From a3e4fdd8a5eefedfa2c8b72510bd1d92c55110b3 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Fri, 5 Sep 2025 11:48:37 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=8E=A8=20Moves=20kinematics=20config=20to?= =?UTF-8?q?=20kinematics=20file?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- esp32/include/kinematics.h | 58 ++++++++++++++++- esp32/include/motion.h | 1 - .../motion_states/kinematic_constraints.h | 62 ------------------- esp32/include/motion_states/state.h | 1 - 4 files changed, 57 insertions(+), 65 deletions(-) delete mode 100644 esp32/include/motion_states/kinematic_constraints.h diff --git a/esp32/include/kinematics.h b/esp32/include/kinematics.h index 953fafd..865dfbd 100644 --- a/esp32/include/kinematics.h +++ b/esp32/include/kinematics.h @@ -2,7 +2,63 @@ #define Kinematics_h #include -#include + +class KinConfig { + public: +#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 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 = 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; + 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; +#endif + + 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}}; + + static constexpr float default_feet_positions[4][4] = { + {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 = W / 3; + static constexpr float max_body_shift_z = W / 3; + + 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 = max_leg_reach * 0.8; + static constexpr float max_step_height = max_leg_reach / 2; + + // Default constant + static constexpr float default_step_depth = 0.002; + static constexpr float default_body_height = min_body_height + body_height_range / 2; + static constexpr float default_step_height = default_body_height / 2; +}; struct alignas(16) body_state_t { float omega {0}, phi {0}, psi {0}, xm {0}, ym {KinConfig::default_body_height}, zm {0}; diff --git a/esp32/include/motion.h b/esp32/include/motion.h index f0e5f5b..a3114b0 100644 --- a/esp32/include/motion.h +++ b/esp32/include/motion.h @@ -11,7 +11,6 @@ #include #include #include -#include #include #define DEFAULT_STATE false diff --git a/esp32/include/motion_states/kinematic_constraints.h b/esp32/include/motion_states/kinematic_constraints.h deleted file mode 100644 index 10f5969..0000000 --- a/esp32/include/motion_states/kinematic_constraints.h +++ /dev/null @@ -1,62 +0,0 @@ -#pragma once - -#include -#include -#include - -class KinConfig { - public: -#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 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 = 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; - 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; -#endif - - 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}}; - - static constexpr float default_feet_positions[4][4] = { - {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 = W / 3; - static constexpr float max_body_shift_z = W / 3; - - 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 = max_leg_reach * 0.8; - static constexpr float max_step_height = max_leg_reach / 2; - - // Default constant - static constexpr float default_step_depth = 0.002; - 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/motion_states/state.h b/esp32/include/motion_states/state.h index 432f3a0..02c22a1 100644 --- a/esp32/include/motion_states/state.h +++ b/esp32/include/motion_states/state.h @@ -1,7 +1,6 @@ #pragma once #include -#include #include class MotionState {