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

✨ Makes kinematics params be based on config

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-04 19:08:54 +02:00
parent 1799889712
commit 767d1157df
4 changed files with 31 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/gait/kinematic_constraints.h
+17 -13
View File
@@ -16,10 +16,10 @@ class KinConfig {
#elif defined(SPOTMICRO_ESP32_MINI) #elif defined(SPOTMICRO_ESP32_MINI)
static constexpr float coxa = 35.0f / 100.0f; static constexpr float coxa = 35.0f / 100.0f;
static constexpr float coxa_offset = 0.0f / 100.0f; static constexpr float coxa_offset = 0.0f / 100.0f;
static constexpr float femur = 52.0f / 100.0f; static constexpr float femur = 60.0f / 100.0f;
static constexpr float tibia = 65.0f / 100.0f; static constexpr float tibia = 60.0f / 100.0f;
static constexpr float L = 120.0f / 100.0f; static constexpr float L = 160.0f / 100.0f;
static constexpr float W = 78.5f / 100.0f; static constexpr float W = 80.0f / 100.0f;
#elif defined(SPOTMICRO_YERTLE) #elif defined(SPOTMICRO_YERTLE)
static constexpr float coxa = 35.0f / 100.0f; static constexpr float coxa = 35.0f / 100.0f;
static constexpr float coxa_offset = 0.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}}; {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] = { static constexpr float default_feet_positions[4][4] = {
{mountOffsets[0][0], -1, mountOffsets[0][2] + coxa, 1}, {mountOffsets[0][0], 0, mountOffsets[0][2] + coxa, 1},
{mountOffsets[1][0], -1, mountOffsets[1][2] - coxa, 1}, {mountOffsets[1][0], 0, mountOffsets[1][2] - coxa, 1},
{mountOffsets[2][0], -1, mountOffsets[2][2] + coxa, 1}, {mountOffsets[2][0], 0, mountOffsets[2][2] + coxa, 1},
{mountOffsets[3][0], -1, mountOffsets[3][2] - coxa, 1}, {mountOffsets[3][0], 0, mountOffsets[3][2] - coxa, 1},
}; };
// Max constants // Max constants
static constexpr float max_roll = 15 * (float)M_PI_2; static constexpr float max_roll = 15 * (float)M_PI_2;
static constexpr float max_pitch = 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_x = W / 3;
static constexpr float max_body_shift_z = 0.25; 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; static constexpr float max_step_length = 1;
// Default constant // Default constant
static constexpr float default_step_depth = 0.002; static constexpr float default_step_depth = 0.002;
static constexpr float default_body_height = 0.5; static constexpr float default_body_height = min_body_height + body_height_range / 2;
static constexpr float default_step_height = 0.4; static constexpr float default_step_height = default_body_height / 2;
}; };
esp32/include/gait/state.h
+1 -1
View File
@@ -16,7 +16,7 @@ struct gait_state_t {
class GaitState { class GaitState {
protected: protected:
virtual const char *name() const = 0; 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; gait_state_t gait_state;
esp32/include/kinematics.h
+8 -34
View File
@@ -2,9 +2,10 @@
#define Kinematics_h #define Kinematics_h
#include <utils/math_utils.h> #include <utils/math_utils.h>
#include <gait/kinematic_constraints.h>
struct alignas(16) body_state_t { 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]; float feet[4][4];
void updateFeet(const float newFeet[4][4]) { COPY_2D_ARRAY_4x4(feet, newFeet); } 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 { class Kinematics {
private: private:
#if defined(SPOTMICRO_ESP32) static constexpr float coxa = KinConfig::coxa;
static constexpr float coxa = 60.5f / 100.0f; static constexpr float coxa_offset = KinConfig::coxa_offset;
static constexpr float coxa_offset = 10.0f / 100.0f; static constexpr float femur = KinConfig::femur;
static constexpr float femur = 111.2f / 100.0f; static constexpr float tibia = KinConfig::tibia;
static constexpr float tibia = 118.5f / 100.0f;
static constexpr float L = 207.5f / 100.0f; static constexpr float L = KinConfig::L;
static constexpr float W = 78.0f / 100.0f; static constexpr float W = KinConfig::W;
#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 mountOffsets[4][3] = { 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}}; {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; body_state_t currentState;
public: 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 calculate_inverse_kinematics(const body_state_t body_state, float result[12]) {
esp_err_t ret = ESP_OK; esp_err_t ret = ESP_OK;
esp32/include/motion.h
+5 -5
View File
@@ -39,7 +39,7 @@ class MotionService {
socket.onSubscribe(ANGLES_EVENT, socket.onSubscribe(ANGLES_EVENT,
std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2)); 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) { void anglesEvent(JsonVariant &root, int originId) {
@@ -63,7 +63,7 @@ class MotionService {
void handleInput(JsonVariant &root, int originId) { void handleInput(JsonVariant &root, int originId) {
command.fromJson(root); 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) { switch (motionState) {
case MOTION_STATE::STAND: { case MOTION_STATE::STAND: {
@@ -71,7 +71,7 @@ class MotionService {
body_state.psi = command.ry * KinConfig::max_pitch; body_state.psi = command.ry * KinConfig::max_pitch;
body_state.xm = command.ly * KinConfig::max_body_shift_x; body_state.xm = command.ly * KinConfig::max_body_shift_x;
body_state.zm = command.lx * KinConfig::max_body_shift_z; 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; break;
} }
case MOTION_STATE::WALK: { case MOTION_STATE::WALK: {
@@ -159,8 +159,8 @@ class MotionService {
MOTION_STATE motionState = MOTION_STATE::DEACTIVATED; MOTION_STATE motionState = MOTION_STATE::DEACTIVATED;
unsigned long _lastUpdate; unsigned long _lastUpdate;
body_state_t target_body_state = {0, 0, 0, 0, 0, 0}; body_state_t target_body_state;
body_state_t body_state = {0, 0, 0, 0, 0, 0}; body_state_t body_state;
gait_state_t gait_state; gait_state_t gait_state;
float new_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; float new_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};