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_contact_surface(self, T):
# We can't construct a ContactSurface directly. So, we need to evaluate
# hydroelastic contact in order to get a result that we can assess.
RigidTransform = RigidTransform_[T]
RigidTransformd = RigidTransform_[float]
SceneGraph = mut.SceneGraph_[T]
FramePoseVector = mut.FramePoseVector_[T]
scene_graph = SceneGraph()
s_id = scene_graph.RegisterSource("source")
# Add a compliant "moving" ball.
f_id = scene_graph.RegisterFrame(
source_id=s_id, frame=mut.GeometryFrame("frame"))
g_id0 = scene_graph.RegisterGeometry(
source_id=s_id, frame_id=f_id,
geometry=mut.GeometryInstance(X_PG=RigidTransformd(),
shape=mut.Sphere(1.), name="sphere"))
props = mut.ProximityProperties()
mut.AddCompliantHydroelasticProperties(
resolution_hint=1.0, hydroelastic_modulus=1e5, properties=props)
scene_graph.AssignRole(s_id, g_id0, props)
# Add a rigd half space.
g_id1 = scene_graph.RegisterAnchoredGeometry(
source_id=s_id,
geometry=mut.GeometryInstance(X_PG=RigidTransformd(),
shape=mut.HalfSpace(), name="plane"))
props = mut.ProximityProperties()
mut.AddRigidHydroelasticProperties(properties=props)
scene_graph.AssignRole(s_id, g_id1, props)
context = scene_graph.CreateDefaultContext()
# Provide poses so we can evaluate the query object. The poses should
# lead to a single collision.
poses = FramePoseVector()
poses.set_value(id=f_id, value=RigidTransform([0, 0, 0.5]))
scene_graph.get_source_pose_port(s_id).FixValue(context, poses)
query_object = scene_graph.get_query_output_port().Eval(context)
# Test both mesh representations.
for rep in (mut.HydroelasticContactRepresentation.kTriangle,
mut.HydroelasticContactRepresentation.kPolygon):
expect_triangles = (
rep == mut.HydroelasticContactRepresentation.kTriangle)
results = query_object.ComputeContactSurfaces(rep)
self.assertEqual(len(results), 1)
contact_surface = results[0]
self.assertLess(g_id0, g_id1) # confirm M = 0 and N = 1.
self.assertEqual(contact_surface.id_M(), g_id0)
self.assertEqual(contact_surface.id_N(), g_id1)
self.assertGreater(contact_surface.num_faces(), 0)
self.assertGreater(contact_surface.num_vertices(), 0)
self.assertGreater(contact_surface.area(face_index=0), 0)
self.assertGreater(contact_surface.total_area(), 0)
contact_surface.face_normal(face_index=0)
contact_surface.centroid(face_index=0)
contact_surface.centroid()
self.assertEqual(contact_surface.is_triangle(), expect_triangles)
self.assertEqual(contact_surface.representation(), rep)
if expect_triangles:
# Details of mesh are tested in geometry_hydro_test.py
contact_surface.tri_mesh_W()
field = contact_surface.tri_e_MN()
# Only triangle mesh fields can evaluate barycentric coords.
field.Evaluate(e=0, b=(0.25, 0.5, 0.25))
else:
# Details of mesh are tested in geometry_hydro_test.py
contact_surface.poly_mesh_W()
field = contact_surface.poly_e_MN()
# Only the Polygonal mesh has gradients pre-computed.
field.EvaluateGradient(e=0)
# APIs available to both Triangle and Polygon-based fields.
field.EvaluateAtVertex(v=0)
field.EvaluateCartesian(e=0, p_MQ=(0.25, 0.25, 0))
