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 __init__(self, tf):
geom = PhysicsGeometryInfo()
geom.register_geometry(tf=drake_tf_to_torch_tf(
RigidTransform(p=[0., 0., .1 / 2.])),
geometry=pydrake_geom.Cylinder(radius=0.05,
length=0.1),
color=np.array([0.3, 0.8, 0.5, 1.0]))
super().__init__(tf=tf, physics_geometry_info=geom, observed=True)
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]))
def __init__(self, tf):
geom = PhysicsGeometryInfo()
rgba = np.random.uniform([0.85, 0.75, 0.45, 1.0],
[0.95, 0.85, 0.55, 1.0])
geom.register_geometry(
tf=drake_tf_to_torch_tf(
RigidTransform(p=[0., 0., .01 / 2.],
rpy=RollPitchYaw(0., np.pi / 2., 0.))),
geometry=pydrake_geom.Cylinder(radius=0.01, length=0.15),
color=rgba,
)
super().__init__(tf=tf, physics_geometry_info=geom, observed=True)
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_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]))
# Test for existence of deprecated accessors.
with catch_drake_warnings(expected_count=3):
cylinder.get_radius()
cylinder.get_length()
sphere.get_radius()
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()])
