🪠 Removes feature code

app/src/lib/components/Visualization.svelte

@@ -45,7 +45,7 @@
     ];
 
     let settings = {
-        'Internal kinematic':true,
+        'Internal kinematic':false,
         'Robot transform controls':false,
         'Auto orient robot':true,
         'Trace feet':debug,
@@ -87,14 +87,6 @@
                     settings.x = data.ly / 2 
                     settings.z = data.lx / 2
                     break;
-
-                case ModesEnum.Walk:
-                    // target_position.yaw += data.lx / 100
-                    // const angle = radToDeg(Math.atan2(data.rx, data.ry)) - 90 + $mpu.heading + target_position.yaw;
-                    // const distance = Math.sqrt(data.rx**2 + data.ry**2) / 25
-                    // target_position.x = distance * Math.cos(degToRad(angle))
-                    // target_position.z = distance * Math.sin(degToRad(angle))
-                    break;
             }
         })
 	});
@@ -267,6 +259,11 @@
         settings.z = -robot.position.x * 100
     }
 
+    const updateTargetPosition = () => {
+        target.position.x = lerp(target.position.x, target_position.x, 0.5)
+        target.position.z = lerp(target.position.z, target_position.z, 0.5)
+    }
+
 	const render = () => {
 		const robot = sceneManager.model;
 		if (!robot) return;
@@ -293,10 +290,7 @@
         }
 
         orient_robot(robot, toes) 
-
+        updateTargetPosition();
-        target.position.x = lerp(target.position.x, target_position.x, 0.5)
-        // target.position.y = lerp(target.position.y, robot.position.y, 0.5)
-        target.position.z = lerp(target.position.z, target_position.z, 0.5)
 	};
 
 

app/src/lib/kinematic.ts

