예제 #1
0
 def adjust_torso(self):
     # move torso up
     tgoal = SingleJointPositionGoal()
     tgoal.position = 0.238  # all the way up is 0.300
     tgoal.min_duration = rospy.Duration( 2.0 )
     tgoal.max_velocity = 1.0
     self.torso_sac.send_goal_and_wait(tgoal)
예제 #2
0
 def set_torso_state(self, jval, wait=False):
     """ Sets goal for torso using provided value"""
     
     if not (type(jval) is list):
         jval = [jval] 
     
     if len(jval) == 0:
         rospy.logwarn("No torso target given!")
         self.torso_done = True
         return
     
     #HACK: it looks like with a value outside this range the actionserver doesn't return..
     if jval[0] < 0.012:
         jval[0] = 0.2
     if jval[0] > 0.3:
         jval[0] = 0.3
     self.torso_done = False
     
     goal = SingleJointPositionGoal()
     goal.position = jval[0]
     goal.min_duration = rospy.Duration(self.time_to_reach)
     
     self.torso_client.send_goal(goal, done_cb=self.__torso_done_cb())
     if wait:
         self.torso_client.wait_for_result()
예제 #3
0
def move_body_torso(position):
    service = 'torso_controller/position_joint_action'
    goal = SingleJointPositionGoal()
    goal.position = position
    goal.min_duration = roslib.rostime.Duration(2.0)
    goal.max_velocity = 1.0
     
    client = actionlib.SimpleActionClient(service, SingleJointPositionAction)
    client.send_goal(goal, done_cb=task_completed)
    client.wait_for_result()
예제 #4
0
    def move(self, position):
        assert (position >= 0.0 and position <= 0.2)
        self.position = position

        up = SingleJointPositionGoal()
        up.position = self.position
        up.min_duration = rospy.Duration(2.0)
        up.max_velocity = 1.0

        self.torso_client.send_goal(up)
        self.torso_client.wait_for_result()
    def get_sm(self):
        sm = smach.StateMachine(outcomes=['succeeded','preempted','aborted'])
        
        with sm:
            smach.StateMachine.add('NAV_APPROACH',
                    self.sm_nav_approach.get_sm(),
                    transitions={'succeeded' : 'TORSO_SETUP',
                                 'shutdown' : 'aborted'})

            # move torso up
            tgoal = SingleJointPositionGoal()
            tgoal.position = 0.300  # all the way up is 0.300
            tgoal.min_duration = rospy.Duration( 2.0 )
            tgoal.max_velocity = 1.0
            smach.StateMachine.add(
                'TORSO_SETUP',
                SimpleActionState( 'torso_controller/position_joint_action',
                                   SingleJointPositionAction,
                                   goal = tgoal),
                transitions = { 'succeeded': 'R_UNTUCK' })

            # Untucks the right arm
            smach.StateMachine.add(
                'R_UNTUCK',
                ServiceState( 'traj_playback/untuck_r_arm',
                              TrajPlaybackSrv,
                              request = TrajPlaybackSrvRequest( False )), # if True, reverse trajectory
                transitions = { 'succeeded' : 'R_ADJUST_MIRROR' })

            # Adjusts the mirror
            smach.StateMachine.add(
                'R_ADJUST_MIRROR',
                ServiceState( 'traj_playback/r_adjust_mirror',
                              TrajPlaybackSrv,
                              request = TrajPlaybackSrvRequest( False )), # if True, reverse trajectory
                transitions = { 'succeeded' : 'L_UNTUCK' })

            # Untucks the left arm
            smach.StateMachine.add(
                'L_UNTUCK',
                ServiceState( 'traj_playback/untuck_l_arm',
                              TrajPlaybackSrv,
                              request = TrajPlaybackSrvRequest( False )), # if True, reverse trajectory
                transitions = { 'succeeded' : 'HEAD_REG_ALL' })

            smach.StateMachine.add('HEAD_REG_ALL',
                    self.sm_ell_reg.get_sm(),
                    transitions = { 'succeeded' : 'SETUP_TASK_CONTROLLER' })

            smach.StateMachine.add(
                'SETUP_TASK_CONTROLLER',
                SetupTaskController())

        return sm
