def test_collision_filtering(self):
sg = mut.SceneGraph()
sg_context = sg.CreateDefaultContext()
geometries = mut.GeometrySet()
# Mutate SceneGraph model
dut = sg.collision_filter_manager()
dut.Apply(
mut.CollisionFilterDeclaration().ExcludeBetween(
geometries, geometries))
dut.Apply(
mut.CollisionFilterDeclaration().ExcludeWithin(geometries))
# Mutate context data
dut = sg.collision_filter_manager(sg_context)
dut.Apply(
mut.CollisionFilterDeclaration().ExcludeBetween(
geometries, geometries))
dut.Apply(
mut.CollisionFilterDeclaration().ExcludeWithin(geometries))
# TODO(2021-11-01) Remove these with deprecation resolution.
# Legacy API
with catch_drake_warnings(expected_count=4):
sg.ExcludeCollisionsBetween(geometries, geometries)
sg.ExcludeCollisionsBetween(sg_context, geometries, geometries)
sg.ExcludeCollisionsWithin(geometries)
sg.ExcludeCollisionsWithin(sg_context, geometries)
def test_collision_filtering(self):
sg = mut.SceneGraph()
sg_context = sg.CreateDefaultContext()
geometries = mut.GeometrySet()
# Confirm that both invocations provide access.
for dut in (sg.collision_filter_manager(),
sg.collision_filter_manager(sg_context)):
self.assertIsInstance(dut, mut.CollisionFilterManager)
# We'll test against the Context-variant, assuming that if the API
# works for an instance from one source, it'll work for both.
dut = sg.collision_filter_manager(sg_context)
dut.Apply(declaration=mut.CollisionFilterDeclaration().ExcludeBetween(
geometries, geometries))
dut.Apply(declaration=mut.CollisionFilterDeclaration().ExcludeWithin(
geometries))
dut.Apply(declaration=mut.CollisionFilterDeclaration().AllowBetween(
set_A=geometries, set_B=geometries))
dut.Apply(declaration=mut.CollisionFilterDeclaration().AllowWithin(
geometry_set=geometries))
id = dut.ApplyTransient(declaration=mut.CollisionFilterDeclaration().
ExcludeWithin(geometries))
self.assertTrue(dut.has_transient_history())
self.assertTrue(dut.IsActive(filter_id=id))
self.assertTrue(dut.RemoveDeclaration(filter_id=id))
def test_collision_filtering(self):
sg = mut.SceneGraph()
sg_context = sg.CreateDefaultContext()
geometries = mut.GeometrySet()
sg.ExcludeCollisionsBetween(geometries, geometries)
sg.ExcludeCollisionsBetween(sg_context, geometries, geometries)
sg.ExcludeCollisionsWithin(geometries)
sg.ExcludeCollisionsWithin(sg_context, geometries)
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_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(len(inspector.GetGeometryIds(mut.GeometrySet())), 0)
self.assertEqual(len(inspector.GetGeometryIds(mut.GeometrySet(),
mut.Role.kProximity)),
0)
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)
