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;
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;
esp32/lib/ESP32-sveltekit/Gait/GaitState.h
+16 -13
View File
@@ -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<float, BEZIER_POINTS> 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<void(float, float, float *, float, float *)> 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;