def offset_hand(pose_input, unit_vector=[1, 0, 0], offset_dist=0.1):
""" Find the pre grasp pose by offsetting the hand backwards from its current position
grasp_pose: the pose of the grasp location
offset_dist: the amount to offset off the object
Returns
---
pre_grasp_pose: the offsetted pose of the object
"""
#use the unit z vector because thats the direction out of the hand
quat = pose_input.orientation[0:4]
quat_inv = tf.transformations.quaternion_inverse(quat)
direction = spacial_location.qv_mult(quat, unit_vector)
pre_grasp_position = pose_input.position - direction * offset_dist
pre_grasp_pose = Pose(pre_grasp_position, pose_input.orientation)
pose_input.show_position_marker(ident=1, label="grasp pose")
pre_grasp_pose.show_position_marker(ident=2, label="pregrasp pose")
# import pdb; pdb.set_trace()
return pre_grasp_pose
def _offset_hand(self, grasp_pose, offset_dist=0.1):
""" Find the pre grasp pose by offsetting the hand backwards from its current position
grasp_pose: the pose of the grasp location
offset_dist: the amount to offset off the object
Returns
---
pre_grasp_pose: the offsetted pose of the object
"""
#use the unit z vector because thats the direction out of the hand
unit_z_vector = np.asarray([0, 0, 1])
direction = spacial_location.qv_mult(grasp_pose.orientation[0:4],
unit_z_vector)
#print "direction", direction
print "grasp_pose.position: ", grasp_pose.position
grasp_pose.show_position_marker(ident=1, label="grasp pose")
pre_grasp_position = grasp_pose.position - direction * offset_dist
pre_grasp_pose = Pose(pre_grasp_position, grasp_pose.orientation)
pre_grasp_pose.show_position_marker(ident=2, label="pregrasp pose")
return pre_grasp_pose