From 767d1157df0e910ba00ba9f06679d80d9a93777f Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Thu, 4 Sep 2025 19:08:54 +0200 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20Makes=20kinematics=20params=20be=20?= =?UTF-8?q?based=20on=20config?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- esp32/include/gait/kinematic_constraints.h | 30 +++++++++------- esp32/include/gait/state.h | 2 +- esp32/include/kinematics.h | 42 +++++----------------- esp32/include/motion.h | 10 +++--- 4 files changed, 31 insertions(+), 53 deletions(-) diff --git a/esp32/include/gait/kinematic_constraints.h b/esp32/include/gait/kinematic_constraints.h index 23359a0..a9277c4 100644 --- a/esp32/include/gait/kinematic_constraints.h +++ b/esp32/include/gait/kinematic_constraints.h @@ -16,10 +16,10 @@ class KinConfig { #elif defined(SPOTMICRO_ESP32_MINI) static constexpr float coxa = 35.0f / 100.0f; static constexpr float coxa_offset = 0.0f / 100.0f; - static constexpr float femur = 52.0f / 100.0f; - static constexpr float tibia = 65.0f / 100.0f; - static constexpr float L = 120.0f / 100.0f; - static constexpr float W = 78.5f / 100.0f; + static constexpr float femur = 60.0f / 100.0f; + static constexpr float tibia = 60.0f / 100.0f; + static constexpr float L = 160.0f / 100.0f; + static constexpr float W = 80.0f / 100.0f; #elif defined(SPOTMICRO_YERTLE) static constexpr float coxa = 35.0f / 100.0f; static constexpr float coxa_offset = 0.0f; @@ -33,25 +33,29 @@ class KinConfig { {L / 2, 0, W / 2}, {L / 2, 0, -W / 2}, {-L / 2, 0, W / 2}, {-L / 2, 0, -W / 2}}; static constexpr float default_feet_positions[4][4] = { - {mountOffsets[0][0], -1, mountOffsets[0][2] + coxa, 1}, - {mountOffsets[1][0], -1, mountOffsets[1][2] - coxa, 1}, - {mountOffsets[2][0], -1, mountOffsets[2][2] + coxa, 1}, - {mountOffsets[3][0], -1, mountOffsets[3][2] - coxa, 1}, + {mountOffsets[0][0], 0, mountOffsets[0][2] + coxa, 1}, + {mountOffsets[1][0], 0, mountOffsets[1][2] - coxa, 1}, + {mountOffsets[2][0], 0, mountOffsets[2][2] + coxa, 1}, + {mountOffsets[3][0], 0, mountOffsets[3][2] - coxa, 1}, }; // Max constants static constexpr float max_roll = 15 * (float)M_PI_2; static constexpr float max_pitch = 15 * (float)M_PI_2; - static constexpr float max_body_shift_x = 0.25; - static constexpr float max_body_shift_z = 0.25; + static constexpr float max_body_shift_x = W / 3; + static constexpr float max_body_shift_z = W / 3; - static constexpr float max_body_height = 1; + static constexpr float max_leg_reach = femur + tibia - coxa_offset; + + static constexpr float min_body_height = max_leg_reach * 0.45; + static constexpr float max_body_height = max_leg_reach * 0.9; + static constexpr float body_height_range = max_body_height - min_body_height; static constexpr float max_step_length = 1; // Default constant static constexpr float default_step_depth = 0.002; - static constexpr float default_body_height = 0.5; - static constexpr float default_step_height = 0.4; + static constexpr float default_body_height = min_body_height + body_height_range / 2; + static constexpr float default_step_height = default_body_height / 2; }; diff --git a/esp32/include/gait/state.h b/esp32/include/gait/state.h index b08a599..e65d072 100644 --- a/esp32/include/gait/state.h +++ b/esp32/include/gait/state.h @@ -16,7 +16,7 @@ struct gait_state_t { class GaitState { protected: virtual const char *name() const = 0; - static constexpr const float (&default_feet_pos)[4][4] = Kinematics::default_feet_positions; + static constexpr const float (&default_feet_pos)[4][4] = KinConfig::default_feet_positions; gait_state_t gait_state; diff --git a/esp32/include/kinematics.h b/esp32/include/kinematics.h index dcb0de5..f27a3fd 100644 --- a/esp32/include/kinematics.h +++ b/esp32/include/kinematics.h @@ -2,9 +2,10 @@ #define Kinematics_h #include +#include struct alignas(16) body_state_t { - float omega, phi, psi, xm, ym, zm; + float omega {0}, phi {0}, psi {0}, xm {0}, ym {KinConfig::default_body_height}, zm {0}; float feet[4][4]; void updateFeet(const float newFeet[4][4]) { COPY_2D_ARRAY_4x4(feet, newFeet); } @@ -21,33 +22,13 @@ struct alignas(16) body_state_t { class Kinematics { private: -#if defined(SPOTMICRO_ESP32) - static constexpr float coxa = 60.5f / 100.0f; - static constexpr float coxa_offset = 10.0f / 100.0f; - static constexpr float femur = 111.2f / 100.0f; - static constexpr float tibia = 118.5f / 100.0f; + static constexpr float coxa = KinConfig::coxa; + static constexpr float coxa_offset = KinConfig::coxa_offset; + static constexpr float femur = KinConfig::femur; + static constexpr float tibia = KinConfig::tibia; - static constexpr float L = 207.5f / 100.0f; - static constexpr float W = 78.0f / 100.0f; -#elif defined(SPOTMICRO_ESP32_MINI) - static constexpr float coxa = 35.0f / 100.0f; - static constexpr float coxa_offset = 0.0f / 100.0f; - static constexpr float femur = 52.0f / 100.0f; - static constexpr float tibia = 65.0f / 100.0f; - - static constexpr float L = 120.0f / 100.0f; - static constexpr float W = 78.5f / 100.0f; -#elif defined(SPOTMICRO_YERTLE) - static constexpr float coxa = 35.0f / 100.0f; - static constexpr float coxa_offset = 0.0f; - static constexpr float femur = 130.0f / 100.0f; - static constexpr float tibia = 130.0f / 100.0f; - - static constexpr float L = 240.0f / 100.0f; - static constexpr float W = 78.0f / 100.0f; -#else -#error "Must define either SPOTMICRO_ESP32 or SPOTMICRO_YERTLE" -#endif + static constexpr float L = KinConfig::L; + static constexpr float W = KinConfig::W; static constexpr float mountOffsets[4][3] = { {L / 2, 0, W / 2}, {L / 2, 0, -W / 2}, {-L / 2, 0, W / 2}, {-L / 2, 0, -W / 2}}; @@ -61,13 +42,6 @@ class Kinematics { body_state_t currentState; public: - static constexpr float default_feet_positions[4][4] = { - {mountOffsets[0][0], -1, mountOffsets[0][2] + coxa, 1}, - {mountOffsets[1][0], -1, mountOffsets[1][2] - coxa, 1}, - {mountOffsets[2][0], -1, mountOffsets[2][2] + coxa, 1}, - {mountOffsets[3][0], -1, mountOffsets[3][2] - coxa, 1}, - }; - esp_err_t calculate_inverse_kinematics(const body_state_t body_state, float result[12]) { esp_err_t ret = ESP_OK; diff --git a/esp32/include/motion.h b/esp32/include/motion.h index 86dd639..c7e74eb 100644 --- a/esp32/include/motion.h +++ b/esp32/include/motion.h @@ -39,7 +39,7 @@ class MotionService { socket.onSubscribe(ANGLES_EVENT, std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2)); - body_state.updateFeet(kinematics.default_feet_positions); + body_state.updateFeet(KinConfig::default_feet_positions); } void anglesEvent(JsonVariant &root, int originId) { @@ -63,7 +63,7 @@ class MotionService { void handleInput(JsonVariant &root, int originId) { command.fromJson(root); - body_state.ym = command.h - 0.5f; + body_state.ym = KinConfig::min_body_height + command.h * KinConfig::body_height_range; switch (motionState) { case MOTION_STATE::STAND: { @@ -71,7 +71,7 @@ class MotionService { body_state.psi = command.ry * KinConfig::max_pitch; body_state.xm = command.ly * KinConfig::max_body_shift_x; body_state.zm = command.lx * KinConfig::max_body_shift_z; - body_state.updateFeet(kinematics.default_feet_positions); + body_state.updateFeet(KinConfig::default_feet_positions); break; } case MOTION_STATE::WALK: { @@ -159,8 +159,8 @@ 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}; + body_state_t target_body_state; + body_state_t body_state; gait_state_t gait_state; float new_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};