예제 #6
0
파일: torso.py 프로젝트: vjampala/cse481
    def move(self, position):
        assert(position >= 0.0 and position <= 0.2)
        self.position = position
        
        up = SingleJointPositionGoal()
        up.position = self.position
        up.min_duration = rospy.Duration(2.0)
        up.max_velocity = 1.0

        self.torso_client.send_goal(up)
        self.torso_client.wait_for_result()
예제 #7
0
def sm_grasp():
    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])

    with sm:
        # Setup arm pose (out of way for perception)
        tgoal = SingleJointPositionGoal()
        tgoal.position = 0.190  # all the way up is 0.200
        tgoal.min_duration = rospy.Duration( 2.0 )
        tgoal.max_velocity = 1.0
        smach.StateMachine.add(
            'TORSO_SETUP',
            SimpleActionState( 'torso_controller/position_joint_action',
                               SingleJointPositionAction,
                               goal = tgoal),
            transitions = { 'succeeded': 'THREE_TRIES' })

        # We will run the grasper at most 3 times.
        smach.StateMachine.add(
            'THREE_TRIES',
            NTries( 3 ),
            transitions = {'succeeded':'PERCEIVE_SETUP',
                           'aborted':'aborted'})

        # get hand out of face
        smach.StateMachine.add(
            'PERCEIVE_SETUP',
            ServiceState( '/rfid_handoff/grasp', HandoffSrv ),
            transitions = { 'succeeded' : 'PERCEIVE_OBJECT' })
            

        # Setment objects
        smach.StateMachine.add(
            'PERCEIVE_OBJECT',
            ServiceState( '/obj_segment_inst',
                          DetectTableInst,
                          request = DetectTableInstRequest( 1.0 ),
                          response_slots = ['grasp_points']), # PoseArray
            transitions = {'succeeded':'GRASP_SETUP'},
            remapping = {'grasp_points':'object_poses'}) #output

        # Setup arm pose (out of way for perception)
        smach.StateMachine.add(
            'GRASP_SETUP',
            SimpleActionState( 'overhead_grasp_setup',
                               OverheadGraspSetupAction,
                               goal = OverheadGraspSetupGoal( True )), # disable new look
            transitions = { 'succeeded': 'GRASP' })

        # Actually perform grasp of some object in front of robot on table
        def grasp_goal_cb( userdata, goal ):
            # grasp_poses is PoseArray in base_link
            mgp = userdata.grasp_poses.poses[0]
            
            ggoal = OverheadGraspGoal()
            ggoal.is_grasp = True
            ggoal.grasp_type = OverheadGraspGoal.MANUAL_GRASP
            
            ggoal.x = mgp.position.x + 0.05 # Converts base_link -> torso_lift_link (only for x,y)
            ggoal.y = mgp.position.y

            o = mgp.orientation
            r,p,y = tft.euler_from_quaternion(( o.x, o.y, o.z, o.w ))
            ggoal.rot = y
            
            return ggoal

        smach.StateMachine.add(
            'GRASP',
            SimpleActionState( 'overhead_grasp',
                               OverheadGraspAction,
                               goal_cb = grasp_goal_cb,
                               input_keys = ['grasp_poses']),
            remapping = {'grasp_poses':'object_poses'},
            transitions = { 'succeeded': 'succeeded',
                            'aborted':'THREE_TRIES' })
            
    return sm
