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)