From 9705628f291b1251aa5f66f34227b79c8bdf8b60 Mon Sep 17 00:00:00 2001
From: Rune Harlyk <rune@harlyk.dk>
Date: Sat, 6 May 2023 02:24:50 +0200
Subject: [PATCH] =?UTF-8?q?=F0=9F=92=AF=20Adds=20initial=20IK=20and=20serv?=
 =?UTF-8?q?o=20functionality?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 include/IK_config.h    |  42 +++++++
 include/config.h       |  15 ++-
 include/ik_task.h      |  27 +++++
 include/servo.h        |  16 +++
 include/servo_config.h |  93 +++++++++++++++
 include/spot_ik.h      |  19 +++
 src/ik_task.cpp        | 139 ++++++++++++++++++++++
 src/main.cpp           |  15 ++-
 src/servo.cpp          |  38 ++++++
 src/spot_ik.cpp        | 264 +++++++++++++++++++++++++++++++++++++++++
 10 files changed, 659 insertions(+), 9 deletions(-)
 create mode 100644 include/IK_config.h
 create mode 100644 include/ik_task.h
 create mode 100644 include/servo.h
 create mode 100644 include/servo_config.h
 create mode 100644 include/spot_ik.h
 create mode 100644 src/ik_task.cpp
 create mode 100644 src/servo.cpp
 create mode 100644 src/spot_ik.cpp

diff --git a/include/IK_config.h b/include/IK_config.h
new file mode 100644
index 0000000..5e9cbc1
--- /dev/null
+++ b/include/IK_config.h
@@ -0,0 +1,42 @@
+#ifndef IK_CONFIG_H
+#define IK_CONFIG_H
+
+#include <Arduino.h>
+
+// SERVOS
+#define Servo_Foot     0
+#define Servo_Leg      1
+#define Servo_Shoulder 2
+
+#define LEG_LF  0 // 0, 1, 2
+#define LEG_RF  1 // 3, 4, 5
+#define LEG_LB  2 // 6, 7, 8
+#define LEG_RB  3 // 9, 10, 11
+
+#define L1  60.5  // y Distance between Shoulder Servo and Leg
+#define L2  10    // z Distance between Shoulder Servo and Leg
+#define L3  100.7 // Length of upper leg
+#define L4  118.5 // Length of lower leg
+#define L   207.5 // Distance between front and back servos
+#define W   78    // Distance between left and right shoulder
+
+// predefined calculations
+#define L1L1  3660.25  //L1*L1
+// #define L1L2  3760.25  //L1*L1+L2*L2
+// #define LL12  1210     //2*L1*L2
+#define L3L3  10140.49 //L3*L3
+#define L4L4  14042.25 //L4*L4
+#define LL34  23865.9  //2*L3*L4
+
+#define SERVO_STEP_ANGLE 2
+#define MOTION_STEP_ANGLE 5
+#define MOTION_STEP_MOVEMENT 5
+#define MOTION_STEP_ALFA 0.20
+
+extern const int16_t servo_min[12] ;
+extern const float servo_conversion[12] ;
+extern const float theta_range[3][2];
+extern const int8_t servo_invert[12];
+extern int16_t servo_angles[4][3];
+
+#endif
diff --git a/include/config.h b/include/config.h
index bd33c2b..12e3122 100644
--- a/include/config.h
+++ b/include/config.h
@@ -33,6 +33,7 @@
  */
 #define SCREEN_WIDTH 128
 #define SCREEN_HEIGHT 64
+#define SCREEN_RESET -1
 
 
 /*
@@ -54,4 +55,16 @@
  */
 #define USS_LEFT 12
 #define USS_RIGHT 13