예제 #8
0
    def get_nav_prep_sm(self):
        nav_prep_sm = smach.StateMachine(outcomes=['succeeded','preempted','shutdown', 'aborted'])
        with nav_prep_sm:

            # make sure the robot is clear of obstacles
            # make sure the arms are tucked with
            # rosrun pr2_tuckarm tuck_arms.py r t l t
            # wait for the user to click on the head so the robot can approach
            smach.StateMachine.add(
                'WAIT_FOR_HEAD_CLICK',
                ClickMonitor(),
                transitions={'click' : 'PROCESS_NAV_POSE',
                             'shutdown' : 'shutdown'},
                remapping={'click_pose' : 'head_click_pose_global'}) # output (PoseStamped)

            # prepare the navigation pose for move_base
            # gets a point aligned with the normal and a distance away (nav_approach_dist)
            smach.StateMachine.add(
                'PROCESS_NAV_POSE',
                self.get_nav_approach_pose(),
                transitions={'succeeded' : 'MOVE_BASE', 
                             'tf_failure' : 'WAIT_FOR_HEAD_CLICK'},
                remapping={'head_click_pose' : 'head_click_pose_global', # input (PoseStamped)
                           'nav_pose_ps' : 'nav_pose_ps_global'}) # output (PoseStamped)

            # moves the base using nav stack to the appropirate location for moving forward
            smach.StateMachine.add(
                'MOVE_BASE',
                SimpleActionState('/move_base',
                                  MoveBaseAction,
                                  goal_slots=['target_pose'], # PoseStamped
                                  outcomes=['succeeded','aborted','preempted']),
                transitions = { 'succeeded' : 'CHECK_HEADING',
                                'aborted' : 'WAIT_FOR_HEAD_CLICK' },
                remapping = {'target_pose':'nav_pose_ps_global'}) # input (PoseStamped)

            # checks the current angle and returns the error
            smach.StateMachine.add(
                'CHECK_HEADING',
                CheckHeading( listener = self.tf_listener ),
                transitions = { 'succeeded':'ADJUST_HEADING' },
                remapping = { 'target_pose':'nav_pose_ps_global', # input (PoseStamped)
                              'angular_error':'angular_error' }) # output (float)

            # corrects for the error in angle
            smach.StateMachine.add(
                'ADJUST_HEADING',
                ServiceState( '/rotate_backup',
                              RotateBackupSrv,
                              request_slots = ['rotate']), # float (displace = 0.0)
                transitions = { 'succeeded':'MOVE_FORWARD_DIST' },
                remapping = {'rotate':'angular_error'})
            
            # approaches the touching position by moving forward nav_approach_dist meters
            # checks for collisions and will return shutdown if it detects a collsion
            smach.StateMachine.add(
                'MOVE_FORWARD_DIST',
                self.get_nav_approach(),
                transitions = {'succeeded' : 'TORSO_SETUP',
                               'shutdown' : 'shutdown'},
                remapping={'nav_dist' : 'nav_dist_global'})

            # move torso up
            tgoal = SingleJointPositionGoal()
            tgoal.position = 0.300  # all the way up is 0.300
            tgoal.min_duration = rospy.Duration( 2.0 )
            tgoal.max_velocity = 1.0
            smach.StateMachine.add(
                'TORSO_SETUP',
                SimpleActionState( 'torso_controller/position_joint_action',
                                   SingleJointPositionAction,
                                   goal = tgoal),
                transitions = { 'succeeded': 'UNFOLD' })

            # Unfolds the arms
            smach.StateMachine.add(
                'UNFOLD',
                ServiceState( 'traj_playback/unfold',
                              TrajPlaybackSrv,
                              request = TrajPlaybackSrvRequest( False )), # if True, reverse trajectory
                transitions = { 'succeeded':'succeeded' })

        return nav_prep_sm
예제 #9
0
        #     transitions = {'succeeded':'succeeded'})

        

            
    return sm

if False:
    rospy.init_node('localization_trial')

    sm = smach.StateMachine( outcomes=['succeeded','aborted','preempted'] )
    with sm:
        # Just a precation
        tgoal = SingleJointPositionGoal()
        tgoal.position = 0.040  # all the way up is 0.200
        tgoal.min_duration = rospy.Duration( 2.0 )
        tgoal.max_velocity = 1.0
        smach.StateMachine.add(
            'TORSO_SETUP',
            SimpleActionState( 'torso_controller/position_joint_action',
                               SingleJointPositionAction,
                               goal = tgoal),
            transitions = { 'succeeded': 'succeeded' })

    sm.execute()
    

