From 6c542c49147f21023db09421e79c68e5752bcb37 Mon Sep 17 00:00:00 2001
From: swayneleong <15723182159@163.com>
Date: Fri, 30 May 2025 11:10:14 +0800
Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E6=AC=A7=E6=8B=89=E8=A7=92?=
 =?UTF-8?q?=E8=BD=AC=E6=8D=A2?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Massage/MassageControl/hardware/dobot_api.py  | 19 ++++++++++++++++++-
 .../MassageControl/hardware/dobot_nova5.py    |  8 ++++----
 Massage/MassageControl/hardware/test.py       |  2 +-
 .../scripts/coordinate_transform.py           | 10 +++-------
 4 files changed, 26 insertions(+), 13 deletions(-)

diff --git a/Massage/MassageControl/hardware/dobot_api.py b/Massage/MassageControl/hardware/dobot_api.py
index bf9c3b2..27bde60 100644
--- a/Massage/MassageControl/hardware/dobot_api.py
+++ b/Massage/MassageControl/hardware/dobot_api.py
@@ -222,6 +222,15 @@ class DobotApiDashboard(DobotApi):
         except:
             return False
 
+    def Disconnect(self):
+        if self.socket_dobot:
+            try:
+                self.socket_dobot.shutdown(socket.SHUT_RDWR)
+                self.socket_dobot.close()
+            except Exception as e:
+                print(f"断开连接时出错: {e}")
+            finally:
+                self.socket_dobot = None
 
     def EnableRobot(self, load=0.0, centerX=0.0, centerY=0.0, centerZ=0.0, isCheck=-1,):
         """
@@ -3052,7 +3061,15 @@ class DobotApiFeedBack(DobotApi):
         except:
             return False
 
-        
+    def Disconnect(self):
+        if self.socket_dobot:
+            try:
+                self.socket_dobot.shutdown(socket.SHUT_RDWR)
+                self.socket_dobot.close()
+            except Exception as e:
+                print(f"断开连接时出错: {e}")
+            finally:
+                self.socket_dobot = None
 
     def feedBackData(self):
         """
diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py
index 6021271..d773512 100644
--- a/Massage/MassageControl/hardware/dobot_nova5.py
+++ b/Massage/MassageControl/hardware/dobot_nova5.py
@@ -147,11 +147,7 @@ class dobot_nova5:
             self.dashboard = None
             self.feedFour = None
 
-
-
-
         atexit.register(self.exit_task)
-
         self.init_pos = [-45.0009079*np.pi/180,55.5785141*np.pi/180,-120.68821716*np.pi/180,5.11103201*np.pi/180,90.00195312*np.pi/180,-90.00085449*np.pi/180]
 
     # 为状态管理而封装的初始化函数
@@ -212,6 +208,10 @@ class dobot_nova5:
         )
         time.sleep(1)
         # self.move_joint(self.off_pos)
+        self.stop_feedback.set()
+        self.feedback_thread.join()
+        self.dashboard.Disconnect()
+        self.feedFour.Disconnect()
 
     def parseResultId(self,valueRecv):
         # 解析返回值
diff --git a/Massage/MassageControl/hardware/test.py b/Massage/MassageControl/hardware/test.py
index 296df0d..f823475 100644
--- a/Massage/MassageControl/hardware/test.py
+++ b/Massage/MassageControl/hardware/test.py
@@ -7,7 +7,7 @@ from scipy.spatial.transform import Rotation as R
 frames = [
     {"origin": [0, 0, 0], "euler": [0, 0, 0]},
     {"origin": [2, 2, 2], "euler": [0, 150, 90]},
-    {"origin": [3, 3, 3], "euler": [-30, -180, 0]},
+    {"origin": [3, 3, 3], "euler": [-180, -30, 0]},
 ]
 
 def draw_frame(ax, origin, rot_matrix, length=0.5):
diff --git a/Massage/aucpuncture2point/scripts/coordinate_transform.py b/Massage/aucpuncture2point/scripts/coordinate_transform.py
index d5a5bf6..fd3c774 100644
--- a/Massage/aucpuncture2point/scripts/coordinate_transform.py
+++ b/Massage/aucpuncture2point/scripts/coordinate_transform.py
@@ -379,14 +379,10 @@ class CoordinateTransformer:
             # 复制位置坐标
             temp_point[:, :3] = point[:, :3]
             
-            # 计算欧拉角
-            # temp_point[0][3] = -math.asin(point[0][4])
-            # temp_point[0][4] = -math.atan2(-point[0][3], point[0][5])
-            # temp_point[0][5] = 0.0
 
-            temp_point[0][3] = -math.asin(point[0][4])
-            temp_point[0][4] = math.atan2(point[0][3], point[0][5])
-            temp_point[0][5] = -np.pi/2
+            temp_point[0][3] = math.atan2(point[0][3], point[0][5])
+            temp_point[0][4] = -math.asin(point[0][4])
+            temp_point[0][5] = 0
             
             # 确保角度在[-pi, pi]范围内
             if temp_point[0][5] > np.pi: