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

✨ Introduces motion as a state machine

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-04 23:11:59 +02:00
committed by Rune Harlyk
parent c85ac41ebc
commit b66ddc3e81
6 changed files with 118 additions and 123 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/gait/idle_state.h
-8
View File
@@ -1,8 +0,0 @@
#pragma once
#include <gait/state.h>
class IdleState : public GaitState {
protected:
const char *name() const override { return "Idle"; }
};
esp32/include/gait/rest_state.h
+14 -9
View File
@@ -2,17 +2,22 @@
#include <gait/state.h>
class RestState : public GaitState {
class RestState : public MotionState {
protected:
const char *name() const override { return "Rest"; }
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;
body_state.xm = 0;
body_state.ym = KinConfig::default_body_height / 2;
body_state.zm = 0;
body_state.updateFeet(default_feet_pos);
virtual void begin() {
target_body_state.xm = 0;
target_body_state.ym = KinConfig::min_body_height;
target_body_state.zm = 0;
target_body_state.omega = 0;
target_body_state.phi = 0;
target_body_state.psi = 0;
target_body_state.updateFeet(KinConfig::default_feet_positions);
}
void step(body_state_t &body_state, float dt = 0.02f) override {
lerpToBody(body_state);
updateFeet(body_state);
}
};
esp32/include/gait/stand_state.h
+23 -8
View File
@@ -2,16 +2,31 @@
#include <gait/state.h>
class StandState : public GaitState {
class StandState : public MotionState {
protected:
const char *name() const override { return "Stand"; }
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;
body_state.xm = command.ly / 2 / 100;
body_state.zm = command.lx / 2 / 100;
body_state.updateFeet(default_feet_pos);
virtual void begin() {
target_body_state.xm = 0;
target_body_state.ym = KinConfig::min_body_height + 0.5 * KinConfig::body_height_range;
target_body_state.zm = 0;
target_body_state.omega = 0;
target_body_state.phi = 0;
target_body_state.psi = 0;
target_body_state.updateFeet(KinConfig::default_feet_positions);
}
void handleCommand(const CommandMsg &cmd) override {
target_body_state.ym = KinConfig::min_body_height + cmd.h * KinConfig::body_height_range;
target_body_state.psi = cmd.ry * KinConfig::max_pitch;
target_body_state.phi = cmd.rx * KinConfig::max_roll;
target_body_state.xm = cmd.ly * KinConfig::max_body_shift_x;
target_body_state.zm = cmd.lx * KinConfig::max_body_shift_z;
target_body_state.updateFeet(KinConfig::default_feet_positions);
}
void step(body_state_t &body_state, float dt = 0.02f) override {
lerpToBody(body_state);
updateFeet(body_state);
}
};
esp32/include/gait/state.h
+20 -20
View File
@@ -4,35 +4,35 @@
#include <gait/kinematic_constraints.h>
#include <message_types.h>
struct gait_state_t {
float step_height {KinConfig::default_step_height};
float step_x {0};
float step_z {0};
float step_angle {0};
float step_velocity {0.5};
float step_depth {KinConfig::default_step_depth};
};
class GaitState {
class MotionState {
protected:
virtual const char *name() const = 0;
virtual const char* name() const = 0;
static constexpr const float (&default_feet_pos)[4][4] = KinConfig::default_feet_positions;
body_state_t target_body_state;
static constexpr float default_smoothing_factor = 0.06f;
gait_state_t gait_state;
void lerpToBody(body_state_t& body_state, const float smoothing_factor = default_smoothing_factor) {
body_state.xm = lerp(body_state.xm, target_body_state.xm, smoothing_factor);
body_state.ym = lerp(body_state.ym, target_body_state.ym, smoothing_factor);
body_state.zm = lerp(body_state.zm, target_body_state.zm, smoothing_factor);
body_state.phi = lerp(body_state.phi, target_body_state.phi, smoothing_factor);
body_state.psi = lerp(body_state.psi, target_body_state.psi, smoothing_factor);
}
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;
void updateFeet(body_state_t& body_state, const float smoothing_factor = default_smoothing_factor) {
if (target_body_state.feet != body_state.feet) {
body_state.updateFeet(target_body_state.feet);
}
}
public:
virtual ~MotionState() {}
virtual void begin() { ESP_LOGI("Gait Planner", "Starting %s", name()); }
virtual void end() { ESP_LOGI("Gait Planner", "Ending %s", name()); }
virtual void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) { this->mapCommand(command); }
virtual void handleCommand(const CommandMsg& cmd) {}
virtual void step(body_state_t& body_state, float dt = 0.02f) {}
};
esp32/include/gait/walk_state.h
+25 -3
View File
@@ -5,9 +5,18 @@
#include <array>
#include <functional>
struct gait_state_t {
float step_height {KinConfig::default_step_height};
float step_x {0};
float step_z {0};
float step_angle {0};
float step_velocity {0.5};
float step_depth {KinConfig::default_step_depth};
};
enum class WALK_GAIT { TROT, CRAWL };
class WalkState : public GaitState {
class WalkState : public MotionState {
private:
WALK_GAIT mode = WALK_GAIT::TROT;
float phase_time = 0.0f;
@@ -15,6 +24,7 @@ class WalkState : public GaitState {
float stand_offset = 0.6f;
float step_length = 0.0f;
float speed_factor = 1;
gait_state_t gait_state;
struct ShiftState {
float start_x = 0.0f;
@@ -75,8 +85,9 @@ class WalkState : public GaitState {
for (int i = 0; i < 4; ++i) phase_offset[i] = std::fmod(std::fabs(offsets[i]), 1.f);
}
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
this->mapCommand(command);
void step(body_state_t &body_state, float dt = 0.02f) override {
body_state.ym = lerp(body_state.ym, target_body_state.ym, default_smoothing_factor);
body_state.psi = lerp(body_state.psi, target_body_state.psi, default_smoothing_factor);
step_length = std::hypot(gait_state.step_x, gait_state.step_z);
if (gait_state.step_x < 0.0f) step_length = -step_length;
updatePhase(dt);
@@ -85,6 +96,17 @@ class WalkState : public GaitState {
}
protected:
void handleCommand(const CommandMsg &cmd) override {
target_body_state.ym = KinConfig::min_body_height + cmd.h * KinConfig::body_height_range;
target_body_state.psi = cmd.ry * KinConfig::max_pitch;
gait_state.step_height = cmd.s1 * KinConfig::max_step_height;
gait_state.step_x = cmd.ly * KinConfig::max_step_length;
gait_state.step_z = -cmd.lx * KinConfig::max_step_length;
gait_state.step_velocity = cmd.s;
gait_state.step_angle = cmd.rx;
gait_state.step_depth = KinConfig::default_step_depth;
}
void updatePhase(float dt) {
if (gait_state.step_x == 0 && gait_state.step_z == 0 && gait_state.step_angle == 0) {
phase_time = 0;
esp32/include/motion.h
+36 -75
View File
@@ -9,13 +9,14 @@
#include <gait/state.h>
#include <gait/walk_state.h>
#include <gait/stand_state.h>
#include <gait/rest_state.h>
#include <gait/kinematic_constraints.h>
#include <message_types.h>
#define DEFAULT_STATE false
#define ANGLES_EVENT "angles"
#define INPUT_EVENT "input"
#define POSITION_EVENT "position"
#define MODE_EVENT "mode"
#define WALK_GAIT_EVENT "walk_gait"
@@ -35,8 +36,6 @@ class MotionService {
socket.onEvent(ANGLES_EVENT, [&](JsonVariant &root, int originId) { anglesEvent(root, originId); });
socket.onEvent(POSITION_EVENT, [&](JsonVariant &root, int originId) { positionEvent(root, originId); });
socket.onSubscribe(ANGLES_EVENT,
std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2));
@@ -51,40 +50,18 @@ class MotionService {
syncAngles(String(originId));
}
void positionEvent(JsonVariant &root, int originId) {
JsonArray array = root.as<JsonArray>();
body_state.omega = array[0];
body_state.phi = array[1];
body_state.psi = array[2];
body_state.xm = array[3];
body_state.ym = array[4];
body_state.zm = array[5];
void setState(MotionState *newState) {
_servoController->activate();
if (state) {
state->end();
}
state = newState;
if (state) state->begin();
}
void handleInput(JsonVariant &root, int originId) {
command.fromJson(root);
target_body_state.ym = KinConfig::min_body_height + command.h * KinConfig::body_height_range;
target_body_state.psi = command.ry * KinConfig::max_pitch;
switch (motionState) {
case MOTION_STATE::STAND: {
target_body_state.phi = command.rx * KinConfig::max_roll;
target_body_state.xm = command.ly * KinConfig::max_body_shift_x;
target_body_state.zm = command.lx * KinConfig::max_body_shift_z;
body_state.updateFeet(KinConfig::default_feet_positions);
break;
}
case MOTION_STATE::WALK: {
gait_state.step_height = command.s1 * KinConfig::max_step_height;
gait_state.step_x = command.ly * KinConfig::max_step_length;
gait_state.step_z = -command.lx * KinConfig::max_step_length;
gait_state.step_velocity = command.s;
gait_state.step_angle = command.rx;
gait_state.step_depth = KinConfig::default_step_depth;
break;
}
}
if (state) state->handleCommand(command);
}
void handleWalkGait(JsonVariant &root, int originId) {
@@ -92,17 +69,26 @@ class MotionService {
WALK_GAIT walkGait = static_cast<WALK_GAIT>(root.as<int>());
if (walkGait == WALK_GAIT::TROT)
this->walkGait.set_mode_trot();
walkState.set_mode_trot();
else
this->walkGait.set_mode_crawl();
walkState.set_mode_crawl();
}
void handleMode(JsonVariant &root, int originId) {
motionState = static_cast<MOTION_STATE>(root.as<int>());
ESP_LOGV("MotionService", "Mode %d", motionState);
motionState == MOTION_STATE::DEACTIVATED ? _servoController->deactivate() : _servoController->activate();
MOTION_STATE mode = static_cast<MOTION_STATE>(root.as<int>());
ESP_LOGV("MotionService", "Mode %d", mode);
switch (mode) {
case MOTION_STATE::REST: setState(&restState); break;
case MOTION_STATE::STAND: setState(&standState); break;
case MOTION_STATE::WALK: setState(&walkState); break;
case MOTION_STATE::DEACTIVATED:
setState(nullptr);
_servoController->deactivate();
break;
default: setState(nullptr); break;
}
JsonDocument doc;
doc.set(static_cast<int>(motionState));
doc.set(static_cast<int>(mode));
JsonVariant data = doc.as<JsonVariant>();
socket.emit(MODE_EVENT, data, String(originId).c_str());
}
@@ -121,27 +107,10 @@ class MotionService {
}
bool updateMotion() {
switch (motionState) {
case MOTION_STATE::DEACTIVATED: return false;
case MOTION_STATE::IDLE: return false;
case MOTION_STATE::CALIBRATION: update_angles(calibration_angles, new_angles); break;
case MOTION_STATE::REST: update_angles(rest_angles, new_angles); break;
case MOTION_STATE::STAND: {
body_state.xm = lerp(body_state.xm, target_body_state.xm, smoothing_factor);
body_state.ym = lerp(body_state.ym, target_body_state.ym, smoothing_factor);
body_state.zm = lerp(body_state.zm, target_body_state.zm, smoothing_factor);
body_state.phi = lerp(body_state.phi, target_body_state.phi + _peripherals->angleY(), smoothing_factor);
body_state.psi = lerp(body_state.psi, target_body_state.psi + _peripherals->angleX(), smoothing_factor);
kinematics.calculate_inverse_kinematics(body_state, new_angles);
break;
}
case MOTION_STATE::WALK:
body_state.ym = lerp(body_state.ym, target_body_state.ym, smoothing_factor);
body_state.psi = lerp(body_state.psi, target_body_state.psi + _peripherals->angleX(), smoothing_factor);
walkGait.step(body_state, command);
kinematics.calculate_inverse_kinematics(body_state, new_angles);
break;
}
if (!state) return false;
state->step(body_state);
kinematics.calculate_inverse_kinematics(body_state, new_angles);
return update_angles(new_angles, angles);
}
@@ -163,31 +132,23 @@ class MotionService {
ServoController *_servoController;
Peripherals *_peripherals;
Kinematics kinematics;
WalkState walkGait;
CommandMsg command = {0, 0, 0, 0, 0, 0, 0};
friend class GaitState;
friend class MotionState;
MOTION_STATE motionState = MOTION_STATE::DEACTIVATED;
unsigned long _lastUpdate;
MotionState *state = nullptr;
RestState restState;
StandState standState;
WalkState walkState;
body_state_t target_body_state;
body_state_t body_state;
gait_state_t gait_state;
const float smoothing_factor = 0.06f;
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};
float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1};
#if defined(SPOTMICRO_ESP32) || defined(SPOTMICRO_ESP32_MINI)
float rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
float calibration_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
#elif defined(SPOTMICRO_YERTLE)
float rest_angles[12] = {0, 45, -45, 0, 45, -45, 0, 45, -45, 0, 45, -45};
float calibration_angles[12] = {0, 90, 0, 0, 90, 0, 0, 90, 0, 0, 90, 0};
#endif
};
#endif