def setup_scene(rbt, config):
if config["with_ground"] is True:
AddFlatTerrainToWorld(rbt)
for i, instance_config in enumerate(config["instances"]):
add_single_instance_to_rbt(rbt,
config,
instance_config,
i,
floating_base_type=FloatingBaseType.kFixed)
# Add camera geometry!
camera_link = RigidBody()
camera_link.set_name("camera_link")
# necessary so this last link isn't pruned by the rbt.compile() call
camera_link.set_spatial_inertia(np.eye(6))
camera_link.add_joint(
rbt.world(),
RollPitchYawFloatingJoint("camera_floating_base", np.eye(4)))
rbt.add_rigid_body(camera_link)
# - Add frame for camera fixture.
camera_frame = RigidBodyFrame(name="rgbd_camera_frame",
body=camera_link,
xyz=[0.0, 0., 0.],
rpy=[0., 0., 0.])
rbt.addFrame(camera_frame)
rbt.compile()
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_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 test_rigid_body_api(self):
# Tests as much of the RigidBody API as is possible in isolation.
# Adding collision geometry is *not* tested here, as it needs to
# be done in the context of the RigidBodyTree.
body = RigidBody()
name = "body"
body.set_name(name)
self.assertEqual(body.get_name(), name)
inertia = np.eye(6)
body.set_spatial_inertia(inertia)
self.assertTrue(np.allclose(inertia, body.get_spatial_inertia()))
# Try adding a joint to a dummy body.
body_joint = PrismaticJoint("z", np.eye(4), np.array([0., 0., 1.]))
self.assertFalse(body.has_joint())
dummy_body = RigidBody()
body.add_joint(dummy_body, body_joint)
self.assertEqual(body.getJoint(), body_joint)
self.assertTrue(body.has_joint())
# Try adding visual geometry.
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.AddVisualElement(box_visual_element)
body_visual_elements = body.get_visual_elements()
self.assertEqual(len(body_visual_elements), 1)
self.assertEqual(body_visual_elements[0].getGeometry().getShape(),
box_visual_element.getGeometry().getShape())
# Test collision-related methods.
self.assertEqual(body.get_num_collision_elements(), 0)
self.assertEqual(len(body.get_collision_element_ids()), 0)
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")
rbt.compile()
# 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)
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 test_rigid_body_api(self):
# Tests as much of the RigidBody API as is possible in isolation.
# Adding collision geometry is *not* tested here, as it needs to
# be done in the context of the RigidBodyTree.
body = RigidBody()
name = "body"
body.set_name(name)
self.assertEqual(body.get_name(), name)
inertia = np.eye(6)
body.set_spatial_inertia(inertia)
self.assertTrue(np.allclose(inertia, body.get_spatial_inertia()))
# Try adding a joint to a dummy body.
body_joint = PrismaticJoint("z", np.eye(4),
np.array([0., 0., 1.]))
self.assertFalse(body.has_joint())
dummy_body = RigidBody()
body.add_joint(dummy_body, body_joint)
self.assertEqual(body.getJoint(), body_joint)
self.assertTrue(body.has_joint())
# Try adding visual geometry.
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.AddVisualElement(box_visual_element)
body_visual_elements = body.get_visual_elements()
self.assertEqual(len(body_visual_elements), 1)
self.assertEqual(body_visual_elements[0].getGeometry().getShape(),
box_visual_element.getGeometry().getShape())
# Test collision-related methods.
self.assertEqual(body.get_num_collision_elements(), 0)
self.assertEqual(len(body.get_collision_element_ids()), 0)