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 check_state_validity(self, joint_angles, ordered_collision_operations = None, \
robot_state = None, link_padding = None):
req = GetStateValidityRequest()
if robot_state != None:
req.robot_state = robot_state
req.robot_state.joint_state.name.extend(self.joint_names)
req.robot_state.joint_state.position.extend(joint_angles)
req.robot_state.joint_state.header.stamp = rospy.Time.now()
req.check_collisions = True
if ordered_collision_operations != None:
req.ordered_collision_operations = ordered_collision_operations
if link_padding != None:
req.link_padding = link_padding
try:
res = self._check_state_validity_service(req)
except rospy.ServiceException, e:
rospy.logwarn("Check state validity call failed! error msg: %s"%e)
return 0
def test_alter_padding(self):
req = GetRobotStateRequest()
cur_state = self.get_robot_state_server.call(req)
state_req = GetStateValidityRequest()
state_req.robot_state = cur_state.robot_state
state_req.check_collisions = True
res = self.get_state_validity_server.call(state_req)
#no big padding initially
self.failIf(res.error_code.val != res.error_code.SUCCESS)
padd = LinkPadding()
padd.link_name = "r_end_effector"
padd.padding = .03
state_req.link_padding = []
state_req.link_padding.append(padd)
res = self.get_state_validity_server.call(state_req)
#should be in collision
self.failIf(res.error_code.val == res.error_code.SUCCESS)
self.assertEqual(res.error_code.val,
res.error_code.COLLISION_CONSTRAINTS_VIOLATED)
#should be some contacts
self.failIf(len(res.contacts) == 0)
#check that revert works
state_req.link_padding = []
res = self.get_state_validity_server.call(state_req)
self.failIf(res.error_code.val != res.error_code.SUCCESS)
def check_state_validity(self, joint_angles, ordered_collision_operations = None, \
robot_state = None, link_padding = None):
req = GetStateValidityRequest()
if robot_state != None:
req.robot_state = robot_state
req.robot_state.joint_state.name.extend(self.joint_names)
req.robot_state.joint_state.position.extend(joint_angles)
req.robot_state.joint_state.header.stamp = rospy.Time.now()
req.check_collisions = True
if ordered_collision_operations != None:
req.ordered_collision_operations = ordered_collision_operations
if link_padding != None:
req.link_padding = link_padding
try:
res = self._check_state_validity_service(req)
except rospy.ServiceException, e:
rospy.logwarn("Check state validity call failed! error msg: %s" %
e)
return 0
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 test_alter_padding(self):
req = GetRobotStateRequest()
cur_state = self.get_robot_state_server.call(req)
state_req = GetStateValidityRequest()
state_req.robot_state = cur_state.robot_state
state_req.check_collisions = True
res = self.get_state_validity_server.call(state_req)
#no big padding initially
self.failIf(res.error_code.val != res.error_code.SUCCESS)
padd = LinkPadding()
padd.link_name = "r_end_effector"
padd.padding = .03
state_req.link_padding = []
state_req.link_padding.append(padd)
res = self.get_state_validity_server.call(state_req)
#should be in collision
self.failIf(res.error_code.val == res.error_code.SUCCESS)
self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED)
#should be some contacts
self.failIf(len(res.contacts) == 0)
#check that revert works
state_req.link_padding = []
res = self.get_state_validity_server.call(state_req)
self.failIf(res.error_code.val != res.error_code.SUCCESS)
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 test_get_state_validity(self):
req = GetRobotStateRequest()
cur_state = self.get_robot_state_server.call(req)
#for i in range(len(cur_state.robot_state.joint_state.name)):
# print cur_state.robot_state.joint_state.name[i], cur_state.robot_state.joint_state.position[i]
state_req = GetStateValidityRequest()
state_req.robot_state = cur_state.robot_state
group_req = GetGroupInfoRequest()
group_req.group_name = 'right_arm'
group_res = self.get_group_info_server.call(group_req)
self.failIf(len(group_res.link_names) == 0)
right_arm_links = group_res.link_names
group_req.group_name = ''
group_res = self.get_group_info_server.call(group_req)
self.failIf(len(group_res.link_names) == 0)
state_req.ordered_collision_operations.collision_operations = arm_navigation_msgs_utils.make_disable_allowed_collisions_with_exclusions(group_res.link_names,
right_arm_links)
for i in state_req.ordered_collision_operations.collision_operations:
print 'Disabling ', i.object1
state_req.check_collisions = True
res = self.get_state_validity_server.call(state_req)
#should be in collision
self.failIf(res.error_code.val == res.error_code.SUCCESS)
self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED)
#should be some contacts
self.failIf(len(res.contacts) == 0)
for c in res.contacts:
#getting everything in common frame of base_link
contact_point = PointStamped()
contact_point.header = c.header
contact_point.point = c.position
contact_point_base = self.tf.transformPoint("base_link",contact_point)
gripper_point = PointStamped()
gripper_point.header.stamp = c.header.stamp
gripper_point.header.frame_id = "r_gripper_palm_link"
gripper_point.point.x = 0
gripper_point.point.y = 0
gripper_point.point.z = 0
gripper_point_base = self.tf.transformPoint("base_link", gripper_point)
print 'x diff:', abs(gripper_point_base.point.x-contact_point_base.point.x)
print 'y diff:', abs(gripper_point_base.point.y-contact_point_base.point.y)
print 'z diff:', abs(gripper_point_base.point.z-contact_point_base.point.z)
self.failIf(abs(gripper_point_base.point.x-contact_point_base.point.x) > .031)
self.failIf(abs(gripper_point_base.point.y-contact_point_base.point.y) > .031)
self.failIf(abs(gripper_point_base.point.z-contact_point_base.point.z) > .031)
#now we delete obj1
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "test_object"
obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE
self.obj_pub.publish(obj2)
rospy.sleep(5.)
cur_state = self.get_robot_state_server.call(req)
state_req.robot_state = cur_state.robot_state
res = self.get_state_validity_server.call(state_req)
# base shouldn't collide due to child only links
self.failIf(res.error_code.val != res.error_code.SUCCESS)
# now it should collide
state_req.ordered_collision_operations.collision_operations = []
res = self.get_state_validity_server.call(state_req)
# base shouldn't collide due to child only links
self.failIf(res.error_code.val == res.error_code.SUCCESS)
def test_get_state_validity(self):
req = GetRobotStateRequest()
cur_state = self.get_robot_state_server.call(req)
#for i in range(len(cur_state.robot_state.joint_state.name)):
# print cur_state.robot_state.joint_state.name[i], cur_state.robot_state.joint_state.position[i]
state_req = GetStateValidityRequest()
state_req.robot_state = cur_state.robot_state
group_req = GetGroupInfoRequest()
group_req.group_name = 'right_arm'
group_res = self.get_group_info_server.call(group_req)
self.failIf(len(group_res.link_names) == 0)
right_arm_links = group_res.link_names
group_req.group_name = ''
group_res = self.get_group_info_server.call(group_req)
self.failIf(len(group_res.link_names) == 0)
state_req.ordered_collision_operations.collision_operations = arm_navigation_msgs_utils.make_disable_allowed_collisions_with_exclusions(
group_res.link_names, right_arm_links)
for i in state_req.ordered_collision_operations.collision_operations:
print 'Disabling ', i.object1
state_req.check_collisions = True
res = self.get_state_validity_server.call(state_req)
#should be in collision
self.failIf(res.error_code.val == res.error_code.SUCCESS)
self.assertEqual(res.error_code.val,
res.error_code.COLLISION_CONSTRAINTS_VIOLATED)
#should be some contacts
self.failIf(len(res.contacts) == 0)
for c in res.contacts:
#getting everything in common frame of base_link
contact_point = PointStamped()
contact_point.header = c.header
contact_point.point = c.position
contact_point_base = self.tf.transformPoint(
"base_link", contact_point)
gripper_point = PointStamped()
gripper_point.header.stamp = c.header.stamp
gripper_point.header.frame_id = "r_gripper_palm_link"
gripper_point.point.x = 0
gripper_point.point.y = 0
gripper_point.point.z = 0
gripper_point_base = self.tf.transformPoint(
"base_link", gripper_point)
print 'x diff:', abs(gripper_point_base.point.x -
contact_point_base.point.x)
print 'y diff:', abs(gripper_point_base.point.y -
contact_point_base.point.y)
print 'z diff:', abs(gripper_point_base.point.z -
contact_point_base.point.z)
self.failIf(
abs(gripper_point_base.point.x -
contact_point_base.point.x) > .031)
self.failIf(
abs(gripper_point_base.point.y -
contact_point_base.point.y) > .031)
self.failIf(
abs(gripper_point_base.point.z -
contact_point_base.point.z) > .031)
#now we delete obj1
obj2 = CollisionObject()
obj2.header.stamp = rospy.Time.now()
obj2.header.frame_id = "base_link"
obj2.id = "test_object"
obj2.operation.operation = mapping_msgs.msg.CollisionObjectOperation.REMOVE
self.obj_pub.publish(obj2)
rospy.sleep(5.)
cur_state = self.get_robot_state_server.call(req)
state_req.robot_state = cur_state.robot_state
res = self.get_state_validity_server.call(state_req)
# base shouldn't collide due to child only links
self.failIf(res.error_code.val != res.error_code.SUCCESS)
# now it should collide
state_req.ordered_collision_operations.collision_operations = []
res = self.get_state_validity_server.call(state_req)
# base shouldn't collide due to child only links
self.failIf(res.error_code.val == res.error_code.SUCCESS)
