Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

🎨 Updates and simplifies command handling

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-01 18:41:59 +02:00
parent de3912ff10
commit e5bf10cdb0
14 changed files with 111 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/components/Visualization.svelte
+9 -10
View File
@@ -33,7 +33,7 @@
import SceneBuilder from '$lib/sceneBuilder'
import { lerp, degToRad } from 'three/src/math/MathUtils'
import { GUI } from 'three/addons/libs/lil-gui.module.min.js'
import Kinematic, { type body_state_t } from '$lib/kinematic'
import { type body_state_t } from '$lib/kinematic'
import {
BezierState,
CalibrationState,
@@ -264,16 +264,15 @@
if (sceneManager.isDragging || !settings['Internal kinematic']) return
const controlData = get(outControllerData)
const data = {
stop: controlData[0],
lx: controlData[1],
ly: controlData[2],
rx: controlData[3],
ry: controlData[4],
h: controlData[5],
s: controlData[6],
s1: controlData[7]
lx: controlData[0],
ly: controlData[1],
rx: controlData[2],
ry: controlData[3],
h: controlData[4],
s: controlData[5],
s1: controlData[6]
}
body_state.ym = ((data.h + 127) * 0.35) / 100
body_state.ym = data.h
let planner = planners[get(mode)]
const delta = performance.now() - lastTick
app/src/lib/gait.ts
+9 -10
View File
@@ -14,7 +14,6 @@ export interface gait_state_t {
}
export interface ControllerCommand {
stop: number
lx: number
ly: number
rx: number
@@ -61,11 +60,11 @@ export abstract class GaitState {
map_command(command: ControllerCommand) {
const newCommand = {
step_height: 0.4 + (command.s1 / 128 + 1) / 2,
step_x: command.ly / 128,
step_z: -command.lx / 128,
step_velocity: command.s / 128 + 1,
step_angle: command.rx / 128,
step_height: 0.4 + (command.s1 + 1) / 2,
step_x: command.ly,
step_z: -command.lx,
step_velocity: command.s,
step_angle: command.rx,
step_depth: 0.002
}
@@ -112,10 +111,10 @@ export class StandState extends GaitState {
step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
body_state.omega = 0
body_state.phi = command.rx / 8
body_state.psi = command.ry / 8
body_state.xm = command.ly / 2 / 100
body_state.zm = command.lx / 2 / 100
body_state.phi = command.rx
body_state.psi = command.ry
body_state.xm = command.ly / 4
body_state.zm = command.lx / 4
body_state.feet = this.default_feet_pos
return body_state
}
app/src/routes/controller/Controls.svelte
+18 -13
View File
@@ -1,11 +1,11 @@
<script lang="ts">
import nipplejs from 'nipplejs'
import { onMount } from 'svelte'
import { capitalize, throttler, toInt8 } from '$lib/utilities'
import { capitalize, throttler } from '$lib/utilities'
import { input, outControllerData, mode, modes, type Modes, ModesEnum } from '$lib/stores'
import type { vector } from '$lib/types/models'
import { VerticalSlider } from '$lib/components/input'
import { gamepadAxes, gamepadButtons, hasGamepad } from '$lib/stores/gamepad'
import { gamepadAxes, hasGamepad } from '$lib/stores/gamepad'
import { notifications } from '$lib/components/toasts/notifications'
let throttle = new throttler()
@@ -64,14 +64,13 @@
}
const updateData = () => {
data[0] = 0
data[1] = toInt8($input.left.x, -1, 1)
data[2] = toInt8($input.left.y, -1, 1)
data[3] = toInt8($input.right.x, -1, 1)
data[4] = toInt8($input.right.y, -1, 1)
data[5] = toInt8($input.height, 0, 100)
data[6] = toInt8($input.speed, 0, 100)
data[7] = toInt8($input.s1, 0, 100)
data[0] = $input.left.x
data[1] = $input.left.y
data[2] = $input.right.x
data[3] = $input.right.y
data[4] = $input.height
data[5] = $input.speed
data[6] = $input.s1
outControllerData.set(data)
}
@@ -122,7 +121,11 @@
</div>
<div class="absolute bottom-0 z-10 flex items-end">
<div class="flex items-center flex-col bg-base-300 bg-opacity-50 p-3 pb-2 gap-2 rounded-tr-xl">
<VerticalSlider min={0} max={100} oninput={(e: Event) => handleRange(e, 'height')} />
<VerticalSlider
min={0}
max={1}
step={0.01}
oninput={(e: Event) => handleRange(e, 'height')} />
<label for="height">Ht</label>
</div>
<div
@@ -146,7 +149,8 @@
type="range"
name="s1"
min="0"
max="25"
step="0.01"
max="1"
oninput={e => handleRange(e, 's1')}
class="range range-sm range-primary" />
</div>
@@ -156,7 +160,8 @@
type="range"
name="speed"
min="0"
max="25"
step="0.01"
max="1"
oninput={e => handleRange(e, 'speed')}
class="range range-sm range-primary" />
</div>
esp32/include/gait/bezier_state.h
+7 -6
View File
@@ -36,7 +36,7 @@ class BezierState : public GaitState {
public:
const char *name() const override { return "Bezier"; }
void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
this->mapCommand(command);
step_length = std::hypot(gait_state.step_x, gait_state.step_z);
if (gait_state.step_x < 0.0f) {
@@ -78,8 +78,8 @@ class BezierState : public GaitState {
float delta_pos[3] = {0};
float delta_rot[3] = {0};
float length = step_length / 2.0f;
float angle = std::atan2(gait_state.step_z, step_length) * 2;
float length = step_length * 0.5f;
float angle = std::atan2(gait_state.step_z, step_length);
curve(length, angle, arg, phase, delta_pos);
length = gait_state.step_angle * 2.0f;
@@ -107,9 +107,10 @@ class BezierState : public GaitState {
const float X_POLAR = std::cos(angle);
const float Z_POLAR = std::sin(angle);
const float t = std::clamp(phase, 1e-4f, 1.f - 1e-4f);
float phase_power = 1.0f;
float inv_phase_power = std::pow(1.0f - phase, 11);
const float one_minus_phase = 1.0f - phase;
float inv_phase_power = std::pow(1.0f - t, 11);
const float one_minus_phase = 1.0f - t;
for (int i = 0; i < 12; i++) {
float b = COMBINATORIAL_VALUES[i] * phase_power * inv_phase_power;
@@ -122,7 +123,7 @@ class BezierState : public GaitState {
}
}
static float yawArc(const float feet_pos[4], const float *current_pos) {
static float yawArc(const float feet_pos[3], const float *current_pos) {
const float foot_mag = std::hypot(feet_pos[0], feet_pos[2]);
const float foot_dir = std::atan2(feet_pos[2], feet_pos[0]);
const float offsets[] = {current_pos[0] - feet_pos[0], current_pos[1] - feet_pos[1],
esp32/include/gait/crawl_state.h
+1 -1
View File
@@ -27,7 +27,7 @@ class EightPhaseWalkState : public PhaseGaitState {
}
}
void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
return PhaseGaitState::step(body_state, command, dt);
}
};
esp32/include/gait/four_phase_trot_state.h
+1 -1
View File
@@ -22,7 +22,7 @@ class FourPhaseWalkState : public PhaseGaitState {
}
}
void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
return PhaseGaitState::step(body_state, command, dt);
}
};
esp32/include/gait/phase_state_base.h
+1 -1
View File
@@ -14,7 +14,7 @@ class PhaseGaitState : public GaitState {
uint8_t contact_phases[4][8];
float shifts[4][3];
void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
mapCommand(command);
this->dt = dt;
updatePhase();
esp32/include/gait/rest_state.h
+1 -1
View File
@@ -6,7 +6,7 @@ class RestState : public GaitState {
protected:
const char *name() const override { return "Rest"; }
void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
body_state.omega = 0;
body_state.phi = 0;
body_state.psi = 0;
esp32/include/gait/stand_state.h
+1 -1
View File
@@ -6,7 +6,7 @@ class StandState : public GaitState {
protected:
const char *name() const override { return "Stand"; }
void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
body_state.omega = 0;
body_state.phi = command.rx / 8;
body_state.psi = command.ry / 8;
esp32/include/gait/state.h
+9 -15
View File
@@ -1,6 +1,7 @@
#pragma once
#include <kinematics.h>
#include <message_types.h>
struct gait_state_t {
float step_height;
@@ -11,23 +12,18 @@ struct gait_state_t {
float step_depth;
};
struct ControllerCommand {
int stop;
float lx, ly, rx, ry, h, s, s1;
};
class GaitState {
protected:
virtual const char *name() const = 0;
float default_feet_pos[4][4] = {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}};
static constexpr const float (&default_feet_pos)[4][4] = Kinematics::default_feet_positions;
gait_state_t gait_state = {0.4, 0, 0, 0, 1, 0.002};
void mapCommand(ControllerCommand command) {
this->gait_state.step_height = 0.4 + (command.s1 / 128 + 1) / 2;
this->gait_state.step_x = command.ly / 128;
this->gait_state.step_z = -command.lx / 128;
this->gait_state.step_velocity = command.s / 128 + 1;
this->gait_state.step_angle = command.rx / 128;
virtual void mapCommand(CommandMsg command) {
this->gait_state.step_height = command.s1 / 2;
this->gait_state.step_x = command.ly;
this->gait_state.step_z = -command.lx;
this->gait_state.step_velocity = command.s;
this->gait_state.step_angle = command.rx;
this->gait_state.step_depth = 0.002;
}
@@ -38,7 +34,5 @@ class GaitState {
virtual void end() { ESP_LOGI("Gait Planner", "Ending %s", name()); }
virtual void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) {
this->mapCommand(command);
}
virtual void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) { this->mapCommand(command); }
};
esp32/include/kinematics.h
+2 -2
View File
@@ -30,8 +30,8 @@ class Kinematics {
static constexpr float L = 207.5f / 100.0f;
static constexpr float W = 78.0f / 100.0f;
#elif defined(SPOTMICRO_ESP32_MINI)
static constexpr float l1 = 0.0f / 100.0f;
static constexpr float l2 = 0.0f / 100.0f;
static constexpr float l1 = 0.01f / 100.0f;
static constexpr float l2 = 0.01f / 100.0f;
static constexpr float l3 = 52.0f / 100.0f;
static constexpr float l4 = 65.0f / 100.0f;
esp32/include/message_types.h
+28
View File
@@ -0,0 +1,28 @@
#pragma once
#include <ArduinoJson.h>
struct CommandMsg {
float lx, ly, rx, ry, h, s, s1;
friend void toJson(JsonVariant v, CommandMsg const &c) {
JsonArray arr = v.to<JsonArray>();
arr.add(c.lx);
arr.add(c.ly);
arr.add(c.rx);
arr.add(c.ry);
arr.add(c.h);
arr.add(c.s);
arr.add(c.s1);
}
void fromJson(JsonVariantConst o) {
JsonArrayConst arr = o.as<JsonArrayConst>();
lx = arr[0].as<float>();
ly = arr[1].as<float>();
rx = arr[2].as<float>();
ry = arr[3].as<float>();
h = arr[4].as<float>();
s = arr[5].as<float>();
s1 = arr[6].as<float>();
}
};
esp32/include/motion.h
+21 -14
View File
@@ -10,6 +10,7 @@
#include <gait/state.h>
#include <gait/crawl_state.h>
#include <gait/bezier_state.h>
#include <message_types.h>
#define DEFAULT_STATE false
#define ANGLES_EVENT "angles"
@@ -57,26 +58,29 @@ class MotionService {
}
void handleInput(JsonVariant &root, int originId) {
JsonArray array = root.as<JsonArray>();
command.lx = array[1];
command.ly = array[2];
command.rx = array[3];
command.ry = array[4];
command.h = array[5];
command.s = array[6];
command.s1 = array[7];
command.fromJson(root);
body_state.ym = (command.h + 127.f) * 0.35f / 100;
body_state.ym = command.h * 1.f - 0.5f;
switch (motionState) {
case MOTION_STATE::STAND: {
body_state.phi = command.rx / 8;
body_state.psi = command.ry / 8;
body_state.xm = command.ly / 2 / 100;
body_state.zm = command.lx / 2 / 100;
body_state.phi = command.rx; // * 0.254f;
body_state.psi = command.ry; // * 0.254f;
body_state.xm = command.ly / 4;
body_state.zm = command.lx / 4;
body_state.updateFeet(kinematics.default_feet_positions);
break;
}
case MOTION_STATE::CRAWL:
case MOTION_STATE::WALK: {
gait_state.step_height = 0.4 + (command.s1 + 1) / 2;
gait_state.step_x = command.ly;
gait_state.step_z = -command.lx;
gait_state.step_velocity = command.s;
gait_state.step_angle = command.rx;
gait_state.step_depth = 0.002;
break;
}
}
}
@@ -139,7 +143,7 @@ class MotionService {
private:
ServoController *_servoController;
Kinematics kinematics;
ControllerCommand command = {0, 0, 0, 0, 0, 0, 0, 0};
CommandMsg command = {0, 0, 0, 0, 0, 0, 0};
friend class GaitState;
@@ -149,7 +153,10 @@ class MotionService {
MOTION_STATE motionState = MOTION_STATE::DEACTIVATED;
unsigned long _lastUpdate;
body_state_t target_body_state = {0, 0, 0, 0, 0, 0};
body_state_t body_state = {0, 0, 0, 0, 0, 0};
gait_state_t gait_state = {12, 0, 0, 0, 1, 0.002};
float new_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
float angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
esp32/test/gait_performance.cpp
+3 -2
View File
@@ -7,7 +7,7 @@ void test_gaitPlanner_calculateStep_time() {
BezierState gaitPlanner;
body_state_t body_state = {
128, 0, 0, 0, 0, 0, {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}}};
ControllerCommand command = {0, 0, 0, 0, 0, 0, 0, 0};
CommandMsg command = {0, 0, 0, 0, 0, 0, 0};
const int num_steps = 1000;
unsigned long start = millis();
@@ -20,7 +20,8 @@ void test_gaitPlanner_calculateStep_time() {
unsigned long max_duration = num_steps / 2; // Minimum 0.5 ms per step
char message[50];
snprintf(message, sizeof(message), "The step calculation took: %lu ms (%lu ms per iter)", duration, duration / num_steps);
snprintf(message, sizeof(message), "The step calculation took: %lu ms (%lu ms per iter)", duration,
duration / num_steps);
ESP_LOGI("Test planner", message);
TEST_ASSERT_MESSAGE(duration <= max_duration, message);
}