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

🎨 Cleanup crawl

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-03 16:18:36 +02:00
committed by Rune Harlyk
parent 6f46c1f598
commit 774c546487
1 changed files with 25 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/gait/walk_state.h
+25 -10
View File
@@ -69,25 +69,30 @@ class WalkState : public GaitState {
void updatePhase(float dt) { phase_time = std::fmod(phase_time + dt * gait_state.step_velocity * 2, 1.0f); } void updatePhase(float dt) { phase_time = std::fmod(phase_time + dt * gait_state.step_velocity * 2, 1.0f); }
void updateBodyPosition(body_state_t &body_state, float dt) { void updateBodyPosition(body_state_t &body_state, float dt) {
if (mode != WALK_GAIT::CRAWL) return;
const bool moving = gait_state.step_x != 0.f || gait_state.step_z != 0.f || gait_state.step_angle != 0.f; const bool moving = gait_state.step_x != 0.f || gait_state.step_z != 0.f || gait_state.step_angle != 0.f;
if (!moving) return; if (!moving) return;
const auto c = dynamicStanceCentroid(body_state); const auto c = dynamicStanceCentroid(body_state);
const float k = mode == WALK_GAIT::CRAWL ? 16.f : 10.f; const float a = 1.f - std::exp(-16.f * dt);
const float a = 1.f - std::exp(-k * dt);
body_state.xm += (c[0] - body_state.xm) * a; body_state.xm += (c[0] - body_state.xm) * a;
body_state.zm += (c[2] - body_state.zm) * a; body_state.zm += (c[2] - body_state.zm) * a;
} }
std::array<float, 3> dynamicStanceCentroid(const body_state_t &body_state) const { std::array<float, 3> dynamicStanceCentroid(const body_state_t &body_state) const {
float sx = 0.f, sz = 0.f, sw = 0.f; if (mode != WALK_GAIT::CRAWL) return {body_state.xm, 0.f, body_state.zm};
float sx = 0.f, sz = 0.f;
int n = 0;
for (int i = 0; i < 4; ++i) { for (int i = 0; i < 4; ++i) {
const float w = stanceWeight(i); float p = std::fmod(phase_time + phase_offset[i], 1.f);
sx += body_state.feet[i][0] * w; if (p < 0.f) p += 1.f;
sz += body_state.feet[i][2] * w; if (p <= stand_offset) {
sw += w; sx += default_feet_pos[i][0];
sz += default_feet_pos[i][2];
++n;
}
} }
if (sw == 0.f) return {body_state.xm, 0.f, body_state.zm}; if (n == 0) return {body_state.xm, 0.f, body_state.zm};
return {sx / sw, 0.f, sz / sw}; return {sx / n, 0.f, sz / n};
} }
static float smoothstep01(float t) { static float smoothstep01(float t) {
@@ -127,13 +132,23 @@ class WalkState : public GaitState {
} }
void standController(body_state_t &body_state, const int index, const float phase) { void standController(body_state_t &body_state, const int index, const float phase) {
controller(index, body_state, phase, stanceCurve, &gait_state.step_depth); if (mode == WALK_GAIT::CRAWL)
controller(index, body_state, phase, stanceCurveFlat, &gait_state.step_depth);
else
controller(index, body_state, phase, stanceCurve, &gait_state.step_depth);
} }
void swingController(body_state_t &body_state, const int index, const float phase) { void swingController(body_state_t &body_state, const int index, const float phase) {
controller(index, body_state, phase, bezierCurve, &gait_state.step_height); controller(index, body_state, phase, bezierCurve, &gait_state.step_height);
} }
static void stanceCurveFlat(const float length, const float angle, const float *, const float phase, float *point) {
float step = length * (1.0f - 2.0f * phase);
point[0] += step * std::cos(angle);
point[2] += step * std::sin(angle);
point[1] += 0.f;
}
void controller(const int index, body_state_t &body_state, const float phase, void controller(const int index, body_state_t &body_state, const float phase,
std::function<void(float, float, float *, float, float *)> curve, float *arg) { std::function<void(float, float, float *, float, float *)> curve, float *arg) {
float delta_pos[3] = {0}; float delta_pos[3] = {0};