From 87fe566d0d36dc1ad0fec0e8796d4f16947dbb99 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Thu, 7 Nov 2024 12:47:53 +0100 Subject: [PATCH] =?UTF-8?q?=F0=9F=AB=9A=20Adds=20bezier=20gait?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/src/lib/components/Visualization.svelte | 3 +- app/src/lib/gait.ts | 162 ++++++++++++++++++++ 2 files changed, 164 insertions(+), 1 deletion(-) diff --git a/app/src/lib/components/Visualization.svelte b/app/src/lib/components/Visualization.svelte index 1296165..f0214db 100644 --- a/app/src/lib/components/Visualization.svelte +++ b/app/src/lib/components/Visualization.svelte @@ -29,6 +29,7 @@ import { GUI } from 'three/addons/libs/lil-gui.module.min.js'; import Kinematic, { type body_state_t } from '$lib/kinematic'; import { + BezierState, CalibrationState, EightPhaseWalkState, FourPhaseWalkState, @@ -69,7 +70,7 @@ [ModesEnum.Rest]: new RestState(), [ModesEnum.Stand]: new StandState(), [ModesEnum.Crawl]: new EightPhaseWalkState(), - [ModesEnum.Walk]: new FourPhaseWalkState() + [ModesEnum.Walk]: new BezierState() }; let lastTick = performance.now(); diff --git a/app/src/lib/gait.ts b/app/src/lib/gait.ts index 69ce534..d6eb64a 100644 --- a/app/src/lib/gait.ts +++ b/app/src/lib/gait.ts @@ -255,3 +255,165 @@ export class EightPhaseWalkState extends PhaseGaitState { return super.step(body_state, command, dt); } } + +export class BezierState extends GaitState { + protected name = 'Bezier'; + protected phase = 0; + protected phase_num = 0; + protected dt = 0.02; + protected contact_phases = [ + [1, 0], + [0, 1], + [0, 1], + [1, 0] + ]; + protected step_length: number = 0; + protected body_state!: body_state_t; + protected gait_state!: gait_state_t; + + begin() { + super.begin(); + } + + end() { + super.end(); + } + + step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) { + this.body_state = body_state; + this.gait_state = this.map_command(command); + this.dt = dt / 1000; + 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; + this.gait_state.step_z = -this.gait_state.step_z; + } + this.update_phase(); + this.update_feet_positions(); + return this.body_state; + } + + update_phase() { + this.phase += this.dt * this.gait_state.step_velocity * 2; + if (this.phase >= 1) { + this.phase_num += 1; + this.phase_num %= 2; + this.phase = 0; + } + } + + 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_num]; + 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(index) : this.stance(index); + } + + swing(index: number): number[] { + const control_points = this.get_control_points(); + const t = this.phase; + const n = control_points.length - 1; + + const point = [0, 0, 0]; + for (let i = 0; i <= n; i++) { + const bernstein_poly = this.comb(n, i) * Math.pow(t, i) * Math.pow(1 - t, n - i); + point[0] += bernstein_poly * control_points[i][0]; + point[2] += bernstein_poly * control_points[i][1]; + point[1] += bernstein_poly * control_points[i][2]; + } + this.body_state.feet[index][0] += point[0]; + if (point[0] !== 0 || point[2] !== 0) { + this.body_state.feet[index][1] += point[1]; + } + this.body_state.feet[index][2] += point[2]; + return this.body_state.feet[index]; + } + + stance(index: number): number[] { + const t = this.phase; + const L = this.step_length / 2; + + const X_POLAR = Math.cos((this.gait_state.step_z * Math.PI) / 2); + const Y_POLAR = Math.sin((this.gait_state.step_z * Math.PI) / 2); + + const step = L * (1 - 2 * t); + const X = step * X_POLAR; + const Y = step * Y_POLAR; + let Z = 0; + + if (L !== 0) { + Z = -this.gait_state.step_depth * Math.cos((Math.PI * (X + Y)) / (2 * L)); + } + + this.body_state.feet[index][0] += X; + this.body_state.feet[index][2] += Y; + this.body_state.feet[index][1] += Z; + + return this.body_state.feet[index]; + } + + comb(n: number, k: number): number { + if (k < 0 || k > n) { + return 0; + } + 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); + } + return c; + } + + get_control_points(): number[][] { + const L = this.step_length / 2; + const CH = this.gait_state.step_height; + + const STEP = [ + -L, + -L * 1.4, + -L * 1.5, + -L * 1.5, + -L * 1.5, + 0.0, + 0.0, + 0.0, + L * 1.5, + L * 1.5, + L * 1.4, + L + ]; + + const X_POLAR = Math.cos((this.gait_state.step_z * Math.PI) / 2); + const Y_POLAR = Math.sin((this.gait_state.step_z * Math.PI) / 2); + + const control_points: number[][] = []; + + for (let i = 0; i < STEP.length; i++) { + const X = STEP[i] * X_POLAR; + const Y = STEP[i] * Y_POLAR; + let Z = 0.0; + + if (i === 0 || i === 1 || i === 10 || i === 11) { + Z = 0.0; + } else if (i >= 2 && i <= 6) { + Z = CH * 0.9; + } else if (i >= 7 && i <= 9) { + Z = CH * 1.1; + } + + control_points.push([X, Y, Z]); + } + + return control_points; + } +}