增加RunPoint_P_inPose_M_RAD用于set_position

This commit is contained in:
swayneleong 2025-05-30 11:18:27 +08:00
parent 6c542c4914
commit fedaab298d

View File

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