class ArmMoverWorker(threading.Thread):
    def __init__(self, arm_name):
        '''Worker class that can move or query the arm. By making several of these,
        the arm mover class can do multiple movements/queries at once.
        '''
        self._arm_name = arm_name
        threading.Thread.__init__(self)
        self.daemon = True

        self._current_handle = None
        self._task_cv = threading.Condition()


    def assign_task(self, handle):
        ''' Assign a task to this worker.

        **Args:**
            handle (MovementHandle): Handle to the movement being assigned.

        **Raises:**
            ArmNavError if already doing another task.
        '''
        self._task_cv.acquire()
        try:
            if self._current_handle is not None:
                raise ArmNavError('Movement requested while other movement still runnning!')
            self._current_handle = handle
            self._task_cv.notify()
        finally:
            self._task_cv.release()
    
    def run(self):
        '''Loop and wait to be assigned a task.
        '''
        self._initialize()

        while not rospy.is_shutdown():
            self._task_cv.acquire()
            while (self._current_handle is None) and (not rospy.is_shutdown()):
                self._task_cv.wait(0.01)
            self._task_cv.release()

            if rospy.is_shutdown():
                break

            try:
                rospy.logdebug('ArmMoverWorker starting task: %s(%s)' % (
                        str(self._current_handle.task_func), str(self._current_handle.task_args)))
                self._current_handle.task_func(self, *self._current_handle.task_args)
            except Exception as e:
                self._current_handle._add_error(e)
            
            self._task_cv.acquire()
            self._current_handle._set_in_progress(False)
            self._current_handle = None
            self._task_cv.release()

    def _initialize(self):
        '''Connect up all services and action clients.
        '''
        self._world_interface = WorldInterface()
        self._controller_manager = ControllerManagerClient()
        if self._arm_name in ['right_arm', 'left_arm']:
            self._group_name = self._arm_name
            self._planner = ArmPlanner(self._arm_name)
            self._hand_description = HandDescription(self._arm_name)

            arm_abbr = self._arm_name[0]
            self._joint_controller = '%s_arm_controller' % arm_abbr                
            self._cartesian_controller = '%s_cart' % arm_abbr
            
            self._move_arm_client = actionlib.SimpleActionClient(
                'move_%s' % self._arm_name, arm_navigation_msgs.msg.MoveArmAction)
            self._wait_for_action_server(self._move_arm_client)
            
            #jt_action_name = '/%s_arm_controller/joint_trajectory_action' % arm_abbr
            #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_action_name, JointTrajectoryAction)
            #self._wait_for_action_server(self._joint_trajectory_client)
            
            jt_action_name = '/%s_arm_controller/follow_joint_trajectory' % arm_abbr
            self._joint_trajectory_client = actionlib.SimpleActionClient(jt_action_name, FollowJointTrajectoryAction)
            self._wait_for_action_server(self._joint_trajectory_client)
            
            self._cart_interface = CartesianControllerInterface(self._arm_name)
        elif self._arm_name == 'both':
            self._joint_controller = 'two_arm_controller'
            #jt_two_arm_action_name = '/two_arm_controller/joint_trajectory_action'
            #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_two_arm_action_name, JointTrajectoryAction)
            jt_two_arm_action_name = '/two_arm_controller/follow_joint_trajectory'
            self._joint_trajectory_client = actionlib.SimpleActionClient(jt_two_arm_action_name, FollowJointTrajectoryAction)
            self._wait_for_action_server(self._joint_trajectory_client)
        else:
            raise ValueError('Invalid arm name for worker: %s' % self._arm_name)

    def _wait_for_action_server(self, action_client, max_wait=10., wait_increment=0.1):
        '''
        Wait for the action server corresponding to this action client to be ready.
        
        **Args:**
            **action_client (actionlib.SimpleActionClient):** client for an action

            *max_wait (float):* Total number of seconds to wait before failing

            *wait_increment (float):* Number or seconds to wait between checks to rospy.is_shutdown()
        
        **Raises:**

            **exceptions.ActionFailedError:** if max_wait seconds elapses without server being ready.
        '''
        for ii in range(int(round(max_wait / wait_increment))):
            if rospy.is_shutdown():
                raise ActionFailedError('Could not connect to action server (rospy shutdown requested)')
            
            if action_client.wait_for_server(rospy.Duration(wait_increment)):
                return
        raise ActionFailedError('Could not connect to action server (timeout exceeeded)')

    def _move_to_goal(self, goal,
                try_hard=False, collision_aware_goal=True, planner_timeout=5., ordered_collisions=None,
                bounds=None, planner_id='', cartesian_timeout=5.0):
        '''
        Move the specified arm to the given goal.
        
        This function should only get called indirectly by calling move_arm_to_goal.
        '''
        try:
            reached_goal = False
            
            # check which kind of goal we were given
            if type(goal) == PoseStamped:
                goal_is_pose = True
            elif type(goal) == JointState:
                goal_is_pose = False
            else:
                raise ArmNavError('Invalid goal type %s' % str(type(goal)))
    
            rospy.loginfo('Attempting to use move arm to get to goal')
            try:
                self._move_to_goal_using_move_arm(
                    goal, planner_timeout, ordered_collisions, bounds, planner_id)
                reached_goal = True
            except ArmNavError as e:
                self._current_handle._add_error(e)
                rospy.loginfo('Move arm failed: %s' % str(e))
    
            if (not reached_goal) and try_hard:
                rospy.loginfo('Attempting to move directly to goal')
                try:
                    self._move_to_goal_directly(goal, planner_timeout, bounds, collision_aware=True)
                    reached_goal = True
                except ArmNavError as e:
                    self._current_handle._add_error(e)
                    rospy.loginfo('Collision aware IK failed: %s' % str(e))
    
            if (not reached_goal) and try_hard and (not collision_aware_goal) and goal_is_pose:
                rospy.loginfo('Attempting to move directly to goal, ignoring collisions')
                try:
                    self._move_to_goal_directly(goal, planner_timeout, bounds, collision_aware=False)
                    reached_goal = True
                except ArmNavError as e:
                    self._current_handle._add_error(e)
                    rospy.loginfo('Non-collision aware IK failed: %s' % str(e))
    
            if (not reached_goal) and try_hard and (not collision_aware_goal) and goal_is_pose:
                rospy.loginfo('Attempting to use cartesian controller to move towards goal')
                try:
                    self._move_to_goal_using_cartesian_control(goal, cartesian_timeout, bounds)
                    reached_goal = True
                except ArmNavError as e:
                    self._current_handle._add_error(e)
        finally:
            self._current_handle._set_reached_goal(reached_goal)


    def _move_into_joint_limits(self):
        '''
        Moves the arm into joint limits if it is outside of them.

        This cannot be a collision free move but it is almost always very very short.

        **Raises:**

            **exceptions.ArmNavError:** if IK fails

            **rospy.ServiceException:** if there is a problem calling the IK service

            **ValueError:** if the goal type is wrong
        '''
        joint_state = self._planner.get_closest_joint_state_in_limits()
        self._move_to_goal_directly(joint_state, trajectory_time=0.5)
        self._current_handle._set_reached_goal(True)

    def _move_out_of_collision(self, move_mag=0.3, num_tries=100):
        '''
        Tries to find a small movement that will take the arm out of collision.

        **Args:**

            *move_mag (float):* Max magnitude in radians of movement for each joint.
            
            *num_tries (int):* Number of random joint angles to try before giving up.
            
        **Returns:**
            succeeded (boolean): True if arm was sucessfully moved out of collision.
        '''
        req = GetStateValidityRequest()
        req.robot_state = self.world_interface.get_robot_state()
        req.check_collisions = True
        req.check_path_constraints = False
        req.check_joint_limits = False
        req.group_name = self._arm_name
        res = self._planner.get_state_validity_service(req)
        if res.error_code.val == ArmNavErrorCodes.SUCCESS:
            rospy.logdebug('Current state not in collision')
            return False

        joint_state = self._planner.arm_joint_state()
        current_joint_position = np.array(joint_state.position)
        for ii in range(num_tries):
            joint_position = current_joint_position + np.random.uniform(
                -move_mag, move_mag, (len(joint_state.position),))
            joint_state.position = list(joint_position)
            trajectory_tools.set_joint_state_in_robot_state(joint_state, req.robot_state)
            res = self._planner.get_state_validity_service(req)
            in_collision = (res.error_code.val != ArmNavErrorCodes.SUCCESS)
            rospy.logdebug('%s in collision: %s' % (str(joint_position), str(in_collision)))
            if not in_collision:
                self._move_to_goal_directly(joint_state, None, None, collision_aware=False)
                self._current_handle._set_reached_goal(True)
        self._current_handle._set_reached_goal(False)
    
    def _call_action(self, action_client, goal):
        '''
        Call an action and wait for it to complete.
        
        **Returns:** 
            Result of action.

        **Raises:** 
            
            **exceptions.ArmNavError:** if action fails.
        '''
        action_client.send_goal(goal)
        gs = actionlib_msgs.msg.GoalStatus()

        r = rospy.Rate(100)
        while True:
            if self._current_handle._get_cancel_requested():
                raise ActionFailedError('Preempted (cancel requested)')
            state = action_client.get_state()
            if state in [gs.PENDING, gs.ACTIVE, gs.PREEMPTING, gs.RECALLING]:
                # action is still going
                pass
            elif state in [gs.PREEMPTED, gs.REJECTED, gs.RECALLED, gs.LOST]:
                raise ArmNavError('Action call failed (%d)!' % (state,))
            elif state in [gs.SUCCEEDED, gs.ABORTED]:
                return action_client.get_result() 
            r.sleep()

    def _move_to_goal_directly(self, goal, planner_timeout=5.0, bounds=None,
            collision_aware=True, trajectory_time=5.0):
        '''
        Move directly to the goal.
        
        No planning, just interpolated joint positions.
        
        If goal is a PoseStamped, does IK to find joint positions to put the end effector in that pose.
        Then executes a trajectory where the only point is the goal joint positions.
        
        Note: planner_timeout, collision_aware and bounds only apply to the IK, and so are not used when
        the goal is already a JointState

        **Raises:**

            **exceptions.ArmNavError:** if IK fails

            **rospy.ServiceException:** if there is a problem calling the IK service

            **ValueError:** if the goal type is wrong
        '''
        if type(goal) == JointState:
            joint_state = goal
        elif type(goal) == PoseStamped:
            ik_res = self._planner.get_ik(goal, collision_aware=collision_aware, starting_state=None,
                seed_state=None, timeout=planner_timeout)
            if not ik_res.error_code.val == ArmNavErrorCodes.SUCCESS:
                raise ArmNavError('Unable to get IK for pose', ik_res.error_code)
            joint_state = ik_res.solution.joint_state
        else:
            raise ValueError('Invalid goal type: %s' % str(type(goal)))
        
        trajectory = JointTrajectory()
        trajectory.joint_names = self._planner.joint_names
        jtp = JointTrajectoryPoint()
        jtp.positions = joint_state.position
        jtp.time_from_start = rospy.Duration(trajectory_time)
        trajectory.points.append(jtp)
        self._execute_joint_trajectory(trajectory)

        # should actually check this...
        self._current_handle._set_reached_goal(True)
        
    def _move_to_goal_using_move_arm(self, goal, planner_timeout, ordered_collisions, bounds, planner_id=''):
        '''
        Try using the MoveArm action to get to the goal.
        '''
        self._controller_manager.switch_controllers(start_controllers=[self._joint_controller])
        current_state = self._world_interface.get_robot_state()
        link_name = self._hand_description.hand_frame
        
        if type(goal) == JointState:
            mp_request = conversions.joint_state_to_motion_plan_request(
                goal, link_name, self._group_name, current_state,
                timeout=planner_timeout, bounds=bounds, planner_id=planner_id)
        elif type(goal) == PoseStamped:
            mp_request = conversions.pose_stamped_to_motion_plan_request(
                goal, link_name, self._group_name, starting_state=current_state, 
                timeout=planner_timeout, bounds=bounds, planner_id=planner_id)
        else:
            raise ValueError('Invalid goal type %s' % str(type(goal)))
        
        ma_goal = arm_navigation_msgs.msg.MoveArmGoal()
        ma_goal.motion_plan_request = mp_request
        if ordered_collisions:
            ma_goal.operations = ordered_collisions
        ma_goal.planner_service_name = DEFAULT_PLANNER_SERVICE_NAME
        
        # send goal to move arm
        res = self._call_action(self._move_arm_client, ma_goal)
        if res == None:
            raise ArmNavError('MoveArm failed without setting result')
        elif not res.error_code.val == ArmNavErrorCodes.SUCCESS:
            raise ArmNavError('MoveArm failed', res.error_code)
        else:
            self._current_handle._set_reached_goal(True)

    def _move_to_goal_using_cartesian_control(self, goal, timeout, bounds):
        if type(goal) == PoseStamped:
            pose_stamped = goal
        else:
            raise ValueError('Invalid goal type for cartesian control: %s' % str(type(goal)))
        self._controller_manager.switch_controllers(start_controllers=[self._cartesian_controller])
        self._cart_interface.set_desired_pose(pose_stamped)
        start_time = time.time()
        r = rospy.Rate(100)
        try:
            print 'Current handle'
            print self._current_handle._get_cancel_requested()
            while not self._current_handle._get_cancel_requested():
                print 'Inside while loop'
                # ignores bounds right now and uses defaults... fixme
                if self._cart_interface.reached_desired_pose():
                    self._current_handle._set_reached_goal(True)
                    return
                if (time.time() - start_time) > timeout:
                    raise ArmNavError('Cartesian control move time out', 
                                      ArmNavErrorCodes(ArmNavErrorCodes.TIMED_OUT))
                r.sleep()
        finally:
            self._cart_interface.cancel_desired_pose()
                                   
    def _execute_joint_trajectory(self, trajectory):
        '''
        Executes the given trajectory, switching controllers first if needed.

        **Args:**

            **trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to execute.
        '''
        self._controller_manager.switch_controllers(start_controllers=[self._joint_controller])
        #goal = JointTrajectoryGoal()
        goal = FollowJointTrajectoryGoal()
        goal.trajectory = trajectory
        jt_res = self._call_action(self._joint_trajectory_client, goal)

        # should actually check this
        self._current_handle._set_reached_goal(True)

        return jt_res
    
    def _execute_two_arm_trajectory(self, trajectory):
        '''
        Executes the given trajectory, switching controllers first if needed.

        **Args:**

        **trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to execute.
        '''
        self._controller_manager.switch_controllers(start_controllers=[self._joint_controller])
        #goal = JointTrajectoryGoal()
        goal = FollowJointTrajectoryGoal()
        goal.trajectory = trajectory
        jt_res = self._call_action(self._joint_trajectory_client, goal)

        # should actually check this
        self._current_handle._set_reached_goal(True)
        
        return jt_res