@@ -33,7 +33,7 @@ export default class Kinematic {
 	L: number;
 	W: number;
 
-	DEGREES2RAD = 0.017453292519943;
+	DEG2RAD = 0.017453292519943;
 
 	sHp = sin(Math.PI / 2);
 	cHp = cos(Math.PI / 2);
@@ -115,12 +115,12 @@ export default class Kinematic {
 	}
 
 	bodyIK(p: body_state_t) {
-		const cos_omega = cos(p.omega * this.DEGREES2RAD);
+		const cos_omega = cos(p.omega * this.DEG2RAD);
-		const sin_omega = sin(p.omega * this.DEGREES2RAD);
+		const sin_omega = sin(p.omega * this.DEG2RAD);
-		const cos_phi = cos(p.phi * this.DEGREES2RAD);
+		const cos_phi = cos(p.phi * this.DEG2RAD);
-		const sin_phi = sin(p.phi * this.DEGREES2RAD);
+		const sin_phi = sin(p.phi * this.DEG2RAD);
-		const cos_psi = cos(p.psi * this.DEGREES2RAD);
+		const cos_psi = cos(p.psi * this.DEG2RAD);
-		const sin_psi = sin(p.psi * this.DEGREES2RAD);
+		const sin_psi = sin(p.psi * this.DEG2RAD);
 
 		const Tm: number[][] = [
 			[cos_phi * cos_psi, -sin_psi * cos_phi, sin_phi, p.xm],

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

@@ -19,7 +19,7 @@ export enum ModesEnum {
 	Walk
 }
 
-export const mode: Writable<ModesEnum> = writable(ModesEnum.Walk);
+export const mode: Writable<ModesEnum> = writable(ModesEnum.Idle);
 
 export const outControllerData = writable([0, 0, 0, 0, 0, 70, 0]);
 

esp32/build/firmware/Spot-Micro_esp32cam_0-0-1.md5

@@ -1 +0,0 @@
-e26accd60ea0ebc13e6ab02a735aa5ee

esp32/lib/ESP32-sveltekit/IMUService.h

@@ -1,11 +1,11 @@
 #pragma once
 
-#include "MPU6050_6Axis_MotionApps612.h"
-#include <Adafruit_Sensor.h>
 #include <Adafruit_BMP085_U.h>
 #include <Adafruit_HMC5883_U.h>
+#include <Adafruit_Sensor.h>
 #include <ArduinoJson.h>
 #include <EventSocket.h>
+#include <MPU6050_6Axis_MotionApps612.h>
 
 #define IMU_INTERVAL 500
 #define MAX_ESP_IMU_SIZE 500
@@ -149,7 +149,6 @@ protected:
             doc["x"] = round2(getAngleX());
             doc["y"] = round2(getAngleY());
             doc["z"] = round2(getAngleZ());
-            // doc["imu_temp"] = round2(getTemp());
         }
 #endif
 #if FT_ENABLED(FT_MAG)

esp32/lib/ESP32-sveltekit/Kinematics.h

@@ -1,55 +1,11 @@
 #ifndef Kinematics_h
 #define Kinematics_h
 
-#include <cmath>
 #include <esp_dsp.h>
+#include <cmath>
 
-static esp_err_t inverse(float a[4][4], float b[4][4])
+#define RAD2DEG 57.295779513082321 // 180 / PI
-    {
+#define DEG2RAD 0.017453292519943
-        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, phi, psi, xm, ym, zm;
@@ -64,10 +20,6 @@ typedef struct {
     }
 } body_state_t;
 
-
-#define RAD2DEGREES 57.295779513082321 // 180 / PI
-#define DEGREES2RAD 0.017453292519943
-
 class Kinematics
 {
 private:
@@ -81,7 +33,6 @@ private:
     float point[4];
     float Q1[4][4];
 
-
     const float sHp = sin(PI / 2);
     const float cHp = cos(PI / 2);
 
@@ -129,47 +80,42 @@ public:
     }
 
     esp_err_t bodyIK(body_state_t p) {
-        float cos_omega = cos(p.omega*DEGREES2RAD);
+        float cos_omega = cos(p.omega * DEG2RAD);
-        float sin_omega = sin(p.omega*DEGREES2RAD);
+        float sin_omega = sin(p.omega * DEG2RAD);
-        float cos_phi = cos(p.phi*DEGREES2RAD);
+        float cos_phi = cos(p.phi * DEG2RAD);
-        float sin_phi = sin(p.phi*DEGREES2RAD);
+        float sin_phi = sin(p.phi * DEG2RAD);
-        float cos_psi = cos(p.psi*DEGREES2RAD);
+        float cos_psi = cos(p.psi * DEG2RAD);
-        float sin_psi = sin(p.psi*DEGREES2RAD);
+        float sin_psi = sin(p.psi * DEG2RAD);
 
         float Tm[4][4] = {
             {cos_phi * cos_psi, -sin_psi * cos_phi, sin_phi, p.xm},
-            {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},
+            {sin_omega * sin_phi * cos_psi + sin_psi * cos_omega,
-            {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},
+             -sin_omega * sin_phi * sin_psi + cos_omega * cos_psi,
-            {0, 0, 0, 1}
+             -sin_omega * cos_phi, p.ym},
-        };
+            {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}};
 
-        float point_lf[4][4] = {
+        float point_lf[4][4] = {{cHp, 0, sHp, L / 2},
-            {cHp, 0, sHp, L / 2}, 
                                 {0, 1, 0, 0},
                                 {-sHp, 0, cHp, W / 2},
-            {0, 0, 0, 1}
+                                {0, 0, 0, 1}};
-        };
 
-        float point_rf[4][4] = {
+        float point_rf[4][4] = {{cHp, 0, sHp, L / 2},
-            {cHp, 0, sHp, L / 2}, 
                                 {0, 1, 0, 0},
                                 {-sHp, 0, cHp, -W / 2},
-            {0, 0, 0, 1}
+                                {0, 0, 0, 1}};
-        };
 
-        float point_lb[4][4] = {
+        float point_lb[4][4] = {{cHp, 0, sHp, -L / 2},
-            {cHp, 0, sHp, -L / 2}, 
                                 {0, 1, 0, 0},
                                 {-sHp, 0, cHp, W / 2},
-            {0, 0, 0, 1}
+                                {0, 0, 0, 1}};
-        };
 
-        float point_rb[4][4] = {
+        float point_rb[4][4] = {{cHp, 0, sHp, -L / 2},
-            {cHp, 0, sHp, -L / 2}, 
                                 {0, 1, 0, 0},
                                 {-sHp, 0, cHp, -W / 2},
-            {0, 0, 0, 1}
+                                {0, 0, 0, 1}};
-        };
 
         dspm_mult_f32_ae32((float*)Tm, (float*)point_lf, (float*)Tlf, 4, 4, 4);
         dspm_mult_f32_ae32((float*)Tm, (float*)point_rf, (float*)Trf, 4, 4, 4);
@@ -182,7 +128,7 @@ public:
         float x = point[0], y = point[1], z = point[2];
 
         float F = sqrt(x * x + y * y - l1 * l1);
-        if (isnan(F)) F = l1;
+        F = isnan(F) ? l1 : F;
         float G = F - l2;
         float H = sqrt(G * G + z * z);
 
@@ -190,44 +136,46 @@ public:
         float theta3 = acos((H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4));
         if (isnan(theta3)) theta3 = 0;
         float theta2 = atan2(z, G) - atan2(l4 * sin(theta3), l3 + l4 * cos(theta3));
-        result[0] = theta1 * RAD2DEGREES;
+        result[0] = theta1 * RAD2DEG;
-        result[1] = theta2 * RAD2DEGREES;
+        result[1] = theta2 * RAD2DEG;
-        result[2] = theta3 * RAD2DEGREES;
+        result[2] = theta3 * RAD2DEG;
-    }
-};
-
-typedef struct {
-    float x, z, yaw;
-} position_target_t;
-
-typedef struct {
-    float step_length;
-    float yaw_rate;
-    float lateral_shift;
-    float step_velocity;
-    float swing_period;
-    float clearance_height;
-    float penetration_depth;
-} gait_state_t;
-
-class GaitPlanner 
-{
-    public:
-        GaitPlanner() { }
-
-        ~GaitPlanner() { }
-
-        void update_trajectory(position_target_t position_target, float dt, body_state_t body_state) {
-            // for (int8_t i = 0; i < 4; i++) {
-            //     body_state.feet_position[i][0] = position_target.x;
-            //     body_state.feet_position[i][3] = position_target.z;
-            // }
     }
 
-    private:
+    esp_err_t inverse(float a[4][4], float b[4][4]) {
-        uint16_t time;
+        float s[] = {a[0][0] * a[1][1] - a[1][0] * a[0][1],
-        uint16_t target_yaw;
+                    a[0][0] * a[1][2] - a[1][0] * a[0][2],
+                    a[0][0] * a[1][3] - a[1][0] * a[0][3],
+                    a[0][1] * a[1][2] - a[1][1] * a[0][2],
+                    a[0][1] * a[1][3] - a[1][1] * a[0][3],
+                    a[0][2] * a[1][3] - a[1][2] * a[0][3]};
+        float c[] = {a[2][0] * a[3][1] - a[3][0] * a[2][1],
+                    a[2][0] * a[3][2] - a[3][0] * a[2][2],
+                    a[2][0] * a[3][3] - a[3][0] * a[2][3],
+                    a[2][1] * a[3][2] - a[3][1] * a[2][2],
+                    a[2][1] * a[3][3] - a[3][1] * a[2][3],
+                    a[2][2] * a[3][3] - a[3][2] * a[2][3]};
+        float det = s[0] * c[5] - s[1] * c[4] + s[2] * c[3] + s[3] * c[2] -
+                    s[4] * c[1] + s[5] * c[0];
+        if (det == 0.0) return ESP_FAIL;
+        float invdet = 1.0 / det;
+        b[0][0] = (a[1][1] * c[5] - a[1][2] * c[4] + a[1][3] * c[3]) * invdet;
+        b[0][1] = (-a[0][1] * c[5] + a[0][2] * c[4] - a[0][3] * c[3]) * invdet;
+        b[0][2] = (a[3][1] * s[5] - a[3][2] * s[4] + a[3][3] * s[3]) * invdet;
+        b[0][3] = (-a[2][1] * s[5] + a[2][2] * s[4] - a[2][3] * s[3]) * invdet;
+        b[1][0] = (-a[1][0] * c[5] + a[1][2] * c[2] - a[1][3] * c[1]) * invdet;
+        b[1][1] = (a[0][0] * c[5] - a[0][2] * c[2] + a[0][3] * c[1]) * invdet;
+        b[1][2] = (-a[3][0] * s[5] + a[3][2] * s[2] - a[3][3] * s[1]) * invdet;
+        b[1][3] = (a[2][0] * s[5] - a[2][2] * s[2] + a[2][3] * s[1]) * invdet;
+        b[2][0] = (a[1][0] * c[4] - a[1][1] * c[2] + a[1][3] * c[0]) * invdet;
+        b[2][1] = (-a[0][0] * c[4] + a[0][1] * c[2] - a[0][3] * c[0]) * invdet;
+        b[2][2] = (a[3][0] * s[4] - a[3][1] * s[2] + a[3][3] * s[0]) * invdet;
+        b[2][3] = (-a[2][0] * s[4] + a[2][1] * s[2] - a[2][3] * s[0]) * invdet;
+        b[3][0] = (-a[1][0] * c[3] + a[1][1] * c[1] - a[1][2] * c[0]) * invdet;
+        b[3][1] = (a[0][0] * c[3] - a[0][1] * c[1] + a[0][2] * c[0]) * invdet;
+        b[3][2] = (-a[3][0] * s[3] + a[3][1] * s[1] - a[3][2] * s[0]) * invdet;
+        b[3][3] = (a[2][0] * s[3] - a[2][1] * s[1] + a[2][2] * s[0]) * invdet;
+        return ESP_OK;
+    }
 };
 
