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)
