def move_robot_fast(hand_handle, finger, arm_handle, pose, goal_precision, extra_tool=identity(4), update_finger_cart=True): #hand_handle.update_sensor_data() if update_finger_cart: finger_cart=hand_handle.fingers[finger].get_cart_pos() tool_pose=dot(finger_cart, extra_tool) #print "Finger cart pos", finger_cart arm_handle.setTool(narray_to_kdlframe(tool_pose)) #arm_handle.gotoFrame(pose.reshape(16), wait=0.0, goal_precision=goal_precision) arm_handle.gotoPose(pose[:3,3], pose[:3,:3].reshape(9))
def move_robot(hand_handle, finger, arm_handle, pose, goal_precision, wait=10.0, extra_tool=identity(4), update_finger_cart=True): if update_finger_cart: hand_handle.update_sensor_data() finger_cart=hand_handle.fingers[finger].get_cart_pos() tool_pose=dot(finger_cart, extra_tool) print "Finger cart pos", finger_cart print "SET TOOL" arm_handle.setTool(narray_to_kdlframe(tool_pose)) print "GOTO POSE" arm_handle.gotoFrame(pose.reshape(16), wait=wait, goal_precision=goal_precision) print "END GOTO POSE"