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
