增加RunPoint_P_inPose_M_RAD用于set_position

Massage/MassageControl/hardware/dobot_nova5.py

@@ -314,6 +314,29 @@ class dobot_nova5:
         print(f'After {cnt} retries, still failed to arrive at the joint position: {q}')
         return -1
     
+    def RunPoint_P_inPose_M_RAD(self,pose,**params):
+        '''
+            RUNPOINT_P 以关节运动方式运动到目标点
+            MovJ flag = 0 通过位姿走点，输入为[X Y Z RX RY RZ]  unit: M/RAD
+        '''
+        # 单位 m/rad -> mm/deg
+        max_retry_count = params.get('max_retry_count', 3)
+        cnt = 0
+        poses_list_mm_deg = self.__transform_m_rad_2_mm_deg(pose)
+        while cnt < max_retry_count:
+            cnt += 1
+            command = self.dashboard.MovJ(*poses_list_mm_deg,0)
+
+            is_arrived = self.waitArrival(command)
+            if is_arrived == 0:
+                print(f'Arrived at the joint position: {pose}')
+                return 0
+            else:
+                print(f'Failed to arrive at the joint position: {pose}')
+                return -1
+        print(f'After {cnt} retries, still failed to arrive at the joint position: {pose}')
+        return -1
+                
 
     def ServoJoint(self,joints_list):
         '''