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)