增加了关节角度和米为单位的一些接口

Massage/MassageControl/hardware/dobot_nova5.py

@@ -144,7 +144,44 @@ class dobot_nova5:
                 break
             sleep(0.1)
         return 0
-
+    
+    def RunPoint_P_inPose_M_RAD(self,poses_list):
+            '''
+                RUNPOINT_P 以关节运动方式运动到目标点
+                MovJ flag = 0 通过位姿走点，输入为[X Y Z RX RY RZ]  unit: M/RAD
+            '''
+            # 单位 m/rad -> mm/deg
+            poses_list_mm_deg = self.__transform_m_rad_2_mm_deg(poses_list)
+            # 走点
+            recvmovemess = self.dashboard.MovJ(*poses_list_mm_deg,0)
+            print("MovJ:",recvmovemess)
+            print(self.parseResultId(recvmovemess))
+            currentCommandID = self.parseResultId(recvmovemess)[1]
+            print("指令 ID:", currentCommandID)
+            sleep(0.02)
+            while True: # 完成判断循环
+                ########## 调试注释 #############
+                # print("robot mode",self.feedbackData.robotMode)
+                # print("feedback current ID", self.feedbackData.robotCurrentCommandID)
+                # print("指令 ID:", currentCommandID)
+                ########## 调试注释 #############
+                currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
+                if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
+                    self.dashboard.Stop()
+                    self.dashboard.EmergencyStop(mode=1)
+                    print("当前工作停止，机械臂处于急停状态")
+                if self.feedbackData.robotMode == 10: # 10 当前工程暂停
+                    print("机械臂为暂停状态")
+                    res = self.dashboard.Continue()
+                    code = self.parseResultId(res)[0]
+                    if code == 0:
+                        print("机械臂继续已暂停的运动指令")
+                if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
+                    print("该次运动完成")
+                    break
+                sleep(0.1)
+            return 0
+    
     def RunPoint_P_inJoint(self,joints_list):
         '''
             RUNPOINT_P 以关节运动方式运动到目标点
@@ -180,7 +217,45 @@ class dobot_nova5:
                 break
             sleep(0.1)  # 避免 CPU 占用过高
         return 0
-
+    
+    def RunPoint_P_inJoint_RAD(self,joints_list):
+        '''
+            RUNPOINT_P 以关节运动方式运动到目标点
+            MovJ flag = 1 通过关节角度走点，输入为[J1 J2 J3 J4 J5 J6]  unit: rad
+        '''
+        # 单位 rad -> deg
+        joints_list_deg = self.__transform_rad_2_deg(joints_list)
+        # 走点
+        recvmovemess = self.dashboard.MovJ(*joints_list_deg,1)
+        print("MovJ:",recvmovemess)
+        print(self.parseResultId(recvmovemess))
+        currentCommandID = self.parseResultId(recvmovemess)[1]
+        print("指令 ID:", currentCommandID)
+        sleep(0.02)
+        while True: # 完成判断循环
+            ########## 调试注释 #############
+            # print("robot mode",self.feedbackData.robotMode)
+            # print("feedback current ID", self.feedbackData.robotCurrentCommandID)
+            # print("指令 ID:", currentCommandID)
+            # print("30004",self.dashboard.GetCurrentCommandID())
+            ########## 调试注释 #############
+            currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
+            if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
+                self.dashboard.Stop()
+                self.dashboard.EmergencyStop(mode=1)
+                print("当前工作停止，机械臂处于急停状态")
+            if self.feedbackData.robotMode == 10: # 10 当前工程暂停
+                print("机械臂为暂停状态")
+                res = self.dashboard.Continue()
+                code = self.parseResultId(res)[0]
+                if code == 0:
+                    print("机械臂继续已暂停的运动指令")
+            if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
+                print("该次运动完成")
+                break
+            sleep(0.1)  # 避免 CPU 占用过高
+        return 0
+    
     def RunPoint_L_inPose(self,poses_list):
         '''
             RUNPOINT_L 以直线运动方式运动到目标点
@@ -210,6 +285,37 @@ class dobot_nova5:
             sleep(0.1)
         return 0
     
