def process_touch_pose(ud):
#########################################################################
# tranformation logic for manipulation
# put touch pose in torso frame
frame_B_touch = util.pose_msg_to_mat(ud.touch_click_pose)
torso_B_frame = self.get_transform("torso_lift_link",
ud.touch_click_pose.header.frame_id)
if torso_B_frame is None:
return 'tf_failure'
torso_B_touch_bad = torso_B_frame * frame_B_touch
# rotate pixel23d the right way
t_pos, t_quat = util.pose_mat_to_pq(torso_B_touch_bad)
# rotate so x axis pointing out
quat_flip_rot = tf_trans.quaternion_from_euler(0.0, np.pi/2.0, 0.0)
quat_flipped = tf_trans.quaternion_multiply(t_quat, quat_flip_rot)
# rotate around x axis so the y axis is flat
mat_flipped = tf_trans.quaternion_matrix(quat_flipped)
rot_angle = np.arctan(-mat_flipped[2,1] / mat_flipped[2,2])
quat_ortho_rot = tf_trans.quaternion_from_euler(rot_angle + np.pi, 0.0, 0.0)
quat_flipped_ortho = tf_trans.quaternion_multiply(quat_flipped, quat_ortho_rot)
torso_B_touch = util.pose_pq_to_mat(t_pos, quat_flipped_ortho)
# offset the touch location by the approach tranform
appr_B_tool = self.get_transform(self.tool_approach_frame, self.tool_frame)
if appr_B_tool is None:
return 'tf_failure'
torso_B_touch_appr = torso_B_touch * appr_B_tool
# project the approach back into the wrist
appr_B_wrist = self.get_transform(self.tool_frame,
self.arm + "_wrist_roll_link")
if appr_B_wrist is None:
return 'tf_failure'
torso_B_wrist = torso_B_touch_appr * appr_B_wrist
#########################################################################
# create PoseStamped
appr_wrist_ps = util.pose_mat_to_stamped_msg('torso_lift_link',
torso_B_wrist)
appr_tool_ps = util.pose_mat_to_stamped_msg('torso_lift_link',
torso_B_touch_appr)
touch_tool_ps = util.pose_mat_to_stamped_msg('torso_lift_link',
torso_B_touch)
# visualization
self.wrist_pub.publish(appr_wrist_ps)
self.appr_pub.publish(appr_tool_ps)
self.touch_pub.publish(touch_tool_ps)
# set return values
ud.appr_wrist_mat = torso_B_wrist
ud.appr_tool_ps = appr_tool_ps
ud.touch_tool_ps = touch_tool_ps
return 'succeeded'
def get_transform(self, from_frame, to_frame, time=None):
if time is None:
time = rospy.Time.now()
try:
self.tf_listener.waitForTransform(from_frame, to_frame, time, rospy.Duration(5))
pos, quat = self.tf_listener.lookupTransform(from_frame, to_frame, time)
return util.pose_pq_to_mat(pos, quat)
except (tf.Exception, tf.LookupException, tf.ConnectivityException):
return None
def get_transform(self, from_frame, to_frame, time=rospy.Time()):
pos, quat = self.tf_listener.lookupTransform(from_frame, to_frame, time)
return util.pose_pq_to_mat(pos, quat)
def get_transform(self, from_frame, to_frame, time=rospy.Time()):
pos, quat = self.tf_listener.lookupTransform(from_frame, to_frame,
time)
return util.pose_pq_to_mat(pos, quat)