-#define USS_MAX_DISTANCE 200
\ No newline at end of file
+#define USS_MAX_DISTANCE 200
+
+/*
+ * Button settings
+ */
+#define BUTTON 16
+#define BUTTON_LED 2
+
+/*
+ * PWM controller settings
+ */
+#define SERVO_OSCILLATOR_FREQUENCY 27000000
+#define SERVO_FREQ 50
\ No newline at end of file
diff --git a/include/ik_task.h b/include/ik_task.h
new file mode 100644
index 0000000..f8bafca
--- /dev/null
+++ b/include/ik_task.h
@@ -0,0 +1,27 @@
+#ifndef IK_TASK_H
+#define IK_TASK_H
+
+#include <Arduino.h>
+#include <servo.h>
+#include <IK_config.h>
+#include <spot_ik.h>
+
+#define DEGREES2RAD 0.017453292519943
+
+typedef  struct {
+    float omega;
+    float phi;
+    float psi;
+    float xm;
+    float ym;
+    float zm;
+    bool set;
+} position_t;
+
+void set_orientation_cb(int16_t omega, int16_t phi, int16_t psi, int16_t xm, int16_t ym, int16_t zm);
+
+void reset_position();
+
+void task_ik(void *ignore);
+
+#endif
diff --git a/include/servo.h b/include/servo.h
new file mode 100644
index 0000000..411578e
--- /dev/null
+++ b/include/servo.h
@@ -0,0 +1,16 @@
+#ifndef SERVO_H
+#define SERVO_H
+
+#include <esp_err.h>
+
+typedef struct {
+  uint16_t pulse_0; 
+  uint16_t pulse_180; 
+  int8_t invert;
+} servo_settings_t;
+
+esp_err_t disable_servos();
+esp_err_t setup_pwm_controller();
+esp_err_t set_servo(uint8_t id, uint16_t angle);
+
+#endif
\ No newline at end of file
diff --git a/include/servo_config.h b/include/servo_config.h
new file mode 100644
index 0000000..c789d16
--- /dev/null
+++ b/include/servo_config.h
@@ -0,0 +1,93 @@
+#ifndef SERVO_CONFIG_H
+#define SERVO_CONFIG_H
+
+//#include "servo.h"
+#include "spot_ik.h"
+
+// RF - Right Front Leg
+// lower leg
+#define RF_LOWER_SERVO_CHANNEL 8
+#define RF_LOWER_SERVO_CENTER 306
+#define RF_LOWER_SERVO_RANGE 385
+#define RF_LOWER_SERVO_DIRECTION 1
+#define RF_LOWER_SERVO_CENTER_ANG_DEG 99.86f
+// upper leg
+#define RF_UPPER_SERVO_CHANNEL 7
+#define RF_UPPER_SERVO_CENTER 306
+#define RF_UPPER_SERVO_RANGE 407
+#define RF_UPPER_SERVO_DIRECTION 1
+#define RF_UPPER_SERVO_CENTER_ANG_DEG -31.62f
+// shoulder joint
+#define RF_HIP_SERVO_CHANNEL 6
+#define RF_HIP_SERVO_CENTER 306
+#define RF_HIP_SERVO_RANGE 396
+#define RF_HIP_SERVO_DIRECTION -1
+#define RF_HIP_SERVO_CENTER_ANG_DEG 1.67f
+
+// RB - Right Back Leg
+// lower leg
+#define RB_LOWER_SERVO_CHANNEL 2
+#define RB_LOWER_SERVO_CENTER 306
+#define RB_LOWER_SERVO_RANGE 369
+#define RB_LOWER_SERVO_DIRECTION 1
+#define RB_LOWER_SERVO_CENTER_ANG_DEG 95.37f
+// upper leg
+#define RB_UPPER_SERVO_CHANNEL 1
+#define RB_UPPER_SERVO_CENTER 306
+#define RB_UPPER_SERVO_RANGE 381
+#define RB_UPPER_SERVO_DIRECTION 1
+#define RB_UPPER_SERVO_CENTER_ANG_DEG -37.21f
+// shoulder joint
+#define RB_HIP_SERVO_CHANNEL 0
+#define RB_HIP_SERVO_CENTER 306
+#define RB_HIP_SERVO_RANGE 403
+#define RB_HIP_SERVO_DIRECTION 1
+#define RB_HIP_SERVO_CENTER_ANG_DEG -3.27f
+
+// LB - Left Back Leg
+// lower leg
+#define LB_LOWER_SERVO_CHANNEL 5
+#define LB_LOWER_SERVO_CENTER 306
+#define LB_LOWER_SERVO_RANGE 374
+#define LB_LOWER_SERVO_DIRECTION 1
+#define LB_LOWER_SERVO_CENTER_ANG_DEG -92.65f
+// upper leg
+#define LB_UPPER_SERVO_CHANNEL 4
+#define LB_UPPER_SERVO_CENTER 306
+#define LB_UPPER_SERVO_RANGE 403
+#define LB_UPPER_SERVO_DIRECTION 1
+#define LB_UPPER_SERVO_CENTER_ANG_DEG 91.23f
+// shoulder joint
+#define LB_HIP_SERVO_CHANNEL 3
+#define LB_HIP_SERVO_CENTER 306
+#define LB_HIP_SERVO_RANGE 367
+#define LB_HIP_SERVO_DIRECTION -1
+#define LB_HIP_SERVO_CENTER_ANG_DEG -7.20f
+
+// Lf - Left fRONT Leg
+// lower leg
+#define LF_LOWER_SERVO_CHANNEL 11
+#define LF_LOWER_SERVO_CENTER 306
+#define LF_LOWER_SERVO_RANGE 385
+#define LF_LOWER_SERVO_DIRECTION 1
+#define LF_LOWER_SERVO_CENTER_ANG_DEG -87.43f
+// upper leg
+#define LF_UPPER_SERVO_CHANNEL 10
+#define LF_UPPER_SERVO_CENTER 306
+#define LF_UPPER_SERVO_RANGE 388
+#define LF_UPPER_SERVO_DIRECTION 1
+#define LF_UPPER_SERVO_CENTER_ANG_DEG 38.21f
+// shoulder joint
+#define LF_HIP_SERVO_CHANNEL 9
+#define LF_HIP_SERVO_CENTER 306
+#define LF_HIP_SERVO_RANGE 388
+#define LF_HIP_SERVO_DIRECTION 1
+#define LF_HIP_SERVO_CENTER_ANG_DEG 4.67f
+
+extern const int16_t servo_min[12] ;
+extern const float servo_conversion[12] ;
+extern const float theta_range[3][2];
+extern const int8_t servo_invert[12];
+extern int16_t servo_angles[4][3];
+
+#endif
\ No newline at end of file
diff --git a/include/spot_ik.h b/include/spot_ik.h
new file mode 100644
index 0000000..d340ff0
--- /dev/null
+++ b/include/spot_ik.h
@@ -0,0 +1,19 @@
+#ifndef SPOT_IK_H
+#define SPOT_IK_H
+
+#include "esp_err.h"
+
+typedef struct {
+    float x;
+    float y;
+    float z;
+} point;
+
+esp_err_t leg_IK(float* p, uint8_t leg_id, int16_t servo_angles[3]);
+esp_err_t body_IK(float omega, float phi, float psi, float xm, float ym, float zm);
+esp_err_t spot_IK(float omega, float phi, float psi, float xm, float ym, float zm, int16_t servoangles[4][3]);
+
+void print_matrix(float * matrix, int n, int m, char* name) ;
+void print_int_matrix(int16_t * matrix, int n, int m, char* name, uint8_t newlines);
+
+#endif
\ No newline at end of file
diff --git a/src/ik_task.cpp b/src/ik_task.cpp
new file mode 100644
index 0000000..0f2f201
--- /dev/null
+++ b/src/ik_task.cpp
@@ -0,0 +1,139 @@
+#include <ik_task.h>
+
+static const char* TAG = "IK TASK";
+
+position_t spot_position = {.omega=0,.phi=0,.psi=0,.xm=-40,.ym=-170, .zm=0, .set=1};
+position_t goal_position = {0,};
+int16_t servo_angles[4][3] = {{90, 150, 0}, {90, 30, 180}, {90, 150, 0}, {90, 30, 180}};
+int16_t servo_angles_goal[4][3] = {0,};
+
+void set_orientation_cb(int16_t omega, int16_t phi, int16_t psi, int16_t xm, int16_t ym, int16_t zm) {
+  goal_position.omega = omega;
+  goal_position.phi = phi;
+  goal_position.psi = psi;
+  goal_position.xm  = xm;
+  goal_position.ym = ym;
+  goal_position.zm = zm;
+  goal_position.set = true;
+}
+
+void reset_position() {
+    set_orientation_cb(0, 0, 0, 0, 0, 0);
+}
+
+void set_leg_servos() {
+    for (int l = 0; l<4; l++) {
+    // for (int l = 1; l<2; l++) {    
+        for (int s=0;s<3;s++) {
+            if (servo_angles[l][s] != servo_angles_goal[l][s]) {
+                servo_angles[l][s] = servo_angles_goal[l][s];
+                set_servo(l*3 + s, servo_angles[l][s]);
+            }
+        }
+    }
+}
+
+void iterate_to_position() {
+    ESP_LOGI(TAG, "GOAL (%f,%f,%f - %f,%f,%f)", goal_position.omega, goal_position.phi, goal_position.psi, goal_position.xm, goal_position.ym, goal_position.zm);
+
+    do {
+    spot_position.set = false;
+    int diff = 0;
+
+    if (goal_position.omega != spot_position.omega) {
+        diff = goal_position.omega - spot_position.omega;
+        if (abs(diff) < MOTION_STEP_ANGLE) {
+            spot_position.omega = goal_position.omega ;
+        } else {
+            diff = diff < 0 ?  -MOTION_STEP_ANGLE : MOTION_STEP_ANGLE;
+            spot_position.omega += diff;
+            spot_position.set = true;
+        }
+    }
+
+    if (goal_position.phi != spot_position.phi) {
+        diff = goal_position.phi - spot_position.phi;
+        if (abs(diff) < MOTION_STEP_ANGLE) {
+            spot_position.phi = goal_position.phi ;
+        } else {
+            diff = diff < 0 ?  -MOTION_STEP_ANGLE : MOTION_STEP_ANGLE;
+            spot_position.phi += diff;
+            spot_position.set = true;
+        }
+    }
+
+    if (goal_position.psi != spot_position.psi) {
+        diff = goal_position.psi - spot_position.psi;
+        if (abs(diff) < MOTION_STEP_ANGLE) {
+            spot_position.psi = goal_position.psi ;
+        } else {
+            diff = diff < 0 ?  -MOTION_STEP_ANGLE : MOTION_STEP_ANGLE;
+            spot_position.psi += diff;
+            spot_position.set = true;
+        }
+    }
+
+    if (goal_position.xm != spot_position.xm) {
+        diff = goal_position.xm - spot_position.xm;
+        if (abs(diff) < MOTION_STEP_MOVEMENT) {
+            spot_position.xm = goal_position.xm ;
+        } else {
+            diff = diff < 0 ?  -MOTION_STEP_MOVEMENT : MOTION_STEP_MOVEMENT;
+            spot_position.xm += diff;
+            spot_position.set = true;
+        }
+    }
+
+    if (goal_position.ym != spot_position.ym) {
+        diff = goal_position.ym - spot_position.ym;
+        if (abs(diff) < MOTION_STEP_MOVEMENT) {
+            spot_position.ym = goal_position.ym ;
+        } else {
+            diff = diff < 0 ?  -MOTION_STEP_MOVEMENT : MOTION_STEP_MOVEMENT;
+            spot_position.ym += diff;
+            spot_position.set = true;
+        }
+    }
+
+    if (goal_position.zm != spot_position.zm) {
+        diff = goal_position.zm - spot_position.zm;
+        if (abs(diff) < MOTION_STEP_MOVEMENT) {
+            spot_position.zm = goal_position.zm ;
+        } else {
+            diff = diff < 0 ?  -MOTION_STEP_MOVEMENT : MOTION_STEP_MOVEMENT;
+            spot_position.zm += diff;
+            spot_position.set = true;
+        }
+    }
+
+
+    ESP_LOGI(TAG, "CURRENT (%f,%f,%f - %f,%f,%f) %d", spot_position.omega, spot_position.phi, spot_position.psi, spot_position.xm, spot_position.ym, spot_position.zm, spot_position.set);
+    
+    esp_err_t ret = spot_IK(spot_position.omega*DEGREES2RAD, spot_position.phi*DEGREES2RAD, spot_position.psi*DEGREES2RAD, spot_position.xm, spot_position.ym, spot_position.zm, servo_angles_goal);
+    ESP_LOGD(TAG, "Valid IK %d", ret==ESP_OK);
+    if (ret == ESP_OK) {
+        //print_int_matrix((int16_t*) servo_angles_goal, 4, 3, "servo_angles_goal", false);
+        set_leg_servos();
+    }
+
+    } while (spot_position.set);
+
+
+    //set_new_orientation_act_value((int16_t) spot_position.omega, (int16_t) spot_position.phi, (int16_t) spot_position.psi, (int16_t) spot_position.xm, (int16_t) spot_position.ym, (int16_t) spot_position.zm);
+}
+
+void task_ik(void *ignore){
+    ESP_LOGI(TAG, "Executing on core %d", xPortGetCoreID());
+    esp_err_t ret;
+
+    reset_position();
+    
+    for(;;) {
+        vTaskDelay(100 / portTICK_RATE_MS);
+        if (goal_position.set) {
+            goal_position.set = false;
+            iterate_to_position();
+        }
+    }
+    vTaskDelete(NULL);
+}
diff --git a/src/main.cpp b/src/main.cpp
index a9c37c7..e95c63a 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -20,6 +20,11 @@
 #include <config.h>
 #include <camera.h>
 