+    def RunPoint_L_inPose_M_RAD(self,poses_list):
+        '''
+            RUNPOINT_L 以直线运动方式运动到目标点
+            MovJ flag = 0 通过位姿走点，输入为[X Y Z RX RY RZ]  unit: m/rad
+        '''
+        # 单位 m/rad -> mm/deg
+        poses_list_mm_deg = self.__transform_m_rad_2_mm_deg(poses_list)
+        # 走点
+        recvmovemess = self.dashboard.MovL(*poses_list_mm_deg,0)
+        print("MovL:",recvmovemess)
+        print(self.parseResultId(recvmovemess))
+        currentCommandID = self.parseResultId(recvmovemess)[1]
+        print("指令 ID:", currentCommandID)
+        while True:
+            currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
+            if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
+                self.dashboard.Stop()
+                self.dashboard.EmergencyStop(mode=1)
+                print("当前工作停止，机械臂处于急停状态")
+            if self.feedbackData.robotMode == 10: # 10 当前工程暂停
+                print("机械臂为暂停状态")
+                res = self.dashboard.Continue()
+                code = self.parseResultId(res)[0]
+                if code == 0:
+                    print("机械臂继续已暂停的运动指令")
+            if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
+                print("该次运动完成")
+                break
+            sleep(0.1)
+        return 0
+    
     def RunPoint_L_inJoint(self,joints_list):
         '''
             RUNPOINT_L 以直线运动方式运动到目标点
@@ -238,9 +344,68 @@ class dobot_nova5:
                 break
             sleep(0.1)
         return 0
+    
+    def RunPoint_L_inJoint_RAD(self,joints_list):
+        '''
+            RUNPOINT_L 以直线运动方式运动到目标点
+            MovJ flag = 0 通过位姿走点，输入为[X Y Z RX RY RZ]  unit: rad
+        '''
+        # 单位 rad -> deg
+        joints_list_deg = self.__transform_rad_2_deg(joints_list)
+        # 走点
+        recvmovemess = self.dashboard.MovL(*joints_list_deg,0)
+        print("MovL:",recvmovemess)
+        print(self.parseResultId(recvmovemess))
+        currentCommandID = self.parseResultId(recvmovemess)[1]
+        print("指令 ID:", currentCommandID)
+        while True:
+            currentCommandID = self.parseResultId(self.dashboard.GetCurrentCommandID())[1]
+            if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
+                self.dashboard.Stop()
+                self.dashboard.EmergencyStop(mode=1)
+                print("当前工作停止，机械臂处于急停状态")
+            if self.feedbackData.robotMode == 10: # 10 当前工程暂停
+                print("机械臂为暂停状态")
+                res = self.dashboard.Continue()
+                code = self.parseResultId(res)[0]
+                if code == 0:
+                    print("机械臂继续已暂停的运动指令")
+            if self.feedbackData.robotMode == 5 and self.feedbackData.robotCurrentCommandID == currentCommandID:
+                print("该次运动完成")
+                break
+            sleep(0.1)
+        return 0
 
-    def ServoJoint(self,joints_list,t=-1,aheadtime=-1,gain=-1):
+    def ServoJoint(self,joints_list):
-        tcp_command = f"servoj({joints_list[0]},{joints_list[1]},{joints_list[2]},{joints_list[3]},{joints_list[4]},{joints_list[5]},t={t},aheadtime={aheadtime},gain={gain})"
+        '''
+        关节角度伺服
+        输入为[J1 J2 J3 J4 J5 J6]  unit: deg
+        '''
+        tcp_command = f"servoj({joints_list[0]},{joints_list[1]},{joints_list[2]},{joints_list[3]},{joints_list[4]},{joints_list[5]}, t=0.01, aheadtime=20, gain=200)"
+        self.dashboard.send_data(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收
+        print("ServoJ:",tcp_command)
+        if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
+            self.dashboard.Stop()
+            self.dashboard.EmergencyStop(mode=1)
+            print("当前工作停止，机械臂处于急停状态")
+            return -1
+        if self.feedbackData.robotMode == 10: # 10 当前工程暂停
+            print("机械臂为暂停状态")
+            res = self.dashboard.Continue()
+            code = self.parseResultId(res)[0]
+            if code == 0:
+                print("机械臂继续已暂停的运动指令")
+        return 0
+    
+    def ServoJoint_RAD(self,joints_list):
+        '''
+        关节角度伺服
+        输入为[J1 J2 J3 J4 J5 J6]  unit: rad
+        '''
+        # 单位转换 rad -> deg
+        joints_list_deg = self.__transform_rad_2_deg(joints_list)
+        # 构造字符串
+        tcp_command = f"servoj({joints_list_deg[0]},{joints_list_deg[1]},{joints_list_deg[2]},{joints_list_deg[3]},{joints_list_deg[4]},{joints_list_deg[5]}, t=0.01, aheadtime=20, gain=200)"
         self.dashboard.send_data(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收
         print("ServoJ:",tcp_command)
         if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
@@ -257,6 +422,10 @@ class dobot_nova5:
         return 0
             
     def ServoPose(self,poses_list):
+        '''
+        位姿伺服
+        输入为[X Y Z RX RY RZ]  unit: mm/deg
+        '''
         poses_list = list(map(float, poses_list))
         tcp_command = f"ServoP({poses_list[0]},{poses_list[1]},{poses_list[2]},{poses_list[3]},{poses_list[4]},{poses_list[5]}, t=0.01, aheadtime=20, gain=200)"
         res = self.dashboard.sendRecvMsg(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收
@@ -271,6 +440,30 @@ class dobot_nova5:
             code = self.parseResultId(res)[0]
             if code == 0:
                 print("机械臂继续已暂停的运动指令")
+    
+    def ServoPose_M_RAD(self,poses_list):
+        '''
+        位姿伺服
+        输入为[X Y Z RX RY RZ]  unit: M/RAD
+        '''
+        # 单位 m/rad -> mm/deg
+        poses_list_mm_deg = self.__transform_m_rad_2_mm_deg(poses_list)
+        # 确保数据类型正确
+        poses_list_mm_deg = list(map(float, poses_list_mm_deg))
+        # 构造字符串
+        tcp_command = f"ServoP({poses_list[0]},{poses_list[1]},{poses_list[2]},{poses_list[3]},{poses_list[4]},{poses_list[5]}, t=0.01, aheadtime=20, gain=200)"
+        res = self.dashboard.sendRecvMsg(tcp_command) # 通过反馈状态来进行异常处理，故这里只需要发，不需要收
+        print("ServoP msg:",res)
+        if self.feedbackData.robotMode == 9 or self.feedbackData.robotMode == 11: 
+            self.dashboard.Stop()
+            self.dashboard.EmergencyStop(mode=1)
+            print("当前工作停止，机械臂处于急停状态")
+        if self.feedbackData.robotMode == 10: # 10 当前工程暂停
+            print("机械臂为暂停状态")
+            res = self.dashboard.Continue()
+            code = self.parseResultId(res)[0]
+            if code == 0:
+                print("机械臂继续已暂停的运动指令")
 
     def setEndEffector(self,i,tool_i):
         '''
