🏍️ Adds position event for updating kinematics

app/src/lib/components/Visualization.svelte

@@ -2,7 +2,7 @@
 	import { onDestroy, onMount } from 'svelte';
 	import { BufferGeometry, Line, LineBasicMaterial, Vector3, type NormalBufferAttributes } from 'three';
 	import uzip from 'uzip';
-	import { model, outControllerData, servoAnglesOut } from '$lib/stores';
+	import { kinematicData, 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';
@@ -38,9 +38,8 @@
         [-100, -100, -100, 1],
     ];
 
-
     let settings = {
-        'Internal kinematic':true,
+        'Internal kinematic':false,
         'Trace feet':debug,
         'Trace points': 30,
         'Fix camera on robot': true,
@@ -99,18 +98,29 @@
         general.add(settings, 'Internal kinematic')
 
         const kinematic = gui_panel.addFolder('Kinematics');
-        kinematic.add(settings, 'omega', -20, 20)
+        kinematic.add(settings, 'omega', -20, 20).onChange(updateKinematicPosition)
-        kinematic.add(settings, 'phi', -30, 30)
+        kinematic.add(settings, 'phi', -30, 30).onChange(updateKinematicPosition)
-        kinematic.add(settings, 'psi', -20, 15)
+        kinematic.add(settings, 'psi', -20, 15).onChange(updateKinematicPosition)
-        kinematic.add(settings, 'x', -90, 90)
+        kinematic.add(settings, 'x', -90, 90).onChange(updateKinematicPosition)
-        kinematic.add(settings, 'y', 0, 200)
+        kinematic.add(settings, 'y', 0, 200).onChange(updateKinematicPosition)
-        kinematic.add(settings, 'z', -130, 130)
+        kinematic.add(settings, 'z', -130, 130).onChange(updateKinematicPosition)
  
         const visibility = gui_panel.addFolder('Visualization');
         visibility.add(settings, 'Trace feet')
         visibility.add(settings, 'Trace points', 1, 1000, 1)
     }
 
+    const updateKinematicPosition = () => {
+        kinematicData.set([
+                settings.omega,
+                settings.phi,
+                settings.psi,
+                settings.x,
+                settings.y,
+                settings.z
+        ])
+    }
+
 	const cacheModelFiles = async () => {
 		let data = await fetch('/stl.zip').then((data) => data.arrayBuffer());
 
@@ -201,7 +211,11 @@
         if (settings['Internal kinematic']) calculate_kinematics()
 
 		robot.position.y = robot.position.y - Math.min(...toes.map(toe => toe.y));
-		robot.rotation.z = lerp(robot.rotation.z, degToRad($mpu.heading + 90), 0.1);
+		robot.position.z = lerp(robot.position.z, -settings.x / 100, 0.1);
+
+		robot.rotation.z = lerp(robot.rotation.z, degToRad(settings.phi + $mpu.heading + 90), 0.1);
+        robot.rotation.y = lerp(robot.rotation.y, degToRad(settings.omega - settings.z / 2.5), 0.1);
+        robot.rotation.x = lerp(robot.rotation.x, degToRad(settings.psi - 90), 0.1);
 
 		for (let i = 0; i < $jointNames.length; i++) {
 			currentModelAngles[i] = lerp(

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

@@ -23,6 +23,8 @@ export const mode: Writable<ModesEnum> = writable(ModesEnum.Idle);
 
 export const outControllerData = writable([0, 0, 0, 0, 0, 70, 0]);
 
+export const kinematicData = writable([0, 0, 0, 0, 100, 0]);
+
 export const input: Writable<ControllerInput> = writable({
 	left: { x: 0, y: 0 },
 	right: { x: 0, y: 0 },

app/src/routes/+layout.svelte

@@ -13,7 +13,7 @@
 	import Menu from './menu.svelte';
 	import Statusbar from './statusbar.svelte';
 	import Login from './login.svelte';
-	import { ModesEnum, mode, outControllerData, servoAngles, servoAnglesOut, socket } from '$lib/stores';
+	import { ModesEnum, kinematicData, mode, outControllerData, servoAngles, servoAnglesOut, socket } from '$lib/stores';
 	import type { Analytics, Battery, DownloadOTA } from '$lib/types/models';
 	import { api } from '$lib/api';
 
@@ -29,6 +29,7 @@
         outControllerData.subscribe((data) => socket.sendEvent("input", {data}));
         mode.subscribe((data) => socket.sendEvent("mode", {data}));
         servoAnglesOut.subscribe((data) => socket.sendEvent("angles", {data}));
+        kinematicData.subscribe((data) => socket.sendEvent("position", {data}));
 	});
 
     onDestroy(() => {

esp32/lib/ESP32-sveltekit/MotionService.h

@@ -9,6 +9,7 @@
 #define LIGHT_SETTINGS_ENDPOINT_PATH "/api/input"
 #define ANGLES_EVENT "angles"
 #define INPUT_EVENT "input"
+#define POSITION_EVENT "position"
 #define MODE_EVENT "mode"
 
 enum class MOTION_STATE
@@ -32,12 +33,16 @@ class MotionService
         _socket->registerEvent(INPUT_EVENT);
         _socket->registerEvent(ANGLES_EVENT);
         _socket->registerEvent(MODE_EVENT);
+        _socket->registerEvent(POSITION_EVENT);
 
         _socket->onEvent(INPUT_EVENT, [&](JsonObject &root, int originId) { handleInput(root, originId); });
 
         _socket->onEvent(MODE_EVENT, [&](JsonObject &root, int originId) { handleMode(root, originId); });
 
         _socket->onEvent(ANGLES_EVENT, [&](JsonObject &root, int originId) { anglesEvent(root, originId); });
+
+        _socket->onEvent(POSITION_EVENT, [&](JsonObject &root, int originId) { positionEvent(root, originId); });
+
         _socket->onSubscribe(ANGLES_EVENT,
                              std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2));
     }
@@ -52,6 +57,18 @@ class MotionService
         syncAngles(String(originId));
     }
 
+    void positionEvent(JsonObject &root, int originId)
+    {
+        JsonArray array = root["data"].as<JsonArray>();
+        position.omega = array[0];
+        position.phi = array[1];
+        position.psi = array[2];
+        position.xm = array[3];
+        position.ym = array[4];
+        position.zm = array[5];
+        // syncAngles(String(originId));
+    }
+
     void handleInput(JsonObject &root, int originId)
     {
         JsonArray array = root["data"].as<JsonArray>();
@@ -59,6 +76,20 @@ class MotionService
         {
             input[i] = array[i];
         }
+        float lx = input[1];
+        float ly = input[2];
+        float rx = input[3];
+        float ry = input[4];
+        float h = input[5];
+        float s = input[6];
+        position = {
+            0, 
+            rx / 4, 
+            ry / 4, 
+            ly / 2, 
+            (h + 128) * (float)0.7, 
+            lx / 2
+        };
         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]);
     }
 
