🦾 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;
     }
 
@@ -55,6 +95,9 @@
         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')
         visibility.add(settings, 'Trace points', 1, 1000, 1)

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.inverse(this.Tlf), Lp[0])),
-			this.legIK(this.multiplyVector(this.Ix, this.multiplyVector(this.inverse(this.Trf), Lp[1]))),
+			...this.legIK(
-			this.legIK(this.multiplyVector(this.inverse(this.Tlb), Lp[2])),
+				this.multiplyVector(this.Ix, this.multiplyVector(this.inverse(this.Trf), Lp[1]))
-			this.legIK(this.multiplyVector(this.Ix, this.multiplyVector(this.inverse(this.Trb), Lp[3])))
+			),
+			...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 },

esp32/lib/ESP32-sveltekit/Kinematics.h

@@ -4,14 +4,55 @@
 #include <cmath>
 #include <esp_dsp.h>
 
+static esp_err_t inverse(float a[4][4], float b[4][4])
+    {
+        float s0 = a[0][0] * a[1][1] - a[1][0] * a[0][1];
+        float s1 = a[0][0] * a[1][2] - a[1][0] * a[0][2];
+        float s2 = a[0][0] * a[1][3] - a[1][0] * a[0][3];
+        float s3 = a[0][1] * a[1][2] - a[1][1] * a[0][2];
+        float s4 = a[0][1] * a[1][3] - a[1][1] * a[0][3];
+        float s5 = a[0][2] * a[1][3] - a[1][2] * a[0][3];
+
+        float c5 = a[2][2] * a[3][3] - a[3][2] * a[2][3];
+        float c4 = a[2][1] * a[3][3] - a[3][1] * a[2][3];
+        float c3 = a[2][1] * a[3][2] - a[3][1] * a[2][2];
+        float c2 = a[2][0] * a[3][3] - a[3][0] * a[2][3];
+        float c1 = a[2][0] * a[3][2] - a[3][0] * a[2][2];
+        float c0 = a[2][0] * a[3][1] - a[3][0] * a[2][1];
+
+        // Should check for 0 determinant
+        float det = (s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0);
+
+        if (det == 0.0) return ESP_FAIL;
+
+        float invdet = 1.0 / det;
+
+
+        b[0][0] = ( a[1][1] * c5 - a[1][2] * c4 + a[1][3] * c3) * invdet;
+        b[0][1] = (-a[0][1] * c5 + a[0][2] * c4 - a[0][3] * c3) * invdet;
+        b[0][2] = ( a[3][1] * s5 - a[3][2] * s4 + a[3][3] * s3) * invdet;
+        b[0][3] = (-a[2][1] * s5 + a[2][2] * s4 - a[2][3] * s3) * invdet;
+
+        b[1][0] = (-a[1][0] * c5 + a[1][2] * c2 - a[1][3] * c1) * invdet;
+        b[1][1] = ( a[0][0] * c5 - a[0][2] * c2 + a[0][3] * c1) * invdet;
+        b[1][2] = (-a[3][0] * s5 + a[3][2] * s2 - a[3][3] * s1) * invdet;
+        b[1][3] = ( a[2][0] * s5 - a[2][2] * s2 + a[2][3] * s1) * invdet;
+
+        b[2][0] = ( a[1][0] * c4 - a[1][1] * c2 + a[1][3] * c0) * invdet;
+        b[2][1] = (-a[0][0] * c4 + a[0][1] * c2 - a[0][3] * c0) * invdet;
+        b[2][2] = ( a[3][0] * s4 - a[3][1] * s2 + a[3][3] * s0) * invdet;
+        b[2][3] = (-a[2][0] * s4 + a[2][1] * s2 - a[2][3] * s0) * invdet;
+
+        b[3][0] = (-a[1][0] * c3 + a[1][1] * c1 - a[1][2] * c0) * invdet;
+        b[3][1] = ( a[0][0] * c3 - a[0][1] * c1 + a[0][2] * c0) * invdet;
+        b[3][2] = (-a[3][0] * s3 + a[3][1] * s1 - a[3][2] * s0) * invdet;
+        b[3][3] = ( a[2][0] * s3 - a[2][1] * s1 + a[2][2] * s0) * invdet;
+
+        return ESP_OK;
+    }
+
 typedef  struct {
-    float omega;
+    float omega, phi, psi, xm, ym, zm;
-    float phi;
-    float psi;
-    float xm;
-    float ym;
-    float zm;
-    bool set;
 } position_t;
 
 
