From abefdd6c21b250268a50418bf4941db9a856d62c Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Thu, 1 Aug 2024 22:47:34 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=AA=98=20Adds=20translated=20c++=20Gait?= =?UTF-8?q?=20code?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- esp32/lib/ESP32-sveltekit/Gait/GaitState.h | 221 +++++++++++++++++---- esp32/lib/ESP32-sveltekit/MotionService.h | 3 +- 2 files changed, 186 insertions(+), 38 deletions(-) diff --git a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h index 3465b22..c4feece 100644 --- a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h +++ b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h @@ -1,60 +1,207 @@ -#ifndef GAIT_STATE_H -#define GAIT_STATE_H - #include +struct gait_state_t { + float step_height; + float step_x; + float step_z; + float step_angle; + float step_velocity; +}; + +struct ControllerCommand { + int stop; + float lx, ly, rx, ry, h, s; +}; + class GaitState { - private: + protected: + virtual const char* name() const = 0; + float default_feet_positions[4][4] = {{1, -1, 1, 1}, {1, -1, -1, 1}, {-1, -1, 1, 1}, {-1, -1, -1, 1}}; + public: - GaitState() { ESP_LOGI("GaitState", "%s constructor", name); } + virtual float getDefaultHeight() const { return 0.5f; } - virtual ~GaitState() = 0; + virtual void begin() { ESP_LOGI("Gait Planner", "Starting %s", name()) } - virtual void begin() = 0; + virtual void end() { ESP_LOGI("Gait Planner", "Ending %s", name()) } - virtual void end() = 0; - - virtual void loop() = 0; - - const char *name; - - static body_state_t body_state; + virtual void step(body_state_t body_state, ControllerCommand command, float dt = 0.02f) {} }; class IdleState : public GaitState { - public: - IdleState() { name = "Idle"; } - ~IdleState() {} + protected: + const char* name() const override { return "Idle"; } +}; - void begin() override { ESP_LOGI("GaitState", "IdleState begin"); } +class RestState : public GaitState { + protected: + const char* name() const override { return "Rest"; } - void end() override { ESP_LOGI("GaitState", "IdleState end"); } - - void loop() override { ESP_LOGI("GaitState", "IdleState loop"); } + void step(body_state_t body_state, ControllerCommand command, float dt = 0.02f) override { + body_state.omega = 0; + body_state.phi = 0; + body_state.psi = 0; + body_state.xm = 0; + body_state.ym = getDefaultHeight() / 2; + body_state.zm = 0; + body_state.updateFeet(default_feet_positions); + } }; class StandState : public GaitState { - public: - StandState() { name = "Stand"; } - ~StandState() {} + protected: + const char* name() const override { return "Stand"; } - void begin() override { ESP_LOGI("GaitState", "StandState begin"); } - - void end() override { ESP_LOGI("GaitState", "StandState end"); } - - void loop() override { ESP_LOGI("GaitState", "StandState loop"); } + void step(body_state_t body_state, ControllerCommand command, float dt = 0.02f) override { + body_state.omega = 0; + body_state.phi = command.rx / 8; + body_state.psi = command.ry / 8; + body_state.xm = command.ly / 2 / 100; + body_state.zm = command.lx / 2 / 100; + body_state.updateFeet(default_feet_positions); + } }; -class WalkState : public GaitState { - public: - WalkState() { name = "Walk"; } - ~WalkState() {} +class PhaseGaitState : public GaitState { + protected: + int tick = 0; + int phase = 0; + float phase_time = 0; + virtual int num_phases() const = 0; + virtual int phase_length() const = 0; + virtual float swing_stand_ratio() const = 0; - void begin() override { ESP_LOGI("GaitState", "WalkState begin"); } + uint8_t contact_phases[4][8]; + float shifts[4][3]; - void end() override { ESP_LOGI("GaitState", "WalkState end"); } + gait_state_t gait_state; + float dt = 0.02f; - void loop() override { ESP_LOGI("GaitState", "WalkState loop"); } + void step(body_state_t body_state, ControllerCommand command, float dt = 0.02f) override { + this->gait_state = mapCommand(command); + this->dt = dt; + updatePhase(); + updateBodyPosition(body_state); + updateFeetPositions(body_state.feet); + } + + void updatePhase() { + tick += 1; + phase_time = static_cast(tick) / phase_length(); + + if (tick % phase_length() == 0) { + phase += 1; + if (phase == num_phases()) phase = 0; + tick = 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(float feet[4][4]) { + for (int i = 0; i < 4; ++i) { + updateFootPosition(feet[i], i); + } + } + + void updateFootPosition(float foot[4], int index) { + bool contact = contact_phases[index][phase]; + contact ? stand(foot, index) : swing(foot, index); + } + + void stand(float foot[4], int index) { + float delta_pos[3] = {-gait_state.step_x * dt * swing_stand_ratio(), 0, + -gait_state.step_z * dt * swing_stand_ratio()}; + + foot[0] += delta_pos[0]; + foot[1] = default_feet_positions[index][1]; + foot[2] += delta_pos[2]; + } + + void swing(float foot[4], int index) { + float delta_pos[3] = {gait_state.step_x * dt, 0, gait_state.step_z * dt}; + + if (gait_state.step_x == 0) { + delta_pos[0] = (default_feet_positions[index][0] - foot[0]) * dt * 8; + } + + if (gait_state.step_z == 0) { + delta_pos[2] = (default_feet_positions[index][2] - foot[2]) * dt * 8; + } + + foot[0] += delta_pos[0]; + foot[1] = default_feet_positions[index][1] + std::sin(phase_time * M_PI) * gait_state.step_height; + foot[2] += delta_pos[2]; + } + + gait_state_t mapCommand(ControllerCommand command) { + gait_state_t state; + state.step_height = 0.4f; + state.step_x = std::round(command.ly * 10) / 10 * 3; + state.step_z = -std::round(command.lx * 10) / 10 * 3; + state.step_velocity = 1; + state.step_angle = 0; + return state; + } }; -#endif \ No newline at end of file +class FourPhaseWalkState : public PhaseGaitState { + protected: + const char* name() const override { return "Four phase walk"; } + + int num_phases() const override { return 4; } + + int phase_length() const override { return 15; } + + 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, ControllerCommand command, float dt = 0.02f) override { + return PhaseGaitState::step(body_state, command, dt); + } +}; + +class EightPhaseWalkState : public PhaseGaitState { + protected: + const char* name() const override { return "Eight phase walk"; } + + int num_phases() const override { return 8; } + + int phase_length() const override { return 20; } + + 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, 0, 1, 1, 1, 1}, {1, 1, 1, 1, 1, 0, 1, 1}, {1, 1, 1, 1, 1, 1, 1, 0}}; + float shift_values[4][3] = {{-0.3f, 0, -0.2f}, {-0.3f, 0, 0.2f}, {0.3f, 0, -0.2f}, {0.3f, 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, ControllerCommand command, float dt = 0.02f) override { + return PhaseGaitState::step(body_state, command, dt); + } +}; \ No newline at end of file diff --git a/esp32/lib/ESP32-sveltekit/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h index 4fcf2f5..72902fe 100644 --- a/esp32/lib/ESP32-sveltekit/MotionService.h +++ b/esp32/lib/ESP32-sveltekit/MotionService.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -146,7 +147,7 @@ class MotionService { float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1}; float default_feet_positions[4][4] = { - {100, -100, 100, 1}, {100, -100, -100, 1}, {-100, -100, 100, 1}, {-100, -100, -100, 1}}; + {1, -1, 1, 1}, {1, -1, -1, 1}, {-1, -1, 1, 1}, {-1, -1, -1, 1}}; float angles[12] = { 0,