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

✨ Introduces kinmatics config to sync mapping between variants

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-04 18:02:38 +02:00
parent 0b5d7b1534
commit 1799889712
4 changed files with 78 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/gait/kinematic_constraints.h
+57
View File
@@ -0,0 +1,57 @@
#pragma once
#include <kinematics.h>
#include <utils/math_utils.h>
#include <cmath>
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 = 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;
#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], -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},
};
// 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_height = 1;
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;
};
esp32/include/gait/rest_state.h
+1 -1
View File
@@ -11,7 +11,7 @@ class RestState : public GaitState {
body_state.phi = 0;
body_state.psi = 0;
body_state.xm = 0;
body_state.ym = getDefaultHeight() / 2;
body_state.ym = KinConfig::default_body_height / 2;
body_state.zm = 0;
body_state.updateFeet(default_feet_pos);
}
esp32/include/gait/state.h
+9 -9
View File
@@ -1,22 +1,24 @@
#pragma once
#include <kinematics.h>
#include <gait/kinematic_constraints.h>
#include <message_types.h>
struct gait_state_t {
float step_height;
float step_x;
float step_z;
float step_angle;
float step_velocity;
float step_depth;
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 {
protected:
virtual const char *name() const = 0;
static constexpr const float (&default_feet_pos)[4][4] = Kinematics::default_feet_positions;
gait_state_t gait_state = {0.4, 0, 0, 0, 1, 0.002};
gait_state_t gait_state;
virtual void mapCommand(CommandMsg command) {
this->gait_state.step_height = command.s1 / 2;
@@ -28,8 +30,6 @@ class GaitState {
}
public:
virtual float getDefaultHeight() const { return 0.5f; }
virtual void begin() { ESP_LOGI("Gait Planner", "Starting %s", name()); }
virtual void end() { ESP_LOGI("Gait Planner", "Ending %s", name()); }