From fedaab298d82df082e0e96b4e5bfa24aa9ea45ad Mon Sep 17 00:00:00 2001
From: swayneleong <15723182159@163.com>
Date: Fri, 30 May 2025 11:18:27 +0800
Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0RunPoint=5FP=5FinPose=5FM=5FR?=
 =?UTF-8?q?AD=E7=94=A8=E4=BA=8Eset=5Fposition?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../MassageControl/hardware/dobot_nova5.py    | 23 +++++++++++++++++++
 1 file changed, 23 insertions(+)

diff --git a/Massage/MassageControl/hardware/dobot_nova5.py b/Massage/MassageControl/hardware/dobot_nova5.py
index d773512..d8d1c19 100644
--- a/Massage/MassageControl/hardware/dobot_nova5.py
+++ b/Massage/MassageControl/hardware/dobot_nova5.py
@@ -313,6 +313,29 @@ class dobot_nova5:
                 return -1
         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):