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_geometry_instance_api(self):
RigidTransform = RigidTransform_[float]
geometry = mut.GeometryInstance(X_PG=RigidTransform(),
shape=mut.Sphere(1.),
name="sphere")
self.assertIsInstance(geometry.id(), mut.GeometryId)
geometry.set_pose(RigidTransform([1, 0, 0]))
self.assertIsInstance(geometry.pose(), RigidTransform)
self.assertIsInstance(geometry.shape(), mut.Shape)
self.assertIsInstance(geometry.release_shape(), mut.Shape)
self.assertEqual(geometry.name(), "sphere")
geometry.set_proximity_properties(mut.ProximityProperties())
geometry.set_illustration_properties(mut.IllustrationProperties())
geometry.set_perception_properties(mut.PerceptionProperties())
self.assertIsInstance(geometry.mutable_proximity_properties(),
mut.ProximityProperties)
self.assertIsInstance(geometry.proximity_properties(),
mut.ProximityProperties)
self.assertIsInstance(geometry.mutable_illustration_properties(),
mut.IllustrationProperties)
self.assertIsInstance(geometry.illustration_properties(),
mut.IllustrationProperties)
self.assertIsInstance(geometry.mutable_perception_properties(),
mut.PerceptionProperties)
self.assertIsInstance(geometry.perception_properties(),
mut.PerceptionProperties)
def test_geometry_properties_api(self):
# Test perception/ illustration properties (specifically Rgba).
test_vector = [0., 0., 1., 1.]
test_color = mut.Rgba(0., 0., 1., 1.)
phong_props = mut.MakePhongIllustrationProperties(test_vector)
self.assertIsInstance(phong_props, mut.IllustrationProperties)
actual_color = phong_props.GetProperty("phong", "diffuse")
self.assertEqual(actual_color, test_color)
# Ensure that we can create it manually.
phong_props = mut.IllustrationProperties()
phong_props.AddProperty("phong", "diffuse", test_color)
actual_color = phong_props.GetProperty("phong", "diffuse")
self.assertEqual(actual_color, test_color)
# Test proximity properties.
prop = mut.ProximityProperties()
self.assertEqual(str(prop), "[__default__]")
default_group = prop.default_group_name()
self.assertTrue(prop.HasGroup(group_name=default_group))
self.assertEqual(prop.num_groups(), 1)
self.assertTrue(default_group in prop.GetGroupNames())
prop.AddProperty(group_name=default_group, name="test", value=3)
self.assertTrue(prop.HasProperty(group_name=default_group,
name="test"))
self.assertEqual(
prop.GetProperty(group_name=default_group, name="test"), 3)
self.assertEqual(
prop.GetPropertyOrDefault(group_name=default_group,
name="empty",
default_value=5), 5)
group_values = prop.GetPropertiesInGroup(group_name=default_group)
for name, value in group_values.items():
self.assertIsInstance(name, str)
self.assertIsInstance(value, AbstractValue)
def test_geometry_properties_api(self):
self.assertIsInstance(
mut.MakePhongIllustrationProperties([0, 0, 1, 1]),
mut.IllustrationProperties)
prop = mut.ProximityProperties()
self.assertEqual(str(prop), "[__default__]")
default_group = prop.default_group_name()
self.assertTrue(prop.HasGroup(group_name=default_group))
self.assertEqual(prop.num_groups(), 1)
self.assertTrue(default_group in prop.GetGroupNames())
prop.AddProperty(group_name=default_group, name="test", value=3)
self.assertTrue(prop.HasProperty(group_name=default_group,
name="test"))
self.assertEqual(
prop.GetProperty(group_name=default_group, name="test"), 3)
self.assertEqual(
prop.GetPropertyOrDefault(group_name=default_group,
name="empty",
default_value=5), 5)
group_values = prop.GetPropertiesInGroup(group_name=default_group)
for name, value in group_values.items():
self.assertIsInstance(name, str)
self.assertIsInstance(value, AbstractValue)
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_frame"))
global_frame_2 = scene_graph.RegisterFrame(
source_id=global_source,
parent_id=global_frame,
frame=mut.GeometryFrame("anchored_frame"))
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="sphere"))
global_geometry_2 = scene_graph.RegisterGeometry(
source_id=global_source,
geometry_id=global_geometry,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere"))
anchored_geometry = scene_graph.RegisterAnchoredGeometry(
source_id=global_source,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere"))
self.assertIsInstance(scene_graph.get_source_pose_port(global_source),
InputPort)
self.assertIsInstance(scene_graph.get_pose_bundle_output_port(),
OutputPort)
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_frames(), 3)
self.assertEqual(inspector.num_sources(), 2)
self.assertEqual(inspector.num_geometries(), 3)
# 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.GetIllustrationProperties(geometry_id=global_geometry),
mut.IllustrationProperties)
self.assertIsInstance(
inspector.GetPerceptionProperties(geometry_id=global_geometry),
mut.PerceptionProperties)
def test_geometry_properties_api(self):
# Test perception/ illustration properties (specifically Rgba).
test_vector = [0., 0., 1., 1.]
test_color = mut.Rgba(0., 0., 1., 1.)
phong_props = mut.MakePhongIllustrationProperties(test_vector)
self.assertIsInstance(phong_props, mut.IllustrationProperties)
actual_color = phong_props.GetProperty("phong", "diffuse")
self.assertEqual(actual_color, test_color)
# Ensure that we can create it manually.
phong_props = mut.IllustrationProperties()
phong_props.AddProperty("phong", "diffuse", test_color)
actual_color = phong_props.GetProperty("phong", "diffuse")
self.assertEqual(actual_color, test_color)
# Test proximity properties.
prop = mut.ProximityProperties()
self.assertEqual(str(prop), "[__default__]")
default_group = prop.default_group_name()
self.assertTrue(prop.HasGroup(group_name=default_group))
self.assertEqual(prop.num_groups(), 1)
self.assertTrue(default_group in prop.GetGroupNames())
prop.AddProperty(group_name=default_group, name="test", value=3)
self.assertTrue(prop.HasProperty(group_name=default_group,
name="test"))
self.assertEqual(
prop.GetProperty(group_name=default_group, name="test"), 3)
self.assertEqual(
prop.GetPropertyOrDefault(
group_name=default_group, name="empty", default_value=5),
5)
group_values = prop.GetPropertiesInGroup(group_name=default_group)
for name, value in group_values.items():
self.assertIsInstance(name, str)
self.assertIsInstance(value, AbstractValue)
# Remove the property.
self.assertTrue(prop.RemoveProperty(group_name=default_group,
name="test"))
self.assertFalse(prop.HasProperty(group_name=default_group,
name="test"))
# Update a property.
prop.AddProperty(group_name=default_group, name="to_update", value=17)
self.assertTrue(prop.HasProperty(group_name=default_group,
name="to_update"))
self.assertEqual(
prop.GetProperty(group_name=default_group, name="to_update"), 17)
prop.UpdateProperty(group_name=default_group, name="to_update",
value=20)
self.assertTrue(prop.HasProperty(group_name=default_group,
name="to_update"))
self.assertEqual(
prop.GetProperty(group_name=default_group, name="to_update"),
20)
# Property copying.
for PropertyType in [mut.ProximityProperties,
mut.IllustrationProperties,
mut.PerceptionProperties]:
props = PropertyType()
props.AddProperty("g", "p", 10)
self.assertTrue(props.HasProperty("g", "p"))
props_copy = PropertyType(other=props)
self.assertTrue(props_copy.HasProperty("g", "p"))
props_copy2 = copy.copy(props)
self.assertTrue(props_copy2.HasProperty("g", "p"))
props_copy3 = copy.deepcopy(props)
self.assertTrue(props_copy3.HasProperty("g", "p"))
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_frame"))
scene_graph.RegisterFrame(
source_id=global_source, parent_id=global_frame,
frame=mut.GeometryFrame("anchored_frame"))
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"))
scene_graph.RegisterGeometry(
source_id=global_source, geometry_id=global_geometry,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere2"))
scene_graph.RegisterAnchoredGeometry(
source_id=global_source,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere3"))
self.assertIsInstance(
scene_graph.get_source_pose_port(global_source), InputPort)
self.assertIsInstance(
scene_graph.get_pose_bundle_output_port(), OutputPort)
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.all_frame_ids()), 3)
self.assertTrue(inspector.world_frame_id()
in inspector.all_frame_ids())
self.assertTrue(global_frame in inspector.all_frame_ids())
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)
self.assertEqual(
inspector.NumGeometriesWithRole(role=mut.Role.kUnassigned), 3)
self.assertEqual(inspector.NumDynamicGeometries(), 2)
self.assertEqual(inspector.NumAnchoredGeometries(), 1)
self.assertEqual(len(inspector.GetCollisionCandidates()), 0)
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_frame")
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), 0)
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)),
0)
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_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_frame"))
scene_graph.RegisterFrame(source_id=global_source,
parent_id=global_frame,
frame=mut.GeometryFrame("anchored_frame"))
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"))
scene_graph.RegisterGeometry(source_id=global_source,
geometry_id=global_geometry,
geometry=mut.GeometryInstance(
X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere2"))
scene_graph.RegisterAnchoredGeometry(source_id=global_source,
geometry=mut.GeometryInstance(
X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere3"))
self.assertIsInstance(scene_graph.get_source_pose_port(global_source),
InputPort)
self.assertIsInstance(scene_graph.get_pose_bundle_output_port(),
OutputPort)
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.all_frame_ids()), 3)
self.assertTrue(
inspector.world_frame_id() in inspector.all_frame_ids())
self.assertTrue(global_frame in inspector.all_frame_ids())
self.assertIsInstance(inspector.world_frame_id(), mut.FrameId)
self.assertEqual(inspector.num_geometries(), 3)
self.assertEqual(len(inspector.GetAllGeometryIds()), 3)
self.assertEqual(
inspector.NumGeometriesWithRole(role=mut.Role.kUnassigned), 3)
self.assertEqual(inspector.NumDynamicGeometries(), 2)
self.assertEqual(inspector.NumAnchoredGeometries(), 1)
self.assertEqual(len(inspector.GetCollisionCandidates()), 0)
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))
with catch_drake_warnings(expected_count=2):
self.assertTrue(inspector.SourceIsRegistered(id=global_source))
self.assertEqual(inspector.GetSourceName(source_id=global_source),
"anchored")
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_frame")
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),
0)
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)), 0)
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)
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_frame"))
scene_graph.RegisterFrame(
source_id=global_source, parent_id=global_frame,
frame=mut.GeometryFrame("anchored_frame"))
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"))
scene_graph.RegisterGeometry(
source_id=global_source, geometry_id=global_geometry,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere2"))
scene_graph.RegisterAnchoredGeometry(
source_id=global_source,
geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](),
shape=mut.Sphere(1.),
name="sphere3"))
self.assertIsInstance(
scene_graph.get_source_pose_port(global_source), InputPort)
self.assertIsInstance(
scene_graph.get_pose_bundle_output_port(), OutputPort)
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(inspector.num_geometries(), 3)
self.assertEqual(len(inspector.GetAllGeometryIds()), 3)
self.assertEqual(
inspector.NumGeometriesWithRole(role=mut.Role.kUnassigned), 3)
self.assertEqual(inspector.NumDynamicGeometries(), 2)
self.assertEqual(inspector.NumAnchoredGeometries(), 1)
self.assertTrue(inspector.SourceIsRegistered(id=global_source))
self.assertEqual(inspector.GetSourceName(id=global_source), "anchored")
self.assertEqual(inspector.GetFrameId(global_geometry), global_frame)
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)),
0)
self.assertEqual(
inspector.GetGeometryIdByName(frame_id=global_frame,
role=mut.Role.kUnassigned,
name="sphere1"),
global_geometry)
self.assertEqual(
inspector.GetName(frame_id=global_frame), "anchored_frame")
self.assertEqual(
inspector.GetName(geometry_id=global_geometry), "sphere1")
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.GetIllustrationProperties(geometry_id=global_geometry),
mut.IllustrationProperties)
self.assertIsInstance(
inspector.GetPerceptionProperties(geometry_id=global_geometry),
mut.PerceptionProperties)
self.assertIsInstance(
inspector.CloneGeometryInstance(geometry_id=global_geometry),
mut.GeometryInstance)
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)