Exemple #2
0
def plate_main():
    pub = rospy.Publisher('darrt_trajectory', MarkerArray)
    rospy.loginfo('Waiting for action')
    rospy.loginfo('Doing detection')
    wi = WorldInterface()
    psi = get_planning_scene_interface()
    detector = TablewareDetection()
    plate = wi.collision_object('plate')
    good_detection = False
    goal = DARRTGoal()
    if plate:
        rospy.loginfo('Use last detection?')
        good_detection = (raw_input() == 'y')
        ops = PoseStamped()
        ops.header = copy.deepcopy(plate.header)
        ops.pose = copy.deepcopy(plate.poses[0])
        goal.pickup_goal = PickupGoal('right_arm', om.GraspableObject(reference_frame_id=ops.header.frame_id), 
                                      object_pose_stamped=ops, collision_object_name='plate',
                                      collision_support_surface_name='serving_table')
    while not good_detection:
        det = detector.detect_objects(add_table_to_map=False, 
                                      add_objects_as_mesh=False, table_name='serving_table')
        goal.pickup_goal = get_plate(det.pickup_goals, det.table.pose, wi)
        psi.reset()
        if not goal.pickup_goal:
            rospy.loginfo('Nothing to pick up!')
        rospy.loginfo('Good detection?')
        good_detection = (raw_input() == 'y')
    if not goal.pickup_goal:
        rospy.loginfo('Nothing to pick up!')
        return
    add_map_tables(wi)
    psi.reset()
    client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
    client.wait_for_server()

    goal.pickup_goal.arm_name = 'right_arm'
    goal.pickup_goal.desired_grasps = plate_grasps(goal.pickup_goal.object_pose_stamped, 
                                                   goal.pickup_goal.target.reference_frame_id)
    goal.pickup_goal.lift.desired_distance = 0.3
    place_pose_stamped = PoseStamped()
    place_pose_stamped.header.frame_id = wi.world_frame 
    # place_pose_stamped.pose.position.x = 1.3
    # place_pose_stamped.pose.position.y = -0.3
    # place_pose_stamped.pose.position.z = 1.01
    place_pose_stamped.pose.position.x = 0.0
    place_pose_stamped.pose.position.y = 0.0
    place_pose_stamped.pose.position.z = 0.76
    place_pose_stamped.pose.orientation.w = 1.0
    # place_pose_stamped.pose.orientation.x = 0.1
    # place_pose_stamped.pose.orientation.y = 0.1
    # place_pose_stamped.pose.orientation.w = np.sqrt(0.98)
    #place_pose_stamped = copy.deepcopy(goal.pickup_goal.object_pose_stamped)
    #place_pose_stamped.pose.position.x -= 0.2
    #place_pose_stamped.pose.position.y -= 0.2
    #place_pose_stamped.pose.position.z += 0.2
    goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped],
                                collision_support_surface_name = 'dirty_table',
                                collision_object_name = goal.pickup_goal.collision_object_name)
    goal.place_goal.approach.direction.header.frame_id = wi.world_frame
    goal.place_goal.approach.direction.vector.x = np.sqrt(0.18)
    goal.place_goal.approach.direction.vector.y = -np.sqrt(0.18)
    goal.place_goal.approach.direction.vector.z = -0.8
    goal.place_goal.approach.desired_distance = 0.2
    goal.primitives = [goal.PICK, goal.PLACE, goal.PUSH, goal.BASE_TRANSIT]
    goal.min_grasp_distance_from_surface = 0.19
    goal.object_sampling_fraction = 0.7
    goal.retreat_distance = 0.3
    goal.debug_level = 2
    goal.do_pause = False
    goal.execute = False
    goal.planning_time = 6000
    goal.tries = 1
    goal.goal_bias = 0.2
    rospy.loginfo('Sending goal')
    client.send_goal_and_wait(goal)
    rospy.loginfo('Returned')
    result = client.get_result()

    marray = MarkerArray()
    if result.error_code == result.SUCCESS:
        #pickle everything!! great excitement
        filename = 'trajectory_and_objects.pck'
        rospy.loginfo('Pickling to '+filename)
        #the last bit allows us to recreate the planning
        pickle.dump([client.get_result(), goal, wi.collision_objects(), wi.get_robot_state()], open(filename, 'wb'))
        rospy.loginfo('Successful write')
        for t in result.primitive_trajectories:
            marray.markers += vt.trajectory_markers(t, ns='trajectory', resolution=3).markers
        for (i, m) in enumerate(marray.markers):
            m.id = i
            (m.color.r, m.color.g, m.color.b) = vt.hsv_to_rgb(i/float(len(marray.markers))*300.0, 1, 1)
        while not rospy.is_shutdown():
            pub.publish(marray)
            rospy.sleep(0.1)
