From 215bfdf5825bdb1cf918fb8f0c90ecc2bec94c30 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Thu, 1 Aug 2024 20:45:20 +0200
Subject: [PATCH] =?UTF-8?q?=F0=9F=8E=B2=20Adds=20all=20gait=20planners=20f?=
 =?UTF-8?q?or=20internal=20kinematics?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 app/src/lib/components/Visualization.svelte | 18 ++++++++++++++----
 app/src/lib/gait.ts                         | 21 ++++++++++++++++++---
 app/src/lib/stores/model-store.ts           |  3 ++-
 3 files changed, 34 insertions(+), 8 deletions(-)

diff --git a/app/src/lib/components/Visualization.svelte b/app/src/lib/components/Visualization.svelte
index 8e70f20..b5e9142 100644
--- a/app/src/lib/components/Visualization.svelte
+++ b/app/src/lib/components/Visualization.svelte
@@ -10,7 +10,7 @@
 	import { lerp, degToRad } from 'three/src/math/MathUtils';
     import { GUI } from 'three/addons/libs/lil-gui.module.min.js';
 	import Kinematic, { type body_state_t } from '$lib/kinematic';
-    import {EightPhaseWalkState, FourPhaseWalkState, StandState} from '$lib/gait'
+    import {EightPhaseWalkState, FourPhaseWalkState, 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';
@@ -36,8 +36,13 @@
     let target_position = {x: 0, z: 0, yaw: 0}
 
     let kinematic = new Kinematic()
-    let gaitPlanner = new FourPhaseWalkState()
-    let standState = new StandState()
+    let planners = {
+        [ModesEnum.Idle]: new IdleState(),
+        [ModesEnum.Rest]: new RestState(),
+        [ModesEnum.Stand]: new StandState(),
+        [ModesEnum.Crawl]: new EightPhaseWalkState(),
+        [ModesEnum.Walk]: new FourPhaseWalkState()
+    }
     const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1]
     const Lp = [
         [1, -1, 1, 1],
@@ -244,10 +249,15 @@
                 h: controlData[5],
                 s: controlData[6],
             };
-        let planner = get(mode) === ModesEnum.Walk ? gaitPlanner : standState
+            body_state.ym = ((data.h + 128) * 0.75) / 100;
+        let planner = planners[get(mode)]
         body_state = planner.step(body_state, data);
 
+        settings.omega = body_state.omega
+        settings.phi = body_state.phi
+        settings.psi = body_state.psi 
         settings.xm = body_state.xm
+        settings.ym = body_state.ym
         settings.zm = body_state.zm
     }
 
diff --git a/app/src/lib/gait.ts b/app/src/lib/gait.ts
index 8b000f0..03a68d7 100644
--- a/app/src/lib/gait.ts
+++ b/app/src/lib/gait.ts
@@ -36,7 +36,7 @@ export abstract class GaitState {
 		];
 	}
 
-	protected static get default_height() {
+	protected get default_height() {
 		return 0.5;
 	}
 
@@ -47,7 +47,7 @@ export abstract class GaitState {
 		console.log('Ending', this.name);
 	}
 	step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
-		console.log('Stepping', this.name);
+		return body_state;
 	}
 
 	map_command(command: ControllerCommand): gait_state_t {
@@ -66,6 +66,21 @@ export class IdleState extends GaitState {
 	protected name = 'Idle';
 }
 
+export class RestState extends GaitState {
+	protected name = 'Rest';
+
+	step(body_state: body_state_t, command: ControllerCommand, dt: number = 0.02) {
+		body_state.omega = 0;
+		body_state.phi = 0;
+		body_state.psi = 0;
+		body_state.xm = 0;
+		body_state.ym = this.default_height / 2;
+		body_state.zm = 0;
+		body_state.feet = this.default_feet_pos;
+		return body_state;
+	}
+}
+
 export class StandState extends GaitState {
 	protected name = 'Stand';
 
@@ -75,7 +90,7 @@ export class StandState extends GaitState {
 		body_state.psi = command.ry / 8;
 		body_state.xm = command.ly / 2 / 100;
 		body_state.zm = command.lx / 2 / 100;
-		body_state.ym = ((command.h + 128) * 0.75) / 100;
+		body_state.feet = this.default_feet_pos;
 		return body_state;
 	}
 }
diff --git a/app/src/lib/stores/model-store.ts b/app/src/lib/stores/model-store.ts
index 791ce58..ab2cbd6 100644
--- a/app/src/lib/stores/model-store.ts
+++ b/app/src/lib/stores/model-store.ts
@@ -8,7 +8,7 @@ export const jointNames = persistentStore('joint_names', []);
 
 export const model = writable();
 
-export const modes = ['idle', 'rest', 'stand', 'walk'] as const;
+export const modes = ['idle', 'rest', 'stand', 'Crawl', 'walk'] as const;
 
 export type Modes = (typeof modes)[number];
 
@@ -16,6 +16,7 @@ export enum ModesEnum {
 	Idle,
 	Rest,
 	Stand,
+	Crawl,
 	Walk
 }