From fc0914ded4de97994bb6835587ca517e9c0e74b3 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Mon, 11 Nov 2024 17:46:31 +0100 Subject: [PATCH] =?UTF-8?q?=F0=9F=8F=8F=20Updates=20gait=20to=20use=20leg?= =?UTF-8?q?=20phase=20time=20instead=20of=20contact=20phase?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/src/lib/gait.ts | 32 ++++++++++++---------- esp32/lib/ESP32-sveltekit/Gait/GaitState.h | 29 +++++++++++--------- 2 files changed, 33 insertions(+), 28 deletions(-) diff --git a/app/src/lib/gait.ts b/app/src/lib/gait.ts index 2fa7946..4a93f8a 100644 --- a/app/src/lib/gait.ts +++ b/app/src/lib/gait.ts @@ -20,6 +20,7 @@ export interface ControllerCommand { ry: number; h: number; s: number; + s1: number; } export abstract class GaitState { @@ -64,7 +65,7 @@ export abstract class GaitState { map_command(command: ControllerCommand) { const newCommand = { - step_height: 0.4, + step_height: 0.4 + (command.s1 / 128 + 1) / 2, step_x: Math.floor(fromInt8(command.ly, -1, 1) * 10) / 10, step_z: -(Math.floor(fromInt8(command.lx, -1, 1) * 10) / 10), step_velocity: command.s / 128 + 1, @@ -268,13 +269,8 @@ export class BezierState extends GaitState { protected name = 'Bezier'; protected phase = 0; protected phase_num = 0; - protected contact_phases = [ - [1, 0], - [0, 1], - [0, 1], - [1, 0] - ]; protected step_length: number = 0; + offset = [0, 0.5, 0.5, 0]; begin() { super.begin(); @@ -311,36 +307,42 @@ export class BezierState extends GaitState { } update_foot_position(index: number): number[] { - const contact = this.contact_phases[index][this.phase_num]; + let phase = this.phase + this.offset[index]; + if (phase >= 1) { + phase -= 1; + } this.body_state.feet[index][0] = this.default_feet_pos[index][0]; this.body_state.feet[index][1] = this.default_feet_pos[index][1]; this.body_state.feet[index][2] = this.default_feet_pos[index][2]; - return contact ? this.swing_controller(index) : this.stand_controller(index); + return phase <= 0.75 ? + this.stand_controller(index, phase / 0.75) + : this.swing_controller(index, (phase - 0.75) / (1 - 0.75)); } - stand_controller(index: number) { + stand_controller(index: number, phase: number) { let depth = this.gait_state.step_depth; - return this.controller(index, stance_curve, depth); + return this.controller(index, phase, stance_curve, depth); } - swing_controller(index: number) { + swing_controller(index: number, phase: number) { let height = this.gait_state.step_height; - return this.controller(index, bezier_curve, height); + return this.controller(index, phase, bezier_curve, height); } controller( index: number, + phase: number, controller: (length: number, angle: number, ...args: number[]) => number[], ...args: number[] ) { let length = this.step_length / 2; let angle = Math.atan2(this.gait_state.step_z, this.step_length) * 2; - const delta_pos = controller(length, angle, ...args, this.phase); + const delta_pos = controller(length, angle, ...args, phase); length = this.gait_state.step_angle * 2; angle = yawArc(this.default_feet_pos[index], this.body_state.feet[index]); - const delta_rot = controller(length, angle, ...args, this.phase); + const delta_rot = controller(length, angle, ...args, phase); this.body_state.feet[index][0] += delta_pos[0] + delta_rot[0] * 0.2; this.body_state.feet[index][2] += delta_pos[2] + delta_rot[2] * 0.2; diff --git a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h index 962fa52..003ffb9 100644 --- a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h +++ b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h @@ -209,8 +209,8 @@ class EightPhaseWalkState : public PhaseGaitState { class BezierState : public GaitState { private: float phase_time = 0.0f; - uint8_t phase = 0; - static constexpr uint8_t contact_phases[4][2] = {{1, 0}, {0, 1}, {0, 1}, {1, 0}}; + static constexpr float offset[4] = {0.f, 0.5f, 0.5f, 0.f}; + static constexpr float standOffset = 0.75f; static constexpr uint8_t BEZIER_POINTS = 12; float step_length = 0.0f; static constexpr std::array COMBINATORIAL_VALUES = { @@ -249,9 +249,7 @@ class BezierState : public GaitState { void updatePhase(float dt) { phase_time += dt * gait_state.step_velocity * 2; - if (phase_time >= 1.0f) { - phase ^= 1; phase_time = 0; } } @@ -263,33 +261,38 @@ class BezierState : public GaitState { } void updateFootPosition(body_state_t &body_state, const int index) { - bool contact = contact_phases[index][phase] == 1; 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]; - contact ? standController(body_state, index) : swingController(body_state, index); + float leg_phase = phase_time + offset[index]; + if (leg_phase >= 1) { + leg_phase -= 1; + } + const bool contact = leg_phase <= standOffset; + contact ? standController(body_state, index, leg_phase / 0.75) + : swingController(body_state, index, (leg_phase - 0.75) / (1 - 0.75)); } - void standController(body_state_t &body_state, const int index) { - controller(index, body_state, stanceCurve, &gait_state.step_depth); + void standController(body_state_t &body_state, const int index, const float phase) { + controller(index, body_state, phase, stanceCurve, &gait_state.step_depth); } - void swingController(body_state_t &body_state, const int index) { - controller(index, body_state, bezierCurve, &gait_state.step_height); + void swingController(body_state_t &body_state, const int index, const float phase) { + controller(index, body_state, phase, bezierCurve, &gait_state.step_height); } - void controller(const int index, body_state_t &body_state, + void controller(const int index, body_state_t &body_state, const float phase, std::function curve, float *arg) { float delta_pos[3] = {0, 0, 0}; float delta_rot[3] = {0, 0, 0}; float length = step_length / 2.0f; float angle = std::atan2(gait_state.step_z, step_length) * 2; - curve(length, angle, arg, phase_time, delta_pos); + curve(length, angle, arg, phase, delta_pos); length = gait_state.step_angle * 2.0f; angle = yawArc(default_feet_pos[index], body_state.feet[index]); - curve(length, angle, arg, phase_time, delta_rot); + curve(length, angle, arg, phase, delta_rot); body_state.feet[index][0] += delta_pos[0] + delta_rot[0] * 0.2; if (step_length || gait_state.step_angle) body_state.feet[index][1] += delta_pos[1] + delta_rot[1] * 0.2;