From b66ddc3e815261ea52aee35dbe5afb80333c0082 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Thu, 4 Sep 2025 23:11:59 +0200 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20Introduces=20motion=20as=20a=20stat?= =?UTF-8?q?e=20machine?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- esp32/include/gait/idle_state.h | 8 --- esp32/include/gait/rest_state.h | 23 ++++--- esp32/include/gait/stand_state.h | 31 ++++++--- esp32/include/gait/state.h | 40 +++++------ esp32/include/gait/walk_state.h | 28 +++++++- esp32/include/motion.h | 111 ++++++++++--------------------- 6 files changed, 118 insertions(+), 123 deletions(-) delete mode 100644 esp32/include/gait/idle_state.h diff --git a/esp32/include/gait/idle_state.h b/esp32/include/gait/idle_state.h deleted file mode 100644 index ddbd763..0000000 --- a/esp32/include/gait/idle_state.h +++ /dev/null @@ -1,8 +0,0 @@ -#pragma once - -#include - -class IdleState : public GaitState { - protected: - const char *name() const override { return "Idle"; } -}; \ No newline at end of file diff --git a/esp32/include/gait/rest_state.h b/esp32/include/gait/rest_state.h index ed4561f..89d76d1 100644 --- a/esp32/include/gait/rest_state.h +++ b/esp32/include/gait/rest_state.h @@ -2,17 +2,22 @@ #include -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); } }; \ No newline at end of file diff --git a/esp32/include/gait/stand_state.h b/esp32/include/gait/stand_state.h index 4631e1a..2ab473f 100644 --- a/esp32/include/gait/stand_state.h +++ b/esp32/include/gait/stand_state.h @@ -2,16 +2,31 @@ #include -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); } }; \ No newline at end of file diff --git a/esp32/include/gait/state.h b/esp32/include/gait/state.h index e65d072..99cc80d 100644 --- a/esp32/include/gait/state.h +++ b/esp32/include/gait/state.h @@ -4,35 +4,35 @@ #include #include -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) {} }; \ No newline at end of file diff --git a/esp32/include/gait/walk_state.h b/esp32/include/gait/walk_state.h index e3c9246..cf97bb8 100644 --- a/esp32/include/gait/walk_state.h +++ b/esp32/include/gait/walk_state.h @@ -5,9 +5,18 @@ #include #include +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; diff --git a/esp32/include/motion.h b/esp32/include/motion.h index 946ef13..15c91d1 100644 --- a/esp32/include/motion.h +++ b/esp32/include/motion.h @@ -9,13 +9,14 @@ #include #include +#include +#include #include #include #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(); - 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(root.as()); 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(root.as()); - ESP_LOGV("MotionService", "Mode %d", motionState); - motionState == MOTION_STATE::DEACTIVATED ? _servoController->deactivate() : _servoController->activate(); + MOTION_STATE mode = static_cast(root.as()); + 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(motionState)); + doc.set(static_cast(mode)); JsonVariant data = doc.as(); 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