+#include <servo.h>
+#include <ik_task.h>
+
+static const char* TAG = "MAIN";
+
 DNSServer dnsServer;
 AsyncWebSocket ws(WEBSOCKET_PATH);
 AsyncEventSource events(EVENTSOURCE_PATH);
@@ -31,8 +36,7 @@ NewPing sonar[2] = {
 };
 
 MPU6050 mpu(Wire);
-Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
-Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
+Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, SCREEN_RESET);
 
 bool MPU_READY = false;
 bool PWM_READY = false;
@@ -101,11 +105,6 @@ bool setupMPU(){
   return 1;
 }
 
-bool setupPWMController(){
-  pwm.begin();
-  pwm.setOscillatorFrequency(27000000);
-  pwm.setPWMFreq(50);
-  return 1;
 }
 
 void setup(){
@@ -117,7 +116,7 @@ void setup(){
 
   OLED_READY = setupOLED();
   MPU_READY = setupMPU();
-  PWM_READY = setupPWMController();
+  PWM_READY = setup_pwm_controller();
 
   setupCamera();
   setupWiFi();
diff --git a/src/servo.cpp b/src/servo.cpp
new file mode 100644
index 0000000..984497a
--- /dev/null
+++ b/src/servo.cpp
@@ -0,0 +1,38 @@
+#include <Adafruit_PWMServoDriver.h>
+
+#include "servo.h"
+#include "config.h"
+
+#include "servo_config.h"
+
+static const char* TAG = "SERVO";
+
+Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
+
+const int16_t servo_min[12] = {153,118,138,121,116,125,131,150,148,130,158,165};
+const float servo_conversion[12] = {2.011111,2.011111,2.000000,2.050000,1.966667,2.027778,2.038889,1.677778,1.622222,2.027778,1.927778,1.650000};
+const int8_t servo_invert[12] = {1,0,1, 0,1,0,  0,0,1,  1,1,0};
+const float theta_range[3][2] = {{-M_PI / 3, M_PI/3}, {-2 * M_PI/3, M_PI/3}, {0, M_PI}};
+
+esp_err_t disable_servos(){
+  ESP_LOGI(TAG, "Disabling servos");
+  pwm.setPWM(0, 0, 0);
+  return ESP_OK;
+}
+
+esp_err_t setup_pwm_controller(){
+  pwm.begin();
+  pwm.setOscillatorFrequency(SERVO_OSCILLATOR_FREQUENCY);
+  pwm.setPWMFreq(SERVO_FREQ);
+  return ESP_OK;
+}
+
+esp_err_t set_servo(uint8_t id, uint16_t angle) {
+  esp_err_t ret;
+  uint16_t pulse = (uint16_t) (0.5 + servo_min[id] + (angle * servo_conversion[id]));
+  ESP_LOGI(TAG, "setPWM of servo %d, %d degrees -> Pulse %d", id, angle, pulse);
+  ret = pwm.setPWM(id, 0, pulse);
+
+  if (ret == ESP_OK) return ESP_OK;
+  else return ESP_FAIL;
+}
\ No newline at end of file
diff --git a/src/spot_ik.cpp b/src/spot_ik.cpp
new file mode 100644
index 0000000..c1e616f
--- /dev/null
+++ b/src/spot_ik.cpp
@@ -0,0 +1,264 @@
+#include "spot_ik.h"
+#include "IK_config.h"
+
+#include "esp_dsp.h"
+#include "math.h"
+
+// https://github.com/maartenweyn/SpotMicro_ESP32/blob/master/code/esp-idf/ik_test/main/spot_ik.c
+
+static const char* TAG = "IK";
+
+#define RAD2DEGREES 57.295779513082321 // 180 / PI
+#define DEGREES2RAD 0.017453292519943
+
+// float orientation[3] = {M_PI / 8, M_PI / 8, 0}; //A 3x1 arry with Spot's Roll, Pitch, Yaw angles - omega, phi, psi
+// float position[3] = {0, 0, 0};    //A 3x1 array with Spot's X, Y, Z coordinates
+static float Rx[4][4] = {{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 1}}; // X-axis rotation matrix
+static float Ry[4][4] = {{0, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 1}}; // Y-axis rotation matrix
+static float Rz[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}}; // Z-axis rotation matrix
+static float Rxyz[4][4] = {0,};
+static float Tm[4][4] = {0,}; // Transformation Matrix
+static float T[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}; //Translation <Atrix
+static float Trb[4][4] = {0,}; //Tranformation Matrix Right Back
+static float Trf[4][4] = {0,}; //Tranformation Matrix Right Front
+static float Tlb[4][4] = {0,}; //Tranformation Matrix Left Back
+static float Tlf[4][4] = {0,}; //Tranformation Matrix Left Front
+
+static const float Trb2[4][4] = {{0, 0, 1, -L/2}, {0, 1, 0, 0}, {-1, 0, 0, -W/2}, {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Right Back
+static const float Trf2[4][4] = {{0, 0, 1, L/2},  {0, 1, 0, 0}, {-1, 0, 0, -W/2}, {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Right Front
+static const float Tlb2[4][4] = {{0, 0, 1, -L/2},  {0, 1, 0, 0}, {-1, 0, 0, W/2},  {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Left Back
+static const float Tlf2[4][4] = {{0, 0, 1, L/2}, {0, 1, 0, 0}, {-1, 0, 0, W/2},  {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Left Front
+
+static float Lp[4][4] = {{80, -130, 100, 1}, {80, -130, -100, 1}, {-130, -130, 100, 1}, {-130, -130, -100, 1}};
+
+static const float Ix[4][4] = {{-1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};
+
+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;
+}
+
+void print_matrix(float * matrix, int n, int m, char* name) {
+  printf("Matrix %s:\n", name);
+  for (int i=0;i<n;i++) {
+    for (int j=0;j<m;j++)
+      printf("%.2f ", matrix[i*m + j]);
+    printf("\n");
+  }
+}
+
+void print_int_matrix(int16_t * matrix, int n, int m, char* name, uint8_t newlines) {
+  printf("Matrix %s:\n", name);
+  for (int i=0;i<n;i++) {
+    for (int j=0;j<m;j++)
+      printf("%3d ", matrix[i*m + j]);
+    if (newlines) printf("\n");
+  }
+  if (!newlines) printf("\n");
+}
+
+esp_err_t body_IK(float omega, float phi, float psi, float xm, float ym, float zm) {
+  float cos_omega = cos(omega);
+  float sin_omega = sin(omega);
+  float cos_phi   = cos(phi);
+  float sin_phi   = sin(phi);
+  float cos_psi   = cos(psi);
+  float sin_psi   = sin(psi);
+
+  Rx[1][1] = cos_omega;
+  Rx[1][2] = -sin_omega;
+  Rx[2][1] = sin_omega;
+  Rx[2][2] = cos_omega;
+
+  Ry[0][0] = cos_phi;
+  Ry[0][2] = sin_phi;
+  Ry[2][0] = -sin_phi;
+  Ry[2][2] = cos_phi;
+
+  Rz[0][0] = cos_psi;
+  Rz[0][1] = -sin_psi;
+  Rz[1][0] = sin_psi;
+  Rz[1][1] = cos_psi;
+
+  float Rxy[4][4];
+  dspm_mult_f32_ae32((float*) Rx, (float*) Ry, (float*) Rxy, 4, 4, 4);
+  dspm_mult_f32_ae32((float*) Rxy, (float*) Rz, (float*) Rxyz, 4, 4, 4);
+
+  // print_matrix((float*) Rx, 4, 4, "Rx");
+  // print_matrix((float*) Ry, 4, 4, "Ry");
+  // print_matrix((float*) Rz, 4, 4, "Rz");
+  // print_matrix((float*) Rxyz, 4, 4, "Rxyz");
+
+  T[0][3] = xm;
+  T[1][3] = ym;
+  T[2][3] = zm;
+
+  dsps_add_f32_ae32((float*) T, (float*) Rxyz, (float*) Tm, 16, 1, 1, 1);
+  // print_matrix((float*) Tm, 4, 4, "Tm");
+
+  dspm_mult_f32_ae32((float*) Tm, (float*) Trb2, (float*) Trb, 4, 4, 4);
+  dspm_mult_f32_ae32((float*) Tm, (float*) Trf2, (float*) Trf, 4, 4, 4);
+  dspm_mult_f32_ae32((float*) Tm, (float*) Tlf2, (float*) Tlf, 4, 4, 4);
+  dspm_mult_f32_ae32((float*) Tm, (float*) Tlb2, (float*) Tlb, 4, 4, 4);
+  // print_matrix((float*) Trb, 4, 4, "Trb");
+  // print_matrix((float*) Trf, 4, 4, "Trf");
+  // print_matrix((float*) Tlf, 4, 4, "Tlf");
+  // print_matrix((float*) Tlb, 4, 4, "Tlb");
+
+  return ESP_OK;
+}
+
+
+
+esp_err_t leg_IK(float* p, uint8_t leg_id, int16_t* servo_angles) {
+    //float x = (leg_id == LEG_RF || leg_id == LEG_RB) ? -p[0] : p[0];
+    // Rxy Lenght of shoulder-point on x/y plane only
+    float Rxy2 = p[0]*p[0] + p[1]*p[1] - L1L1;
+    if (Rxy2 < 0) Rxy2 = 0;
+    float Rxy = sqrt(Rxy2);
+    // Dxy Distance we need to cover in xy plane
+    float Dxy = Rxy - L2;
+    // Dxyz 3D Distance we need to cover
+    float Dxyz = sqrt(Dxy*Dxy+p[2]*p[2]);
+
+    // ESP_LOGD(tag, "IK Rxy,Dxy,Dxyz %.2f %.2f %.2f", Rxy, Dxy, Dxyz);
+
+    // The angle we need to cover - the angle already covered because of the offset of the servo
+    float theta1 = -atan2(p[1], p[0]) - atan2(Rxy, -L1);
+    if (isnan(theta1)) return -1;
+
+    // ESP_LOGD(tag, "T1 %.2f = (- %.2f - %.2f)", theta1, -atan2(p[1], p[0]), atan2(Rxy, -L1));
+
+    float d=(Dxyz*Dxyz - L3L3 - L4L4)/(LL34);
+    float theta3 = acos(d);
+    if (isnan(theta3)) return -3;
+
+    float theta2 = atan2(p[2], Dxy) - atan2(L4 * sin(theta3), L3 + L4 * cos(theta3));
+    if (isnan(theta2)) return -2;
+    //  ESP_LOGD(tag, "IK %.2f - atan2( %.2f, %.2f) (%.2f)", atan2(p[2], Dxy), L4 * sin(theta3), L3 + L4 * cos(theta3), atan2(L4 * sin(theta3), L3 + L4 * cos(theta3)));
+
+
+    if (servo_invert[leg_id*3]) {
+      servo_angles[0] = 90 - (int16_t) (0.5 + theta1 * RAD2DEGREES);
+    } else {
+      servo_angles[0] = 90 + (int16_t) (0.5 + theta1 * RAD2DEGREES);
+    }
+
+    if (servo_invert[leg_id*3 + 1]) {
+      servo_angles[1] = 120 + (int16_t) (0.5 + theta2 * RAD2DEGREES);
+    } else {
+      servo_angles[1] = 60 - (int16_t) (0.5 + theta2 * RAD2DEGREES);
+    }
+
+    if (servo_invert[leg_id*3 + 2]) {
+      servo_angles[2] = 180 - (int16_t) (0.5 + theta3 * RAD2DEGREES);
+    } else {
+      servo_angles[2] = (int16_t) (0.5 + theta3 * RAD2DEGREES);
+    }
+
+    if (servo_angles[1] < 0) servo_angles[1] += 360;
+    if (servo_angles[1] > 360) servo_angles[1] -= 360;
+
+    // ESP_LOGD(tag, "IK T2 %.2f -> %d", theta2, servo_angles[1]);
+    // ESP_LOGD(tag, "IK D %.2f, T3 %.2f -> %d", d, theta3, servo_angles[2]);
+
+
+    if (theta1 < theta_range[0][0] || theta1 > theta_range[0][1]) return -4;
+    if (theta2 < theta_range[1][0] || theta2 > theta_range[1][1]) return -5;
+    if (theta3 < theta_range[2][0] || theta3 > theta_range[2][1]) return -6;
+
+
+    return ESP_OK;
+}
+
+esp_err_t spot_IK(float omega, float phi, float psi, float xm, float ym, float zm, int16_t servoangles[4][3]) {
+  esp_err_t ret = ESP_OK;
+  body_IK(omega, phi, psi, xm, ym, zm);
+
+  float inv[4][4];
+  float Q1[4][4];
+  float Q[4];
+
+
+  ret += inverse(Tlf, inv);
+  dspm_mult_f32_ae32((float*) inv, (float*) Lp[0], (float*) Q, 4, 4, 1);
+  // print_matrix((float*) Q, 1, 4, "Q LF");
+  ret += leg_IK((float*) Q, LEG_LF, (int16_t*) servoangles[0]);
+  //printf("leg_IK return %d\n", ret);
+  //print_int_matrix((int16_t*) servo_angles[0], 1, 3, "servo_angles LF", 1);
+
+
+
+  // printf("\n\nRF-----\n");
+  ret += inverse(Trf, inv);
+  // print_matrix((float*) Ix, 4, 4, "Ix");
+  // print_matrix((float*) inv, 4, 4, "inv Trf");
+  dspm_mult_f32_ae32((float*) Ix, (float*) inv, (float*) Q1, 4, 4, 4);
+  // print_matrix((float*) Q1, 4, 4, "Q1 RF");
+  dspm_mult_f32_ae32((float*) Q1, (float*) Lp[1], (float*) Q, 4, 4, 1);
+  // print_matrix((float*) Q, 1, 4, "Q RF");
+  ret += leg_IK((float*) Q, LEG_RF, (int16_t*) servoangles[1]);
+  //printf("leg_IK return %d\n", ret);
+  //print_int_matrix((int16_t*) servo_angles[1], 1, 3, "servo_angles RF");
+
+  ret += inverse(Tlb, inv);
+  dspm_mult_f32_ae32((float*) inv, (float*) Lp[2], (float*) Q, 4, 4, 1);
+  // print_matrix((float*) Q, 1, 4, "Q LB");
+  ret += leg_IK((float*) Q, LEG_LB, (int16_t*) servoangles[2]);
+  //printf("leg_IK return %d\n", ret);
+  //print_int_matrix((int16_t*) servo_angles[2], 1, 3, "servo_angles LB");
+
+  // printf("\n\nRB-----\n");
+  ret += inverse(Trb, inv);
+  // print_matrix((float*) inv, 4, 4, "inv Trf");
+  dspm_mult_f32_ae32((float*) Ix, (float*) inv, (float*) Q1, 4, 4, 4);
+  // print_matrix((float*) Q1, 4, 4, "Q1 RF");
+  dspm_mult_f32_ae32((float*) Q1, (float*) Lp[3], (float*) Q, 4, 4, 1);
+
+  // print_matrix((float*) Q, 1, 4, "Q RB");
+  ret += leg_IK((float*) Q, LEG_RB, (int16_t*) servoangles[3]);
+  //printf("leg_IK return %d\n", ret);
+  //print_int_matrix((int16_t*) servo_angles[3], 1, 3, "servo_angles RB");
+
+  return ret;
+}
\ No newline at end of file