fetch_plug.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 from math import *
00010 import copy
00011 import tf
00012 import PyKDL
00013 
00014 from std_srvs.srv import *
00015 
00016 from actionlib_msgs.msg import *
00017 from pr2_plugs_msgs.msg import *
00018 from pr2_common_action_msgs.msg import *
00019 from pr2_controllers_msgs.msg import *
00020 from trajectory_msgs.msg import *
00021 
00022 from pr2_arm_move_ik.tools import *
00023 from tf_conversions.posemath import fromMsg, toMsg
00024 from geometry_msgs.msg import *
00025 from joint_trajectory_action_tools.tools import get_action_goal as get_generator_goal
00026 
00027 # State machine classes
00028 from pr2_plugs_actions.tf_util import TFUtil
00029 import smach
00030 from smach import *
00031 from smach_ros import *
00032 
00033 import actionlib
00034 
00035 __all__ = ['construct_sm']
00036 
00037 
00038 def construct_sm():
00039     TFUtil()
00040     # Define fixed goals
00041 
00042     # Open gripper goal
00043     open_gripper_goal = Pr2GripperCommandGoal()
00044     open_gripper_goal.command.position = 0.07
00045     open_gripper_goal.command.max_effort = 99999
00046 
00047     # Close gripper goal
00048     close_gripper_goal = Pr2GripperCommandGoal()
00049     close_gripper_goal.command.position = 0.0
00050     close_gripper_goal.command.max_effort = 99999
00051 
00052     # Construct state machine
00053     sm = StateMachine(
00054             ['succeeded','aborted','preempted'],
00055             output_keys = ['plug_on_base_pose', 'gripper_plug_grasp_pose'])
00056 
00057     # Hardcoded poses for approach / grasping
00058     sm.userdata.gripper_plug_grasp_pose_approach = toMsg(PyKDL.Frame(PyKDL.Vector(0, 0.05, 0)).Inverse())
00059     sm.userdata.gripper_plug_grasp_pose = toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(pi/2, 0.0, -pi/9), PyKDL.Vector(-0.03, 0.0, 0.005)).Inverse())
00060 
00061     # Define nominal sequence
00062     with sm:
00063         StateMachine.add('RAISE_SPINE',
00064                 SimpleActionState('torso_controller/position_joint_action',
00065                     SingleJointPositionAction,
00066                     goal = SingleJointPositionGoal(position=0.16)),
00067                 {'succeeded':'MOVE_ARM_BASE_DETECT_POSE'})
00068 
00069         # Move arm to detect the plug on the base
00070         StateMachine.add('MOVE_ARM_BASE_DETECT_POSE',
00071                          SimpleActionState('r_arm_controller/joint_trajectory_generator',
00072                                            JointTrajectoryAction, goal = get_generator_goal('pr2_plugs_configuration/detect_plug_on_base')),
00073                          {'succeeded':'DETECT_PLUG_ON_BASE'})
00074 
00075         # Detect the plug
00076         @smach.cb_interface(output_keys=['plug_on_base_pose'])
00077         def store_detect_plug_result(ud, result_state, result):
00078             if result_state == actionlib.GoalStatus.SUCCEEDED:
00079                 ud.plug_on_base_pose = TFUtil.wait_and_transform('base_link', result.plug_pose).pose
00080 
00081         StateMachine.add('DETECT_PLUG_ON_BASE',
00082                 SimpleActionState('detect_plug_on_base',DetectPlugOnBaseAction,
00083                     goal = DetectPlugOnBaseGoal(),
00084                     result_cb = store_detect_plug_result),
00085                          {'succeeded':'MOVE_ARM_BASE_GRASP_POSE',
00086                           'aborted':'MOVE_ARM_BASE_DETECT_POSE'})
00087 
00088         # Move arm to the grasp pose
00089         StateMachine.add('MOVE_ARM_BASE_GRASP_POSE',
00090                          SimpleActionState('r_arm_controller/joint_trajectory_generator',JointTrajectoryAction, goal = get_generator_goal('pr2_plugs_configuration/grasp_plug')),
00091                          {'succeeded':'OPEN_GRIPPER',
00092                           'aborted':'RECOVER_GRASP_TO_DETECT_POSE'})
00093 
00094         StateMachine.add('OPEN_GRIPPER',
00095                          SimpleActionState('r_gripper_controller/gripper_action',
00096                                            Pr2GripperCommandAction,
00097                                            goal = open_gripper_goal),
00098                          {'succeeded':'APPROACH_PLUG'})
00099 
00100         @smach.cb_interface(input_keys = ['plug_on_base_pose',
00101                                           'gripper_plug_grasp_pose_approach',
00102                                           'gripper_plug_grasp_pose'])
00103         def get_approach_plug_goal(ud, goal):
00104             """Get the ik goal for approaching the plug to grasp it """
00105             pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)
00106             goal = ArmMoveIKGoal()
00107             goal.pose.pose = toMsg(fromMsg(ud.plug_on_base_pose)
00108                                    * fromMsg(ud.gripper_plug_grasp_pose_approach).Inverse()
00109                                    * fromMsg(ud.gripper_plug_grasp_pose).Inverse()
00110                                    * pose_gripper_wrist)
00111 
00112             goal.pose.header.stamp = rospy.Time.now()
00113             goal.pose.header.frame_id = 'base_link'
00114             goal.ik_seed = get_action_seed('pr2_plugs_configuration/grasp_plug_seed')
00115             goal.move_duration = rospy.Duration(3.0)
00116 
00117             return goal
00118 
00119         StateMachine.add('APPROACH_PLUG',
00120                          SimpleActionState('r_arm_ik', ArmMoveIKAction, goal_cb = get_approach_plug_goal),
00121                          {'succeeded':'GRASP_PLUG',
00122                           'aborted':'DETECT_PLUG_ON_BASE'})
00123 
00124         @smach.cb_interface(input_keys = ['plug_on_base_pose', 'gripper_plug_grasp_pose'])
00125         def get_grasp_plug_goal(ud, goal):
00126             """Get the ik goal for grasping the plug."""
00127             pose_gripper_wrist = fromMsg(TFUtil.wait_and_lookup('r_gripper_tool_frame', 'r_wrist_roll_link').pose)
00128 
00129             goal = ArmMoveIKGoal()
00130             goal.pose.pose = toMsg(fromMsg(ud.plug_on_base_pose)
00131                                    * fromMsg(ud.gripper_plug_grasp_pose).Inverse()
00132                                    * pose_gripper_wrist)
00133 
00134             goal.pose.header.stamp = rospy.Time.now()
00135             goal.pose.header.frame_id = 'base_link'
00136             goal.ik_seed = get_action_seed('pr2_plugs_configuration/grasp_plug_seed')
00137             goal.move_duration = rospy.Duration(3.0)
00138 
00139             return goal
00140 
00141         StateMachine.add('GRASP_PLUG',
00142                 SimpleActionState('r_arm_ik', ArmMoveIKAction, goal_cb = get_grasp_plug_goal),
00143                 {'succeeded':'CLOSE_GRIPPER',
00144                     'aborted':'DETECT_PLUG_ON_BASE'})
00145         
00146         StateMachine.add('CLOSE_GRIPPER',
00147                 SimpleActionState('r_gripper_controller/gripper_action',
00148                     Pr2GripperCommandAction,
00149                     goal = close_gripper_goal),
00150                 { 'succeeded':'DETECT_PLUG_ON_BASE',
00151                     'aborted':'REMOVE_PLUG'})
00152 
00153         StateMachine.add('REMOVE_PLUG',
00154                 SimpleActionState('r_arm_controller/joint_trajectory_generator',JointTrajectoryAction, goal = get_generator_goal('pr2_plugs_configuration/remove_plug')),
00155                 {'succeeded':'LOWER_SPINE'})
00156             
00157         StateMachine.add('LOWER_SPINE',
00158                 SimpleActionState('torso_controller/position_joint_action',
00159                     SingleJointPositionAction,
00160                     goal = SingleJointPositionGoal(position=0.01)),
00161                 {'succeeded':'succeeded'})
00162         
00163         # Define recovery states
00164         StateMachine.add('RECOVER_GRASP_TO_DETECT_POSE',
00165                 SimpleActionState('r_arm_controller/joint_trajectory_generator',JointTrajectoryAction, goal = get_generator_goal('pr2_plugs_configuration/recover_grasp_to_detect')),
00166                 { 'succeeded':'DETECT_PLUG_ON_BASE',
00167                     'aborted':'RECOVER_GRASP_TO_DETECT_POSE'})
00168     return sm
00169 
00170 if __name__ == "__main__":
00171     rospy.init_node("fetch_plug")#,log_level=rospy.DEBUG)
00172     TFUtil()
00173 
00174     sm_fetch_plug = construct_sm()
00175 
00176     # Run state machine introspection server
00177     intro_server = IntrospectionServer('fetch_plug',sm_fetch_plug,'/RECHARGE/FETCH_PLUG')
00178     intro_server.start()
00179 
00180     # Run state machine action server 
00181     asw = ActionServerWrapper(
00182             'fetch_plug', FetchPlugAction, sm_fetch_plug,
00183             succeeded_outcomes = ['succeeded'],
00184             aborted_outcomes = ['aborted'],
00185             preempted_outcomes = ['preempted'],
00186             pack_result_slots = True)
00187     asw.run_server()
00188 
00189     rospy.spin()
00190 
00191     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