plug_in.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib
00004 roslib.load_manifest('pr2_plugs_actions')
00005 
00006 import rospy
00007 
00008 import os,sys,time
00009 import math
00010 import PyKDL
00011 
00012 from actionlib_msgs.msg import *
00013 from pr2_common_action_msgs.msg import *
00014 from pr2_plugs_msgs.msg import *
00015 from pr2_controllers_msgs.msg import *
00016 from geometry_msgs.msg import *
00017 from trajectory_msgs.msg import *
00018 from move_base_msgs.msg import *
00019 from sensor_msgs.msg import *
00020 
00021 from pr2_arm_move_ik.tools import *
00022 from tf_conversions.posemath import fromMsg, toMsg
00023 from joint_trajectory_action_tools.tools import get_action_goal as get_generator_goal
00024 
00025 # State machine classes
00026 import smach
00027 from smach import *
00028 from smach_ros import *
00029 from pr2_plugs_actions.tf_util import TFUtil
00030 
00031 import actionlib
00032 
00033 def drange(start, stop, step):
00034   r = start
00035   while r < stop:
00036     yield r
00037     r += step
00038 
00039 def get_outlet_to_plug(pose_base_outlet, pose_plug_gripper):
00040     """Get the pose from the outlet to the plug."""
00041     pose_base_gripper = fromMsg(TFUtil.wait_and_lookup("base_link","r_gripper_tool_frame", 
00042                                                        rospy.Time.now(), rospy.Duration(2.0)).pose)
00043 
00044     outlet_to_plug = pose_base_outlet.Inverse() * pose_base_gripper * pose_plug_gripper.Inverse()
00045 
00046     return outlet_to_plug
00047 
00048 @smach.cb_interface(input_keys=['base_to_outlet','gripper_to_plug'])
00049 def get_outlet_to_plug_ik_goal(ud, pose):
00050     """Get an IK goal for a pose in the frame of the outlet"""
00051     pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup("r_gripper_tool_frame", "r_wrist_roll_link", 
00052                                                         rospy.Time.now(), rospy.Duration(2.0)).pose)
00053 
00054     goal = ArmMoveIKGoal()
00055     goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
00056     goal.pose.pose = toMsg(fromMsg(ud.base_to_outlet) * pose * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist)
00057     goal.pose.header.stamp = rospy.Time.now()
00058     goal.pose.header.frame_id = 'base_link'
00059 
00060     return goal
00061 
00062 @smach.cb_interface(input_keys=['gripper_to_plug','base_to_outlet'])
00063 def get_wiggle_goal(ud,goal):
00064     goal = WigglePlugGoal()
00065     goal.gripper_to_plug = ud.gripper_to_plug
00066     goal.base_to_outlet = ud.base_to_outlet
00067     goal.wiggle_period = rospy.Duration(0.5)
00068     goal.insert = 1
00069     return goal
00070 
00071 def construct_sm():
00072     TFUtil()
00073 
00074     # Construct state machine
00075     sm = StateMachine(
00076             ['succeeded','aborted','preempted'],
00077             input_keys = ['base_to_outlet'],
00078             output_keys = ['gripper_to_plug'])
00079 
00080     # Define nominal sequence
00081     with sm:
00082 
00083         # Detect the plug in the gripper
00084         @smach.cb_interface(output_keys=['gripper_to_plug'])
00085         def store_detect_plug_result(ud, result_state, result):
00086             if result_state == GoalStatus.SUCCEEDED:
00087                 gripper_to_plug = TFUtil.wait_and_transform('r_gripper_tool_frame',result.plug_pose).pose
00088                 ud.gripper_to_plug = gripper_to_plug
00089                 print 'gripper_to_plug'
00090                 print gripper_to_plug
00091 
00092         StateMachine.add('DETECT_PLUG_IN_GRIPPER',
00093                 SimpleActionState('detect_plug',
00094                     DetectPlugInGripperAction,
00095                     goal = DetectPlugInGripperGoal(),
00096                     result_cb = store_detect_plug_result),
00097                 {'succeeded':'LOWER_SPINE'})
00098 
00099         StateMachine.add('LOWER_SPINE',
00100                 SimpleActionState('torso_controller/position_joint_action', SingleJointPositionAction,
00101                     goal = SingleJointPositionGoal(position=0.01)),
00102                 {'succeeded':'APPROACH_OUTLET_ITER'})
00103 
00104         # Approach outlet
00105         approach_it = Iterator(
00106                 ['succeeded','preempted','aborted'],
00107                 input_keys = ['base_to_outlet','gripper_to_plug'],
00108                 output_keys = ['outlet_to_plug_contact'],
00109                 it = lambda: drange(-0.07, 0.09, 0.005),
00110                 it_label = 'approach_offset',
00111                 exhausted_outcome = 'aborted')
00112         StateMachine.add('APPROACH_OUTLET_ITER',approach_it,
00113                 {'succeeded':'TWIST_PLUG_ITER',
00114                     'aborted':'FAIL_PULL_BACK_FROM_WALL'})
00115         with approach_it:
00116             approach_sm = StateMachine(
00117                     ['succeeded','preempted','aborted','keep_moving'],
00118                     input_keys=['base_to_outlet','approach_offset','gripper_to_plug'],
00119                     output_keys=['outlet_to_plug_contact'])
00120             Iterator.set_contained_state('APPROACH',approach_sm,
00121                 loop_outcomes=['keep_moving'])
00122 
00123             with approach_sm:
00124                 @smach.cb_interface(
00125                         input_keys=['base_to_outlet','approach_offset','gripper_to_plug'])
00126                 def get_move_closer_goal(ud, goal):
00127                     """Generate an ik goal to move along the local x axis of the outlet."""
00128 
00129                     pose_outlet_plug = PyKDL.Frame(PyKDL.Vector(ud.approach_offset, 0, 0))
00130                     pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)
00131                     base_to_outlet = fromMsg(ud.base_to_outlet)
00132                     gripper_to_plug = fromMsg(ud.gripper_to_plug)
00133 
00134                     pose_base_wrist = base_to_outlet * pose_outlet_plug * gripper_to_plug.Inverse() * pose_gripper_wrist
00135 
00136                     goal = ArmMoveIKGoal()
00137                     goal.pose.pose = toMsg(pose_base_wrist)
00138                     goal.pose.header.stamp = rospy.Time.now()
00139                     goal.pose.header.frame_id = 'base_link'
00140                     goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
00141                     goal.move_duration = rospy.Duration(1.0)
00142                     return goal
00143 
00144                 StateMachine.add('MOVE_CLOSER',
00145                         SimpleActionState('r_arm_ik', ArmMoveIKAction, goal_cb = get_move_closer_goal),
00146                         {'succeeded':'CHECK_FOR_CONTACT','aborted':'CHECK_FOR_CONTACT'})
00147 
00148                 @smach.cb_interface(
00149                         input_keys=['base_to_outlet','approach_offset','gripper_to_plug'],
00150                         output_keys=['outlet_to_plug_contact'])
00151                 def plug_in_contact(ud):
00152                     """Returns true if the plug is in contact with something."""
00153 
00154                     pose_base_gripper = fromMsg(TFUtil.wait_and_lookup('base_link', 'r_gripper_tool_frame').pose)
00155                     pose_outlet_plug = fromMsg(ud.base_to_outlet).Inverse() * pose_base_gripper * fromMsg(ud.gripper_to_plug)
00156 
00157                     # check if difference between desired and measured outlet-plug along x-axis is more than 1 cm
00158                     ud.outlet_to_plug_contact = toMsg(pose_outlet_plug)
00159                     if math.fabs(pose_outlet_plug.p[0] - ud.approach_offset) > 0.01:
00160                         return True
00161                     return False
00162 
00163                 StateMachine.add('CHECK_FOR_CONTACT',
00164                     ConditionState(cond_cb = plug_in_contact),
00165                     {'true':'succeeded','false':'keep_moving'})
00166 
00167 
00168         # Twist the plug to check if it's in the outlet
00169         twist_it = Iterator(
00170                 ['succeeded','preempted','aborted'],
00171                 input_keys = ['base_to_outlet','gripper_to_plug','outlet_to_plug_contact'],
00172                 it = lambda: drange(0.0, 0.25, 0.025),
00173                 output_keys = [],
00174                 it_label = 'twist_angle',
00175                 exhausted_outcome = 'aborted')
00176         with twist_it:
00177             twist_sm = StateMachine(
00178                     ['succeeded','preempted','aborted','keep_moving'],
00179                     input_keys = ['base_to_outlet','gripper_to_plug','twist_angle', 'outlet_to_plug_contact'])
00180             with twist_sm:
00181                 @smach.cb_interface(
00182                         input_keys=['base_to_outlet','gripper_to_plug','twist_angle', 'outlet_to_plug_contact'])
00183                 def get_twist_goal(ud, goal):
00184                     """Generate an ik goal to rotate the plug"""
00185                     pose_contact_plug = PyKDL.Frame(PyKDL.Rotation.RPY(ud.twist_angle, 0, 0))
00186                     pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)
00187 
00188                     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
00189 
00190                     goal = ArmMoveIKGoal()
00191                     goal.pose.pose = toMsg(pose_base_wrist)
00192                     goal.pose.header.stamp = rospy.Time.now()
00193                     goal.pose.header.frame_id = 'base_link'
00194                     goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
00195                     goal.move_duration = rospy.Duration(1.0)
00196                     return goal
00197 
00198                 StateMachine.add('TWIST_PLUG',
00199                         SimpleActionState('r_arm_ik', ArmMoveIKAction, goal_cb = get_twist_goal),
00200                         {'succeeded':'CHECK_PLUG_IN_SOCKET','aborted':'CHECK_PLUG_IN_SOCKET'})
00201 
00202                 # Check for mate
00203                 @smach.cb_interface(input_keys=['roll_effort'],output_keys=['roll_effort'])
00204                 def plug_in_socket(ud):
00205                     """Determine if the plug is in the socket yet"""
00206 
00207                     MIN_EFFORT = 1.0
00208                     ud.roll_effort = None
00209 
00210                     # Local cb
00211                     def joint_states_cb(msg, ud):
00212                         ud.roll_effort = dict(zip(msg.name, msg.effort))['r_wrist_roll_joint']
00213 
00214                     # Subscribe to joint state messages
00215                     joint_sub = rospy.Subscriber("joint_states", JointState, joint_states_cb, ud)
00216 
00217                     # Wait for effort
00218                     while ud.roll_effort is None:
00219                         rospy.sleep(0.05)
00220 
00221                     joint_sub.unregister()
00222                     
00223                     if ud.roll_effort > MIN_EFFORT:
00224                         return True
00225                     
00226                     return False
00227 
00228                 StateMachine.add('CHECK_PLUG_IN_SOCKET',
00229                     ConditionState(cond_cb = plug_in_socket),
00230                     {'true':'succeeded','false':'keep_moving'})
00231 
00232             Iterator.set_contained_state('TWIST',
00233                     twist_sm,
00234                     loop_outcomes=['keep_moving'])
00235 
00236         StateMachine.add('TWIST_PLUG_ITER',twist_it,
00237                 {'succeeded':'STRAIGHTEN_PLUG',
00238                     'aborted':'FAIL_PULL_BACK_FROM_WALL'})
00239 
00240         @smach.cb_interface(input_keys=['base_to_outlet','outlet_to_plug_contact','gripper_to_plug'])
00241         def get_straighten_goal(ud, goal):
00242             """Generate an ik goal to straighten the plug in the outlet."""
00243             pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)
00244 
00245             pose_base_wrist = fromMsg(ud.base_to_outlet) * fromMsg(ud.outlet_to_plug_contact) * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist
00246 
00247             goal = ArmMoveIKGoal()
00248             goal.pose.pose = toMsg(pose_base_wrist)
00249             goal.pose.header.stamp = rospy.Time.now()
00250             goal.pose.header.frame_id = 'base_link'
00251             goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
00252             goal.move_duration = rospy.Duration(0.5)
00253             return goal
00254 
00255         StateMachine.add('STRAIGHTEN_PLUG',
00256             SimpleActionState('r_arm_ik', ArmMoveIKAction, goal_cb = get_straighten_goal),
00257             {'succeeded':'WIGGLE_IN',
00258                 'aborted':'FAIL_PULL_BACK_FROM_WALL'})
00259 
00260         # Wiggle the plug
00261         StateMachine.add('WIGGLE_IN',
00262                 SimpleActionState('wiggle_plug',
00263                     WigglePlugAction,
00264                     goal_cb = get_wiggle_goal),
00265                 {'succeeded':'succeeded',
00266                     'aborted':'FAIL_PULL_BACK_FROM_WALL'})
00267 
00268         ### Recovery states
00269         @smach.cb_interface(input_keys=['base_to_outlet','gripper_to_plug'])
00270         def get_pull_back_goal(ud, goal):
00271             """Generate an ik goal to move along the local x axis of the outlet."""
00272             pose_outlet_plug = PyKDL.Frame(PyKDL.Vector(-0.10, 0, 0))
00273             pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)
00274 
00275             pose_base_wrist = fromMsg(ud.base_to_outlet) * pose_outlet_plug * fromMsg(ud.gripper_to_plug).Inverse() * pose_gripper_wrist
00276 
00277             goal = ArmMoveIKGoal()
00278             goal.pose.pose = toMsg(pose_base_wrist)
00279             goal.pose.header.stamp = rospy.Time.now()
00280             goal.pose.header.frame_id = 'base_link'
00281             goal.ik_seed = get_action_seed('pr2_plugs_configuration/approach_outlet_seed')
00282             goal.move_duration = rospy.Duration(3.0)
00283             return goal
00284 
00285         StateMachine.add('FAIL_PULL_BACK_FROM_WALL',
00286             SimpleActionState('r_arm_ik', ArmMoveIKAction, goal_cb = get_pull_back_goal),
00287             {'succeeded':'aborted',
00288                 'aborted':'FAIL_PULL_BACK_FROM_WALL'})
00289     return sm
00290 
00291 
00292 if __name__ == "__main__":
00293     rospy.init_node("plug_in")#,log_level=rospy.DEBUG)
00294     TFUtil()
00295 
00296     sm_plug_in = construct_sm()
00297 
00298     # Run state machine introspection server
00299     intro_server = IntrospectionServer('plug_in',sm_plug_in,'/RECHARGE/PLUG_IN')
00300     intro_server.start()
00301 
00302     # Run state machine action server 
00303     asw = ActionServerWrapper(
00304             'plug_in', PlugInAction, sm_plug_in,
00305             succeeded_outcomes = ['succeeded'],
00306             aborted_outcomes = ['aborted'],
00307             preempted_outcomes = ['preempted'],
00308             expand_goal_slots = True,
00309             pack_result_slots = True)
00310     asw.run_server()
00311 
00312     rospy.spin()
00313 
00314     intro_server.stop()


pr2_plugs_actions
Author(s): Jon Bohren, Patrick Mihelich, Wim Meeussen, and Melonee Wise
autogenerated on Thu Nov 28 2013 11:47:13