更新欧拉角转换

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):
         """

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):
         # 解析返回值

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):

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][3] = math.atan2(point[0][3], point[0][5])
-            temp_point[0][4] = math.atan2(point[0][3], point[0][5])
+            temp_point[0][4] = -math.asin(point[0][4])
-            temp_point[0][5] = -np.pi/2
+            temp_point[0][5] = 0
             
             # 确保角度在[-pi, pi]范围内
             if temp_point[0][5] > np.pi: