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 is_current_state_in_collision(self, arm_name, check_joint_limits = False):
        '''
        Tells you whether the current arm pose is in collision.

        **Returns:**
            succeeded (boolean): True if arm is in 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 = check_joint_limits
        req.group_name = arm_name
        res = self._planners[arm_name].get_state_validity_service(req)
        if res.error_code.val == ArmNavErrorCodes.SUCCESS:
            rospy.logdebug('Current state not in collision')
            return False
        else:
            rospy.logdebug('Current state in collision')
            return True
    def is_in_collision(self, arm_name, joint_state, check_joint_limits=False):
        '''
        Tells you whether the current arm pose is in collision.

        **Returns:**
            succeeded (boolean): True if arm is in collision.
        '''
        req = GetStateValidityRequest()
        req.robot_state = self._world_interface.get_robot_state()
        trajectory_tools.set_joint_state_in_robot_state(
            joint_state, req.robot_state)
        req.check_collisions = True
        req.check_path_constraints = False
        req.check_joint_limits = check_joint_limits
        req.group_name = self._arm_name
        res = self._planners[arm_name].get_state_validity_service(req)
        if res.error_code.val == ArmNavErrorCodes.SUCCESS:
            rospy.logdebug('Current state not in collision')
            return False
        else:
            rospy.logdebug('Current state in collision')
            return 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)