if __name__ == '__main__':
# if False:
    rospy.init_node('smach_aware_home')
예제 #10
0
        #                        MoveBaseAction,
        #                        goal = go ),  # Back down the hallway
        #     transitions = {'succeeded':'succeeded'})

    return sm


if False:
    rospy.init_node('localization_trial')

    sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
    with sm:
        # Just a precation
        tgoal = SingleJointPositionGoal()
        tgoal.position = 0.040  # all the way up is 0.200
        tgoal.min_duration = rospy.Duration(2.0)
        tgoal.max_velocity = 1.0
        smach.StateMachine.add('TORSO_SETUP',
                               SimpleActionState(
                                   'torso_controller/position_joint_action',
                                   SingleJointPositionAction,
                                   goal=tgoal),
                               transitions={'succeeded': 'succeeded'})

    sm.execute()

if __name__ == '__main__':
    # if False:
    rospy.init_node('smach_aware_home')

    sm = cousins_demo()
예제 #11
0
def cousins_demo():
    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'],
                            input_keys = [ 'person_id' ])

    with sm:

        # Just a precation
        tgoal = SingleJointPositionGoal()
        tgoal.position = 0.040  # all the way up is 0.200, mostly down is 0.040
        tgoal.min_duration = rospy.Duration( 2.0 )
        tgoal.max_velocity = 1.0
        smach.StateMachine.add(
            'TORSO_SETUP',
            SimpleActionState( 'torso_controller/position_joint_action',
                               SingleJointPositionAction,
                               goal = tgoal),
            transitions = { 'succeeded': 'INIT_ARM_POSE' })

        
        smach.StateMachine.add(
            'INIT_ARM_POSE',
            ServiceState( '/traj_playback/hfa_untuck',
                          TrajPlaybackSrv,
                          request = TrajPlaybackSrvRequest( False )), 
            transitions = { 'succeeded':'MOVE_GET_OBJ' })
        

        go = MoveBaseGoal()
        
        go.target_pose.header.frame_id = '/map'
        go.target_pose.header.stamp = rospy.Time(0)
        go.target_pose.pose.position.x = -5.07
        go.target_pose.pose.position.y = 8.725
        go.target_pose.pose.orientation.z = 0.926
        go.target_pose.pose.orientation.w = 0.377

        smach.StateMachine.add(
            'MOVE_GET_OBJ',
            SimpleActionState( '/move_base',
                               MoveBaseAction,
                               goal = go ),
            transitions = {'succeeded':'READY_HANDOFF_INIT'})


        smach.StateMachine.add(
            'READY_HANDOFF_INIT',
            PrintStr('Hand me an object [ENTER]'), 
            transitions = { 'succeeded':'HANDOFF_INIT' })

        smach.StateMachine.add(
            'HANDOFF_INIT',
            ServiceState( 'rfid_handoff/initialize',
                          HandoffSrv,
                          request = HandoffSrvRequest()), 
            transitions = { 'succeeded':'CONFIRM_HANDOFF' })

        smach.StateMachine.add(
            'CONFIRM_HANDOFF',
            ConfirmObj('Does the robot have the object [\'yes\' to proceed]?'), 
            transitions = { 'succeeded':'ROTATE_AFTER_HANDOFF',
                            'aborted':'READY_HANDOFF_INIT'} )
        
        smach.StateMachine.add(
            'ROTATE_AFTER_HANDOFF',
            ServiceState( '/rotate_backup',
                          RotateBackupSrv,
                          request = RotateBackupSrvRequest( 3.14, 0.0)),  # Full 180-deg spin.
            transitions = { 'succeeded':'MOVE_DELIVERY' })

        gd = MoveBaseGoal()
        gd.target_pose.header.frame_id = '/map'
        gd.target_pose.header.stamp = rospy.Time(0)
        gd.target_pose.pose.position.x = 2.956
        gd.target_pose.pose.position.y = 3.047
        gd.target_pose.pose.orientation.z = 0.349
        gd.target_pose.pose.orientation.w = 0.937

        smach.StateMachine.add(
            'MOVE_DELIVERY',
            SimpleActionState( '/move_base',
                               MoveBaseAction,
                               goal = gd ),
            transitions = {'succeeded':'DELIVERY'})

        sm_delivery = sm_rfid_delivery.sm_delivery()
        smach.StateMachine.add(
            'DELIVERY', # outcomes: succeeded, aborted, preempted
            sm_delivery,
            remapping = { 'tagid' : 'person_id'}, #input
            transitions = { 'succeeded': 'TUCKL_SUCC',
                            'aborted': 'TUCKL_ABOR'})

        smach.StateMachine.add(
            'TUCKL_SUCC',
            ServiceState( '/robotis/servo_left_pan_moveangle',
                          MoveAng,
                          request = MoveAngRequest( 1.350, 0.3, 0)), 
            transitions = { 'succeeded':'TUCKR_SUCC' })
        smach.StateMachine.add(
            'TUCKR_SUCC',
            ServiceState( '/robotis/servo_right_pan_moveangle',
                          MoveAng,
                          request = MoveAngRequest( -1.350, 0.3, 0)), 
            transitions = { 'succeeded':'BACKUP_AFTER_DELIVERY' })

        smach.StateMachine.add(
            'TUCKL_ABOR',
            ServiceState( '/robotis/servo_left_pan_moveangle',
                          MoveAng,
                          request = MoveAngRequest( 1.350, 0.3, 0)), 
            transitions = { 'succeeded':'TUCKR_ABOR' })
        smach.StateMachine.add(
            'TUCKR_ABOR',
            ServiceState( '/robotis/servo_right_pan_moveangle',
                          MoveAng,
                          request = MoveAngRequest( -1.350, 0.3, 0)), 
            transitions = { 'succeeded':'aborted' })


        smach.StateMachine.add(
            'BACKUP_AFTER_DELIVERY',
            ServiceState( '/rotate_backup',
                          RotateBackupSrv,
                          request = RotateBackupSrvRequest(0.0, -0.50)), 
            transitions = { 'succeeded':'ROTATE_AFTER_DELIVERY' })

        smach.StateMachine.add(
            'ROTATE_AFTER_DELIVERY',
            ServiceState( '/rotate_backup',
                          RotateBackupSrv,
                          request = RotateBackupSrvRequest( 3.14, 0.0)), 
            transitions = { 'succeeded':'MOVE_FINAL' })

        # Kitchen
        # gk = MoveBaseGoal()
        # gk.target_pose.header.frame_id = '/map'
        # gk.target_pose.header.stamp = rospy.Time(0)
        # gk.target_pose.pose.position.x = -1.61
        # gk.target_pose.pose.position.y =  0.88
        # gk.target_pose.pose.orientation.z = 0.91
        # gk.target_pose.pose.orientation.w = 0.40

        smach.StateMachine.add(
            'MOVE_FINAL',
            SimpleActionState( '/move_base',
                               MoveBaseAction,
                               goal = go ),  # Back down the hallway
            transitions = {'succeeded':'succeeded'})

        

            
    return sm
