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)