diff --git a/esp32/lib/ESP32-sveltekit/Kinematics.h b/esp32/lib/ESP32-sveltekit/Kinematics.h
index 6003aee..c205aa3 100644
--- a/esp32/lib/ESP32-sveltekit/Kinematics.h
+++ b/esp32/lib/ESP32-sveltekit/Kinematics.h
@@ -41,28 +41,33 @@ public:
 
         L = 140;
         W = 75;
-
-        float Ix_data[] = {-1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
-        dspm::Mat Ix(Ix_data, 4, 4);
     }
     ~Kinematics(){}
 
     esp_err_t calculate_inverse_kinematics(float lp[4][4], position_t p, float result[12]) {
+
+        Tlf.clear();
+        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};
+        dspm::Mat Ix(Ix_data, 4, 4);
         
         esp_err_t res = bodyIK(p);
 
-        for (int i = 0; i < 4; ++i) {
-            dspm::Mat temp(lp[i], 4, 1);
-            dspm::Mat result_vec(4, 1);
+        dspm::Mat result_vec(4, 4);
+        result_vec = (Tlf.inverse() * dspm::Mat(lp[0], 4, 1));
+        legIK(result_vec.data, result);
 
-            if (i == 1 || i == 3) {
-                result_vec = Ix * ((i == 1 ? Trf.inverse() : Trb.inverse()) * temp);
-            } else {
-                result_vec = (i == 0 ? Tlf.inverse() : Tlb.inverse()) * temp;
-            }
+        result_vec = Ix * (Trf.inverse() * dspm::Mat(lp[1], 4, 1));
+        legIK(result_vec.data, result + 3);
 
-            legIK(result_vec.data, &result[i * 3]);
-        }
+        result_vec = (Tlb.inverse() * dspm::Mat(lp[2], 4, 1));
+        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;
     }
diff --git a/esp32/lib/ESP32-sveltekit/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h
index ac8fd0c..81a22d2 100644
--- a/esp32/lib/ESP32-sveltekit/MotionService.h
+++ b/esp32/lib/ESP32-sveltekit/MotionService.h
@@ -106,7 +106,13 @@ class MotionService
                     {-100, -100,  100, 1},
                     {-100, -100, -100, 1}
                 };
-                position_t p = {0, 0, 0, 0, static_cast<float>(input[5]), 0, input[0]};
+                float lx = static_cast<float>(input[1]);
+                float ly = static_cast<float>(input[2]);
+                float rx = static_cast<float>(input[3]);
+                float ry = static_cast<float>(input[4]);
+                float h = static_cast<float>(input[5]);
+                float s = static_cast<float>(input[6]);
+                position_t p = {0, rx / 4, ry / 4, ly / 2, h, lx / 2, input[0]};
                 float new_angles[12] = {0,};
                 float dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1};
 
@@ -120,7 +126,7 @@ class MotionService
                     }
                 }
                 if (updated) {
-                    ESP_LOGI("MotionService", "New angles: %f %f %f %f %f %f %f %f %f %f %f %f", new_angles[0], new_angles[1], new_angles[2], new_angles[3], new_angles[4], new_angles[5], new_angles[6], new_angles[7], new_angles[8], new_angles[9], new_angles[10], new_angles[11]);
+                    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]);
                 }
                 break;
             }