예제 #12
0
def sm_grasp():
    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])

    with sm:
        # Setup arm pose (out of way for perception)
        tgoal = SingleJointPositionGoal()
        tgoal.position = 0.190  # all the way up is 0.200
        tgoal.min_duration = rospy.Duration(2.0)
        tgoal.max_velocity = 1.0
        smach.StateMachine.add('TORSO_SETUP',
                               SimpleActionState(
                                   'torso_controller/position_joint_action',
                                   SingleJointPositionAction,
                                   goal=tgoal),
                               transitions={'succeeded': 'THREE_TRIES'})

        # We will run the grasper at most 3 times.
        smach.StateMachine.add('THREE_TRIES',
                               NTries(3),
                               transitions={
                                   'succeeded': 'PERCEIVE_SETUP',
                                   'aborted': 'aborted'
                               })

        # get hand out of face
        smach.StateMachine.add('PERCEIVE_SETUP',
                               ServiceState('/rfid_handoff/grasp', HandoffSrv),
                               transitions={'succeeded': 'PERCEIVE_OBJECT'})

        # Setment objects
        smach.StateMachine.add(
            'PERCEIVE_OBJECT',
            ServiceState('/obj_segment_inst',
                         DetectTableInst,
                         request=DetectTableInstRequest(1.0),
                         response_slots=['grasp_points']),  # PoseArray
            transitions={'succeeded': 'GRASP_SETUP'},
            remapping={'grasp_points': 'object_poses'})  #output

        # Setup arm pose (out of way for perception)
        smach.StateMachine.add(
            'GRASP_SETUP',
            SimpleActionState(
                'overhead_grasp_setup',
                OverheadGraspSetupAction,
                goal=OverheadGraspSetupGoal(True)),  # disable new look
            transitions={'succeeded': 'GRASP'})

        # Actually perform grasp of some object in front of robot on table
        def grasp_goal_cb(userdata, goal):
            # grasp_poses is PoseArray in base_link
            mgp = userdata.grasp_poses.poses[0]

            ggoal = OverheadGraspGoal()
            ggoal.is_grasp = True
            ggoal.grasp_type = OverheadGraspGoal.MANUAL_GRASP

            ggoal.x = mgp.position.x + 0.05  # Converts base_link -> torso_lift_link (only for x,y)
            ggoal.y = mgp.position.y

            o = mgp.orientation
            r, p, y = tft.euler_from_quaternion((o.x, o.y, o.z, o.w))
            ggoal.rot = y

            return ggoal

        smach.StateMachine.add('GRASP',
                               SimpleActionState('overhead_grasp',
                                                 OverheadGraspAction,
                                                 goal_cb=grasp_goal_cb,
                                                 input_keys=['grasp_poses']),
                               remapping={'grasp_poses': 'object_poses'},
                               transitions={
                                   'succeeded': 'succeeded',
                                   'aborted': 'THREE_TRIES'
                               })

    return sm
