🎨 Renames states folder

2025-09-04 23:18:57 +02:00
parent b66ddc3e81
commit f82fa051f2
7 changed files with 10 additions and 10 deletions
@@ -0,0 +1,38 @@
+#pragma once
+
+#include <kinematics.h>
+#include <motion_states/kinematic_constraints.h>
+#include <message_types.h>
+
+class MotionState {
+  protected:
+    virtual const char* name() const = 0;
+    static constexpr const float (&default_feet_pos)[4][4] = KinConfig::default_feet_positions;
+    body_state_t target_body_state;
+    static constexpr float default_smoothing_factor = 0.06f;
+
+    void lerpToBody(body_state_t& body_state, const float smoothing_factor = default_smoothing_factor) {
+        body_state.xm = lerp(body_state.xm, target_body_state.xm, smoothing_factor);
+        body_state.ym = lerp(body_state.ym, target_body_state.ym, smoothing_factor);
+        body_state.zm = lerp(body_state.zm, target_body_state.zm, smoothing_factor);
+        body_state.phi = lerp(body_state.phi, target_body_state.phi, smoothing_factor);
+        body_state.psi = lerp(body_state.psi, target_body_state.psi, smoothing_factor);
+    }
+
+    void updateFeet(body_state_t& body_state, const float smoothing_factor = default_smoothing_factor) {
+        if (target_body_state.feet != body_state.feet) {
+            body_state.updateFeet(target_body_state.feet);
+        }
+    }
+
+  public:
+    virtual ~MotionState() {}
+
+    virtual void begin() { ESP_LOGI("Gait Planner", "Starting %s", name()); }
+
+    virtual void end() { ESP_LOGI("Gait Planner", "Ending %s", name()); }
+
+    virtual void handleCommand(const CommandMsg& cmd) {}
+
+    virtual void step(body_state_t& body_state, float dt = 0.02f) {}
+};