@@ -469,7 +662,42 @@ class dobot_nova5:
     def __del__(self):
         del self.dashboard
         del self.feedFour
+    ## 内部工具函数
+
+    def __transform_m_rad_2_mm_deg(self,input_list):
+        '''
+        接受列表 [X Y Z RX RY RZ]
+        将单位 m/rad 转为 mm/deg
+        '''
+        if len(input_list) != 6:
+            raise ValueError("Input list must contain exactly 6 elements [X, Y, Z, RX, RY, RZ]")
+        
+        # 转换位置单位：m -> mm (乘以 1000)
+        x_mm = input_list[0] * 1000.0
+        y_mm = input_list[1] * 1000.0
+        z_mm = input_list[2] * 1000.0
+        
+        # 转换角度单位：rad -> deg (乘以 180/pi)
+        rx_deg = math.degrees(input_list[3])
+        ry_deg = math.degrees(input_list[4])
+        rz_deg = math.degrees(input_list[5])
+        
+        return [x_mm, y_mm, z_mm, rx_deg, ry_deg, rz_deg]
     
+    def __transform_rad_2_deg(self,input_list):
+        '''
+        接受列表 [J1 J2 J3 J4 J5 J6]
+        将单位rad转为deg
+        '''
+        if len(input_list) != 6:
+            raise ValueError("Input list must contain exactly 6 elements [J1, J2, J3, J4, J5, J6]")
+        # 转换角度单位: rad -> deg
+        J_deg = []
+        for i in range(6):
+            J_deg.append(math.degrees(input_list[i]))
+        return J_deg
+
+
     ## 为反馈线程封装
     
     def Get_feedInfo_angle(self):

logs/test.py

@@ -1,5 +1,42 @@
-import random
+# import random
-import numpy as np
+# import numpy as np
 
-random_array = np.random.rand(3)  # 生成长度为 3 的数组
+# random_array = np.random.rand(3)  # 生成长度为 3 的数组
-print(random_array/1000)
+# print(random_array/1000)
+
+import math
+
+def __transform_mm_deg_2_m_rad(input_list):
+    '''
+    接受列表 [X Y Z RX RY RZ]
+    将单位 m/rad 转为 mm/deg
+    '''
+    if len(input_list) != 6:
+        raise ValueError("Input list must contain exactly 6 elements [X, Y, Z, RX, RY, RZ]")
+    
+    # 转换位置单位：m -> mm (乘以 1000)
+    x_mm = input_list[0] * 1000.0
+    y_mm = input_list[1] * 1000.0
+    z_mm = input_list[2] * 1000.0
+    
+    # 转换角度单位：rad -> deg (乘以 180/pi)
+    rx_deg = math.degrees(input_list[3])
+    ry_deg = math.degrees(input_list[4])
+    rz_deg = math.degrees(input_list[5])
+    
+    return [x_mm, y_mm, z_mm, rx_deg, ry_deg, rz_deg]
+
+def __transform_rad_2_deg(input_list):
+        '''
+        接受列表 [J1 J2 J3 J4 J5 J6]
+        将单位rad转为deg
+        '''
+        if len(input_list) != 6:
+            raise ValueError("Input list must contain exactly 6 elements [J1, J2, J3, J4, J5, J6]")
+        # 转换角度单位: rad -> deg
+        J_deg = []
+        for i in range(6):
+            J_deg.append(math.degrees(input_list[i]))
+        return J_deg
+
+print(__transform_rad_2_deg([6.283185307179586, 3.141592653589793, 6.283185307179586,6.283185307179586, 3.141592653589793, 6.283185307179586]))