From 5ce7a0b4847cb2e668b1c588bb767fa04c61a71f Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Fri, 23 May 2025 17:49:17 +0300
Subject: [PATCH] remove old cartesian method and add new

---
 utils.py | 41 ++++++-----------------------------------
 1 file changed, 6 insertions(+), 35 deletions(-)

diff --git a/utils.py b/utils.py
index fd2276d..a15c916 100644
--- a/utils.py
+++ b/utils.py
@@ -140,11 +140,16 @@ class RobotClient:
             raise RuntimeError(f"Inverse kinematics failed: {ret}. \n Was sended {cmd}")
         return ret["ret"]["joint"]
 
-    def move_to_pose(self, pose: dict):
+    def move_to_pose_joint(self, pose: dict):
         joint_start, _, _ = self.get_current_waypoint()
         joint_target = self.inverse_kin(joint_start, pose["pos"], pose["ori"])
         self.move_joint(joint_target)
 
+    def move_to_pose_cart(self, pose: dict):
+        joint_start, _, _ = self.get_current_waypoint()
+        joint_target = self.inverse_kin(joint_start, pose["pos"], pose["ori"])
+        self.move_line(joint_target)
+
     def add_waypoint(self, joint_radian: list):
         cmd = {"command": "add_waypoint", "joint_radian": joint_radian}
         ret = self._send_single_json(cmd)
@@ -157,40 +162,6 @@ class RobotClient:
         if not any(r.get("msg") == "succ" for r in ret):
             raise RuntimeError(f"move_track failed: {ret}")
 
-    def move_linear_cartesian(self, pose: dict, steps: int = 50):
-        joint_start, pos_start, ori_start_wxyz = self.get_current_waypoint()
-
-        # Позиции
-        pos_start = np.array(pos_start)
-        pos_end = np.array(pose["pos"])
-
-        # Ориентации: перевести из [w, x, y, z] в [x, y, z, w]
-        ori_start_xyzw = [ori_start_wxyz[1], ori_start_wxyz[2], ori_start_wxyz[3], ori_start_wxyz[0]]
-        ori_end_xyzw = [pose["ori"][1], pose["ori"][2], pose["ori"][3], pose["ori"][0]]
-
-        # SLERP подготовка
-        key_times = [0, 1]
-        key_rots = R.from_quat([ori_start_xyzw, ori_end_xyzw])
-        slerp = Slerp(key_times, key_rots)
-
-        for i in range(steps + 1):
-            alpha = i / steps
-
-            # Интерполяция позиции
-            interp_pos = (1 - alpha) * pos_start + alpha * pos_end
-
-            # SLERP ориентации
-            interp_rot = slerp([alpha])[0]
-            interp_ori_xyzw = interp_rot.as_quat(canonical=True)
-            interp_ori_wxyz = [interp_ori_xyzw[3], *interp_ori_xyzw[:3]]
-
-            # Инверсная кинематика
-            joint_interp = self.inverse_kin(joint_start, interp_pos.tolist(), interp_ori_wxyz)
-            self.add_waypoint(joint_interp)
-
-        self.move_track()
-        time.sleep(10)
-
     def get_robot_state(self) -> int:
         cmd = {"command": "get_robot_state"}
         ret = self._send_single_json(cmd)