def test_hydro_proximity_properties(self):
"""
Tests the utility functions (related to hydroelastic contact) for
setting values in ProximityProperties (as defined in
proximity_properties.h).
"""
props = mut.ProximityProperties()
res_hint = 0.175
E = 1e8
mut.AddRigidHydroelasticProperties(resolution_hint=res_hint,
properties=props)
self.assertTrue(props.HasProperty("hydroelastic", "compliance_type"))
self.assertFalse(mut_testing.PropertiesIndicateCompliantHydro(props))
self.assertTrue(props.HasProperty("hydroelastic", "resolution_hint"))
self.assertEqual(props.GetProperty("hydroelastic", "resolution_hint"),
res_hint)
props = mut.ProximityProperties()
mut.AddRigidHydroelasticProperties(properties=props)
self.assertTrue(props.HasProperty("hydroelastic", "compliance_type"))
self.assertFalse(mut_testing.PropertiesIndicateCompliantHydro(props))
self.assertFalse(props.HasProperty("hydroelastic", "resolution_hint"))
props = mut.ProximityProperties()
res_hint = 0.275
mut.AddCompliantHydroelasticProperties(resolution_hint=res_hint,
hydroelastic_modulus=E,
properties=props)
self.assertTrue(props.HasProperty("hydroelastic", "compliance_type"))
self.assertTrue(mut_testing.PropertiesIndicateCompliantHydro(props))
self.assertTrue(props.HasProperty("hydroelastic", "resolution_hint"))
self.assertEqual(props.GetProperty("hydroelastic", "resolution_hint"),
res_hint)
self.assertTrue(
props.HasProperty("hydroelastic", "hydroelastic_modulus"))
self.assertEqual(
props.GetProperty("hydroelastic", "hydroelastic_modulus"), E)
props = mut.ProximityProperties()
slab_thickness = 0.275
mut.AddCompliantHydroelasticPropertiesForHalfSpace(
slab_thickness=slab_thickness,
hydroelastic_modulus=E,
properties=props)
self.assertTrue(props.HasProperty("hydroelastic", "compliance_type"))
self.assertTrue(mut_testing.PropertiesIndicateCompliantHydro(props))
self.assertTrue(props.HasProperty("hydroelastic", "slab_thickness"))
self.assertEqual(props.GetProperty("hydroelastic", "slab_thickness"),
slab_thickness)
self.assertTrue(
props.HasProperty("hydroelastic", "hydroelastic_modulus"))
self.assertEqual(
props.GetProperty("hydroelastic", "hydroelastic_modulus"), E)
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))
def test_scene_graph_api(self, T):
SceneGraph = mut.SceneGraph_[T]
InputPort = InputPort_[T]
OutputPort = OutputPort_[T]
scene_graph = SceneGraph()
global_source = scene_graph.RegisterSource("anchored")
global_frame = scene_graph.RegisterFrame(
source_id=global_source,
frame=mut.GeometryFrame("anchored_frame1"))
scene_graph.RegisterFrame(
source_id=global_source, parent_id=global_frame,
frame=mut.GeometryFrame("anchored_frame2"))
global_geometry = scene_graph.RegisterGeometry(
source_id=global_source, frame_id=global_frame,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere1"))
# We'll explicitly give sphere_2 a rigid hydroelastic representation.
sphere_2 = scene_graph.RegisterGeometry(
source_id=global_source, geometry_id=global_geometry,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere2"))
props = mut.ProximityProperties()
mut.AddRigidHydroelasticProperties(resolution_hint=1, properties=props)
scene_graph.AssignRole(source_id=global_source, geometry_id=sphere_2,
properties=props)
# We'll explicitly give sphere_3 a compliant hydroelastic
# representation.
sphere_3 = scene_graph.RegisterAnchoredGeometry(
source_id=global_source,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere3"))
props = mut.ProximityProperties()
mut.AddCompliantHydroelasticProperties(
resolution_hint=1, hydroelastic_modulus=1e8, properties=props)
scene_graph.AssignRole(source_id=global_source, geometry_id=sphere_3,
properties=props)
self.assertIsInstance(
scene_graph.get_source_pose_port(global_source), InputPort)
self.assertIsInstance(
scene_graph.get_query_output_port(), OutputPort)
# Test limited rendering API.
scene_graph.AddRenderer("test_renderer",
mut.render.MakeRenderEngineVtk(
mut.render.RenderEngineVtkParams()))
self.assertTrue(scene_graph.HasRenderer("test_renderer"))
self.assertEqual(scene_graph.RendererCount(), 1)
# Test SceneGraphInspector API
inspector = scene_graph.model_inspector()
self.assertEqual(inspector.num_sources(), 2)
self.assertEqual(inspector.num_frames(), 3)
self.assertEqual(len(inspector.GetAllFrameIds()), 3)
self.assertTrue(inspector.world_frame_id()
in inspector.GetAllFrameIds())
self.assertTrue(global_frame in inspector.GetAllFrameIds())
self.assertIsInstance(inspector.world_frame_id(), mut.FrameId)
self.assertEqual(inspector.num_geometries(), 3)
self.assertEqual(len(inspector.GetAllGeometryIds()), 3)
# Test both GeometrySet API as well as SceneGraphInspector's
# GeometrySet API.
empty_set = mut.GeometrySet()
self.assertEqual(
len(inspector.GetGeometryIds(empty_set)),
0)
self.assertEqual(
len(inspector.GetGeometryIds(empty_set, mut.Role.kProximity)),
0)
# Cases 1.a: Explicit frame, constructor
# N.B. Only in this case (1.a), do we test for non-kwarg usages of
# functions. In other tests,
frame_set_options = [
# Frame scalar.
mut.GeometrySet(frame_id=global_frame),
# Frame list.
mut.GeometrySet(frame_ids=[global_frame]),
# Frame list, no kwargs.
mut.GeometrySet([global_frame]),
# Frame list w/ (empty) geometry list.
mut.GeometrySet(geometry_ids=[], frame_ids=[global_frame]),
# Frame list w/ (empty) geometry list, no kwargs.
mut.GeometrySet([], [global_frame]),
]
# Case 1.b: Explicit frame, via Add().
# - Frame scalar.
cur = mut.GeometrySet()
cur.Add(frame_id=global_frame)
frame_set_options.append(cur)
# - Frame list.
cur = mut.GeometrySet()
cur.Add(frame_ids=[global_frame])
frame_set_options.append(cur)
# - Frame list w/ (empty) geometry list.
cur = mut.GeometrySet()
cur.Add(geometry_ids=[], frame_ids=[global_frame])
frame_set_options.append(cur)
# Cases 1.*: Test 'em all.
for frame_set in frame_set_options:
ids = inspector.GetGeometryIds(frame_set)
# N.B. Per above, we have 2 geometries that have been affixed to
# global frame ("sphere1" and "sphere2").
self.assertEqual(len(ids), 2)
# Cases 2.a: Explicit geometry, constructor (with non-kwarg check).
geometry_set_options = [
# Geometry scalar.
mut.GeometrySet(geometry_id=global_geometry),
# Geometry list.
mut.GeometrySet(geometry_ids=[global_geometry]),
# Geometry list, no kwargs.
mut.GeometrySet([global_geometry]),
# Geometry list w/ (empty) frame list.
mut.GeometrySet(geometry_ids=[global_geometry], frame_ids=[]),
# Geometry list w/ (empty) frame list, no kwargs.
mut.GeometrySet([global_geometry], []),
]
# Cases 2.b: Explicit geometry, via Add().
# - Geometry scalar.
cur = mut.GeometrySet()
cur.Add(geometry_id=global_geometry)
geometry_set_options.append(cur)
# - Geometry list.
cur = mut.GeometrySet()
cur.Add(geometry_ids=[global_geometry])
geometry_set_options.append(cur)
# - Geometry list w/ (empty) frame list.
cur = mut.GeometrySet()
cur.Add(geometry_ids=[global_geometry], frame_ids=[])
geometry_set_options.append(cur)
# Cases 1.*: Test 'em all.
for geometry_set in geometry_set_options:
ids = inspector.GetGeometryIds(geometry_set)
self.assertEqual(len(ids), 1)
# Only the first sphere has no proximity properties. The latter two
# have hydroelastic properties (rigid and compliant, respectively).
self.assertEqual(
inspector.NumGeometriesWithRole(role=mut.Role.kUnassigned), 1)
self.assertIsNone(
inspector.maybe_get_hydroelastic_mesh(
geometry_id=global_geometry))
self.assertIsInstance(
inspector.maybe_get_hydroelastic_mesh(
geometry_id=sphere_2), mut.TriangleSurfaceMesh)
self.assertIsInstance(
inspector.maybe_get_hydroelastic_mesh(
geometry_id=sphere_3), mut.VolumeMesh)
self.assertEqual(inspector.NumDynamicGeometries(), 2)
self.assertEqual(inspector.NumAnchoredGeometries(), 1)
# Sphere 2 and 3 have proximity roles; the pair is a candidate.
self.assertEqual(len(inspector.GetCollisionCandidates()), 1)
self.assertTrue(inspector.SourceIsRegistered(source_id=global_source))
# TODO(SeanCurtis-TRI) Remove this call at the same time as deprecating
# the subsequent deprecation tests; it is only here to show that the
# non-keyword call invokes the non-deprecated overload.
self.assertTrue(inspector.SourceIsRegistered(global_source))
self.assertEqual(inspector.NumFramesForSource(source_id=global_source),
2)
self.assertTrue(global_frame in inspector.FramesForSource(
source_id=global_source))
self.assertTrue(inspector.BelongsToSource(
frame_id=global_frame, source_id=global_source))
self.assertEqual(inspector.GetOwningSourceName(frame_id=global_frame),
"anchored")
self.assertEqual(
inspector.GetName(frame_id=global_frame), "anchored_frame1")
self.assertEqual(inspector.GetFrameGroup(frame_id=global_frame), 0)
self.assertEqual(
inspector.NumGeometriesForFrame(frame_id=global_frame), 2)
self.assertEqual(inspector.NumGeometriesForFrameWithRole(
frame_id=global_frame, role=mut.Role.kProximity), 1)
self.assertEqual(len(inspector.GetGeometries(frame_id=global_frame)),
2)
self.assertTrue(
global_geometry in inspector.GetGeometries(frame_id=global_frame))
self.assertEqual(
len(inspector.GetGeometries(frame_id=global_frame,
role=mut.Role.kProximity)),
1)
self.assertEqual(
inspector.GetGeometryIdByName(frame_id=global_frame,
role=mut.Role.kUnassigned,
name="sphere1"),
global_geometry)
self.assertTrue(inspector.BelongsToSource(
geometry_id=global_geometry, source_id=global_source))
self.assertEqual(
inspector.GetOwningSourceName(geometry_id=global_geometry),
"anchored")
self.assertEqual(inspector.GetFrameId(global_geometry), global_frame)
self.assertEqual(
inspector.GetName(geometry_id=global_geometry), "sphere1")
self.assertIsInstance(inspector.GetShape(geometry_id=global_geometry),
mut.Sphere)
self.assertIsInstance(
inspector.GetPoseInParent(geometry_id=global_geometry),
RigidTransform_[float])
self.assertIsInstance(
inspector.GetPoseInFrame(geometry_id=global_geometry),
RigidTransform_[float])
self.assertIsInstance(inspector.geometry_version(),
mut.GeometryVersion)
# Check AssignRole bits.
proximity = mut.ProximityProperties()
perception = mut.PerceptionProperties()
perception.AddProperty("label", "id", mut.render.RenderLabel(0))
illustration = mut.IllustrationProperties()
props = [
proximity,
perception,
illustration,
]
context = scene_graph.CreateDefaultContext()
for prop in props:
# Check SceneGraph mutating variant.
scene_graph.AssignRole(
source_id=global_source, geometry_id=global_geometry,
properties=prop, assign=mut.RoleAssign.kNew)
# Check Context mutating variant.
scene_graph.AssignRole(
context=context, source_id=global_source,
geometry_id=global_geometry, properties=prop,
assign=mut.RoleAssign.kNew)
# Check property accessors.
self.assertIsInstance(
inspector.GetProximityProperties(geometry_id=global_geometry),
mut.ProximityProperties)
self.assertIsInstance(
inspector.GetProperties(geometry_id=global_geometry,
role=mut.Role.kProximity),
mut.ProximityProperties)
self.assertIsInstance(
inspector.GetIllustrationProperties(geometry_id=global_geometry),
mut.IllustrationProperties)
self.assertIsInstance(
inspector.GetProperties(geometry_id=global_geometry,
role=mut.Role.kIllustration),
mut.IllustrationProperties)
self.assertIsInstance(
inspector.GetPerceptionProperties(geometry_id=global_geometry),
mut.PerceptionProperties)
self.assertIsInstance(
inspector.GetProperties(geometry_id=global_geometry,
role=mut.Role.kPerception),
mut.PerceptionProperties)
self.assertIsInstance(
inspector.CloneGeometryInstance(geometry_id=global_geometry),
mut.GeometryInstance)
self.assertTrue(inspector.CollisionFiltered(
geometry_id1=global_geometry, geometry_id2=global_geometry))
roles = [
mut.Role.kProximity,
mut.Role.kPerception,
mut.Role.kIllustration,
]
for role in roles:
self.assertEqual(
scene_graph.RemoveRole(
source_id=global_source, geometry_id=global_geometry,
role=role),
1)
def test_proximity_properties(self):
"""
Tests the utility functions (not related to hydroelastic contact) for
setting values in ProximityProperties (as defined in
proximity_properties.h).
"""
props = mut.ProximityProperties()
reference_friction = CoulombFriction(0.25, 0.125)
mut.AddContactMaterial(dissipation=2.7,
point_stiffness=3.9,
friction=reference_friction,
properties=props)
self.assertTrue(
props.HasProperty("material", "hunt_crossley_dissipation"))
self.assertEqual(
props.GetProperty("material", "hunt_crossley_dissipation"), 2.7)
self.assertTrue(
props.HasProperty("material", "point_contact_stiffness"))
self.assertEqual(
props.GetProperty("material", "point_contact_stiffness"), 3.9)
self.assertTrue(props.HasProperty("material", "coulomb_friction"))
stored_friction = props.GetProperty("material", "coulomb_friction")
self.assertEqual(stored_friction.static_friction(),
reference_friction.static_friction())
self.assertEqual(stored_friction.dynamic_friction(),
reference_friction.dynamic_friction())
props = mut.ProximityProperties()
res_hint = 0.175
E = 1e8
mut.AddRigidHydroelasticProperties(resolution_hint=res_hint,
properties=props)
self.assertTrue(props.HasProperty("hydroelastic", "compliance_type"))
self.assertFalse(mut_testing.PropertiesIndicateCompliantHydro(props))
self.assertTrue(props.HasProperty("hydroelastic", "resolution_hint"))
self.assertEqual(props.GetProperty("hydroelastic", "resolution_hint"),
res_hint)
props = mut.ProximityProperties()
mut.AddRigidHydroelasticProperties(properties=props)
self.assertTrue(props.HasProperty("hydroelastic", "compliance_type"))
self.assertFalse(mut_testing.PropertiesIndicateCompliantHydro(props))
self.assertFalse(props.HasProperty("hydroelastic", "resolution_hint"))
props = mut.ProximityProperties()
res_hint = 0.275
mut.AddCompliantHydroelasticProperties(resolution_hint=res_hint,
hydroelastic_modulus=E,
properties=props)
self.assertTrue(props.HasProperty("hydroelastic", "compliance_type"))
self.assertTrue(mut_testing.PropertiesIndicateCompliantHydro(props))
self.assertTrue(props.HasProperty("hydroelastic", "resolution_hint"))
self.assertEqual(props.GetProperty("hydroelastic", "resolution_hint"),
res_hint)
self.assertTrue(
props.HasProperty("hydroelastic", "hydroelastic_modulus"))
self.assertEqual(
props.GetProperty("hydroelastic", "hydroelastic_modulus"), E)
props = mut.ProximityProperties()
slab_thickness = 0.275
mut.AddCompliantHydroelasticPropertiesForHalfSpace(
slab_thickness=slab_thickness,
hydroelastic_modulus=E,
properties=props)
self.assertTrue(props.HasProperty("hydroelastic", "compliance_type"))
self.assertTrue(mut_testing.PropertiesIndicateCompliantHydro(props))
self.assertTrue(props.HasProperty("hydroelastic", "slab_thickness"))
self.assertEqual(props.GetProperty("hydroelastic", "slab_thickness"),
slab_thickness)
self.assertTrue(
props.HasProperty("hydroelastic", "hydroelastic_modulus"))
self.assertEqual(
props.GetProperty("hydroelastic", "hydroelastic_modulus"), E)