def test_shapes(self): RigidTransform = RigidTransform_[float] sphere = mut.Sphere(radius=1.0) self.assertEqual(sphere.radius(), 1.0) cylinder = mut.Cylinder(radius=1.0, length=2.0) self.assertEqual(cylinder.radius(), 1.0) self.assertEqual(cylinder.length(), 2.0) box = mut.Box(width=1.0, depth=2.0, height=3.0) self.assertEqual(box.width(), 1.0) self.assertEqual(box.depth(), 2.0) self.assertEqual(box.height(), 3.0) numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0])) capsule = mut.Capsule(radius=1.0, length=2.0) self.assertEqual(capsule.radius(), 1.0) self.assertEqual(capsule.length(), 2.0) ellipsoid = mut.Ellipsoid(a=1.0, b=2.0, c=3.0) self.assertEqual(ellipsoid.a(), 1.0) self.assertEqual(ellipsoid.b(), 2.0) self.assertEqual(ellipsoid.c(), 3.0) X_FH = mut.HalfSpace.MakePose(Hz_dir_F=[0, 1, 0], p_FB=[1, 1, 1]) self.assertIsInstance(X_FH, RigidTransform) box_mesh_path = FindResourceOrThrow( "drake/systems/sensors/test/models/meshes/box.obj") mesh = mut.Mesh(absolute_filename=box_mesh_path, scale=1.0) self.assertEqual(mesh.filename(), box_mesh_path) self.assertEqual(mesh.scale(), 1.0) convex = mut.Convex(absolute_filename=box_mesh_path, scale=1.0) self.assertEqual(convex.filename(), box_mesh_path) self.assertEqual(convex.scale(), 1.0)
def test_shape_constructors(self): box_mesh_path = FindResourceOrThrow( "drake/systems/sensors/test/models/meshes/box.obj") shapes = [ mut.Sphere(radius=1.0), mut.Cylinder(radius=1.0, length=2.0), mut.Box(width=1.0, depth=2.0, height=3.0), mut.HalfSpace(), mut.Mesh(absolute_filename=box_mesh_path, scale=1.0), mut.Convex(absolute_filename=box_mesh_path, scale=1.0) ] for shape in shapes: self.assertIsInstance(shape, mut.Shape)
def test_shape_constructors(self): shapes = [ mut.Sphere(radius=1.0), mut.Cylinder(radius=1.0, length=2.0), mut.Box(width=1.0, depth=2.0, height=3.0), mut.Capsule(radius=1.0, length=2.0), mut.Ellipsoid(a=1.0, b=2.0, c=3.0), mut.HalfSpace(), mut.Mesh(absolute_filename="arbitrary/path", scale=1.0), mut.Convex(absolute_filename="arbitrary/path", scale=1.0), mut.MeshcatCone(height=1.23, a=3.45, b=6.78) ] for shape in shapes: self.assertIsInstance(shape, mut.Shape) shape_cls = type(shape) shape_copy = shape.Clone() self.assertIsInstance(shape_copy, shape_cls) self.assertIsNot(shape, shape_copy)
def test_shapes(self): sphere = mut.Sphere(radius=1.0) self.assertEqual(sphere.radius(), 1.0) cylinder = mut.Cylinder(radius=1.0, length=2.0) self.assertEqual(cylinder.radius(), 1.0) self.assertEqual(cylinder.length(), 2.0) box = mut.Box(width=1.0, depth=2.0, height=3.0) self.assertEqual(box.width(), 1.0) self.assertEqual(box.depth(), 2.0) self.assertEqual(box.height(), 3.0) numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0])) box_mesh_path = FindResourceOrThrow( "drake/systems/sensors/test/models/meshes/box.obj") mesh = mut.Mesh(absolute_filename=box_mesh_path, scale=1.0) self.assertEqual(mesh.filename(), box_mesh_path) self.assertEqual(mesh.scale(), 1.0) convex = mut.Convex(absolute_filename=box_mesh_path, scale=1.0) self.assertEqual(convex.filename(), box_mesh_path) self.assertEqual(convex.scale(), 1.0)
def test_shapes(self): # We'll test some invariants on all shapes as inherited from the Shape # API. def assert_shape_api(shape): self.assertIsInstance(shape, mut.Shape) shape_cls = type(shape) shape_copy = shape.Clone() self.assertIsInstance(shape_copy, shape_cls) self.assertIsNot(shape, shape_copy) # Note: these are ordered alphabetical order and not in the declared # order in shape_specification.h box = mut.Box(width=1.0, depth=2.0, height=3.0) assert_shape_api(box) self.assertEqual(box.width(), 1.0) self.assertEqual(box.depth(), 2.0) self.assertEqual(box.height(), 3.0) assert_pickle( self, box, lambda shape: [shape.width( ), shape.depth(), shape.height()]) numpy_compare.assert_float_equal(box.size(), np.array([1.0, 2.0, 3.0])) self.assertAlmostEqual(mut.CalcVolume(box), 6.0, 1e-14) capsule = mut.Capsule(radius=1.0, length=2.0) assert_shape_api(capsule) self.assertEqual(capsule.radius(), 1.0) self.assertEqual(capsule.length(), 2.0) assert_pickle( self, capsule, lambda shape: [shape.radius(), shape.length()]) junk_path = "arbitrary/path" convex = mut.Convex(absolute_filename=junk_path, scale=1.0) assert_shape_api(convex) self.assertEqual(convex.filename(), junk_path) self.assertEqual(convex.scale(), 1.0) assert_pickle( self, convex, lambda shape: [shape.filename(), shape.scale()]) cylinder = mut.Cylinder(radius=1.0, length=2.0) assert_shape_api(cylinder) self.assertEqual(cylinder.radius(), 1.0) self.assertEqual(cylinder.length(), 2.0) assert_pickle( self, cylinder, lambda shape: [shape.radius(), shape.length()]) ellipsoid = mut.Ellipsoid(a=1.0, b=2.0, c=3.0) assert_shape_api(ellipsoid) self.assertEqual(ellipsoid.a(), 1.0) self.assertEqual(ellipsoid.b(), 2.0) self.assertEqual(ellipsoid.c(), 3.0) assert_pickle(self, ellipsoid, lambda shape: [shape.a(), shape.b(), shape.c()]) X_FH = mut.HalfSpace.MakePose(Hz_dir_F=[0, 1, 0], p_FB=[1, 1, 1]) self.assertIsInstance(X_FH, RigidTransform) mesh = mut.Mesh(absolute_filename=junk_path, scale=1.0) assert_shape_api(mesh) self.assertEqual(mesh.filename(), junk_path) self.assertEqual(mesh.scale(), 1.0) assert_pickle( self, mesh, lambda shape: [shape.filename(), shape.scale()]) sphere = mut.Sphere(radius=1.0) assert_shape_api(sphere) self.assertEqual(sphere.radius(), 1.0) assert_pickle(self, sphere, mut.Sphere.radius) cone = mut.MeshcatCone(height=1.2, a=3.4, b=5.6) assert_shape_api(cone) self.assertEqual(cone.height(), 1.2) self.assertEqual(cone.a(), 3.4) self.assertEqual(cone.b(), 5.6) assert_pickle(self, cone, lambda shape: [shape.height( ), shape.a(), shape.b()])
def build_mbp(seed=0, verts_geom=False, convex_collision_geom=True): # Make some random lumpy objects trimeshes = [] np.random.seed(42) for k in range(3): # Make a small random number of triangles and chull it # to get a lumpy object mesh = trimesh.creation.random_soup(5) mesh = trimesh.convex.convex_hull(mesh) trimeshes.append(mesh) # Create Drake geometry from those objects by adding a small # sphere at each vertex sphere_rad = 0.05 cmap = plt.cm.get_cmap('jet') builder = DiagramBuilder() mbp, scene_graph = AddMultibodyPlantSceneGraph( builder, MultibodyPlant(time_step=0.001)) # Add ground friction = CoulombFriction(0.9, 0.8) g = pydrake_geom.Box(100., 100., 0.5) tf = RigidTransform(p=[0., 0., -0.25]) mbp.RegisterVisualGeometry(body=mbp.world_body(), X_BG=tf, shape=g, name="ground", diffuse_color=[1.0, 1.0, 1.0, 1.0]) mbp.RegisterCollisionGeometry(body=mbp.world_body(), X_BG=tf, shape=g, name="ground", coulomb_friction=friction) for i, mesh in enumerate(trimeshes): inertia = SpatialInertia(mass=1.0, p_PScm_E=np.zeros(3), G_SP_E=UnitInertia(0.01, 0.01, 0.01)) body = mbp.AddRigidBody(name="body_%d" % i, M_BBo_B=inertia) color = cmap(np.random.random()) if verts_geom: for j, vert in enumerate(mesh.vertices): g = pydrake_geom.Sphere(radius=sphere_rad) tf = RigidTransform(p=vert) mbp.RegisterVisualGeometry(body=body, X_BG=tf, shape=g, name="body_%d_color_%d" % (i, j), diffuse_color=color) mbp.RegisterCollisionGeometry(body=body, X_BG=tf, shape=g, name="body_%d_collision_%d" % (i, j), coulomb_friction=friction) # And add mesh itself for vis path = "/tmp/part_%d.obj" % i trimesh.exchange.export.export_mesh(mesh, path) g = pydrake_geom.Convex(path) mbp.RegisterVisualGeometry(body=body, X_BG=RigidTransform(), shape=g, name="body_%d_base" % i, diffuse_color=color) if convex_collision_geom: mbp.RegisterCollisionGeometry(body=body, X_BG=RigidTransform(), shape=g, name="body_%d_base_col" % i, coulomb_friction=friction) mbp.SetDefaultFreeBodyPose(body, RigidTransform(p=[i % 3, i / 3., 1.])) mbp.Finalize() return builder, mbp, scene_graph