From 54a04197700167ed6e22dc401095800d563a8bd0 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Mon, 1 Sep 2025 22:23:02 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=8E=A8=20Cleans=20up=20gait=20handling=20?= =?UTF-8?q?code?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/src/lib/components/Visualization.svelte | 16 +- app/src/lib/gait.ts | 254 ++++++------------ app/src/lib/stores/model-store.ts | 41 +-- app/src/lib/types/models.ts | 1 + app/src/routes/+layout.svelte | 10 +- app/src/routes/controller/Controls.svelte | 34 ++- esp32/include/gait/crawl_state.h | 33 --- esp32/include/gait/four_phase_trot_state.h | 28 -- esp32/include/gait/phase_state_base.h | 78 ------ .../gait/{bezier_state.h => walk_state.h} | 83 +++++- esp32/include/motion.h | 29 +- esp32/test/gait_performance.cpp | 2 +- 12 files changed, 246 insertions(+), 363 deletions(-) delete mode 100644 esp32/include/gait/crawl_state.h delete mode 100644 esp32/include/gait/four_phase_trot_state.h delete mode 100644 esp32/include/gait/phase_state_base.h rename esp32/include/gait/{bezier_state.h => walk_state.h} (64%) diff --git a/app/src/lib/components/Visualization.svelte b/app/src/lib/components/Visualization.svelte index 02f54de..18b1ee0 100644 --- a/app/src/lib/components/Visualization.svelte +++ b/app/src/lib/components/Visualization.svelte @@ -22,7 +22,10 @@ servoAngles, mpu, jointNames, - currentKinematic + currentKinematic, + walkGait, + walkGaits, + walkGaitToMode } from '$lib/stores' import { extractFootColor, @@ -34,14 +37,7 @@ import { lerp, degToRad } from 'three/src/math/MathUtils' import { GUI } from 'three/addons/libs/lil-gui.module.min.js' import { type body_state_t } from '$lib/kinematic' - import { - BezierState, - CalibrationState, - EightPhaseWalkState, - IdleState, - RestState, - StandState - } from '$lib/gait' + import { BezierState, CalibrationState, IdleState, RestState, StandState } from '$lib/gait' import { radToDeg } from 'three/src/math/MathUtils.js' import type { URDFRobot } from 'urdf-loader' import { get } from 'svelte/store' @@ -78,7 +74,6 @@ [ModesEnum.Calibration]: new CalibrationState(), [ModesEnum.Rest]: new RestState(), [ModesEnum.Stand]: new StandState(), - [ModesEnum.Crawl]: new EightPhaseWalkState(), [ModesEnum.Walk]: new BezierState() } let lastTick = performance.now() @@ -117,6 +112,7 @@ await populateModelCache() await createScene() servoAngles.subscribe(updateAnglesFromStore) + walkGait.subscribe(gait => planners[ModesEnum.Walk].set_mode(walkGaitToMode(gait))) if (panel) createPanel() }) diff --git a/app/src/lib/gait.ts b/app/src/lib/gait.ts index f77b24b..ae9b72c 100644 --- a/app/src/lib/gait.ts +++ b/app/src/lib/gait.ts @@ -2,8 +2,6 @@ import { get } from 'svelte/store' import type { body_state_t } from './kinematic' import { currentKinematic } from './stores/featureFlags' -const { sin } = Math - export interface gait_state_t { step_height: number step_x: number @@ -79,7 +77,8 @@ export class IdleState extends GaitState { export class CalibrationState extends GaitState { protected name = 'Calibration' - step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { + // eslint-disable-next-line @typescript-eslint/no-unused-vars + step(body_state: body_state_t, _command: ControllerCommand) { body_state.omega = 0 body_state.phi = 0 body_state.psi = 0 @@ -94,7 +93,8 @@ export class CalibrationState extends GaitState { export class RestState extends GaitState { protected name = 'Rest' - step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { + // eslint-disable-next-line @typescript-eslint/no-unused-vars + step(body_state: body_state_t, _command: ControllerCommand) { body_state.omega = 0 body_state.phi = 0 body_state.psi = 0 @@ -109,7 +109,7 @@ export class RestState extends GaitState { export class StandState extends GaitState { protected name = 'Stand' - step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { + step(body_state: body_state_t, command: ControllerCommand) { body_state.omega = 0 body_state.phi = command.rx * 10 * (Math.PI / 2) body_state.psi = command.ry * 10 * (Math.PI / 2) @@ -120,157 +120,41 @@ export class StandState extends GaitState { } } -abstract class PhaseGaitState extends GaitState { - protected tick = 0 - protected phase = 0 - protected phase_time = 0 - protected abstract num_phases: number - protected abstract phase_speed_factor: number - protected abstract swing_stand_ratio: number - - protected contact_phases!: number[][] - protected shifts!: number[][] - - step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { - super.step(body_state, command, dt) - this.update_phase() - this.update_body_position() - this.update_feet_positions() - return this.body_state - } - - update_phase() { - this.phase_time += this.dt * this.phase_speed_factor * this.gait_state.step_velocity - - if (this.phase_time >= 1) { - this.phase += 1 - if (this.phase == this.num_phases) this.phase = 0 - this.phase_time = 0 - } - } - - update_body_position() { - if (this.num_phases === 4) return - - const shift = this.shifts[Math.floor(this.phase / 2)] - - this.body_state.xm += (shift[0] - this.body_state.xm) * this.dt * 4 - this.body_state.zm += (shift[2] - this.body_state.zm) * this.dt * 4 - } - - update_feet_positions() { - for (let i = 0; i < 4; i++) { - this.body_state.feet[i] = this.update_foot_position(i) - } - } - - update_foot_position(index: number): number[] { - const contact = this.contact_phases[index][this.phase] - return contact ? this.stand(index) : this.swing(index) - } - - stand(index: number): number[] { - const delta_pos = [ - -this.gait_state.step_x * this.dt * this.swing_stand_ratio, - 0, - -this.gait_state.step_z * this.dt * this.swing_stand_ratio - ] - - this.body_state.feet[index][0] = this.body_state.feet[index][0] + delta_pos[0] - this.body_state.feet[index][1] = this.default_feet_pos[index][1] - this.body_state.feet[index][2] = this.body_state.feet[index][2] + delta_pos[2] - return this.body_state.feet[index] - } - - swing(index: number): number[] { - const delta_pos = [this.gait_state.step_x * this.dt, 0, this.gait_state.step_z * this.dt] - - if (this.gait_state.step_x == 0) { - delta_pos[0] = - (this.default_feet_pos[index][0] - this.body_state.feet[index][0]) * this.dt * 8 - } - - if (this.gait_state.step_z == 0) { - delta_pos[2] = - (this.default_feet_pos[index][2] - this.body_state.feet[index][2]) * this.dt * 8 - } - - this.body_state.feet[index][0] = this.body_state.feet[index][0] + delta_pos[0] - this.body_state.feet[index][1] = - this.default_feet_pos[index][1] + sin(this.phase_time * Math.PI) * this.gait_state.step_height - this.body_state.feet[index][2] = this.body_state.feet[index][2] + delta_pos[2] - return this.body_state.feet[index] - } -} - -export class FourPhaseWalkState extends PhaseGaitState { - protected name = 'Four phase walk' - protected num_phases = 4 - protected phase_speed_factor = 6 - protected contact_phases = [ - [1, 0, 1, 1], - [1, 1, 1, 0], - [1, 1, 1, 0], - [1, 0, 1, 1] - ] - protected swing_stand_ratio = 1 / (this.num_phases - 1) - - begin() { - super.begin() - } - - end() { - super.end() - } - - step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { - return super.step(body_state, command, dt) - } -} - -export class EightPhaseWalkState extends PhaseGaitState { - protected name = 'Eight phase walk' - protected num_phases = 8 - protected phase_speed_factor = 4 - protected contact_phases = [ - [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] - ] - protected shifts = [ - [-0.05, 0, -0.2], - [0.3, 0, 0.2], - [-0.05, 0, 0.2], - [0.3, 0, -0.2] - ] - protected swing_stand_ratio = 1 / (this.num_phases - 1) - - begin() { - super.begin() - } - - end() { - super.end() - } - - step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { - return super.step(body_state, command, dt) - } -} - export class BezierState extends GaitState { protected name = 'Bezier' protected phase = 0 protected phase_num = 0 - protected step_length: number = 0 - protected stand_offset = 0.75 - offset = [0, 0.5, 0.5, 0] + protected step_length = 0 + protected stand_offset = 0.85 + protected mode: 'crawl' | 'trot' = 'trot' + offset = [0, 0.5, 0.75, 0.25] + + constructor() { + super() + this.set_mode(this.mode) + } begin() { super.begin() } + set_mode(mode: 'crawl' | 'trot', duty?: number, order?: [number, number, number, number]) { + console.log('BezierState set_mode', mode) + + this.mode = mode + if (mode === 'crawl') { + this.stand_offset = duty ?? 0.85 + const o = order ?? [0, 3, 1, 2] + const base = [0, 0.25, 0.5, 0.75] + const offsets = new Array(4).fill(0) + for (let i = 0; i < 4; i++) offsets[o[i]] = base[i] + this.offset = offsets + } else { + this.stand_offset = duty ?? 0.6 + this.offset = order ? (order.map(v => v % 1) as number[]) : [0, 0.5, 0.5, 0] + } + } + end() { super.end() } @@ -278,34 +162,78 @@ export class BezierState extends GaitState { step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { super.step(body_state, command, dt) this.step_length = Math.sqrt(this.gait_state.step_x ** 2 + this.gait_state.step_z ** 2) - if (this.gait_state.step_x < 0) { - this.step_length = -this.step_length - } + if (this.gait_state.step_x < 0) this.step_length = -this.step_length this.update_phase() + this.update_body_position() this.update_feet_positions() return this.body_state } update_phase() { - this.phase += this.dt * this.gait_state.step_velocity * 2 + const m = this.gait_state + if (m.step_x === 0 && m.step_z === 0 && m.step_angle === 0) return + this.phase += this.dt * m.step_velocity * 2 if (this.phase >= 1) { - this.phase_num += 1 - this.phase_num %= 2 + this.phase_num = (this.phase_num + 1) % 2 this.phase = 0 } } - update_feet_positions() { + protected phase_lead = 0.08 + protected feather = 0.05 + + update_body_position() { + const m = this.gait_state + const moving = m.step_x !== 0 || m.step_z !== 0 || m.step_angle !== 0 + if (!moving) return + const c = this.dynamic_stance_centroid() + const k = this.mode === 'crawl' ? 16 : 10 + const a = 1 - Math.exp(-k * this.dt) + this.body_state.xm += (c[0] - this.body_state.xm) * a + this.body_state.zm += (c[2] - this.body_state.zm) * a + } + + protected dynamic_stance_centroid(): number[] { + let sx = 0, + sz = 0, + sw = 0 for (let i = 0; i < 4; i++) { - this.body_state.feet[i] = this.update_foot_position(i) + const w = this.stance_weight(i) + sx += this.body_state.feet[i][0] * w + sz += this.body_state.feet[i][2] * w + sw += w } + if (sw === 0) return [this.body_state.xm, 0, this.body_state.zm] + return [sx / sw, 0, sz / sw] + } + + protected stance_weight(i: number): number { + const s = this.stand_offset + const e = this.feather + let p = this.phase + this.offset[i] + this.phase_lead + p -= Math.floor(p) + if (p < s - e) return 1 + if (p > s + e && p < 1 - e) return 0 + if (p <= s + e) { + const t = (p - (s - e)) / (2 * e) + return 1 - this.smoothstep01(t) + } + const q = p >= 1 - e ? (p - (1 - e)) / e : (e - p) / e + return this.smoothstep01(q) + } + + protected smoothstep01(t: number): number { + const x = Math.max(0, Math.min(1, t)) + return x * x * (3 - 2 * x) + } + + update_feet_positions() { + for (let i = 0; i < 4; i++) this.body_state.feet[i] = this.update_foot_position(i) } update_foot_position(index: number): number[] { let phase = this.phase + this.offset[index] - if (phase >= 1) { - phase -= 1 - } + 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] @@ -356,10 +284,7 @@ const stance_curve = (length: number, angle: number, depth: number, phase: numbe const X = step * X_POLAR const Z = step * Y_POLAR let Y = 0 - - if (length !== 0) { - Y = -depth * Math.cos((Math.PI * (X + Y)) / (2 * length)) - } + if (length !== 0) Y = -depth * Math.cos((Math.PI * (X + Y)) / (2 * length)) return [X, Y, Z] } @@ -390,6 +315,7 @@ const bezier_curve = (length: number, angle: number, height: number, phase: numb } return point } + const get_control_points = (length: number, angle: number, height: number): number[][] => { const X_POLAR = Math.cos(angle) const Z_POLAR = Math.sin(angle) @@ -440,8 +366,6 @@ const comb = (n: number, k: number): number => { if (k === 0 || k === n) return 1 k = Math.min(k, n - k) let c = 1 - for (let i = 0; i < k; i++) { - c = (c * (n - i)) / (i + 1) - } + for (let i = 0; i < k; i++) c = (c * (n - i)) / (i + 1) return c } diff --git a/app/src/lib/stores/model-store.ts b/app/src/lib/stores/model-store.ts index 75c0af3..c1ac068 100644 --- a/app/src/lib/stores/model-store.ts +++ b/app/src/lib/stores/model-store.ts @@ -8,30 +8,39 @@ export const jointNames = persistentStore('joint_names', []) export const model = writable() -export const modes = [ - 'deactivated', - 'idle', - 'calibration', - 'rest', - 'stand', - 'crawl', - 'walk' -] as const +export const modes = ['deactivated', 'idle', 'calibration', 'rest', 'stand', 'walk'] as const export type Modes = (typeof modes)[number] export enum ModesEnum { - Deactivated, - Idle, - Calibration, - Rest, - Stand, - Crawl, - Walk + Deactivated = 0, + Idle = 1, + Calibration = 2, + Rest = 3, + Stand = 4, + Walk = 5 +} + +export enum WalkGaits { + Trot = 0, + Crawl = 1 +} + +export const walkGaits = ['trot', 'crawl'] as const + +export const walkGaitLabels: Record = { + [WalkGaits.Trot]: 'Trot', + [WalkGaits.Crawl]: 'Crawl' +} + +export const walkGaitToMode = (gait: WalkGaits): 'trot' | 'crawl' => { + return gait === WalkGaits.Trot ? 'trot' : 'crawl' } export const mode: Writable = writable(ModesEnum.Deactivated) +export const walkGait: Writable = writable(WalkGaits.Trot) + export const outControllerData = writable([0, 0, 0, 0, 0, 1, 0]) export const kinematicData = writable([0, 0, 0, 0, 1, 0]) diff --git a/app/src/lib/types/models.ts b/app/src/lib/types/models.ts index 53f2387..cfc42a1 100644 --- a/app/src/lib/types/models.ts +++ b/app/src/lib/types/models.ts @@ -8,6 +8,7 @@ export enum MessageTopic { i2cScan = 'i2cScan', peripheralSettings = 'peripheralSettings', otastatus = 'otastatus', + gait = 'walk_gait', servoState = 'servoState', servoPWM = 'servoPWM', WiFiSettings = 'WiFiSettings', diff --git a/app/src/routes/+layout.svelte b/app/src/routes/+layout.svelte index 7a3b213..e51bd04 100644 --- a/app/src/routes/+layout.svelte +++ b/app/src/routes/+layout.svelte @@ -19,7 +19,8 @@ servoAnglesOut, socket, location, - useFeatureFlags + useFeatureFlags, + walkGait } from '$lib/stores' import { type Analytics, type DownloadOTA } from '$lib/types/models' import { MessageTopic } from '$lib/types/models' @@ -38,12 +39,9 @@ addEventListeners() - outControllerData.subscribe(data => { - console.log(data) - - socket.sendEvent(MessageTopic.input, data) - }) + outControllerData.subscribe(data => socket.sendEvent(MessageTopic.input, data)) mode.subscribe(data => socket.sendEvent(MessageTopic.mode, data)) + walkGait.subscribe(data => socket.sendEvent(MessageTopic.gait, data)) servoAnglesOut.subscribe(data => socket.sendEvent(MessageTopic.angles, data)) kinematicData.subscribe(data => socket.sendEvent(MessageTopic.position, data)) }) diff --git a/app/src/routes/controller/Controls.svelte b/app/src/routes/controller/Controls.svelte index 0b1ae66..0ee77a6 100644 --- a/app/src/routes/controller/Controls.svelte +++ b/app/src/routes/controller/Controls.svelte @@ -2,7 +2,18 @@ import nipplejs from 'nipplejs' import { onMount } from 'svelte' import { capitalize, throttler } from '$lib/utilities' - import { input, outControllerData, mode, modes, type Modes, ModesEnum } from '$lib/stores' + import { + input, + outControllerData, + mode, + modes, + type Modes, + ModesEnum, + walkGaits, + WalkGaits, + walkGait, + walkGaitLabels + } from '$lib/stores' import type { vector } from '$lib/types/models' import { VerticalSlider } from '$lib/components/input' import { gamepadAxes, hasGamepad } from '$lib/stores/gamepad' @@ -13,7 +24,7 @@ let right: nipplejs.JoystickManager let throttle_timing = 40 - let data = new Array(8) + let data = new Array(7) $effect(() => { if ($hasGamepad) { @@ -100,6 +111,10 @@ const changeMode = (modeValue: Modes) => { mode.set(modes.indexOf(modeValue)) } + + const changeWalkGait = (walkGaitValue: WalkGaits) => { + walkGait.set(walkGaitValue) + }
@@ -141,7 +156,20 @@ {/each}
- {#if $mode === ModesEnum.Walk || $mode === ModesEnum.Crawl} + {#if $mode === ModesEnum.Walk} +
+ {#each Object.values(WalkGaits) as gaitValue} + {#if typeof gaitValue === 'number'} + + {/if} + {/each} +
+
diff --git a/esp32/include/gait/crawl_state.h b/esp32/include/gait/crawl_state.h deleted file mode 100644 index 2b341e8..0000000 --- a/esp32/include/gait/crawl_state.h +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -#include - -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); - } -}; \ No newline at end of file diff --git a/esp32/include/gait/four_phase_trot_state.h b/esp32/include/gait/four_phase_trot_state.h deleted file mode 100644 index 7332656..0000000 --- a/esp32/include/gait/four_phase_trot_state.h +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -#include - -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); - } -}; \ No newline at end of file diff --git a/esp32/include/gait/phase_state_base.h b/esp32/include/gait/phase_state_base.h deleted file mode 100644 index b03042c..0000000 --- a/esp32/include/gait/phase_state_base.h +++ /dev/null @@ -1,78 +0,0 @@ -#pragma once - -#include - -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]; - } -}; \ No newline at end of file diff --git a/esp32/include/gait/bezier_state.h b/esp32/include/gait/walk_state.h similarity index 64% rename from esp32/include/gait/bezier_state.h rename to esp32/include/gait/walk_state.h index 66fbcb0..411c0c0 100644 --- a/esp32/include/gait/bezier_state.h +++ b/esp32/include/gait/walk_state.h @@ -5,13 +5,18 @@ #include #include -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 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 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 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 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; } }; \ No newline at end of file diff --git a/esp32/include/motion.h b/esp32/include/motion.h index 4c6920d..bf9c6fc 100644 --- a/esp32/include/motion.h +++ b/esp32/include/motion.h @@ -8,8 +8,7 @@ #include #include -#include -#include +#include #include #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()); + + WALK_GAIT walkGait = static_cast(root.as()); + 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(root.as()); 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 crawlGait = std::make_unique(); - std::unique_ptr walkGait = std::make_unique(); - MOTION_STATE motionState = MOTION_STATE::DEACTIVATED; unsigned long _lastUpdate; diff --git a/esp32/test/gait_performance.cpp b/esp32/test/gait_performance.cpp index 5bd7085..839472f 100644 --- a/esp32/test/gait_performance.cpp +++ b/esp32/test/gait_performance.cpp @@ -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};