sm_grasp_cleanup_demo.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 #
00003 # Copyright (c) 2009, Georgia Tech Research Corporation
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #     * Redistributions of source code must retain the above copyright
00009 #       notice, this list of conditions and the following disclaimer.
00010 #     * Redistributions in binary form must reproduce the above copyright
00011 #       notice, this list of conditions and the following disclaimer in the
00012 #       documentation and/or other materials provided with the distribution.
00013 #     * Neither the name of the Georgia Tech Research Corporation nor the
00014 #       names of its contributors may be used to endorse or promote products
00015 #       derived from this software without specific prior written permission.
00016 #
00017 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
00018 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
00021 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00022 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00023 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00024 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00025 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00026 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 #
00028 
00029 #  \author Travis Deyle and Kelsey Hawkins (Healthcare Robotics Lab, Georgia Tech.)
00030 
00031 GRASP_LOCATION  =  [ 0.50, -0.30,  0.00]
00032 PLACE_LOCATIONS = [[ 0.58,  0.13,  0.00],
00033                    [ 0.58,  0.21,  0.00],
00034                    [ 0.58,  0.29,  0.00]]
00035 
00036 import sys
00037 
00038 import roslib
00039 roslib.load_manifest('hrl_pr2_experiments')
00040 import rospy
00041 
00042 import smach
00043 from smach_ros import SimpleActionState, ServiceState, IntrospectionServer
00044 import actionlib
00045 import tf.transformations as tft
00046 
00047 from pr2_grasp_behaviors.msg import OverheadGraspAction, OverheadGraspSetupAction
00048 from pr2_grasp_behaviors.msg import OverheadGraspGoal, OverheadGraspSetupGoal
00049 from pr2_controllers_msgs.msg import SingleJointPositionAction, SingleJointPositionGoal
00050 from hrl_trajectory_playback.srv import TrajPlaybackSrv, TrajPlaybackSrvRequest
00051 
00052 # Overhead grasping requres:
00053 #   run: hrl_pr2_gains/change_gains_grasp.sh
00054 #   roslaunch pr2_grasping_behaviors overhead_grasping_server_trained.launch
00055 
00056 class NTries(smach.State):
00057     def __init__(self, n):
00058         smach.State.__init__(self, outcomes=['succeeded', 'aborted'],
00059                                    output_keys=['ntries_counter'])
00060         self.counter = 0
00061         self.n = n
00062 
00063     def execute(self, userdata):
00064         self.counter += 1
00065         userdata.ntries_counter = self.counter
00066 
00067         if self.counter <= self.n:
00068             rospy.logout( 'Executing NTries: On #%d of %d' % (self.counter, self.n))
00069             return 'succeeded'
00070         else:
00071             return 'aborted'
00072 
00073 def sm_grasp():
00074     if len(sys.argv) < 2 or sys.argv[1] not in ['r', 'l']:
00075         print "First arg should be 'r' or 'l'"
00076         return None
00077     arm = sys.argv[1]
00078     if arm == 'r':
00079         arm_mult = 1
00080     else:
00081         arm_mult = -1
00082 
00083     # Create a SMACH state machine
00084     sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])
00085 
00086     with sm:
00087         # Setup arm pose (out of way for perception)
00088         tgoal = SingleJointPositionGoal()
00089         #tgoal.position = 0.190  # all the way up is 0.200
00090         tgoal.position = 0.210  # all the way up is 0.200
00091         tgoal.min_duration = rospy.Duration( 2.0 )
00092         tgoal.max_velocity = 1.0
00093         smach.StateMachine.add(
00094             'TORSO_SETUP',
00095             SimpleActionState( 'torso_controller/position_joint_action',
00096                                SingleJointPositionAction,
00097                                goal = tgoal),
00098             transitions = { 'succeeded': 'ARM_UNTUCK' })
00099 
00100         smach.StateMachine.add(
00101             'ARM_UNTUCK',
00102             ServiceState('traj_playback/' + arm + '_arm_untuck', TrajPlaybackSrv),
00103             transitions = { 'succeeded': 'GRASP_BEGIN_SETUP' })
00104 
00105         # Setup arm pose (out of way for perception)
00106         smach.StateMachine.add(
00107             'GRASP_BEGIN_SETUP',
00108             SimpleActionState( arm + '_overhead_grasp_setup',
00109                                OverheadGraspSetupAction,
00110                                goal = OverheadGraspSetupGoal()), 
00111             transitions = { 'succeeded': 'DEMO_START' })
00112 
00113         @smach.cb_interface(outcomes=['succeeded'])
00114         def wait_for_enter(ud):
00115             raw_input("Press enter to begin cleanup demo.")
00116             return 'succeeded'
00117         smach.StateMachine.add(
00118             'DEMO_START',
00119             smach.CBState(wait_for_enter),
00120             transitions = {'succeeded': 'THREE_OBJECTS'})
00121 
00122         # We will pick up 3 objects.
00123         smach.StateMachine.add(
00124             'THREE_OBJECTS',
00125             NTries( 3 ),
00126             transitions = {'succeeded':'THREE_TRIES',
00127                            'aborted':'RESET_ARMS'},
00128             remapping={'ntries_counter':'object_number'})
00129 
00130         # We will run the grasper at most 3 times.
00131         grasp_tries = NTries( 3 )
00132         smach.StateMachine.add(
00133             'THREE_TRIES',
00134             grasp_tries,
00135             transitions = {'succeeded':'GRASP_SETUP',
00136                            'aborted':'aborted'})
00137 
00138         # Setup arm pose (out of way for perception)
00139         smach.StateMachine.add(
00140             'GRASP_SETUP',
00141             SimpleActionState( arm + '_overhead_grasp_setup',
00142                                OverheadGraspSetupAction,
00143                                goal = OverheadGraspSetupGoal()), 
00144             transitions = { 'succeeded': 'GRASP' })
00145 
00146         def grasp_goal_cb(userdata, goal):
00147             ############################################################
00148             # Creating grasp goal
00149             grasp_goal = OverheadGraspGoal()
00150             grasp_goal.is_grasp = True
00151             grasp_goal.disable_head = False
00152             grasp_goal.disable_coll = False
00153             grasp_goal.grasp_type = OverheadGraspGoal.VISION_GRASP
00154             grasp_goal.x = GRASP_LOCATION[0]
00155             grasp_goal.y = arm_mult * GRASP_LOCATION[1]
00156             grasp_goal.behavior_name = "overhead_grasp"
00157             grasp_goal.sig_level = 0.999
00158             ############################################################
00159             return grasp_goal
00160 
00161         smach.StateMachine.add(
00162             'GRASP',
00163             SimpleActionState( arm + '_overhead_grasp',
00164                                OverheadGraspAction,
00165                                goal_cb = grasp_goal_cb),
00166             transitions = { 'succeeded': 'PLACE',
00167                             'aborted':'THREE_TRIES' })
00168 
00169         def place_goal_cb(userdata, goal):
00170             print "object Number", userdata.object_number
00171             ############################################################
00172             # Creating place place_goal
00173             place_goal = OverheadGraspGoal()
00174             place_goal.is_grasp = False
00175             place_goal.disable_head = False
00176             place_goal.disable_coll = False
00177             place_goal.grasp_type = OverheadGraspGoal.MANUAL_GRASP
00178             place_goal.x = PLACE_LOCATIONS[userdata.object_number-1][0]
00179             place_goal.y = arm_mult * PLACE_LOCATIONS[userdata.object_number-1][1]
00180             place_goal.roll = PLACE_LOCATIONS[userdata.object_number-1][2]
00181             place_goal.behavior_name = "overhead_grasp"
00182             place_goal.sig_level = 0.999
00183             ############################################################
00184             return place_goal
00185 
00186         def clear_grasp_tries(userdata, status, result):
00187             grasp_tries.counter = 0
00188 
00189         smach.StateMachine.add(
00190             'PLACE',
00191             SimpleActionState( arm + '_overhead_grasp',
00192                                OverheadGraspAction,
00193                                goal_cb = place_goal_cb,
00194                                result_cb = clear_grasp_tries,
00195                                input_keys = ['object_number']),
00196             transitions = { 'succeeded': 'THREE_OBJECTS',
00197                             'aborted':'THREE_OBJECTS' })
00198 
00199         # Setup arm pose (out of way for perception)
00200         smach.StateMachine.add(
00201             'RESET_ARMS',
00202             SimpleActionState( arm + '_overhead_grasp_setup',
00203                                OverheadGraspSetupAction,
00204                                goal = OverheadGraspSetupGoal()), 
00205             transitions = { 'succeeded': 'ARM_TUCK' })
00206 
00207         smach.StateMachine.add(
00208             'ARM_TUCK',
00209             ServiceState('traj_playback/' + arm + '_arm_untuck', TrajPlaybackSrv,
00210                          request=TrajPlaybackSrvRequest(True)),
00211             transitions = { 'succeeded': 'succeeded' })
00212             
00213     return sm
00214 
00215 
00216 
00217 if __name__ == '__main__':
00218     rospy.init_node('smach_sm_grasp')
00219 
00220     sm = sm_grasp()
00221 
00222     sis = IntrospectionServer('Grasp Cleanup', sm, '/SM_GRASP_CLEANUP')
00223     sis.start()
00224 
00225     outcome = sm.execute()
00226     
00227     sis.stop()
00228 
00229     
00230 


hrl_hfa_2011
Author(s): Kelsey Hawkins
autogenerated on Wed Nov 27 2013 12:20:53