def test_get_base_state_validity(self):
state_req = GetStateValidityRequest()
state_req.check_collisions = True
#empty state should be ok
res = self.get_state_validity_server.call(state_req)
self.failIf(res.error_code.val != res.error_code.SUCCESS)
#should be in collision
state_req.robot_state.multi_dof_joint_state.joint_names.append(
"base_joint")
state_req.robot_state.multi_dof_joint_state.frame_ids.append(
"odom_combined")
state_req.robot_state.multi_dof_joint_state.child_frame_ids.append(
"base_footprint")
pose = Pose()
pose.position.x = 4.0
pose.orientation.w = 1.0
state_req.robot_state.multi_dof_joint_state.poses.append(pose)
res = self.get_state_validity_server.call(state_req)
self.failIf(res.error_code.val == res.error_code.SUCCESS)
self.assertEqual(res.error_code.val,
res.error_code.COLLISION_CONSTRAINTS_VIOLATED)
#shouldn't be in collision as it doesn't have right parent_frame
state_req.robot_state.multi_dof_joint_state.frame_ids[0] = ""
res = self.get_state_validity_server.call(state_req)
self.failIf(res.error_code.val != res.error_code.SUCCESS)
#should be in collision now
state_req.robot_state.joint_state.name.append("floating_trans_x")
state_req.robot_state.joint_state.position.append(4.0)
res = self.get_state_validity_server.call(state_req)
self.failIf(res.error_code.val == res.error_code.SUCCESS)
self.assertEqual(res.error_code.val,
res.error_code.COLLISION_CONSTRAINTS_VIOLATED)
#should be in collision now
#moving back, but should still be in collision
state_req.robot_state.joint_state.position[0] = 3.3
res = self.get_state_validity_server.call(state_req)
self.failIf(res.error_code.val == res.error_code.SUCCESS)
self.assertEqual(res.error_code.val,
res.error_code.COLLISION_CONSTRAINTS_VIOLATED)
#but at a different theta should be ok
state_req.robot_state.joint_state.name.append("floating_rot_z")
state_req.robot_state.joint_state.position.append(.7071)
state_req.robot_state.joint_state.name.append("floating_rot_w")
state_req.robot_state.joint_state.position.append(.7071)
res = self.get_state_validity_server.call(state_req)
self.failIf(res.error_code.val != res.error_code.SUCCESS)
def test_get_base_state_validity(self):
state_req = GetStateValidityRequest()
state_req.check_collisions = True
#empty state should be ok
res = self.get_state_validity_server.call(state_req)
self.failIf(res.error_code.val != res.error_code.SUCCESS)
#should be in collision
state_req.robot_state.multi_dof_joint_state.joint_names.append("base_joint")
state_req.robot_state.multi_dof_joint_state.frame_ids.append("odom_combined")
state_req.robot_state.multi_dof_joint_state.child_frame_ids.append("base_footprint")
pose = Pose()
pose.position.x = 4.0
pose.orientation.w = 1.0
state_req.robot_state.multi_dof_joint_state.poses.append(pose)
res = self.get_state_validity_server.call(state_req)
self.failIf(res.error_code.val == res.error_code.SUCCESS)
self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED)
#shouldn't be in collision as it doesn't have right parent_frame
state_req.robot_state.multi_dof_joint_state.frame_ids[0] = ""
res = self.get_state_validity_server.call(state_req)
self.failIf(res.error_code.val != res.error_code.SUCCESS)
#should be in collision now
state_req.robot_state.joint_state.name.append("floating_trans_x");
state_req.robot_state.joint_state.position.append(4.0);
res = self.get_state_validity_server.call(state_req)
self.failIf(res.error_code.val == res.error_code.SUCCESS)
self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED)
#should be in collision now
#moving back, but should still be in collision
state_req.robot_state.joint_state.position[0] = 3.3;
res = self.get_state_validity_server.call(state_req)
self.failIf(res.error_code.val == res.error_code.SUCCESS)
self.assertEqual(res.error_code.val, res.error_code.COLLISION_CONSTRAINTS_VIOLATED)
#but at a different theta should be ok
state_req.robot_state.joint_state.name.append("floating_rot_z");
state_req.robot_state.joint_state.position.append(.7071);
state_req.robot_state.joint_state.name.append("floating_rot_w");
state_req.robot_state.joint_state.position.append(.7071);
res = self.get_state_validity_server.call(state_req)
self.failIf(res.error_code.val != res.error_code.SUCCESS)
def _create_state_validity_request(self):
# set up a request message for state checks
state_req = GetStateValidityRequest()
state_req.check_collisions = True
state_req.robot_state.joint_state.header.stamp = rospy.Time.now()
# get the current joint state
currState = self._current_joint_state()
#print str(currState)
if currState is None:
return None
# set the joint name and joint value arrays in the state check request message
for name in currState.name:
state_req.robot_state.joint_state.name.append(name)
state_req.robot_state.joint_state.position.append(currState.position[currState.name.index(name)])
#print str(state_req)
return state_req
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 _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_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 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 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 _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 setUp(self):
rospy.init_node('test_attached_object_collisions')
self.obj_pub = rospy.Publisher('collision_object',
CollisionObject,
latch=True)
self.att_pub = rospy.Publisher('attached_collision_object',
AttachedCollisionObject,
latch=True)
rospy.wait_for_service(default_prefix + '/get_state_validity')
self.get_state_validity_server = rospy.ServiceProxy(
default_prefix + '/get_state_validity', GetStateValidity)
self.state_req = GetStateValidityRequest()
self.state_req.robot_state.joint_state.name = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.state_req.robot_state.joint_state.position = [
float(0.0) for _ in range(7)
]
self.state_req.check_collisions = True
self.table = CollisionObject()
self.table.header.stamp = rospy.Time.now()
self.table.header.frame_id = "base_link"
self.table.id = "tabletop"
self.table.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
self.table.shapes = [Shape() for _ in range(1)]
self.table.shapes[0].type = Shape.BOX
self.table.shapes[0].dimensions = [float() for _ in range(3)]
self.table.shapes[0].dimensions[0] = 1.0
self.table.shapes[0].dimensions[1] = 1.0
self.table.shapes[0].dimensions[2] = .05
self.table.poses = [Pose() for _ in range(1)]
self.table.poses[0].position.x = 1.0
self.table.poses[0].position.y = 0
self.table.poses[0].position.z = .5
self.table.poses[0].orientation.x = 0
self.table.poses[0].orientation.y = 0
self.table.poses[0].orientation.z = 0
self.table.poses[0].orientation.w = 1
#not publishing table right away
self.att_object = AttachedCollisionObject()
self.att_object.object.header.stamp = rospy.Time.now()
self.att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
self.att_object.link_name = "r_gripper_r_finger_tip_link"
self.att_object.object.operation.operation = mapping_msgs.msg.CollisionObjectOperation.ADD
self.att_object.object = CollisionObject()
self.att_object.object.header.stamp = rospy.Time.now()
self.att_object.object.header.frame_id = "r_gripper_r_finger_tip_link"
self.att_object.object.id = "pole"
self.att_object.object.shapes = [Shape() for _ in range(1)]
self.att_object.object.shapes[0].type = Shape.CYLINDER
self.att_object.object.shapes[0].dimensions = [
float() for _ in range(2)
]
self.att_object.object.shapes[0].dimensions[0] = .02
self.att_object.object.shapes[0].dimensions[1] = 1.2
self.att_object.object.poses = [Pose() for _ in range(1)]
self.att_object.object.poses[0].position.x = 0
self.att_object.object.poses[0].position.y = 0
self.att_object.object.poses[0].position.z = 0
self.att_object.object.poses[0].orientation.x = 0
self.att_object.object.poses[0].orientation.y = 0
self.att_object.object.poses[0].orientation.z = 0
self.att_object.object.poses[0].orientation.w = 1
self.att_pub.publish(self.att_object)
self.touch_links = [
'r_gripper_palm_link', 'r_gripper_r_finger_link',
'r_gripper_l_finger_link', 'r_gripper_r_finger_tip_link',
'r_gripper_l_finger_tip_link'
]
rospy.sleep(2.)
# One-time initialization of the environment server:
SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff"
rospy.wait_for_service(SET_PLANNING_SCENE_DIFF_NAME)
set_planning_scene_server = rospy.ServiceProxy(SET_PLANNING_SCENE_DIFF_NAME,
SetPlanningSceneDiff)
# Just send an empty message:
res = set_planning_scene_server.call(SetPlanningSceneDiffRequest())
# One-time initialization of the state validity server:
rospy.wait_for_service('/planning_scene_validity_server/get_state_validity')
state_validity_server = rospy.ServiceProxy(
'/planning_scene_validity_server/get_state_validity', GetStateValidity)
# Set up a request message for state checks:
state_req = GetStateValidityRequest()
state_req.check_collisions = True
state_req.robot_state.joint_state.header.stamp = rospy.Time.now()
# -------------------------------------------- Usage ----------------------------------------
# Get the current joint state, which is presumably collision free:
currState = currentJointState()
print str(currState)
# Set the joint name and joint value arrays in the state check request message
# just for the right shoulder joints. The joint service uses the current joint
# states for any unfilled joint fields in the request message:
for name in currState.name:
state_req.robot_state.joint_state.name.append(name)
rospy.init_node('test_collision_check')
# One-time initialization of the environment server:
SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff"
rospy.wait_for_service(SET_PLANNING_SCENE_DIFF_NAME);
set_planning_scene_server = rospy.ServiceProxy(SET_PLANNING_SCENE_DIFF_NAME, SetPlanningSceneDiff)
# Just send an empty message:
res = set_planning_scene_server.call(SetPlanningSceneDiffRequest())
# One-time initialization of the state validity server:
rospy.wait_for_service('/planning_scene_validity_server/get_state_validity');
state_validity_server = rospy.ServiceProxy('/planning_scene_validity_server/get_state_validity', GetStateValidity)
# Set up a request message for state checks:
state_req = GetStateValidityRequest()
state_req.check_collisions = True
state_req.robot_state.joint_state.header.stamp = rospy.Time.now()
# -------------------------------------------- Usage ----------------------------------------
# Get the current joint state, which is presumably collision free:
currState = currentJointState();
print str(currState)
# Set the joint name and joint value arrays in the state check request message
# just for the right shoulder joints. The joint service uses the current joint
# states for any unfilled joint fields in the request message:
for name in currState.name:
state_req.robot_state.joint_state.name.append(name)
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)
