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

🎨 Cleans up gait handling code

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-01 22:23:02 +02:00
committed by Rune Harlyk
parent d7a6bffe0a
commit 54a0419770
12 changed files with 246 additions and 363 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/gait/crawl_state.h
-33
View File
@@ -1,33 +0,0 @@
#pragma once
#include <gait/phase_state_base.h>
class EightPhaseWalkState : public PhaseGaitState {
protected:
const char *name() const override { return "Eight phase walk"; }
int num_phases() const override { return 8; }
float phase_speed_factor() const override { return 4; }
float swing_stand_ratio() const override { return 1.0f / (num_phases() - 1); }
public:
EightPhaseWalkState() {
uint8_t contact[4][8] = {
{1, 0, 1, 1, 1, 1, 1, 1}, {1, 1, 1, 1, 1, 0, 1, 1}, {1, 1, 1, 1, 1, 1, 1, 0}, {1, 1, 1, 0, 1, 1, 1, 1}};
float shift_values[4][3] = {{-0.05f, 0, -0.2f}, {0.25f, 0, 0.2f}, {-0.05f, 0, 0.2f}, {0.25f, 0, -0.2f}};
for (uint8_t i = 0; i < 4; ++i) {
for (uint8_t j = 0; j < 8; ++j) {
contact_phases[i][j] = contact[i][j];
}
for (uint8_t j = 0; j < 3; ++j) {
shifts[i][j] = shift_values[i][j];
}
}
}
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
return PhaseGaitState::step(body_state, command, dt);
}
};
esp32/include/gait/four_phase_trot_state.h
-28
View File
@@ -1,28 +0,0 @@
#pragma once
#include <gait/phase_state_base.h>
class FourPhaseWalkState : public PhaseGaitState {
protected:
const char *name() const override { return "Four phase walk"; }
int num_phases() const override { return 4; }
float phase_speed_factor() const override { return 6; }
float swing_stand_ratio() const override { return 1.0f / (num_phases() - 1); }
public:
FourPhaseWalkState() {
uint8_t contact[4][4] = {{1, 0, 1, 1}, {1, 1, 1, 0}, {1, 1, 1, 0}, {1, 0, 1, 1}};
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
contact_phases[i][j] = contact[i][j];
}
}
}
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
return PhaseGaitState::step(body_state, command, dt);
}
};
esp32/include/gait/phase_state_base.h
-78
View File
@@ -1,78 +0,0 @@
#pragma once
#include <gait/state.h>
class PhaseGaitState : public GaitState {
protected:
int phase = 0;
float phase_time = 0;
virtual int num_phases() const = 0;
virtual float phase_speed_factor() const = 0;
virtual float swing_stand_ratio() const = 0;
float dt = 0.02f;
uint8_t contact_phases[4][8];
float shifts[4][3];
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
mapCommand(command);
this->dt = dt;
updatePhase();
updateBodyPosition(body_state);
updateFeetPositions(body_state);
}
void updatePhase() {
phase_time += dt * phase_speed_factor() * gait_state.step_velocity;
if (phase_time >= 1.0f) {
phase += 1;
if (phase == num_phases()) phase = 0;
phase_time = 0;
}
}
void updateBodyPosition(body_state_t &body_state) {
if (num_phases() == 4) return;
const auto &shift = shifts[phase / 2];
body_state.xm += (shift[0] - body_state.xm) * dt * 4;
body_state.zm += (shift[2] - body_state.zm) * dt * 4;
}
void updateFeetPositions(body_state_t &body_state) {
for (int i = 0; i < 4; ++i) {
updateFootPosition(body_state, i);
}
}
void updateFootPosition(body_state_t &body_state, int index) {
bool contact = contact_phases[index][phase];
contact ? stand(body_state, index) : swing(body_state, index);
}
void stand(body_state_t &body_state, int index) {
float delta_pos[3] = {-gait_state.step_x * dt * swing_stand_ratio(), 0,
-gait_state.step_z * dt * swing_stand_ratio()};
body_state.feet[index][0] += delta_pos[0];
body_state.feet[index][1] = default_feet_pos[index][1];
body_state.feet[index][2] += delta_pos[2];
}
void swing(body_state_t &body_state, int index) {
float delta_pos[3] = {gait_state.step_x * dt, 0, gait_state.step_z * dt};
if (std::fabs(gait_state.step_x) < 0.01) {
delta_pos[0] = (default_feet_pos[index][0] - body_state.feet[index][0]) * dt * 8;
}
if (std::fabs(gait_state.step_z) < 0.01) {
delta_pos[2] = (default_feet_pos[index][2] - body_state.feet[index][2]) * dt * 8;
}
body_state.feet[index][0] += delta_pos[0];
body_state.feet[index][1] = default_feet_pos[index][1] + std::sin(phase_time * M_PI) * gait_state.step_height;
body_state.feet[index][2] += delta_pos[2];
}
};
esp32/include/gait/bezier_state.h → esp32/include/gait/walk_state.h
+72 -11
View File
@@ -5,13 +5,18 @@
#include <array>
#include <functional>
class BezierState : public GaitState {
enum class WALK_GAIT { TROT, CRAWL };
class WalkState : public GaitState {
private:
WALK_GAIT mode = WALK_GAIT::TROT;
float phase_time = 0.0f;
static constexpr float PHASE_OFFSET[4] = {0.f, 0.5f, 0.5f, 0.f};
static constexpr float STAND_OFFSET = 0.75f;
static constexpr uint8_t BEZIER_POINTS = 12;
float phase_offset[4] = {0.f, 0.5f, 0.5f, 0.f};
float stand_offset = 0.6f;
float step_length = 0.0f;
float phase_lead = 0.08f;
float feather = 0.05f;
static constexpr uint8_t BEZIER_POINTS = 12;
static constexpr std::array<float, BEZIER_POINTS> COMBINATORIAL_VALUES = {
combinatorial_constexpr(11, 0), // 1
combinatorial_constexpr(11, 1), // 11
@@ -36,6 +41,19 @@ class BezierState : public GaitState {
public:
const char *name() const override { return "Bezier"; }
void set_mode_crawl(float duty = 0.85f, std::array<int, 4> order = {0, 3, 1, 2}) {
mode = WALK_GAIT::CRAWL;
stand_offset = duty;
const float base[4] = {0.f, 0.25f, 0.5f, 0.75f};
for (int i = 0; i < 4; ++i) phase_offset[order[i]] = base[i];
}
void set_mode_trot(float duty = 0.6f, std::array<float, 4> offsets = {0.f, 0.5f, 0.5f, 0.f}) {
mode = WALK_GAIT::TROT;
stand_offset = duty;
for (int i = 0; i < 4; ++i) phase_offset[i] = std::fmod(std::fabs(offsets[i]), 1.f);
}
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
this->mapCommand(command);
step_length = std::hypot(gait_state.step_x, gait_state.step_z);
@@ -43,26 +61,69 @@ class BezierState : public GaitState {
step_length = -step_length;
}
updatePhase(dt);
updateBodyPosition(body_state, dt);
updateFeetPositions(body_state);
}
protected:
void updatePhase(float dt) { phase_time = std::fmod(phase_time + dt * gait_state.step_velocity * 2, 1.0f); }
void updateFeetPositions(body_state_t &body_state) {
void updateBodyPosition(body_state_t &body_state, float dt) {
const bool moving = gait_state.step_x != 0.f || gait_state.step_z != 0.f || gait_state.step_angle != 0.f;
if (!moving) return;
const auto c = dynamicStanceCentroid(body_state);
const float k = mode == WALK_GAIT::CRAWL ? 16.f : 10.f;
const float a = 1.f - std::exp(-k * dt);
body_state.xm += (c[0] - body_state.xm) * a;
body_state.zm += (c[2] - body_state.zm) * a;
}
std::array<float, 3> dynamicStanceCentroid(const body_state_t &body_state) const {
float sx = 0.f, sz = 0.f, sw = 0.f;
for (int i = 0; i < 4; ++i) {
updateFootPosition(body_state, i);
const float w = stanceWeight(i);
sx += body_state.feet[i][0] * w;
sz += body_state.feet[i][2] * w;
sw += w;
}
if (sw == 0.f) return {body_state.xm, 0.f, body_state.zm};
return {sx / sw, 0.f, sz / sw};
}
static float smoothstep01(float t) {
const float x = std::clamp(t, 0.f, 1.f);
return x * x * (3.f - 2.f * x);
}
float stanceWeight(int i) const {
const float s = stand_offset;
const float e = feather;
float p = std::fmod(phase_time + phase_offset[i] + phase_lead, 1.f);
if (p < 0.f) p += 1.f;
if (p < s - e) return 1.f;
if (p > s + e && p < 1.f - e) return 0.f;
if (p <= s + e) {
const float t = (p - (s - e)) / (2.f * e);
return 1.f - smoothstep01(t);
}
const float q = p >= 1.f - e ? (p - (1.f - e)) / e : (e - p) / e;
return smoothstep01(q);
}
void updateFeetPositions(body_state_t &body_state) {
for (int i = 0; i < 4; ++i) updateFootPosition(body_state, i);
}
void updateFootPosition(body_state_t &body_state, const int index) {
body_state.feet[index][0] = this->default_feet_pos[index][0];
body_state.feet[index][1] = this->default_feet_pos[index][1];
body_state.feet[index][2] = this->default_feet_pos[index][2];
const float leg_phase = std::fmod(phase_time + PHASE_OFFSET[index], 1.0f);
const bool contact = leg_phase <= STAND_OFFSET;
contact ? standController(body_state, index, leg_phase / STAND_OFFSET)
: swingController(body_state, index, (leg_phase - STAND_OFFSET) / (1 - STAND_OFFSET));
const float leg_phase = std::fmod(phase_time + phase_offset[index], 1.0f);
const bool contact = leg_phase <= stand_offset;
if (contact)
standController(body_state, index, leg_phase / stand_offset);
else
swingController(body_state, index, (leg_phase - stand_offset) / (1.f - stand_offset));
}
void standController(body_state_t &body_state, const int index, const float phase) {
@@ -131,6 +192,6 @@ class BezierState : public GaitState {
const float offset_mag = std::hypot(offsets[0], offsets[2]);
const float offset_mod = std::atan2(offset_mag, foot_mag);
return M_PI_2 + foot_dir + offset_mod;
return (float)M_PI_2 + foot_dir + offset_mod;
}
};