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"
