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;
}
};
esp32/include/motion.h
+17 -12
View File
@@ -8,8 +8,7 @@
#include <utils/math_utils.h>
#include <gait/state.h>
#include <gait/crawl_state.h>
#include <gait/bezier_state.h>
#include <gait/walk_state.h>
#include <message_types.h>
#define DEFAULT_STATE false
@@ -17,8 +16,9 @@
#define INPUT_EVENT "input"
#define POSITION_EVENT "position"
#define MODE_EVENT "mode"
#define WALK_GAIT_EVENT "walk_gait"
enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, CRAWL, WALK };
enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, WALK };
class MotionService {
public:
@@ -29,6 +29,8 @@ class MotionService {
socket.onEvent(MODE_EVENT, [&](JsonVariant &root, int originId) { handleMode(root, originId); });
socket.onEvent(WALK_GAIT_EVENT, [&](JsonVariant &root, int originId) { handleWalkGait(root, originId); });
socket.onEvent(ANGLES_EVENT, [&](JsonVariant &root, int originId) { anglesEvent(root, originId); });
socket.onEvent(POSITION_EVENT, [&](JsonVariant &root, int originId) { positionEvent(root, originId); });
@@ -71,7 +73,6 @@ class MotionService {
body_state.updateFeet(kinematics.default_feet_positions);
break;
}
case MOTION_STATE::CRAWL:
case MOTION_STATE::WALK: {
gait_state.step_height = 0.4 + (command.s1 + 1) / 2;
gait_state.step_x = command.ly;
@@ -84,6 +85,16 @@ class MotionService {
}
}
void handleWalkGait(JsonVariant &root, int originId) {
ESP_LOGI("MotionService", "Walk Gait %d", root.as<int>());
WALK_GAIT walkGait = static_cast<WALK_GAIT>(root.as<int>());
if (walkGait == WALK_GAIT::TROT)
this->walkGait.set_mode_trot();
else
this->walkGait.set_mode_crawl();
}
void handleMode(JsonVariant &root, int originId) {
motionState = static_cast<MOTION_STATE>(root.as<int>());
ESP_LOGV("MotionService", "Mode %d", motionState);
@@ -114,12 +125,8 @@ class MotionService {
case MOTION_STATE::CALIBRATION: update_angles(calibration_angles, new_angles, false); break;
case MOTION_STATE::REST: update_angles(rest_angles, new_angles, false); break;
case MOTION_STATE::STAND: kinematics.calculate_inverse_kinematics(body_state, new_angles); break;
case MOTION_STATE::CRAWL:
crawlGait->step(body_state, command);
kinematics.calculate_inverse_kinematics(body_state, new_angles);
break;
case MOTION_STATE::WALK:
walkGait->step(body_state, command);
walkGait.step(body_state, command);
kinematics.calculate_inverse_kinematics(body_state, new_angles);
break;
}
@@ -143,13 +150,11 @@ class MotionService {
private:
ServoController *_servoController;
Kinematics kinematics;
WalkState walkGait;
CommandMsg command = {0, 0, 0, 0, 0, 0, 0};
friend class GaitState;
std::unique_ptr<GaitState> crawlGait = std::make_unique<EightPhaseWalkState>();
std::unique_ptr<GaitState> walkGait = std::make_unique<BezierState>();
MOTION_STATE motionState = MOTION_STATE::DEACTIVATED;
unsigned long _lastUpdate;
esp32/test/gait_performance.cpp
+1 -1
View File
@@ -4,7 +4,7 @@
#include "gait/bezier_state.h"
void test_gaitPlanner_calculateStep_time() {
BezierState gaitPlanner;
WalkState gaitPlanner;
body_state_t body_state = {
128, 0, 0, 0, 0, 0, {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}}};
CommandMsg command = {0, 0, 0, 0, 0, 0, 0};