@@ -21,18 +62,26 @@ typedef  struct {
 class Kinematics
 {
 private:
-    dspm::Mat Tlf;
+    float Trb[4][4] = {0,};
-    dspm::Mat Trf;
+    float Trf[4][4] = {0,};
-    dspm::Mat Tlb;
+    float Tlb[4][4] = {0,};
-    dspm::Mat Trb;
+    float Tlf[4][4] = {0,};
 
-    float inverse[4][4];
+    const float Ix[4][4] = {{-1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};
+    float inv[4][4];
+    float point[4];
+    float Q1[4][4];
 
-    dspm::Mat Ix;
+
+    const float sHp = sin(PI / 2);
+    const float cHp = cos(PI / 2);
+
+    float point_lf[4][4];
 
 public:
     float l1, l2, l3, l4;
     float L, W;
+
     Kinematics(){
         l1 = 50;
         l2 = 20;
@@ -45,31 +94,29 @@ public:
     ~Kinematics(){}
 
     esp_err_t calculate_inverse_kinematics(float lp[4][4], position_t p, float result[12]) {
+        esp_err_t ret = ESP_OK;
 
-        Tlf.clear();
+        ret = bodyIK(p);
-        Trf.clear();
-        Tlb.clear();
-        Trb.clear();
 
-        float Ix_data[] = {-1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
+        ret += inverse(Tlf, inv);
-        dspm::Mat Ix(Ix_data, 4, 4);
+        dspm_mult_f32_ae32((float*) inv, (float*) lp[0], (float*) point, 4, 4, 1);
+        legIK((float*) point, result);
 
-        esp_err_t res = bodyIK(p);
+        ret += inverse(Trf, inv);
+        dspm_mult_f32_ae32((float*) Ix, (float*) inv, (float*) Q1, 4, 4, 4);
+        dspm_mult_f32_ae32((float*) Q1, (float*) lp[1], (float*) point, 4, 4, 1);
+        legIK((float*) point, result + 3);
 
-        dspm::Mat result_vec(4, 4);
+        ret += inverse(Tlb, inv);
-        result_vec = (Tlf.inverse() * dspm::Mat(lp[0], 4, 1));
+        dspm_mult_f32_ae32((float*) inv, (float*) lp[2], (float*) point, 4, 4, 1);
-        legIK(result_vec.data, result);
+        legIK((float*) point, result + 6);
 
-        result_vec = Ix * (Trf.inverse() * dspm::Mat(lp[1], 4, 1));
+        ret += inverse(Trb, inv);
-        legIK(result_vec.data, result + 3);
+        dspm_mult_f32_ae32((float*) Ix, (float*) inv, (float*) Q1, 4, 4, 4);
+        dspm_mult_f32_ae32((float*) Q1, (float*) lp[3], (float*) point, 4, 4, 1);
+        legIK((float*) point, result + 9);
 
-        result_vec = (Tlb.inverse() * dspm::Mat(lp[2], 4, 1));
+        return ret;
-        legIK(result_vec.data, result + 6);
-
-        result_vec = Ix * (Trb.inverse() * dspm::Mat(lp[3], 4, 1));
-        legIK(result_vec.data, result + 9);
-
-        return res;
     }
 
     esp_err_t bodyIK(position_t p) {
@@ -80,96 +127,63 @@ public:
         float cos_psi = cos(p.psi*DEGREES2RAD);
         float sin_psi = sin(p.psi*DEGREES2RAD);
         
-        float Rx_data[] = {
+        float Tm[4][4] = {
-            1, 0, 0, 0,
+            {cos_phi * cos_psi, -sin_psi * cos_phi, sin_phi, p.xm},
-            0, cos_omega, -sin_omega, 0,
+            {sin_omega * sin_phi * cos_psi + sin_psi * cos_omega, -sin_omega * sin_phi * sin_psi + cos_omega * cos_psi, -sin_omega * cos_phi, p.ym},
-            0, sin_omega, cos_omega, 0,
+            {sin_omega * sin_psi - sin_phi * cos_omega * cos_psi, sin_omega * cos_psi + sin_phi * sin_psi * cos_omega, cos_omega * cos_phi, p.zm},
-            0, 0, 0, 1
+            {0, 0, 0, 1}
         };
-        dspm::Mat Rx(Rx_data, 4, 4);
 
-        float Ry_data[] = {
+        float point_lf[4][4] = {
-            cos_phi, 0, sin_phi, 0,
+            {cHp, 0, sHp, L / 2}, 
-            0, 1, 0, 0,
+            {0, 1, 0, 0}, 
-            -sin_phi, 0, cos_phi, 0,
+            {-sHp, 0, cHp, W / 2}, 
-            0, 0, 0, 1
+            {0, 0, 0, 1}
         };
-        dspm::Mat Ry(Ry_data, 4, 4);
 
-        float Rz_data[] = {
+        float point_rf[4][4] = {
-            cos_psi, -sin_psi, 0, 0,
+            {cHp, 0, sHp, L / 2}, 
-            sin_psi, cos_psi, 0, 0,
+            {0, 1, 0, 0}, 
-            0, 0, 1, 0,
+            {-sHp, 0, cHp, -W / 2}, 
-            0, 0, 0, 1
+            {0, 0, 0, 1}
         };
-        dspm::Mat Rz(Rz_data, 4, 4);
 
-        dspm::Mat Rxyz = Rx * Ry * Rz;
+        float point_lb[4][4] = {
-
+            {cHp, 0, sHp, -L / 2}, 
-        float T_data[] = {
+            {0, 1, 0, 0}, 
-            0, 0, 0, p.xm,
+            {-sHp, 0, cHp, W / 2}, 
-            0, 0, 0, p.ym,
+            {0, 0, 0, 1}
-            0, 0, 0, p.zm,
-            0, 0, 0, 0
         };
-        dspm::Mat T(T_data, 4, 4);
 
-        dspm::Mat Tm = T + Rxyz;
+        float point_rb[4][4] = {
-
+            {cHp, 0, sHp, -L / 2}, 
-        float sHp = sin(M_PI / 2);
+            {0, 1, 0, 0}, 
-        float cHp = cos(M_PI / 2);
+            {-sHp, 0, cHp, -W / 2}, 
-
+            {0, 0, 0, 1}
-        float points_lf[] = {
-            cHp, 0, sHp, L / 2,
-            0, 1, 0, 0,
-            -sHp, 0, cHp, W / 2,
-            0, 0, 0, 1
         };
-        Tlf = Tm * dspm::Mat(points_lf, 4, 4);
 
-        float points_rf[] = {
+        dspm_mult_f32_ae32((float*) Tm, (float*) point_lf, (float*) Trb, 4, 4, 4);
-            cHp, 0, sHp, L / 2,
+        dspm_mult_f32_ae32((float*) Tm, (float*) point_rf, (float*) Trf, 4, 4, 4);
-            0, 1, 0, 0,
+        dspm_mult_f32_ae32((float*) Tm, (float*) point_lb, (float*) Tlf, 4, 4, 4);
-            -sHp, 0, cHp, -W / 2,
+        dspm_mult_f32_ae32((float*) Tm, (float*) point_rb, (float*) Tlb, 4, 4, 4);
-            0, 0, 0, 1
-        };
-        Trf = Tm * dspm::Mat(points_rf, 4, 4);
-
-        float points_lb[] = {
-            cHp, 0, sHp, -L / 2,
-            0, 1, 0, 0,
-            -sHp, 0, cHp, W / 2,
-            0, 0, 0, 1
-        };
-        Tlb = Tm * dspm::Mat(points_lb, 4, 4);
-
-        float points_rb[] = {
-            cHp, 0, sHp, -L / 2,
-            0, 1, 0, 0,
-            -sHp, 0, cHp, -W / 2,
-            0, 0, 0, 1
-        };
-        Trb = Tm * dspm::Mat(points_rb, 4, 4);
         return ESP_OK;
     }
 
     void legIK(float point[4], float result[3]) {
-        float x = point[0];
+        float x = point[0], y = point[1], z = point[2];
-        float y = point[1];
-        float z = point[2];
 
         float F = sqrt(x * x + y * y - l1 * l1);
         if (isnan(F)) F = l1;
         float G = F - l2;
         float H = sqrt(G * G + z * z);
 
-        result[0] = -atan2(y, x) - atan2(F, -l1);
+        float theta1 = -atan2(y, x) - atan2(F, -l1);
-        result[2] = acos((H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4));
+        float theta3 = acos((H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4));
-        if (isnan(result[2])) result[2] = 0;
+        if (isnan(theta3)) theta3 = 0;
-        result[1] = atan2(z, G) - atan2(l4 * sin(result[2]), l3 + l4 * cos(result[2]));
+        float theta2 = atan2(z, G) - atan2(l4 * sin(theta3), l3 + l4 * cos(theta3));
-        result[0] *= RAD2DEGREES;
+        result[0] = theta1 * RAD2DEGREES;
-        result[1] *= RAD2DEGREES;
+        result[1] = theta2 * RAD2DEGREES;
-        result[2] *= RAD2DEGREES;
+        result[2] = theta3 * RAD2DEGREES;
     }
 };
 

esp32/lib/ESP32-sveltekit/MotionService.h

@@ -59,7 +59,7 @@ class MotionService
         {
             input[i] = array[i];
         }
-        ESP_LOGI("MotionService", "Input: %d %d %d %d %d %d %d", input[0], input[1], input[2], input[3], input[4], input[5], input[6]);
+        ESP_LOGI("MotionService", "Input: %.0f %.0f %.0f %.0f %.0f %.0f %.0f", input[0], input[1], input[2], input[3], input[4], input[5], input[6]);
     }
 
     void handleMode(JsonObject &root, int originId)
@@ -73,13 +73,13 @@ class MotionService
 
     void syncAngles(const String &originId = "", bool sync = false) {
         char output[100];
-        sprintf(output, "[%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d]", angles[0], angles[1], angles[2], angles[3], angles[4],
+        sprintf(output, "[%0.f,%0.f,%0.f,%0.f,%0.f,%0.f,%0.f,%0.f,%0.f,%0.f,%0.f,%0.f]", angles[0], angles[1], angles[2], angles[3], angles[4],
                 angles[5], angles[6], angles[7], angles[8], angles[9], angles[10], angles[11]);
         _socket->emit(ANGLES_EVENT, output, String(originId).c_str());
 
     }
 
-    int lerp(int start, int end, float t) {
+    float lerp(float start, float end, float t) {
         return (1 - t) * start + t * end;
     }
 
@@ -90,7 +90,7 @@ class MotionService
                 break;
             case MOTION_STATE::REST:
                 for (int i = 0; i < 12; i++) {
-                    int16_t new_angle = lerp(angles[i], rest_angles[i], 0.5);
+                    float new_angle = lerp(angles[i], rest_angles[i], 0.5);
                     if (new_angle != angles[i]) {
                         angles[i] = new_angle;
                         updated = true;
@@ -106,27 +106,34 @@ class MotionService
                     {-100, -100,  100, 1},
                     {-100, -100, -100, 1}
                 };
-                float lx = static_cast<float>(input[1]);
+                float lx = input[1];
-                float ly = static_cast<float>(input[2]);
+                float ly = input[2];
-                float rx = static_cast<float>(input[3]);
+                float rx = input[3];
-                float ry = static_cast<float>(input[4]);
+                float ry = input[4];
-                float h = static_cast<float>(input[5]);
+                float h = input[5];
-                float s = static_cast<float>(input[6]);
+                float s = input[6];
-                position_t p = {0, rx / 4, ry / 4, ly / 2, h, lx / 2, input[0]};
+                position_t p = {
+                    0, 
+                    rx / 4, 
+                    ry / 4, 
+                    ly / 2, 
+                    (h+128)*0.7, 
+                    lx / 2
+                };
                 float new_angles[12] = {0,};
                 float dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1};
 
                 kinematics.calculate_inverse_kinematics(lp, p, new_angles);
 
                 for (int i = 0; i < 12; i++) {
-                    int16_t new_angle = lerp(angles[i], new_angles[i] * dir[i], 0.3);
+                    float new_angle = lerp(angles[i], new_angles[i] * dir[i], 0.3);
                     if (new_angle != angles[i]) {
                         angles[i] = new_angle;
                         updated = true;
                     }
                 }
                 if (updated) {
-                    ESP_LOGI("MotionService", "New angles: %f %f %f %f %f %f %f %f %f %f %f %f", angles[0], angles[1], angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9], angles[10], angles[11]);
+                    ESP_LOGI("MotionService", "New angles: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f", angles[0], angles[1], angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9], angles[10], angles[11]);
                 }
                 break;
             }
@@ -156,10 +163,10 @@ class MotionService
 
     constexpr static int MotionInterval = 100;
 
-    int8_t input[7] = {0, 0, 0, 0, 0, 0, 0};
+    float input[7] = {0, 0, 0, 0, 0, 0, 0};
-    int16_t angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+    float angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-    int16_t rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
+    float rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
-    int16_t stand_angles[12] = {0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90};
+    float stand_angles[12] = {0, 45, -90, 0, 45, -90, 0, 45, -90, 0, 45, -90};
     MOTION_STATE motionState = MOTION_STATE::IDLE;
     unsigned long _lastUpdate;
     int dir = 2;