@@ -76,7 +107,6 @@ class MotionService
         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());
-
     }
 
     float lerp(float start, float end, float t) {
@@ -99,31 +129,8 @@ class MotionService
                 break;
 
             case MOTION_STATE::STAND: {
-
-                float lp[4][4] = {
-                    { 100, -100,  100, 1},
-                    { 100, -100, -100, 1},
-                    {-100, -100,  100, 1},
-                    {-100, -100, -100, 1}
-                };
-                float lx = input[1];
-                float ly = input[2];
-                float rx = input[3];
-                float ry = input[4];
-                float h = input[5];
-                float s = input[6];
-                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, position, new_angles);
-
-                kinematics.calculate_inverse_kinematics(lp, p, new_angles);
 
                 for (int i = 0; i < 12; i++) {
                     float new_angle = lerp(angles[i], new_angles[i] * dir[i], 0.3);
@@ -138,10 +145,20 @@ class MotionService
                 break;
             }
             case MOTION_STATE::WALK:
-                angles[1] += dir;
+                float new_angles[12] = {0,};
-                if (angles[1] >= 90) dir = -1;
+                lp[0][1] += walk_dir;
-                if (angles[1] <= 0) dir = 1;
+                if (lp[0][1] >= 200) walk_dir = -1;
-                updated = true;
+                if (lp[0][1] <= 100) walk_dir = 1;
+
+                kinematics.calculate_inverse_kinematics(lp, position, new_angles);
+
+                for (int i = 0; i < 12; i++) {
+                    float new_angle = lerp(angles[i], new_angles[i] * dir[i], 0.3);
+                    if (new_angle != angles[i]) {
+                        angles[i] = new_angle;
+                        updated = true;
+                    }
+                }
                 break;
         }
         return updated;
@@ -163,13 +180,22 @@ class MotionService
 
     constexpr static int MotionInterval = 100;
 
+    position_t position = {0, 0, 0, 0, 0, 0};
+    float dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1};
+    float lp[4][4] = {
+        { 100, -100,  100, 1},
+        { 100, -100, -100, 1},
+        {-100, -100,  100, 1},
+        {-100, -100, -100, 1}
+    };
+
     float input[7] = {0, 0, 0, 0, 0, 0, 0};
     float angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
     float rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
     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;
+    int walk_dir = 2;
 };
 
 #endif