class ArmMoverWorker(threading.Thread):
    def __init__(self, arm_name):
        '''Worker class that can move or query the arm. By making several of these,
        the arm mover class can do multiple movements/queries at once.
        '''
        self._arm_name = arm_name
        threading.Thread.__init__(self)
        self.daemon = True

        self._current_handle = None
        self._task_cv = threading.Condition()

    def assign_task(self, handle):
        ''' Assign a task to this worker.

        **Args:**
            handle (MovementHandle): Handle to the movement being assigned.

        **Raises:**
            ArmNavError if already doing another task.
        '''
        self._task_cv.acquire()
        try:
            if self._current_handle is not None:
                raise ArmNavError(
                    'Movement requested while other movement still runnning!')
            self._current_handle = handle
            self._task_cv.notify()
        finally:
            self._task_cv.release()

    def run(self):
        '''Loop and wait to be assigned a task.
        '''
        self._initialize()

        while not rospy.is_shutdown():
            self._task_cv.acquire()
            while (self._current_handle is None) and (not rospy.is_shutdown()):
                self._task_cv.wait(0.01)
            self._task_cv.release()

            if rospy.is_shutdown():
                break

            try:
                rospy.logdebug('ArmMoverWorker starting task: %s(%s)' %
                               (str(self._current_handle.task_func),
                                str(self._current_handle.task_args)))
                self._current_handle.task_func(self,
                                               *self._current_handle.task_args)
            except Exception as e:
                self._current_handle._add_error(e)

            self._task_cv.acquire()
            self._current_handle._set_in_progress(False)
            self._current_handle = None
            self._task_cv.release()

    def _initialize(self):
        '''Connect up all services and action clients.
        '''
        self._world_interface = WorldInterface()
        self._controller_manager = ControllerManagerClient()
        if self._arm_name in ['right_arm', 'left_arm']:
            self._group_name = self._arm_name
            self._planner = ArmPlanner(self._arm_name)
            self._hand_description = HandDescription(self._arm_name)

            arm_abbr = self._arm_name[0]
            self._joint_controller = '%s_arm_controller' % arm_abbr
            self._cartesian_controller = '%s_cart' % arm_abbr

            self._move_arm_client = actionlib.SimpleActionClient(
                'move_%s' % self._arm_name,
                arm_navigation_msgs.msg.MoveArmAction)
            self._wait_for_action_server(self._move_arm_client)

            #jt_action_name = '/%s_arm_controller/joint_trajectory_action' % arm_abbr
            #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_action_name, JointTrajectoryAction)
            #self._wait_for_action_server(self._joint_trajectory_client)

            jt_action_name = '/%s_arm_controller/follow_joint_trajectory' % arm_abbr
            self._joint_trajectory_client = actionlib.SimpleActionClient(
                jt_action_name, FollowJointTrajectoryAction)
            self._wait_for_action_server(self._joint_trajectory_client)

            self._cart_interface = CartesianControllerInterface(self._arm_name)
        elif self._arm_name == 'both':
            self._joint_controller = 'two_arm_controller'
            #jt_two_arm_action_name = '/two_arm_controller/joint_trajectory_action'
            #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_two_arm_action_name, JointTrajectoryAction)
            jt_two_arm_action_name = '/two_arm_controller/follow_joint_trajectory'
            self._joint_trajectory_client = actionlib.SimpleActionClient(
                jt_two_arm_action_name, FollowJointTrajectoryAction)
            self._wait_for_action_server(self._joint_trajectory_client)
        else:
            raise ValueError('Invalid arm name for worker: %s' %
                             self._arm_name)

    def _wait_for_action_server(self,
                                action_client,
                                max_wait=10.,
                                wait_increment=0.1):
        '''
        Wait for the action server corresponding to this action client to be ready.
        
        **Args:**
            **action_client (actionlib.SimpleActionClient):** client for an action

            *max_wait (float):* Total number of seconds to wait before failing

            *wait_increment (float):* Number or seconds to wait between checks to rospy.is_shutdown()
        
        **Raises:**

            **exceptions.ActionFailedError:** if max_wait seconds elapses without server being ready.
        '''
        for ii in range(int(round(max_wait / wait_increment))):
            if rospy.is_shutdown():
                raise ActionFailedError(
                    'Could not connect to action server (rospy shutdown requested)'
                )

            if action_client.wait_for_server(rospy.Duration(wait_increment)):
                return
        raise ActionFailedError(
            'Could not connect to action server (timeout exceeeded)')

    def _move_to_goal(self,
                      goal,
                      try_hard=False,
                      collision_aware_goal=True,
                      planner_timeout=5.,
                      ordered_collisions=None,
                      bounds=None,
                      planner_id='',
                      cartesian_timeout=5.0):
        '''
        Move the specified arm to the given goal.
        
        This function should only get called indirectly by calling move_arm_to_goal.
        '''
        try:
            reached_goal = False

            # check which kind of goal we were given
            if type(goal) == PoseStamped:
                goal_is_pose = True
            elif type(goal) == JointState:
                goal_is_pose = False
            else:
                raise ArmNavError('Invalid goal type %s' % str(type(goal)))

            rospy.loginfo('Attempting to use move arm to get to goal')
            try:
                self._move_to_goal_using_move_arm(goal, planner_timeout,
                                                  ordered_collisions, bounds,
                                                  planner_id)
                reached_goal = True
            except ArmNavError as e:
                self._current_handle._add_error(e)
                rospy.loginfo('Move arm failed: %s' % str(e))

            if (not reached_goal) and try_hard:
                rospy.loginfo('Attempting to move directly to goal')
                try:
                    self._move_to_goal_directly(goal,
                                                planner_timeout,
                                                bounds,
                                                collision_aware=True)
                    reached_goal = True
                except ArmNavError as e:
                    self._current_handle._add_error(e)
                    rospy.loginfo('Collision aware IK failed: %s' % str(e))

            if (not reached_goal) and try_hard and (
                    not collision_aware_goal) and goal_is_pose:
                rospy.loginfo(
                    'Attempting to move directly to goal, ignoring collisions')
                try:
                    self._move_to_goal_directly(goal,
                                                planner_timeout,
                                                bounds,
                                                collision_aware=False)
                    reached_goal = True
                except ArmNavError as e:
                    self._current_handle._add_error(e)
                    rospy.loginfo('Non-collision aware IK failed: %s' % str(e))

            if (not reached_goal) and try_hard and (
                    not collision_aware_goal) and goal_is_pose:
                rospy.loginfo(
                    'Attempting to use cartesian controller to move towards goal'
                )
                try:
                    self._move_to_goal_using_cartesian_control(
                        goal, cartesian_timeout, bounds)
                    reached_goal = True
                except ArmNavError as e:
                    self._current_handle._add_error(e)
        finally:
            self._current_handle._set_reached_goal(reached_goal)

    def _move_into_joint_limits(self):
        '''
        Moves the arm into joint limits if it is outside of them.

        This cannot be a collision free move but it is almost always very very short.

        **Raises:**

            **exceptions.ArmNavError:** if IK fails

            **rospy.ServiceException:** if there is a problem calling the IK service

            **ValueError:** if the goal type is wrong
        '''
        joint_state = self._planner.get_closest_joint_state_in_limits()
        self._move_to_goal_directly(joint_state, trajectory_time=0.5)
        self._current_handle._set_reached_goal(True)

    def _move_out_of_collision(self, move_mag=0.3, num_tries=100):
        '''
        Tries to find a small movement that will take the arm out of collision.

        **Args:**

            *move_mag (float):* Max magnitude in radians of movement for each joint.
            
            *num_tries (int):* Number of random joint angles to try before giving up.
            
        **Returns:**
            succeeded (boolean): True if arm was sucessfully moved out of collision.
        '''
        req = GetStateValidityRequest()
        req.robot_state = self.world_interface.get_robot_state()
        req.check_collisions = True
        req.check_path_constraints = False
        req.check_joint_limits = False
        req.group_name = self._arm_name
        res = self._planner.get_state_validity_service(req)
        if res.error_code.val == ArmNavErrorCodes.SUCCESS:
            rospy.logdebug('Current state not in collision')
            return False

        joint_state = self._planner.arm_joint_state()
        current_joint_position = np.array(joint_state.position)
        for ii in range(num_tries):
            joint_position = current_joint_position + np.random.uniform(
                -move_mag, move_mag, (len(joint_state.position), ))
            joint_state.position = list(joint_position)
            trajectory_tools.set_joint_state_in_robot_state(
                joint_state, req.robot_state)
            res = self._planner.get_state_validity_service(req)
            in_collision = (res.error_code.val != ArmNavErrorCodes.SUCCESS)
            rospy.logdebug('%s in collision: %s' %
                           (str(joint_position), str(in_collision)))
            if not in_collision:
                self._move_to_goal_directly(joint_state,
                                            None,
                                            None,
                                            collision_aware=False)
                self._current_handle._set_reached_goal(True)
        self._current_handle._set_reached_goal(False)

    def _call_action(self, action_client, goal):
        '''
        Call an action and wait for it to complete.
        
        **Returns:** 
            Result of action.

        **Raises:** 
            
            **exceptions.ArmNavError:** if action fails.
        '''
        action_client.send_goal(goal)
        gs = actionlib_msgs.msg.GoalStatus()

        r = rospy.Rate(100)
        while True:
            if self._current_handle._get_cancel_requested():
                raise ActionFailedError('Preempted (cancel requested)')
            state = action_client.get_state()
            if state in [gs.PENDING, gs.ACTIVE, gs.PREEMPTING, gs.RECALLING]:
                # action is still going
                pass
            elif state in [gs.PREEMPTED, gs.REJECTED, gs.RECALLED, gs.LOST]:
                raise ArmNavError('Action call failed (%d)!' % (state, ))
            elif state in [gs.SUCCEEDED, gs.ABORTED]:
                return action_client.get_result()
            r.sleep()

    def _move_to_goal_directly(self,
                               goal,
                               planner_timeout=5.0,
                               bounds=None,
                               collision_aware=True,
                               trajectory_time=5.0):
        '''
        Move directly to the goal.
        
        No planning, just interpolated joint positions.
        
        If goal is a PoseStamped, does IK to find joint positions to put the end effector in that pose.
        Then executes a trajectory where the only point is the goal joint positions.
        
        Note: planner_timeout, collision_aware and bounds only apply to the IK, and so are not used when
        the goal is already a JointState

        **Raises:**

            **exceptions.ArmNavError:** if IK fails

            **rospy.ServiceException:** if there is a problem calling the IK service

            **ValueError:** if the goal type is wrong
        '''
        if type(goal) == JointState:
            joint_state = goal
        elif type(goal) == PoseStamped:
            ik_res = self._planner.get_ik(goal,
                                          collision_aware=collision_aware,
                                          starting_state=None,
                                          seed_state=None,
                                          timeout=planner_timeout)
            if not ik_res.error_code.val == ArmNavErrorCodes.SUCCESS:
                raise ArmNavError('Unable to get IK for pose',
                                  ik_res.error_code)
            joint_state = ik_res.solution.joint_state
        else:
            raise ValueError('Invalid goal type: %s' % str(type(goal)))

        trajectory = JointTrajectory()
        trajectory.joint_names = self._planner.joint_names
        jtp = JointTrajectoryPoint()
        jtp.positions = joint_state.position
        jtp.time_from_start = rospy.Duration(trajectory_time)
        trajectory.points.append(jtp)
        self._execute_joint_trajectory(trajectory)

        # should actually check this...
        self._current_handle._set_reached_goal(True)

    def _move_to_goal_using_move_arm(self,
                                     goal,
                                     planner_timeout,
                                     ordered_collisions,
                                     bounds,
                                     planner_id=''):
        '''
        Try using the MoveArm action to get to the goal.
        '''
        self._controller_manager.switch_controllers(
            start_controllers=[self._joint_controller])
        current_state = self._world_interface.get_robot_state()
        link_name = self._hand_description.hand_frame

        if type(goal) == JointState:
            mp_request = conversions.joint_state_to_motion_plan_request(
                goal,
                link_name,
                self._group_name,
                current_state,
                timeout=planner_timeout,
                bounds=bounds,
                planner_id=planner_id)
        elif type(goal) == PoseStamped:
            mp_request = conversions.pose_stamped_to_motion_plan_request(
                goal,
                link_name,
                self._group_name,
                starting_state=current_state,
                timeout=planner_timeout,
                bounds=bounds,
                planner_id=planner_id)
        else:
            raise ValueError('Invalid goal type %s' % str(type(goal)))

        ma_goal = arm_navigation_msgs.msg.MoveArmGoal()
        ma_goal.motion_plan_request = mp_request
        if ordered_collisions:
            ma_goal.operations = ordered_collisions
        ma_goal.planner_service_name = DEFAULT_PLANNER_SERVICE_NAME

        # send goal to move arm
        res = self._call_action(self._move_arm_client, ma_goal)
        if res == None:
            raise ArmNavError('MoveArm failed without setting result')
        elif not res.error_code.val == ArmNavErrorCodes.SUCCESS:
            raise ArmNavError('MoveArm failed', res.error_code)
        else:
            self._current_handle._set_reached_goal(True)

    def _move_to_goal_using_cartesian_control(self, goal, timeout, bounds):
        if type(goal) == PoseStamped:
            pose_stamped = goal
        else:
            raise ValueError('Invalid goal type for cartesian control: %s' %
                             str(type(goal)))
        self._controller_manager.switch_controllers(
            start_controllers=[self._cartesian_controller])
        self._cart_interface.set_desired_pose(pose_stamped)
        start_time = time.time()
        r = rospy.Rate(100)
        try:
            print 'Current handle'
            print self._current_handle._get_cancel_requested()
            while not self._current_handle._get_cancel_requested():
                print 'Inside while loop'
                # ignores bounds right now and uses defaults... fixme
                if self._cart_interface.reached_desired_pose():
                    self._current_handle._set_reached_goal(True)
                    return
                if (time.time() - start_time) > timeout:
                    raise ArmNavError(
                        'Cartesian control move time out',
                        ArmNavErrorCodes(ArmNavErrorCodes.TIMED_OUT))
                r.sleep()
        finally:
            self._cart_interface.cancel_desired_pose()

    def _execute_joint_trajectory(self, trajectory):
        '''
        Executes the given trajectory, switching controllers first if needed.

        **Args:**

            **trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to execute.
        '''
        self._controller_manager.switch_controllers(
            start_controllers=[self._joint_controller])
        #goal = JointTrajectoryGoal()
        goal = FollowJointTrajectoryGoal()
        goal.trajectory = trajectory
        jt_res = self._call_action(self._joint_trajectory_client, goal)

        # should actually check this
        self._current_handle._set_reached_goal(True)

        return jt_res

    def _execute_two_arm_trajectory(self, trajectory):
        '''
        Executes the given trajectory, switching controllers first if needed.

        **Args:**

        **trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to execute.
        '''
        self._controller_manager.switch_controllers(
            start_controllers=[self._joint_controller])
        #goal = JointTrajectoryGoal()
        goal = FollowJointTrajectoryGoal()
        goal.trajectory = trajectory
        jt_res = self._call_action(self._joint_trajectory_client, goal)

        # should actually check this
        self._current_handle._set_reached_goal(True)

        return jt_res