def test_scene_graph_api(self): scene_graph = mut.SceneGraph() global_source = scene_graph.RegisterSource("anchored") 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 visualization API. # Use a mockable so that we can make a smoke test without side effects. lcm = DrakeMockLcm() for i in range(2): builder = DiagramBuilder() scene_graph = builder.AddSystem(mut.SceneGraph()) if i == 1: mut.ConnectDrakeVisualizer( builder=builder, scene_graph=scene_graph) else: mut.ConnectDrakeVisualizer( builder=builder, scene_graph=scene_graph, pose_bundle_output_port=( scene_graph.get_pose_bundle_output_port())) mut.DispatchLoadMessage( scene_graph=scene_graph, lcm=lcm)
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_id_uniqueness(self): """Every FrameId must be unique.""" scene_graph = mut.SceneGraph() f = mut.FrameId.get_new_id().get_value() g = mut.GeometryFrame("geometry").id().get_value() w = scene_graph.world_frame_id().get_value() self.assertNotEqual(f, g) self.assertNotEqual(w, f) self.assertNotEqual(w, g)
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_optimization(self): """Tests geometry::optimization bindings""" A = np.eye(3) b = [1.0, 1.0, 1.0] prog = MathematicalProgram() x = prog.NewContinuousVariables(3, "x") t = prog.NewContinuousVariables(1, "t") # Test Point. p = np.array([11.1, 12.2, 13.3]) point = mut.optimization.Point(p) self.assertEqual(point.ambient_dimension(), 3) np.testing.assert_array_equal(point.x(), p) point.set_x(x=2*p) np.testing.assert_array_equal(point.x(), 2*p) point.set_x(x=p) # Test HPolyhedron. hpoly = mut.optimization.HPolyhedron(A=A, b=b) self.assertEqual(hpoly.ambient_dimension(), 3) np.testing.assert_array_equal(hpoly.A(), A) np.testing.assert_array_equal(hpoly.b(), b) self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0)) hpoly.AddPointInSetConstraints(prog, x) with self.assertRaisesRegex( RuntimeError, ".*not implemented yet for HPolyhedron.*"): hpoly.ToShapeWithPose() h_box = mut.optimization.HPolyhedron.MakeBox( lb=[-1, -1, -1], ub=[1, 1, 1]) h_unit_box = mut.optimization.HPolyhedron.MakeUnitBox(dim=3) np.testing.assert_array_equal(h_box.A(), h_unit_box.A()) np.testing.assert_array_equal(h_box.b(), h_unit_box.b()) self.assertIsInstance( h_box.MaximumVolumeInscribedEllipsoid(), mut.optimization.Hyperellipsoid) np.testing.assert_array_almost_equal( h_box.ChebyshevCenter(), [0, 0, 0]) # Test Hyperellipsoid. ellipsoid = mut.optimization.Hyperellipsoid(A=A, center=b) self.assertEqual(ellipsoid.ambient_dimension(), 3) np.testing.assert_array_equal(ellipsoid.A(), A) np.testing.assert_array_equal(ellipsoid.center(), b) self.assertTrue(ellipsoid.PointInSet(x=b, tol=0.0)) ellipsoid.AddPointInSetConstraints(prog, x) shape, pose = ellipsoid.ToShapeWithPose() self.assertIsInstance(shape, mut.Ellipsoid) self.assertIsInstance(pose, RigidTransform) scale, witness = ellipsoid.MinimumUniformScalingToTouch(point) self.assertTrue(scale > 0.0) np.testing.assert_array_almost_equal(witness, p) e_ball = mut.optimization.Hyperellipsoid.MakeAxisAligned( radius=[1, 1, 1], center=b) np.testing.assert_array_equal(e_ball.A(), A) np.testing.assert_array_equal(e_ball.center(), b) e_ball2 = mut.optimization.Hyperellipsoid.MakeHypersphere( radius=1, center=b) np.testing.assert_array_equal(e_ball2.A(), A) np.testing.assert_array_equal(e_ball2.center(), b) e_ball3 = mut.optimization.Hyperellipsoid.MakeUnitBall(dim=3) np.testing.assert_array_equal(e_ball3.A(), A) np.testing.assert_array_equal(e_ball3.center(), [0, 0, 0]) # Test VPolytope. vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]]) vpoly = mut.optimization.VPolytope(vertices=vertices) self.assertEqual(vpoly.ambient_dimension(), 2) np.testing.assert_array_equal(vpoly.vertices(), vertices) self.assertTrue(vpoly.PointInSet(x=[1.0, 5.0], tol=1e-8)) vpoly.AddPointInSetConstraints(prog, x[0:2]) v_box = mut.optimization.VPolytope.MakeBox( lb=[-1, -1, -1], ub=[1, 1, 1]) self.assertTrue(v_box.PointInSet([0, 0, 0])) v_unit_box = mut.optimization.VPolytope.MakeUnitBox(dim=3) self.assertTrue(v_unit_box.PointInSet([0, 0, 0])) # Test remaining ConvexSet methods using these instances. self.assertIsInstance(hpoly.Clone(), mut.optimization.HPolyhedron) self.assertTrue(ellipsoid.IsBounded()) hpoly.AddPointInNonnegativeScalingConstraints(prog=prog, x=x, t=t[0]) # Test MakeFromSceneGraph methods. scene_graph = mut.SceneGraph() source_id = scene_graph.RegisterSource("source") frame_id = scene_graph.RegisterFrame( source_id=source_id, frame=mut.GeometryFrame("frame")) box_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=mut.GeometryInstance(X_PG=RigidTransform(), shape=mut.Box(1., 1., 1.), name="sphere")) sphere_geometry_id = scene_graph.RegisterGeometry( source_id=source_id, frame_id=frame_id, geometry=mut.GeometryInstance(X_PG=RigidTransform(), shape=mut.Sphere(1.), name="sphere")) context = scene_graph.CreateDefaultContext() pose_vector = mut.FramePoseVector() pose_vector.set_value(frame_id, RigidTransform()) scene_graph.get_source_pose_port(source_id).FixValue( context, pose_vector) query_object = scene_graph.get_query_output_port().Eval(context) H = mut.optimization.HPolyhedron( query_object=query_object, geometry_id=box_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(H.ambient_dimension(), 3) E = mut.optimization.Hyperellipsoid( query_object=query_object, geometry_id=sphere_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(E.ambient_dimension(), 3) P = mut.optimization.Point( query_object=query_object, geometry_id=sphere_geometry_id, reference_frame=scene_graph.world_frame_id(), maximum_allowable_radius=1.5) self.assertEqual(P.ambient_dimension(), 3) V = mut.optimization.VPolytope( query_object=query_object, geometry_id=box_geometry_id, reference_frame=scene_graph.world_frame_id()) self.assertEqual(V.ambient_dimension(), 3) # Test Iris. obstacles = mut.optimization.MakeIrisObstacles( query_object=query_object, reference_frame=scene_graph.world_frame_id()) options = mut.optimization.IrisOptions() options.require_sample_point_is_contained = True options.iteration_limit = 1 options.termination_threshold = 0.1 region = mut.optimization.Iris( obstacles=obstacles, sample=[2, 3.4, 5], domain=mut.optimization.HPolyhedron.MakeBox( lb=[-5, -5, -5], ub=[5, 5, 5]), options=options) self.assertIsInstance(region, mut.optimization.HPolyhedron) obstacles = [ mut.optimization.HPolyhedron.MakeUnitBox(3), mut.optimization.Hyperellipsoid.MakeUnitBall(3), mut.optimization.Point([0, 0, 0]), mut.optimization.VPolytope.MakeUnitBox(3)] region = mut.optimization.Iris( obstacles=obstacles, sample=[2, 3.4, 5], domain=mut.optimization.HPolyhedron.MakeBox( lb=[-5, -5, -5], ub=[5, 5, 5]), options=options) self.assertIsInstance(region, mut.optimization.HPolyhedron)
def test_render_engine_api(self): class DummyRenderEngine(mut.render.RenderEngine): """Mirror of C++ DummyRenderEngine.""" # See comment below about `rgbd_sensor_test.cc`. latest_instance = None def __init__(self, render_label=None): mut.render.RenderEngine.__init__(self) # N.B. We do not hide these because this is a test class. # Normally, you would want to hide this. self.force_accept = False self.registered_geometries = set() self.updated_ids = {} self.include_group_name = "in_test" self.X_WC = RigidTransform_[float]() self.color_count = 0 self.depth_count = 0 self.label_count = 0 self.color_camera = None self.depth_camera = None self.label_camera = None def UpdateViewpoint(self, X_WC): DummyRenderEngine.latest_instance = self self.X_WC = X_WC def ImplementGeometry(self, shape, user_data): DummyRenderEngine.latest_instance = self def DoRegisterVisual(self, id, shape, properties, X_WG): DummyRenderEngine.latest_instance = self mut.GetRenderLabelOrThrow(properties) if self.force_accept or properties.HasGroup( self.include_group_name ): self.registered_geometries.add(id) return True return False def DoUpdateVisualPose(self, id, X_WG): DummyRenderEngine.latest_instance = self self.updated_ids[id] = X_WG def DoRemoveGeometry(self, id): DummyRenderEngine.latest_instance = self self.registered_geometries.remove(id) def DoClone(self): DummyRenderEngine.latest_instance = self new = DummyRenderEngine() new.force_accept = copy.copy(self.force_accept) new.registered_geometries = copy.copy( self.registered_geometries) new.updated_ids = copy.copy(self.updated_ids) new.include_group_name = copy.copy(self.include_group_name) new.X_WC = copy.copy(self.X_WC) new.color_count = copy.copy(self.color_count) new.depth_count = copy.copy(self.depth_count) new.label_count = copy.copy(self.label_count) new.color_camera = copy.copy(self.color_camera) new.depth_camera = copy.copy(self.depth_camera) new.label_camera = copy.copy(self.label_camera) return new def DoRenderColorImage(self, camera, color_image_out): DummyRenderEngine.latest_instance = self self.color_count += 1 self.color_camera = camera def DoRenderDepthImage(self, camera, depth_image_out): DummyRenderEngine.latest_instance = self self.depth_count += 1 self.depth_camera = camera def DoRenderLabelImage(self, camera, label_image_out): DummyRenderEngine.latest_instance = self self.label_count += 1 self.label_camera = camera engine = DummyRenderEngine() self.assertIsInstance(engine, mut.render.RenderEngine) self.assertIsInstance(engine.Clone(), DummyRenderEngine) # Test implementation of C++ interface by using RgbdSensor. renderer_name = "renderer" builder = DiagramBuilder() scene_graph = builder.AddSystem(mut.SceneGraph()) # N.B. This passes ownership. scene_graph.AddRenderer(renderer_name, engine) sensor = builder.AddSystem(RgbdSensor( parent_id=scene_graph.world_frame_id(), X_PB=RigidTransform(), depth_camera=mut.render.DepthRenderCamera( mut.render.RenderCameraCore( renderer_name, CameraInfo(640, 480, np.pi/4), mut.render.ClippingRange(0.1, 5.0), RigidTransform()), mut.render.DepthRange(0.1, 5.0)))) builder.Connect( scene_graph.get_query_output_port(), sensor.query_object_input_port(), ) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() sensor_context = sensor.GetMyContextFromRoot(diagram_context) image = sensor.color_image_output_port().Eval(sensor_context) # N.B. Because there's context cloning going on under the hood, we # won't be interacting with our originally registered instance. # See `rgbd_sensor_test.cc` as well. current_engine = DummyRenderEngine.latest_instance self.assertIsNot(current_engine, engine) self.assertIsInstance(image, ImageRgba8U) self.assertEqual(current_engine.color_count, 1) image = sensor.depth_image_32F_output_port().Eval(sensor_context) self.assertIsInstance(image, ImageDepth32F) self.assertEqual(current_engine.depth_count, 1) image = sensor.depth_image_16U_output_port().Eval(sensor_context) self.assertIsInstance(image, ImageDepth16U) self.assertEqual(current_engine.depth_count, 2) image = sensor.label_image_output_port().Eval(sensor_context) self.assertIsInstance(image, ImageLabel16I) self.assertEqual(current_engine.label_count, 1)