def test_visual_element_api(self): material_in = [0.3, 0.4, 0.5, 0.6] material_in_2 = [0.6, 0.7, 0.8, 0.9] box = shapes.Box(size=[1., 1., 1.]) visual_element_np = shapes.VisualElement(box, np.eye(4), material_in) visual_element_isom = shapes.VisualElement(box, Isometry3.Identity(), material_in) self.assertTrue( np.allclose(visual_element_np.getMaterial(), material_in)) visual_element_np.setMaterial(material_in_2) self.assertTrue( np.allclose(visual_element_np.getMaterial(), material_in_2))
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") # 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]