예제 #13
0
def sm_grasp():
    if len(sys.argv) < 2 or sys.argv[1] not in ['r', 'l']:
        print "First arg should be 'r' or 'l'"
        return None
    arm = sys.argv[1]
    if arm == 'r':
        arm_mult = 1
    else:
        arm_mult = -1

    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])

    with sm:
        # Setup arm pose (out of way for perception)
        tgoal = SingleJointPositionGoal()
        #tgoal.position = 0.190  # all the way up is 0.200
        tgoal.position = 0.210  # all the way up is 0.200
        tgoal.min_duration = rospy.Duration(2.0)
        tgoal.max_velocity = 1.0
        smach.StateMachine.add('TORSO_SETUP',
                               SimpleActionState(
                                   'torso_controller/position_joint_action',
                                   SingleJointPositionAction,
                                   goal=tgoal),
                               transitions={'succeeded': 'ARM_UNTUCK'})

        smach.StateMachine.add('ARM_UNTUCK',
                               ServiceState(
                                   'traj_playback/' + arm + '_arm_untuck',
                                   TrajPlaybackSrv),
                               transitions={'succeeded': 'GRASP_BEGIN_SETUP'})

        # Setup arm pose (out of way for perception)
        smach.StateMachine.add('GRASP_BEGIN_SETUP',
                               SimpleActionState(
                                   arm + '_overhead_grasp_setup',
                                   OverheadGraspSetupAction,
                                   goal=OverheadGraspSetupGoal()),
                               transitions={'succeeded': 'DEMO_START'})

        @smach.cb_interface(outcomes=['succeeded'])
        def wait_for_enter(ud):
            raw_input("Press enter to begin cleanup demo.")
            return 'succeeded'

        smach.StateMachine.add('DEMO_START',
                               smach.CBState(wait_for_enter),
                               transitions={'succeeded': 'THREE_OBJECTS'})

        # We will pick up 3 objects.
        smach.StateMachine.add('THREE_OBJECTS',
                               NTries(3),
                               transitions={
                                   'succeeded': 'THREE_TRIES',
                                   'aborted': 'RESET_ARMS'
                               },
                               remapping={'ntries_counter': 'object_number'})

        # We will run the grasper at most 3 times.
        grasp_tries = NTries(3)
        smach.StateMachine.add('THREE_TRIES',
                               grasp_tries,
                               transitions={
                                   'succeeded': 'GRASP_SETUP',
                                   'aborted': 'aborted'
                               })

        # Setup arm pose (out of way for perception)
        smach.StateMachine.add('GRASP_SETUP',
                               SimpleActionState(
                                   arm + '_overhead_grasp_setup',
                                   OverheadGraspSetupAction,
                                   goal=OverheadGraspSetupGoal()),
                               transitions={'succeeded': 'GRASP'})

        def grasp_goal_cb(userdata, goal):
            ############################################################
            # Creating grasp goal
            grasp_goal = OverheadGraspGoal()
            grasp_goal.is_grasp = True
            grasp_goal.disable_head = False
            grasp_goal.disable_coll = False
            grasp_goal.grasp_type = OverheadGraspGoal.VISION_GRASP
            grasp_goal.x = GRASP_LOCATION[0]
            grasp_goal.y = arm_mult * GRASP_LOCATION[1]
            grasp_goal.behavior_name = "overhead_grasp"
            grasp_goal.sig_level = 0.999
            ############################################################
            return grasp_goal

        smach.StateMachine.add('GRASP',
                               SimpleActionState(arm + '_overhead_grasp',
                                                 OverheadGraspAction,
                                                 goal_cb=grasp_goal_cb),
                               transitions={
                                   'succeeded': 'PLACE',
                                   'aborted': 'THREE_TRIES'
                               })

        def place_goal_cb(userdata, goal):
            print "object Number", userdata.object_number
            ############################################################
            # Creating place place_goal
            place_goal = OverheadGraspGoal()
            place_goal.is_grasp = False
            place_goal.disable_head = False
            place_goal.disable_coll = False
            place_goal.grasp_type = OverheadGraspGoal.MANUAL_GRASP
            place_goal.x = PLACE_LOCATIONS[userdata.object_number - 1][0]
            place_goal.y = arm_mult * PLACE_LOCATIONS[userdata.object_number -
                                                      1][1]
            place_goal.roll = PLACE_LOCATIONS[userdata.object_number - 1][2]
            place_goal.behavior_name = "overhead_grasp"
            place_goal.sig_level = 0.999
            ############################################################
            return place_goal

        def clear_grasp_tries(userdata, status, result):
            grasp_tries.counter = 0

        smach.StateMachine.add('PLACE',
                               SimpleActionState(arm + '_overhead_grasp',
                                                 OverheadGraspAction,
                                                 goal_cb=place_goal_cb,
                                                 result_cb=clear_grasp_tries,
                                                 input_keys=['object_number']),
                               transitions={
                                   'succeeded': 'THREE_OBJECTS',
                                   'aborted': 'THREE_OBJECTS'
                               })

        # Setup arm pose (out of way for perception)
        smach.StateMachine.add('RESET_ARMS',
                               SimpleActionState(
                                   arm + '_overhead_grasp_setup',
                                   OverheadGraspSetupAction,
                                   goal=OverheadGraspSetupGoal()),
                               transitions={'succeeded': 'ARM_TUCK'})

        smach.StateMachine.add('ARM_TUCK',
                               ServiceState(
                                   'traj_playback/' + arm + '_arm_untuck',
                                   TrajPlaybackSrv,
                                   request=TrajPlaybackSrvRequest(True)),
                               transitions={'succeeded': 'succeeded'})

    return sm