🐎 Adds four phase gait

app/src/lib/components/Visualization.svelte

@@ -3,7 +3,7 @@
 	import { BufferGeometry, Line, LineBasicMaterial, Mesh, MeshBasicMaterial, Object3D, SphereGeometry, Vector3, type NormalBufferAttributes, type Object3DEventMap } from 'three';
 	import uzip from 'uzip';
 	import { ModesEnum, kinematicData, mode, model, outControllerData, servoAnglesOut } from '$lib/stores';
-	import { footColor, isEmbeddedApp, throttler, toeWorldPositions } from '$lib/utilities';
+	import { footColor, fromInt8, isEmbeddedApp, throttler, toeWorldPositions } from '$lib/utilities';
 	import { fileService } from '$lib/services';
 	import { servoAngles, mpu, jointNames } from '$lib/stores';
 	import SceneBuilder from '$lib/sceneBuilder';
@@ -55,6 +55,14 @@
             feet: Lp
         }
 
+    const gait_state = {
+        step_height:0.2,
+        step_x:1,
+        step_z:1,
+        step_velocity:1,
+        step_angle:0
+    }
+
     let settings = {
         'Internal kinematic':true,
         'Robot transform controls':false,
@@ -258,8 +266,12 @@
         const stepRotation = 0
         const stepPeriod = 0
         const direction = 1
-        body_state.feet = bezierGaitPlanner.loop(stepLength, stepAngle, stepRotation, stepPeriod, direction, body_state.feet);
 
+        const controlData = get(outControllerData)
+        gait_state.step_x = Math.floor(fromInt8(controlData[2], -1, 1) * 10) / 10 * 2
+        gait_state.step_z = Math.floor(fromInt8(controlData[1], -1, 1) * 10) / 10 * 2
+        gait_state.step_velocity = fromInt8(controlData[6], -1, 1)
+        body_state.feet = bezierGaitPlanner._loop(body_state, gait_state);
     }
 
     const update_robot_position = (robot:URDFRobot) => {

app/src/lib/kinematic.ts

@@ -1,4 +1,3 @@
-import { radToDeg } from 'three/src/math/MathUtils.js';
 
 export interface body_state_t {
 	omega: number;
@@ -10,6 +9,14 @@ export interface body_state_t {
 	feet: number[][];
 }
 
+export interface gait_state_t {
+	step_height: number;
+	step_x: number;
+	step_z: number;
+	step_angle: number;
+	step_velocity: number;
+}
+
 export interface position {
 	x: number;
 	y: number;
@@ -615,7 +622,7 @@ export class BezierGaitPlanner {
 	private tick = 0;
 	private phase = 0;
 	private phase_time = 0;
-	private total_phases_length = 120;
+	private total_phases_length = 60;
 	private num_phases = 4;
 	private phase_length = this.total_phases_length / num_phases;
 	private sub_phase_tick = 0;
@@ -643,7 +650,9 @@ export class BezierGaitPlanner {
 		[-1, -1, -1, 1]
 	];
 
-	body_state!: body_state_t;
+	private body_state!: body_state_t;
+	private gait_state!: gait_state_t;
+	private dt: number = 0.02;
 
 	constructor(mode: string) {
 		this._frame = Array.from({ length: 4 }, () => Array(3).fill(0));
@@ -661,8 +670,10 @@ export class BezierGaitPlanner {
 		}
 	}
 
-	_loop(body_state: body_state_t) {
+	_loop(body_state: body_state_t, gait_state: gait_state_t, dt: number = 0.02) {
 		this.body_state = body_state;
+		this.gait_state = gait_state;
+		this.dt = dt;
 		this.update_phase();
 		this.update_body_position();
 		this.update_feet_positions();
@@ -694,14 +705,34 @@ export class BezierGaitPlanner {
 	}
 
 	stand(index: number): number[] {
-		this.body_state.feet[index][0] = this.default_feet_pos[index][0] - this.phase_time * 0.5;
-		this.body_state.feet[index][1] = -1;
+		const delta_pos = [
+			(-this.gait_state.step_x * this.dt) / 3,
+			0,
+			(-this.gait_state.step_z * this.dt) / 3
+		];
+
+		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 swing_proportion = this.sub_phase_tick / swing_ticks;
-		this.body_state.feet[index][1] = -sin(this.phase_time * Math.PI);
+		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]) * dt * 8;
+		}
+
+		if (this.gait_state.step_z == 0) {
+			delta_pos[2] = (this.default_feet_pos[index][2] - this.body_state.feet[index][2]) * 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];
 	}
 	private static solve_bin_factor(n: number, k: number): number {

app/src/lib/utilities/math-utilities.ts

@@ -9,3 +9,10 @@ export const toInt8 = (number: number, min: number, max: number) => {
 	let scaled = ((number - min) / (max - min)) * 255 - 128;
 	return Math.max(-128, Math.min(127, Math.round(scaled))) | 0;
 };
+
+export const fromInt8 = (int8: number, min: number, max: number) => {
+	int8 = Math.max(-128, Math.min(127, int8));
+	const scaled = (int8 + 128) / 255;
+	const number = scaled * (max - min) + min;
+	return number;
+};