def execute_linear_move(self, goal): rospy.loginfo("[epc_move_actionlib] Execute linear move.") result = EPCLinearMoveResult() start_pose_stamped = self.tf_listener.transformPose( "torso_lift_link", goal.start_pose) start_pose = util.pose_msg_to_mat(start_pose_stamped) end_pose_stamped = self.tf_listener.transformPose( "torso_lift_link", goal.end_pose) end_pose = util.pose_msg_to_mat(end_pose_stamped) if goal.reset_controllers: self.epc_move.reset_controllers() def preempt_callback(err_pos, err_ang): if self.linear_move_act.is_preempt_requested(): return "preempted" return None result.result = self.epc_move.linear_move( start_pose, end_pose, goal.tool_frame, goal.velocity, goal.err_pos_max, goal.err_ang_max, goal.setup_steps, preempt_callback) rospy.loginfo( "[epc_move_actionlib] Linear move completed with result '%s'." % result.result) if result.result == "succeeded": self.linear_move_act.set_succeeded(result) elif result.result == "preempted": self.linear_move_act.set_preempted(result) else: self.linear_move_act.set_aborted(result)
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 execute_setup(self, goal): rospy.loginfo( "[linear_move_relative] Execute relative arm movement setup.") result = LinearMoveRelativeSetupResult() self.lin_move.reset_controllers() # find setup poses torso_B_frame = self.lin_move.get_transform( "torso_lift_link", goal.goal_pose.header.frame_id) frame_B_goal = util.pose_msg_to_mat(goal.goal_pose) appr_B_tool = self.lin_move.get_transform(self.tool_approach_frame, self.tool_frame) torso_B_tool_appr = torso_B_frame * frame_B_goal * appr_B_tool appr_B_wrist = self.lin_move.get_transform( self.tool_approach_frame, self.arm + "_wrist_roll_link") torso_B_wrist = torso_B_frame * frame_B_goal * appr_B_wrist # wait for arm to stop moving self.wait_arm_moving(stop_wait=True) # start collision detection self.start_detection(joint_detect=True, finger_detect=True, force_detect=False, joint_behavior="overhead_grasp", joint_sig_level=0.99) # move to best estimate for IK for the wrist angles = self.lin_move.biased_IK(torso_B_wrist, self.INIT_ANGS, self.JOINTS_BIAS, num_iters=6) if angles is None: self.move_arm_setup_act.set_aborted(result) return self.lin_move.command_joint_angles(angles, 3.0) # wait for the arm to start moving, then stop moving self.wait_arm_moving(stop_wait=False) rospy.sleep(1.0) self.wait_arm_moving(stop_wait=True) # use the epc control to accruately move to the setup position move_success = self.lin_move.move_to_pose(torso_B_tool_appr) # wait for the arm to start moving, then stop moving self.wait_arm_moving(stop_wait=False) rospy.sleep(1.0) self.wait_arm_moving(stop_wait=True) self.stop_detection() # return result message result.collided = self.coll_state.collided self.move_arm_setup_act.set_succeeded(result) self.coll_state.collided = False rospy.loginfo( "[linear_move_relative] Relative arm movement setup complete.") return True
def execute_direct_move(self, goal): rospy.loginfo("[epc_move_actionlib] Execute direct move.") result = EPCDirectMoveResult() transformed_pose_stamped = self.tf_listener.transformPose("torso_lift_link", goal.target_pose) transformed_pose = util.pose_msg_to_mat(transformed_pose_stamped) if goal.reset_controllers: self.epc_move.reset_controllers() def preempt_callback(err_pos, err_ang): if self.direct_move_act.is_preempt_requested(): return "preempted" return None result.result = self.epc_move.direct_move( transformed_pose, goal.tool_frame, goal.err_pos_goal, goal.err_ang_goal, goal.err_pos_max, goal.err_ang_max, goal.steady_steps, preempt_callback, ) rospy.loginfo("[epc_move_actionlib] Direct move completed with result '%s'." % result.result) if result.result == "succeeded": self.direct_move_act.set_succeeded(result) elif result.result == "preempted": self.direct_move_act.set_preempted(result) else: self.direct_move_act.set_aborted(result)
def execute_move(self, goal): rospy.loginfo("[linear_move_relative] Execute relative arm movement.") result = LinearMoveRelativeResult() torso_B_frame = self.lin_move.get_transform("torso_lift_link", goal.goal_pose.header.frame_id) frame_B_goal = util.pose_msg_to_mat(goal.goal_pose) appr_B_tool = self.lin_move.get_transform(self.tool_approach_frame, self.tool_frame) torso_B_tool_appr = torso_B_frame * frame_B_goal * appr_B_tool torso_B_tool_goal = torso_B_frame * frame_B_goal self.wait_arm_moving(stop_wait=True) move_success = self.lin_move.linear_trajectory(torso_B_tool_appr, torso_B_tool_goal, velocity=self.MOVE_VELOCITY) self.move_arm_act.set_succeeded(result) return if not move_success: rospy.loginfo("[linear_move_relative] Relative arm movement failed.") self.move_arm_act.set_aborted(move_success) return self.wait_arm_moving(stop_wait=False) self.start_detection("overhead_grasp", 1.0) rospy.sleep(2) self.wait_arm_moving(stop_wait=True) self.stop_detection() rospy.loginfo("[linear_move_relative] Relative arm movement complete.") result.collided = self.coll_state.collided self.move_arm_act.set_succeeded(result) self.coll_state.collided = False
def execute_direct_move(self, goal): rospy.loginfo("[epc_move_actionlib] Execute direct move.") result = EPCDirectMoveResult() transformed_pose_stamped = self.tf_listener.transformPose( "torso_lift_link", goal.target_pose) transformed_pose = util.pose_msg_to_mat(transformed_pose_stamped) if goal.reset_controllers: self.epc_move.reset_controllers() def preempt_callback(err_pos, err_ang): if self.direct_move_act.is_preempt_requested(): return "preempted" return None result.result = self.epc_move.direct_move( transformed_pose, goal.tool_frame, goal.err_pos_goal, goal.err_ang_goal, goal.err_pos_max, goal.err_ang_max, goal.steady_steps, preempt_callback) rospy.loginfo( "[epc_move_actionlib] Direct move completed with result '%s'." % result.result) if result.result == "succeeded": self.direct_move_act.set_succeeded(result) elif result.result == "preempted": self.direct_move_act.set_preempted(result) else: self.direct_move_act.set_aborted(result)
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 execute_setup(self, goal): rospy.loginfo("[linear_move_relative] Execute relative arm movement setup.") result = LinearMoveRelativeSetupResult() self.lin_move.reset_controllers() # find setup poses torso_B_frame = self.lin_move.get_transform("torso_lift_link", goal.goal_pose.header.frame_id) frame_B_goal = util.pose_msg_to_mat(goal.goal_pose) appr_B_tool = self.lin_move.get_transform(self.tool_approach_frame, self.tool_frame) torso_B_tool_appr = torso_B_frame * frame_B_goal * appr_B_tool appr_B_wrist = self.lin_move.get_transform(self.tool_approach_frame, self.arm + "_wrist_roll_link") torso_B_wrist = torso_B_frame * frame_B_goal * appr_B_wrist # wait for arm to stop moving self.wait_arm_moving(stop_wait=True) # start collision detection self.start_detection(joint_detect=True, finger_detect=True, force_detect=False, joint_behavior="overhead_grasp", joint_sig_level=0.99) # move to best estimate for IK for the wrist angles = self.lin_move.biased_IK(torso_B_wrist, self.INIT_ANGS, self.JOINTS_BIAS, num_iters=6) if angles is None: self.move_arm_setup_act.set_aborted(result) return self.lin_move.command_joint_angles(angles, 3.0) # wait for the arm to start moving, then stop moving self.wait_arm_moving(stop_wait=False) rospy.sleep(1.0) self.wait_arm_moving(stop_wait=True) # use the epc control to accruately move to the setup position move_success = self.lin_move.move_to_pose(torso_B_tool_appr) # wait for the arm to start moving, then stop moving self.wait_arm_moving(stop_wait=False) rospy.sleep(1.0) self.wait_arm_moving(stop_wait=True) self.stop_detection() # return result message result.collided = self.coll_state.collided self.move_arm_setup_act.set_succeeded(result) self.coll_state.collided = False rospy.loginfo("[linear_move_relative] Relative arm movement setup complete.") return True
def execute_linear_move(self, goal): rospy.loginfo("[epc_move_actionlib] Execute linear move.") result = EPCLinearMoveResult() start_pose_stamped = self.tf_listener.transformPose("torso_lift_link", goal.start_pose) start_pose = util.pose_msg_to_mat(start_pose_stamped) end_pose_stamped = self.tf_listener.transformPose("torso_lift_link", goal.end_pose) end_pose = util.pose_msg_to_mat(end_pose_stamped) if goal.reset_controllers: self.epc_move.reset_controllers() def preempt_callback(err_pos, err_ang): if self.linear_move_act.is_preempt_requested(): return "preempted" return None result.result = self.epc_move.linear_move( start_pose, end_pose, goal.tool_frame, goal.velocity, goal.err_pos_max, goal.err_ang_max, goal.setup_steps, preempt_callback, ) rospy.loginfo("[epc_move_actionlib] Linear move completed with result '%s'." % result.result) if result.result == "succeeded": self.linear_move_act.set_succeeded(result) elif result.result == "preempted": self.linear_move_act.set_preempted(result) else: self.linear_move_act.set_aborted(result)
def execute_move(self, goal): rospy.loginfo("[linear_move_relative] Execute relative arm movement.") result = LinearMoveRelativeResult() torso_B_frame = self.lin_move.get_transform( "torso_lift_link", goal.goal_pose.header.frame_id) frame_B_goal = util.pose_msg_to_mat(goal.goal_pose) appr_B_tool = self.lin_move.get_transform(self.tool_approach_frame, self.tool_frame) torso_B_tool_appr = torso_B_frame * frame_B_goal * appr_B_tool torso_B_tool_goal = torso_B_frame * frame_B_goal self.wait_arm_moving(stop_wait=True) move_success = self.lin_move.linear_trajectory( torso_B_tool_appr, torso_B_tool_goal, velocity=self.MOVE_VELOCITY) self.move_arm_act.set_succeeded(result) return if not move_success: rospy.loginfo( "[linear_move_relative] Relative arm movement failed.") self.move_arm_act.set_aborted(move_success) return self.wait_arm_moving(stop_wait=False) self.start_detection("overhead_grasp", 1.0) rospy.sleep(2) self.wait_arm_moving(stop_wait=True) self.stop_detection() rospy.loginfo("[linear_move_relative] Relative arm movement complete.") result.collided = self.coll_state.collided self.move_arm_act.set_succeeded(result) self.coll_state.collided = False