🦾 Makes embedded kinematics kinda work

app/src/lib/components/Visualization.svelte

@@ -2,13 +2,15 @@
 	import { onDestroy, onMount } from 'svelte';
 	import { BufferGeometry, Line, LineBasicMaterial, Vector3, type NormalBufferAttributes } from 'three';
 	import uzip from 'uzip';
-	import { model, servoAnglesOut } from '$lib/stores';
+	import { model, outControllerData, servoAnglesOut } from '$lib/stores';
 	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';
+	import Kinematic, { type position_t } from '$lib/kinematic';
+	import { radToDeg } from 'three/src/math/MathUtils.js';
 
     export let sky = true
     export let orbit = false
@@ -27,7 +29,10 @@
     let feet_trace = new Array(4).fill([]);
     let trace_lines: BufferGeometry<NormalBufferAttributes>[] = []
 
+    let kinematic = new Kinematic()
+
     let settings = {
+        'Internal kinematic':false,
         'Trace feet':debug,
         'Trace points': 30,
         'Fix camera on robot': true
@@ -38,10 +43,45 @@
         await createScene();
         if (!isEmbeddedApp && panel) createPanel();
         servoAngles.subscribe(updateAnglesFromStore)
+        outControllerData.subscribe((buffer) => {
+            if (!settings['Internal kinematic']) return
+
+            const data = {
+                stop: buffer[0],
+                lx: buffer[1],
+                ly: buffer[2],
+                rx: buffer[3],
+                ry: buffer[4],
+                h: buffer[5],
+                s: buffer[6],
+            };
+
+            const Lp = [
+                [100, -100, 100, 1],
+                [100, -100, -100, 1],
+                [-100, -100, 100, 1],
+                [-100, -100, -100, 1],
+            ];
+            
+            const dir = [-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1]
+
+            const position:position_t = {
+                omega: 0, 
+                phi: data.rx / 4, 
+                psi: data.ry / 4, 
+                xm: data.ly / 2, 
+                ym: (data.h+128)*0.75, 
+                zm: data.lx / 2
+            }
+
+            let new_angles = kinematic.calcIK(Lp, position).map((x, i) => radToDeg(x * dir[i]));
+            modelTargetAngles = new_angles;
+        })
 	});
     
     const updateAnglesFromStore = (angles: number[]) => {
         if (sceneManager.isDragging) return
+        if (settings['Internal kinematic']) return
         modelTargetAngles = angles;
     }
 
@@ -54,6 +94,9 @@
         gui_panel = new GUI({width: 310});
         gui_panel.close();
         gui_panel.domElement.id = 'three-gui-panel';
+
+        const general = gui_panel.addFolder('General');
+        general.add(settings, 'Internal kinematic')
  
         const visibility = gui_panel.addFolder('Visualization');
         visibility.add(settings, 'Trace feet')

app/src/lib/kinematic.ts

@@ -78,14 +78,18 @@ export default class Kinematic {
 		];
 	}
 
-	public calcIK(Lp: number[][], position: position_t): number[][] {
+	public calcIK(Lp: number[][], position: position_t): number[] {
 		this.bodyIK(position);
 
 		return [
-			this.legIK(this.multiplyVector(this.inverse(this.Tlf), Lp[0])),
-			this.legIK(this.multiplyVector(this.Ix, this.multiplyVector(this.inverse(this.Trf), Lp[1]))),
-			this.legIK(this.multiplyVector(this.inverse(this.Tlb), Lp[2])),
-			this.legIK(this.multiplyVector(this.Ix, this.multiplyVector(this.inverse(this.Trb), Lp[3])))
+			...this.legIK(this.multiplyVector(this.inverse(this.Tlf), Lp[0])),
+			...this.legIK(
+				this.multiplyVector(this.Ix, this.multiplyVector(this.inverse(this.Trf), Lp[1]))
+			),
+			...this.legIK(this.multiplyVector(this.inverse(this.Tlb), Lp[2])),
+			...this.legIK(
+				this.multiplyVector(this.Ix, this.multiplyVector(this.inverse(this.Trb), Lp[3]))
+			)
 		];
 	}
 

app/src/lib/stores/model-store.ts

@@ -21,7 +21,7 @@ export enum ModesEnum {
 
 export const mode: Writable<ModesEnum> = writable(ModesEnum.Idle);
 
-export const outControllerData = writable(new Array([0, 0, 0, 0, 0, 70, 0]));
+export const outControllerData = writable([0, 0, 0, 0, 0, 70, 0]);
 
 export const input: Writable<ControllerInput> = writable({
 	left: { x: 0, y: 0 },