🦾 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')