-
 #endif

esp32/lib/ESP32-sveltekit/MotionService.h

@@ -43,8 +43,7 @@ class MotionService
 
         _socket->onEvent(POSITION_EVENT, [&](JsonObject &root, int originId) { positionEvent(root, originId); });
 
-        _socket->onSubscribe(ANGLES_EVENT,
+        _socket->onSubscribe(ANGLES_EVENT, std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2));
-                             std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2));
 
         body_state.updateFeet(default_feet_positions);
     }
@@ -68,7 +67,6 @@ class MotionService
         body_state.xm = array[3];
         body_state.ym = array[4];
         body_state.zm = array[5];
-        // syncAngles(String(originId));
     }
 
     void handleInput(JsonObject &root, int originId)
@@ -91,11 +89,6 @@ class MotionService
                 body_state.zm = lx / 2;
                 break;
             }
-            case MOTION_STATE::WALK:
-                position_target.x = rx / 2;
-                position_target.z = ry / 2;
-                position_target.yaw = lx / 2;
-                break;
         }
     }
 
@@ -135,8 +128,6 @@ class MotionService
                 break;
             }
             case MOTION_STATE::WALK:
-                gaitPlanner.update_trajectory(position_target, MotionInterval, body_state);
-
                 kinematics.calculate_inverse_kinematics(body_state, new_angles);
                 break;
         }
@@ -168,14 +159,12 @@ class MotionService
     SecurityManager *_securityManager;
     TaskManager *_taskManager;
     Kinematics kinematics;
-    GaitPlanner gaitPlanner;
 
     MOTION_STATE motionState = MOTION_STATE::IDLE;
     unsigned long _lastUpdate;
     constexpr static int MotionInterval = 100;
 
     body_state_t body_state = {0,};
-    position_target_t position_target = {0,};
 
     float dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1};
     float default_feet_positions[4][4] = {