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

🏏 Updates gait to use leg phase time instead of contact phase

Browse Source
This commit is contained in:
Rune Harlyk
2024-11-11 17:46:31 +01:00
committed by Rune Harlyk
parent f57d798971
commit fc0914ded4
2 changed files with 33 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/src/lib/gait.ts
+17 -15
View File
@@ -20,6 +20,7 @@ export interface ControllerCommand {
ry: number; ry: number;
h: number; h: number;
s: number; s: number;
s1: number;
} }
export abstract class GaitState { export abstract class GaitState {
@@ -64,7 +65,7 @@ export abstract class GaitState {
map_command(command: ControllerCommand) { map_command(command: ControllerCommand) {
const newCommand = { 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_x: Math.floor(fromInt8(command.ly, -1, 1) * 10) / 10,
step_z: -(Math.floor(fromInt8(command.lx, -1, 1) * 10) / 10), step_z: -(Math.floor(fromInt8(command.lx, -1, 1) * 10) / 10),
step_velocity: command.s / 128 + 1, step_velocity: command.s / 128 + 1,
@@ -268,13 +269,8 @@ export class BezierState extends GaitState {
protected name = 'Bezier'; protected name = 'Bezier';
protected phase = 0; protected phase = 0;
protected phase_num = 0; protected phase_num = 0;
protected contact_phases = [
[1, 0],
[0, 1],
[0, 1],
[1, 0]
];
protected step_length: number = 0; protected step_length: number = 0;
offset = [0, 0.5, 0.5, 0];
begin() { begin() {
super.begin(); super.begin();
@@ -311,36 +307,42 @@ export class BezierState extends GaitState {
} }
update_foot_position(index: number): number[] { 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][0] = this.default_feet_pos[index][0];
this.body_state.feet[index][1] = this.default_feet_pos[index][1]; this.body_state.feet[index][1] = this.default_feet_pos[index][1];
this.body_state.feet[index][2] = this.default_feet_pos[index][2]; 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; 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; let height = this.gait_state.step_height;
return this.controller(index, bezier_curve, height); return this.controller(index, phase, bezier_curve, height);
} }
controller( controller(
index: number, index: number,
phase: number,
controller: (length: number, angle: number, ...args: number[]) => number[], controller: (length: number, angle: number, ...args: number[]) => number[],
...args: number[] ...args: number[]
) { ) {
let length = this.step_length / 2; let length = this.step_length / 2;
let angle = Math.atan2(this.gait_state.step_z, 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; length = this.gait_state.step_angle * 2;
angle = yawArc(this.default_feet_pos[index], this.body_state.feet[index]); 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][0] += delta_pos[0] + delta_rot[0] * 0.2;
this.body_state.feet[index][2] += delta_pos[2] + delta_rot[2] * 0.2; this.body_state.feet[index][2] += delta_pos[2] + delta_rot[2] * 0.2;
esp32/lib/ESP32-sveltekit/Gait/GaitState.h
+16 -13
View File
@@ -209,8 +209,8 @@ class EightPhaseWalkState : public PhaseGaitState {
class BezierState : public GaitState { class BezierState : public GaitState {
private: private:
float phase_time = 0.0f; float phase_time = 0.0f;
uint8_t phase = 0; static constexpr float offset[4] = {0.f, 0.5f, 0.5f, 0.f};
static constexpr uint8_t contact_phases[4][2] = {{1, 0}, {0, 1}, {0, 1}, {1, 0}}; static constexpr float standOffset = 0.75f;
static constexpr uint8_t BEZIER_POINTS = 12; static constexpr uint8_t BEZIER_POINTS = 12;
float step_length = 0.0f; float step_length = 0.0f;
static constexpr std::array<float, BEZIER_POINTS> COMBINATORIAL_VALUES = { static constexpr std::array<float, BEZIER_POINTS> COMBINATORIAL_VALUES = {
@@ -249,9 +249,7 @@ class BezierState : public GaitState {
void updatePhase(float dt) { void updatePhase(float dt) {
phase_time += dt * gait_state.step_velocity * 2; phase_time += dt * gait_state.step_velocity * 2;
if (phase_time >= 1.0f) { if (phase_time >= 1.0f) {
phase ^= 1;
phase_time = 0; phase_time = 0;
} }
} }
@@ -263,33 +261,38 @@ class BezierState : public GaitState {
} }
void updateFootPosition(body_state_t &body_state, const int index) { 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][0] = this->default_feet_pos[index][0];
body_state.feet[index][1] = this->default_feet_pos[index][1]; body_state.feet[index][1] = this->default_feet_pos[index][1];
body_state.feet[index][2] = this->default_feet_pos[index][2]; 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) { void standController(body_state_t &body_state, const int index, const float phase) {
controller(index, body_state, stanceCurve, &gait_state.step_depth); controller(index, body_state, phase, stanceCurve, &gait_state.step_depth);
} }
void swingController(body_state_t &body_state, const int index) { void swingController(body_state_t &body_state, const int index, const float phase) {
controller(index, body_state, bezierCurve, &gait_state.step_height); 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<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, 0, 0}; float delta_pos[3] = {0, 0, 0};
float delta_rot[3] = {0, 0, 0}; float delta_rot[3] = {0, 0, 0};
float length = step_length / 2.0f; float length = step_length / 2.0f;
float angle = std::atan2(gait_state.step_z, step_length) * 2; 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; length = gait_state.step_angle * 2.0f;
angle = yawArc(default_feet_pos[index], body_state.feet[index]); 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; 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; if (step_length || gait_state.step_angle) body_state.feet[index][1] += delta_pos[1] + delta_rot[1] * 0.2;