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")
