def test_rigid_body_tree_collision_api(self):
# This test creates a simple RBT with two spheres in simple
# collision and verifies collision-detection APIs.
rbt = RigidBodyTree()
world_body = rbt.world()
# Both bodies are unit-diameter spheres
# spaced 0.5m from each other along the x axis.
for k in range(2):
body = RigidBody()
body.set_name("body_{}".format(k))
joint = PrismaticJoint("x", np.eye(4), np.array([1.0, 0., 0.]))
body.add_joint(world_body, joint)
body.set_spatial_inertia(np.eye(6))
rbt.add_rigid_body(body)
sphere_element = shapes.Sphere(0.5)
sphere_collision_element = CollisionElement(
sphere_element, np.eye(4))
sphere_collision_element.set_body(body)
rbt.addCollisionElement(sphere_collision_element, body, "default")
rbt.compile()
self.assertTrue(rbt.initialized())
self.assertEqual(rbt.get_num_positions(), 2)
q0 = np.array([-0.25, 0.25])
kinsol = rbt.doKinematics(q0)
# One point in each region of overlap:
# Sphere one is (), sphere two is []
# p0 ( p2 [ p3 ) p4 ] p5
points = np.array([[-1., -0.6, 0., 0.6, 1.], [0., 0., 0., 0., 0],
[0., 0., 0., 0., 0]])
n_pts = points.shape[1]
phi, normal, x, body_x, body_idx = rbt.collisionDetectFromPoints(
cache=kinsol, points=points, use_margins=False)
# Check phi, but leave the other fields as API checks.
self.assertTrue(
np.allclose(phi, np.array([0.25, -0.15, -0.25, -0.15, 0.25])))
self.assertEqual(phi.shape[0], n_pts)
self.assertEqual(normal.shape, (3, n_pts))
self.assertEqual(x.shape, (3, n_pts))
self.assertEqual(normal.shape, (3, n_pts))
point_pairs = rbt.ComputeMaximumDepthCollisionPoints(
cache=kinsol, use_margins=False, throw_if_missing_gradient=True)
self.assertEqual(len(point_pairs), 1)
pp = point_pairs[0]
self.assertEqual(rbt.FindCollisionElement(pp.idA), pp.elementA)
self.assertEqual(rbt.FindCollisionElement(pp.idB), pp.elementB)
possible_points = [np.array([0.5, 0., 0.]), np.array([-0.5, 0.0, 0.0])]
for pt in [pp.ptA, pp.ptB]:
self.assertTrue(
np.allclose(pt, np.array([0.5, 0., 0.]))
or np.allclose(pt, np.array([-0.5, 0., 0.])))
self.assertTrue(np.allclose(np.abs(pp.normal), np.array([1., 0., 0.])))
self.assertEqual(pp.distance, -0.5)
def test_rigid_body_tree_programmatic_construction(self):
# Tests RBT programmatic construction methods by assembling
# a simple RBT with a prismatic and revolute joint, with
# both visual and collision geometry on the last joint.
rbt = RigidBodyTree()
world_body = rbt.world()
# body_1 is connected to the world via a prismatic joint along
# the +z axis.
body_1 = RigidBody()
body_1.set_name("body_1")
body_1_joint = PrismaticJoint("z", np.eye(4),
np.array([0., 0., 1.]))
body_1.add_joint(world_body, body_1_joint)
rbt.add_rigid_body(body_1)
# body_2 is connected to body_1 via a revolute joint around the z-axis.
body_2 = RigidBody()
body_2.set_name("body_2")
body_2_joint = RevoluteJoint("theta", np.eye(4),
np.array([0., 0., 1.]))
body_2.add_joint(body_1, body_2_joint)
box_element = shapes.Box([1.0, 1.0, 1.0])
box_visual_element = shapes.VisualElement(
box_element, np.eye(4), [1., 0., 0., 1.])
body_2.AddVisualElement(box_visual_element)
body_2_visual_elements = body_2.get_visual_elements()
rbt.add_rigid_body(body_2)
box_collision_element = CollisionElement(box_element, np.eye(4))
box_collision_element.set_body(body_2)
rbt.addCollisionElement(box_collision_element, body_2, "default")
# Define a collision filter group containing bodies 1 and 2 and make
# that group ignore itself.
rbt.DefineCollisionFilterGroup(name="test_group")
rbt.AddCollisionFilterGroupMember(
group_name="test_group", body_name="body_1", model_id=0)
rbt.AddCollisionFilterGroupMember(
group_name="test_group", body_name="body_2", model_id=0)
rbt.AddCollisionFilterIgnoreTarget("test_group", "test_group")
self.assertFalse(rbt.initialized())
rbt.compile()
self.assertTrue(rbt.initialized())
# The RBT's position vector should now be [z, theta].
self.assertEqual(body_1.get_position_start_index(), 0)
self.assertEqual(body_2.get_position_start_index(), 1)
self.assertIsNotNone(
rbt.FindCollisionElement(
body_2.get_collision_element_ids()[0]))
def add_box_to_rbt(rbt, box_config, name='box_0'):
# type: (RigidBodyTree, BoxConfig, str) -> List[str]
rigid_body = RigidBody()
rigid_body.set_name(name)
# The fixed joint of rigid body
fixed_joint = FixedJoint(name + 'joint', box_config.box_in_world)
rigid_body.add_joint(rbt.world(), fixed_joint)
# The visual element
box_element = shapes.Box(box_config.dim_xyz)
box_visual_element = shapes.VisualElement(box_element, np.eye(4),
[1., 0., 0., 1.])
rigid_body.AddVisualElement(box_visual_element)
rbt.add_rigid_body(rigid_body)
# The collision element
box_collision_element = CollisionElement(box_element, np.eye(4))
box_collision_element.set_body(rigid_body)
rbt.addCollisionElement(box_collision_element, rigid_body, 'default')
return [name]
def add_cylinder_to_rbt(rbt, cylinder_config, name='cylinder_0'):
# type: (RigidBodyTree, CylinderConfig, str) -> List[str]
# Construct the shape
start_point = np.asarray(cylinder_config.start_point)
end_point = np.asarray(cylinder_config.end_point)
length = np.linalg.norm(start_point - end_point) # type: float
radius = cylinder_config.radius # type: float
cylinder_element = shapes.Cylinder(radius=radius, length=length)
# Compute the transform in world
cylinder_in_world = np.eye(4)
z_axis = (end_point - start_point) / (length)
x_axis = np.array([z_axis[1], -z_axis[0], 0])
x_axis = x_axis / np.linalg.norm(x_axis)
y_axis = np.cross(z_axis, x_axis)
y_axis = y_axis / np.linalg.norm(y_axis)
center = 0.5 * (start_point + end_point)
cylinder_in_world[0:3, 0] = x_axis
cylinder_in_world[0:3, 1] = y_axis
cylinder_in_world[0:3, 2] = z_axis
cylinder_in_world[0:3, 3] = center
# The fixed joint of rigid body
rigid_body = RigidBody()
rigid_body.set_name(name)
fixed_joint = FixedJoint(name + 'joint', cylinder_in_world)
rigid_body.add_joint(rbt.world(), fixed_joint)
# The visual element
cylinder_visual_element = shapes.VisualElement(cylinder_element, np.eye(4),
[1., 0., 0., 1.])
rigid_body.AddVisualElement(cylinder_visual_element)
rbt.add_rigid_body(rigid_body)
# The collision element
cylinder_collision_element = CollisionElement(cylinder_element, np.eye(4))
cylinder_collision_element.set_body(rigid_body)
rbt.addCollisionElement(cylinder_collision_element, rigid_body, 'default')
return [name]
def test_collision_element_api(self):
# Verify construction from both Isometry3d and 4x4 arrays.
box_element = shapes.Box([1.0, 1.0, 1.0])
box_collision_element_np = CollisionElement(box_element, np.eye(4))
box_collision_element_isom = CollisionElement(
box_element, Isometry3.Identity())
body = RigidBody()
box_collision_element_isom.set_body(body)
self.assertEqual(box_collision_element_isom.get_body(), body)
def test_rigid_body_tree_collision_api(self):
# This test creates a simple RBT with two spheres in simple
# collision and verifies collision-detection APIs.
rbt = RigidBodyTree()
world_body = rbt.world()
# Both bodies are unit-diameter spheres
# spaced 0.5m from each other along the x axis.
for k in range(2):
body = RigidBody()
body.set_name("body_{}".format(k))
joint = PrismaticJoint("x", np.eye(4),
np.array([1.0, 0., 0.]))
body.add_joint(world_body, joint)
body.set_spatial_inertia(np.eye(6))
rbt.add_rigid_body(body)
sphere_element = shapes.Sphere(0.5)
sphere_collision_element = CollisionElement(
sphere_element, np.eye(4))
sphere_collision_element.set_body(body)
rbt.addCollisionElement(sphere_collision_element, body, "default")
rbt.compile()
self.assertTrue(rbt.initialized())
self.assertEqual(rbt.get_num_positions(), 2)
q0 = np.array([-0.25, 0.25])
kinsol = rbt.doKinematics(q0)
# One point in each region of overlap:
# Sphere one is (), sphere two is []
# p0 ( p2 [ p3 ) p4 ] p5
points = np.array([[-1., -0.6, 0., 0.6, 1.],
[0., 0., 0., 0., 0],
[0., 0., 0., 0., 0]])
n_pts = points.shape[1]
phi, normal, x, body_x, body_idx = rbt.collisionDetectFromPoints(
cache=kinsol, points=points, use_margins=False)
# Check phi, but leave the other fields as API checks.
self.assertTrue(
np.allclose(phi, np.array([0.25, -0.15, -0.25, -0.15, 0.25])))
self.assertEqual(phi.shape[0], n_pts)
self.assertEqual(normal.shape, (3, n_pts))
self.assertEqual(x.shape, (3, n_pts))
self.assertEqual(normal.shape, (3, n_pts))
point_pairs = rbt.ComputeMaximumDepthCollisionPoints(
cache=kinsol, use_margins=False,
throw_if_missing_gradient=True)
self.assertEqual(len(point_pairs), 1)
pp = point_pairs[0]
self.assertEqual(rbt.FindCollisionElement(pp.idA), pp.elementA)
self.assertEqual(rbt.FindCollisionElement(pp.idB), pp.elementB)
possible_points = [np.array([0.5, 0., 0.]), np.array([-0.5, 0.0, 0.0])]
for pt in [pp.ptA, pp.ptB]:
self.assertTrue(
np.allclose(pt, np.array([0.5, 0., 0.])) or
np.allclose(pt, np.array([-0.5, 0., 0.])))
self.assertTrue(
np.allclose(np.abs(pp.normal), np.array([1., 0., 0.])))
self.assertEqual(pp.distance, -0.5)
