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 make_approach_pose(ud): frame_B_head = util.pose_msg_to_mat(ud.head_click_pose) base_B_frame = self.get_transform("base_link", ud.head_click_pose.header.frame_id) if base_B_frame is None: return 'tf_failure' base_B_head = base_B_frame * frame_B_head norm_z = np.array([base_B_head[0,2], base_B_head[1,2], 0]) norm_z /= np.linalg.norm(norm_z) base_B_head[:3,:3] = np.mat([[-norm_z[0], norm_z[1], 0], [-norm_z[1],-norm_z[0], 0], [ 0, 0, 1]]) # offset in the x-direction by the given parameter offset_B_base = self.get_transform(self.base_offset_frame, "base_link") if offset_B_base is None: return 'tf_failure' nav_pose = base_B_head * offset_B_base print nav_pose nav_pose[:4,3] = nav_pose * np.mat([[-self.nav_approach_dist, 0, 0, 1]]).T print nav_pose now = rospy.Time.now() nav_pose_ps = util.pose_mat_to_stamped_msg('base_link', nav_pose, now) self.tf_listener.waitForTransform("/map", "base_link", now, timeout=rospy.Duration(20)) nav_pose_map_ps = self.tf_listener.transformPose("/map", nav_pose_ps) #self.nav_pub.publish(util.pose_mat_to_stamped_msg('base_link', base_B_head, rospy.Time.now())) print base_B_head, base_B_head.T * base_B_head self.nav_pub.publish(nav_pose_map_ps) ud.nav_pose_ps = nav_pose_map_ps return 'succeeded'
def find_controlled_tool_pose(self, target_pose): # find current tool location t_B_c = self.get_transform("torso_lift_link", self.tool_frame) # find error in position err_pos = target_pose[:3, 3] - t_B_c[:3, 3] # find error in angle err_ang = util.quaternion_dist(target_pose, t_B_c) # find control values u_x = self.x_pid.update_state(err_pos[0, 0]) u_y = self.y_pid.update_state(err_pos[1, 0]) u_z = self.z_pid.update_state(err_pos[2, 0]) + self.grav_comp u_pos = np.mat([u_x, u_y, u_z]).T u_a = self.a_pid.update_state(err_ang) # quat_diff = (np.array(tf_trans.quaternion_from_matrix(target_pose)) - # np.array(tf_trans.quaternion_from_matrix(t_B_c))) # u_qx = self.qx_pid.update_state(quat_diff[0]) # u_qy = self.qy_pid.update_state(quat_diff[1]) # u_qz = self.qz_pid.update_state(quat_diff[2]) # u_qw = self.qw_pid.update_state(quat_diff[3]) # u_quat = np.array([u_qx, u_qy, u_qz, u_qw]) # u_quat = u_quat / np.linalg.norm(u_quat) # find commanded frame of tool # rotation u_a = np.clip(u_a, 0, 1) ei_q_slerp = tf_trans.quaternion_slerp( tf_trans.quaternion_from_matrix(t_B_c), tf_trans.quaternion_from_matrix(target_pose), u_a) new_quat = ei_q_slerp # new_quat = tf_trans.quaternion_multiply(tf_trans.quaternion_from_matrix(t_B_c), # u_quat) t_B_new = np.mat(tf_trans.quaternion_matrix(new_quat)) # position u_pos_clipped = np.clip(u_pos, -self.u_pos_max, self.u_pos_max) t_B_new[:3, 3] = t_B_c[:3, 3] + u_pos_clipped # publish informative topics self.goal_pub.publish( util.pose_mat_to_stamped_msg("torso_lift_link", target_pose)) self.cur_pub.publish( util.pose_mat_to_stamped_msg("torso_lift_link", t_B_c)) self.cmd_pub.publish( util.pose_mat_to_stamped_msg("torso_lift_link", t_B_new)) return t_B_new, err_pos, err_ang
def retreat_cb(ud, goal): # get the current location of the tool start_pose_mat = self.get_transform("torso_lift_link", self.tool_frame) start_pose_ps = util.pose_mat_to_stamped_msg("torso_lift_link", start_pose_mat) # move from the current location to the approach location lm_goal = EPCLinearMoveGoal(start_pose_ps, ud.end_pose, self.tool_frame, False, 0.03, 0.1, 1.0, 3) return lm_goal
def find_controlled_tool_pose(self, target_pose): # find current tool location t_B_c = self.get_transform("torso_lift_link", self.tool_frame) # find error in position err_pos = target_pose[:3,3] - t_B_c[:3,3] # find error in angle err_ang = util.quaternion_dist(target_pose, t_B_c) # find control values u_x = self.x_pid.update_state(err_pos[0,0]) u_y = self.y_pid.update_state(err_pos[1,0]) u_z = self.z_pid.update_state(err_pos[2,0]) + self.grav_comp u_pos = np.mat([u_x, u_y, u_z]).T u_a = self.a_pid.update_state(err_ang) # quat_diff = (np.array(tf_trans.quaternion_from_matrix(target_pose)) - # np.array(tf_trans.quaternion_from_matrix(t_B_c))) # u_qx = self.qx_pid.update_state(quat_diff[0]) # u_qy = self.qy_pid.update_state(quat_diff[1]) # u_qz = self.qz_pid.update_state(quat_diff[2]) # u_qw = self.qw_pid.update_state(quat_diff[3]) # u_quat = np.array([u_qx, u_qy, u_qz, u_qw]) # u_quat = u_quat / np.linalg.norm(u_quat) # find commanded frame of tool # rotation u_a = np.clip(u_a, 0, 1) ei_q_slerp = tf_trans.quaternion_slerp(tf_trans.quaternion_from_matrix(t_B_c), tf_trans.quaternion_from_matrix(target_pose), u_a) new_quat = ei_q_slerp # new_quat = tf_trans.quaternion_multiply(tf_trans.quaternion_from_matrix(t_B_c), # u_quat) t_B_new = np.mat(tf_trans.quaternion_matrix(new_quat)) # position u_pos_clipped = np.clip(u_pos, -self.u_pos_max, self.u_pos_max) t_B_new[:3,3] = t_B_c[:3,3] + u_pos_clipped # publish informative topics self.goal_pub.publish(util.pose_mat_to_stamped_msg("torso_lift_link", target_pose)) self.cur_pub.publish(util.pose_mat_to_stamped_msg("torso_lift_link", t_B_c)) self.cmd_pub.publish(util.pose_mat_to_stamped_msg("torso_lift_link", t_B_new)) return t_B_new, err_pos, err_ang