✨ Introduces kinmatics config to sync mapping between variants

esp32/include/gait/kinematic_constraints.h

@@ -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

@@ -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

@@ -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()); }

esp32/include/motion.h

@@ -9,6 +9,7 @@
 
 #include <gait/state.h>
 #include <gait/walk_state.h>
+#include <gait/kinematic_constraints.h>
 #include <message_types.h>
 
 #define DEFAULT_STATE false
@@ -62,24 +63,24 @@ class MotionService {
     void handleInput(JsonVariant &root, int originId) {
         command.fromJson(root);
 
-        body_state.ym = command.h * 1.f - 0.5f;
+        body_state.ym = command.h - 0.5f;
 
         switch (motionState) {
             case MOTION_STATE::STAND: {
-                body_state.phi = command.rx * 10 * (float)M_PI_2;
-                body_state.psi = command.ry * 10 * (float)M_PI_2;
-                body_state.xm = command.ly / 4;
-                body_state.zm = command.lx / 4;
+                body_state.phi = command.rx * KinConfig::max_roll;
+                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);
                 break;
             }
             case MOTION_STATE::WALK: {
-                gait_state.step_height = 0.4 + (command.s1 + 1) / 2;
-                gait_state.step_x = command.ly;
-                gait_state.step_z = -command.lx;
+                gait_state.step_height = KinConfig::default_step_height + command.s1;
+                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 = 0.002;
+                gait_state.step_depth = KinConfig::default_step_depth;
                 break;
             }
         }
@@ -160,7 +161,7 @@ class MotionService {
 
     body_state_t target_body_state = {0, 0, 0, 0, 0, 0};
     body_state_t body_state = {0, 0, 0, 0, 0, 0};
-    gait_state_t gait_state = {12, 0, 0, 0, 1, 0.002};
+    gait_state_t gait_state;
 
     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};