def get_outlet_to_plug(pose_base_outlet, pose_plug_gripper): """Get the pose from the outlet to the plug.""" pose_base_gripper = fromMsg(TFUtil.wait_and_lookup("base_link","r_gripper_tool_frame", rospy.Time.now(), rospy.Duration(2.0)).pose) outlet_to_plug = pose_base_outlet.Inverse() * pose_base_gripper * pose_plug_gripper.Inverse() return outlet_to_plug
def outlet_to_plug_error(goal): time = rospy.Time.now() try: pose_base_gripper = fromMsg(TFUtil.wait_and_lookup("base_link","r_gripper_tool_frame", time).pose) except rospy.ServiceException, e: rospy.logerr('Could not transform between gripper and wrist at time %f' %time.to_sec()) server.set_aborted() return
def execute_cb(goal): rospy.loginfo("Action server received goal") preempt_timeout = rospy.Duration(10.0) cart_space_goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed') try: pose_gripper_wrist= fromMsg(TFUtil.wait_and_lookup("r_gripper_tool_frame", "r_wrist_roll_link").pose) except rospy.ServiceException, e: rospy.logerr('Could not transform between gripper and wrist at time %f' %time.to_sec()) server.set_aborted() return
def plug_in_contact(ud): """Returns true if the plug is in contact with something.""" pose_base_gripper = fromMsg(TFUtil.wait_and_lookup('base_link', 'r_gripper_tool_frame').pose) pose_outlet_plug = fromMsg(ud.base_to_outlet).Inverse() * pose_base_gripper * fromMsg(ud.gripper_to_plug) # check if difference between desired and measured outlet-plug along x-axis is more than 1 cm ud.outlet_to_plug_contact = toMsg(pose_outlet_plug) if math.fabs(pose_outlet_plug.p[0] - ud.approach_offset) > 0.01: return True return False
def get_outlet_to_plug_ik_goal(ud, pose): """Get an IK goal for a pose in the frame of the outlet""" pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup("r_gripper_tool_frame", "r_wrist_roll_link", rospy.Time.now(), rospy.Duration(2.0)).pose) goal = ArmMoveIKGoal() goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed') goal.pose.pose = toMsg(fromMsg(ud.base_to_outlet) * pose * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist) goal.pose.header.stamp = rospy.Time.now() goal.pose.header.frame_id = 'base_link' return goal
def get_straighten_goal(ud, goal): """Generate an ik goal to straighten the plug in the outlet.""" pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose) pose_base_wrist = fromMsg(ud.base_to_outlet) * fromMsg(ud.outlet_to_plug_contact) * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist goal = ArmMoveIKGoal() goal.pose.pose = toMsg(pose_base_wrist) goal.pose.header.stamp = rospy.Time.now() goal.pose.header.frame_id = 'base_link' goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed') goal.move_duration = rospy.Duration(0.5) return goal
def get_pull_back_goal(ud, goal): """Generate an ik goal to move along the local x axis of the outlet.""" pose_outlet_plug = PyKDL.Frame(PyKDL.Vector(-0.10, 0, 0)) pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose) pose_base_wrist = fromMsg(ud.base_to_outlet) * pose_outlet_plug * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist goal = ArmMoveIKGoal() goal.pose.pose = toMsg(pose_base_wrist) goal.pose.header.stamp = rospy.Time.now() goal.pose.header.frame_id = 'base_link' goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed') goal.move_duration = rospy.Duration(3.0) return goal
def get_twist_goal(ud, goal): """Generate an ik goal to rotate the plug""" pose_contact_plug = PyKDL.Frame(PyKDL.Rotation.RPY(ud.twist_angle, 0, 0)) pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose) pose_base_wrist = fromMsg(ud.base_to_outlet) * fromMsg(ud.outlet_to_plug_contact) * pose_contact_plug * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist goal = ArmMoveIKGoal() goal.pose.pose = toMsg(pose_base_wrist) goal.pose.header.stamp = rospy.Time.now() goal.pose.header.frame_id = 'base_link' goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed') goal.move_duration = rospy.Duration(1.0) return goal
def get_grasp_plug_goal(ud, goal): """Get the ik goal for grasping the plug.""" pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose) goal = ArmMoveIKGoal() goal.pose.pose = toMsg(fromMsg(ud.plug_on_base_pose) * fromMsg(ud.gripper_plug_grasp_pose).Inverse() * pose_gripper_wrist) goal.pose.header.stamp = rospy.Time.now() goal.pose.header.frame_id = 'base_link' goal.ik_seed = get_action_seed('pr2_plugs_configuration/grasp_plug_seed') goal.move_duration = rospy.Duration(3.0) return goal
def execute_cb(goal): rospy.loginfo("Action server received goal") preempt_timeout = rospy.Duration(5.0) # move the spine up rospy.loginfo("Moving up spine...") spine_goal.position = 0.16 if spine_client.send_goal_and_wait(spine_goal, rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED: rospy.logerr('Moving up spine failed') server.set_aborted() return # return plug error = 1.0 free_cord = False iter_idx = 0 while error > ERROR_TOL and not rospy.is_shutdown() and iter_idx < MAX_ITERS: iter_idx += 1 # appraoch in joint space when re-trying if free_cord: rospy.loginfo("Free plug cord by doing approach...") if joint_space_client.send_goal_and_wait(get_action_goal('pr2_plugs_configuration/free_plug_cord'), rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED: rospy.logerr('Free plug cord failed') server.set_aborted() return free_cord = True # move to joint space position rospy.loginfo("Move in joint space...") if joint_space_client.send_goal_and_wait(get_action_goal('pr2_plugs_configuration/stow_plug'), rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED: rospy.logerr('Move approach in joint space failed') server.set_aborted() return # return plug to original location cart_space_goal.ik_seed = get_action_seed('pr2_plugs_configuration/return_plug_seed') pose_plug_gripper = fromMsg(goal.gripper_to_plug_grasp).Inverse() pose_base_plug = fromMsg(goal.base_to_plug) pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose) cart_space_goal.pose.pose = toMsg(pose_base_plug * pose_plug_gripper * pose_gripper_wrist) cart_space_goal.pose.header.stamp = rospy.Time.now() cart_space_goal.pose.header.frame_id = "base_link" cart_space_goal.move_duration = rospy.Duration(3.0) if cart_space_client.send_goal_and_wait(cart_space_goal, rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED: rospy.logerr('Failed to return plug to location where the plug was found when fetching it.') server.set_aborted() return # check if plug in correct location pose_base_gripper_measured = fromMsg(TFUtil.wait_and_lookup('base_link', 'r_gripper_tool_frame', rospy.Time.now()).pose) pose_base_gripper_desired = pose_base_plug * pose_plug_gripper diff = pose_base_gripper_measured.Inverse() * pose_base_gripper_desired error = diff.p.Norm() (r, p, y) = diff.M.GetRPY() error += (r + p + y)/10.0 rospy.loginfo("Diff between desired and actual plug storing poses = %f"%error) # open gripper rospy.loginfo("Open gripper...") gripper_goal.command.position = 0.07 gripper_goal.command.max_effort = 99999 if gripper_client.send_goal_and_wait(gripper_goal, rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED: rospy.logerr('Open gripper failed') server.set_aborted() return # retract arm rospy.loginfo("Releasing plug...") pose_plug_approach = PyKDL.Frame(PyKDL.Vector(0, 0.05, 0)) cart_space_goal.pose.pose = toMsg(pose_base_plug * pose_plug_approach * pose_plug_gripper * pose_gripper_wrist) cart_space_goal.pose.header.stamp = rospy.Time.now() cart_space_goal.pose.header.frame_id = "base_link" cart_space_goal.move_duration = rospy.Duration(3.0) if cart_space_client.send_goal_and_wait(cart_space_goal, rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED: rospy.logerr('Failed to move arm away from where the plug got released') server.set_aborted() return # detect plug on base rospy.loginfo("Detect plug on base...") start_time = rospy.Time.now() while detect_plug_on_base_client.send_goal_and_wait(DetectPlugOnBaseGoal(), rospy.Duration(120.0), preempt_timeout) != GoalStatus.SUCCEEDED: if rospy.Time.now() > start_time + rospy.Duration(60): rospy.logerr("Can't detect plug on base. It is soo dark in here... Giving up, driving away, living on the edge! But let's take an image to make sure.") detect_plug_on_base_client.send_goal_and_wait(DetectPlugOnBaseGoal(record_image=True), rospy.Duration(120.0), preempt_timeout) break rospy.logerr('Detecting plug on base failed, trying again...') # move the spine down rospy.loginfo("Moving down spine...") spine_goal.position = 0.01 spine_client.send_goal(spine_goal) # return result server.set_succeeded(StowPlugResult()) rospy.loginfo("Action server goal finished")