🦘 Cleans gait source

app/src/lib/components/Visualization.svelte

@@ -2,10 +2,9 @@
 	import { onDestroy, onMount } from 'svelte';
 	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 { ModesEnum, kinematicData, mode, model, outControllerData, servoAnglesOut, servoAngles, mpu, jointNames } from '$lib/stores';
-	import { footColor, fromInt8, isEmbeddedApp, throttler, toeWorldPositions } from '$lib/utilities';
+	import { footColor, isEmbeddedApp, throttler, toeWorldPositions } from '$lib/utilities';
 	import { fileService } from '$lib/services';
-	import { servoAngles, mpu, jointNames } from '$lib/stores';
 	import SceneBuilder from '$lib/sceneBuilder';
 	import { lerp, degToRad } from 'three/src/math/MathUtils';
     import { GUI } from 'three/addons/libs/lil-gui.module.min.js';
@@ -36,6 +35,7 @@
     let target_position = {x: 0, z: 0, yaw: 0}
 
     let kinematic = new Kinematic()
+
     let planners = {
         [ModesEnum.Idle]: new IdleState(),
         [ModesEnum.Rest]: new RestState(),
@@ -44,12 +44,6 @@
         [ModesEnum.Walk]: new FourPhaseWalkState()
     }
     const dir = [1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1]
-    const Lp = [
-        [1, -1, 1, 1],
-        [1, -1, -1, 1],
-        [-1, -1, 1, 1],
-        [-1, -1, -1, 1],
-    ];
 
     let body_state = {
         omega: 0,
@@ -58,11 +52,11 @@
         xm: 0,
         ym: 0.5,
         zm: 0,
-            feet: Lp
+        feet: planners[ModesEnum.Idle].default_feet_pos
     }
 
     let settings = {
-        'Internal kinematic':true,
+        'Internal kinematic':false,
         'Robot transform controls':false,
         'Auto orient robot':true,
         'Trace feet':debug,
@@ -85,17 +79,17 @@
         servoAngles.subscribe(updateAnglesFromStore)
 	});
 
+    onDestroy(() => {
+        canvas.remove()
+        gui_panel?.destroy()
+    });
+    
     const updateAnglesFromStore = (angles: number[]) => {
         if (sceneManager.isDragging) return
         if (settings['Internal kinematic']) return
         modelTargetAngles = angles;
     }
 
-	onDestroy(() => {
-        canvas.remove()
-        gui_panel?.destroy()
-    });
-
     const createPanel = () => {
         gui_panel = new GUI({width: 310});
         gui_panel.close();
@@ -170,6 +164,7 @@
         const geometry = new SphereGeometry(0.1, 32, 16 ); 
         const material = new MeshBasicMaterial( { color: 0xffff00 } ); 
         target = new Mesh(geometry, material); 
+
         if (debug) {
             sceneManager.scene.add(target);
             sceneManager.addDragControl(updateAngles)
@@ -250,7 +245,9 @@
             s: controlData[6],
         };
         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
@@ -292,11 +289,7 @@
         sceneManager.transformControl.showZ = settings['Robot transform controls']
         
 		for (let i = 0; i < $jointNames.length; i++) {
-            currentModelAngles[i] = smooth(
+            currentModelAngles[i] = smooth((robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI), modelTargetAngles[i], 0.1);
-                (robot.joints[$jointNames[i]].angle as number) * (180 / Math.PI),
-				modelTargetAngles[i],
-				0.1
-                );
             robot.joints[$jointNames[i]].setJointValue(degToRad(currentModelAngles[i]));
         }
 

app/src/lib/gait.ts

@@ -27,7 +27,7 @@ export abstract class GaitState {
 
 	protected static body_state: body_state_t;
 
-	protected get default_feet_pos() {
+	public get default_feet_pos() {
 		return [
 			[1, -1, 1, 1],
 			[1, -1, -1, 1],
@@ -54,7 +54,7 @@ export abstract class GaitState {
 		return {
 			step_height: 0.4,
 			step_x: (Math.floor(fromInt8(command.ly, -1, 1) * 10) / 10) * 3,
-			step_z: (Math.floor(fromInt8(command.lx, -1, 1) * 10) / 10) * 3,
+			step_z: -(Math.floor(fromInt8(command.lx, -1, 1) * 10) / 10) * 3,
 			step_velocity: 1,
 			step_angle: 0,
 			step_depth: 0.2