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