diff --git a/bindings/pydrake/geometry_py.cc b/bindings/pydrake/geometry_py.cc index fd197264a9d0..7ae74e592be6 100644 --- a/bindings/pydrake/geometry_py.cc +++ b/bindings/pydrake/geometry_py.cc @@ -1,3 +1,5 @@ +#include + #include "pybind11/eigen.h" #include "pybind11/eval.h" #include "pybind11/operators.h" @@ -14,6 +16,8 @@ #include "drake/geometry/geometry_frame.h" #include "drake/geometry/geometry_ids.h" #include "drake/geometry/geometry_instance.h" +#include "drake/geometry/geometry_properties.h" +#include "drake/geometry/geometry_roles.h" #include "drake/geometry/geometry_visualization.h" #include "drake/geometry/proximity/obj_to_surface_mesh.h" #include "drake/geometry/proximity/surface_mesh.h" @@ -27,6 +31,7 @@ namespace drake { namespace pydrake { namespace { +using systems::Context; using systems::LeafSystem; template @@ -126,6 +131,8 @@ void def_geometry_render(py::module m) { render_label.attr("kUnspecified") = RenderLabel::kUnspecified; render_label.attr("kMaxUnreserved") = RenderLabel::kMaxUnreserved; } + + AddValueInstantiation(m); } template @@ -177,60 +184,121 @@ void DoScalarDependentDefinitions(py::module m, T) { // SceneGraph { - auto cls = DefineTemplateClassWithDefault, LeafSystem>( - m, "SceneGraph", param, doc.SceneGraph.doc); + using Class = SceneGraph; + constexpr auto& cls_doc = doc.SceneGraph; + auto cls = DefineTemplateClassWithDefault>( + m, "SceneGraph", param, cls_doc.doc); cls // BR - .def(py::init<>(), doc.SceneGraph.ctor.doc) - .def("get_source_pose_port", &SceneGraph::get_source_pose_port, - py_reference_internal, doc.SceneGraph.get_source_pose_port.doc) + .def(py::init<>(), cls_doc.ctor.doc) + .def("get_source_pose_port", &Class::get_source_pose_port, + py_reference_internal, cls_doc.get_source_pose_port.doc) .def("get_pose_bundle_output_port", - [](SceneGraph* self) -> const systems::OutputPort& { + [](Class* self) -> const systems::OutputPort& { return self->get_pose_bundle_output_port(); }, - py_reference_internal, - doc.SceneGraph.get_pose_bundle_output_port.doc) - .def("get_query_output_port", &SceneGraph::get_query_output_port, - py_reference_internal, doc.SceneGraph.get_query_output_port.doc) - .def("model_inspector", &SceneGraph::model_inspector, - py_reference_internal, doc.SceneGraph.model_inspector.doc) + py_reference_internal, cls_doc.get_pose_bundle_output_port.doc) + .def("get_query_output_port", &Class::get_query_output_port, + py_reference_internal, cls_doc.get_query_output_port.doc) + .def("model_inspector", &Class::model_inspector, py_reference_internal, + cls_doc.model_inspector.doc) .def("RegisterSource", py::overload_cast( // BR - &SceneGraph::RegisterSource), - py::arg("name") = "", doc.SceneGraph.RegisterSource.doc) + &Class::RegisterSource), + py::arg("name") = "", cls_doc.RegisterSource.doc) .def("RegisterFrame", py::overload_cast( - &SceneGraph::RegisterFrame), + &Class::RegisterFrame), py::arg("source_id"), py::arg("frame"), - doc.SceneGraph.RegisterFrame.doc_2args) + cls_doc.RegisterFrame.doc_2args) .def("RegisterFrame", py::overload_cast( - &SceneGraph::RegisterFrame), + &Class::RegisterFrame), py::arg("source_id"), py::arg("parent_id"), py::arg("frame"), - doc.SceneGraph.RegisterFrame.doc_3args) + cls_doc.RegisterFrame.doc_3args) .def("RegisterGeometry", py::overload_cast>( - &SceneGraph::RegisterGeometry), + std::unique_ptr>(&Class::RegisterGeometry), py::arg("source_id"), py::arg("frame_id"), py::arg("geometry"), - doc.SceneGraph.RegisterGeometry - .doc_3args_source_id_frame_id_geometry) + cls_doc.RegisterGeometry.doc_3args_source_id_frame_id_geometry) .def("RegisterGeometry", py::overload_cast>( - &SceneGraph::RegisterGeometry), + std::unique_ptr>(&Class::RegisterGeometry), py::arg("source_id"), py::arg("geometry_id"), py::arg("geometry"), - doc.SceneGraph.RegisterGeometry - .doc_3args_source_id_geometry_id_geometry) + cls_doc.RegisterGeometry.doc_3args_source_id_geometry_id_geometry) .def("RegisterAnchoredGeometry", py::overload_cast>( - &SceneGraph::RegisterAnchoredGeometry), + &Class::RegisterAnchoredGeometry), py::arg("source_id"), py::arg("geometry"), - doc.SceneGraph.RegisterAnchoredGeometry.doc) - .def("AddRenderer", &SceneGraph::AddRenderer, + cls_doc.RegisterAnchoredGeometry.doc) + .def("AddRenderer", &Class::AddRenderer, py::arg("name"), + py::arg("renderer"), cls_doc.AddRenderer.doc) + .def("AddRenderer", WrapDeprecated("Deprecated", &Class::AddRenderer), py::arg("renderer_name"), py::arg("renderer"), - doc.SceneGraph.AddRenderer.doc) - .def_static("world_frame_id", &SceneGraph::world_frame_id, - doc.SceneGraph.world_frame_id.doc); + cls_doc.AddRenderer.doc) + .def("HasRenderer", &Class::HasRenderer, py::arg("name"), + cls_doc.HasRenderer.doc) + .def("RendererCount", &Class::RendererCount, cls_doc.RendererCount.doc) + // - Begin: AssignRole Overloads. + // - - Proximity. + .def("AssignRole", + [](Class& self, SourceId source_id, GeometryId geometry_id, + ProximityProperties properties, RoleAssign assign) { + self.AssignRole(source_id, geometry_id, properties, assign); + }, + py::arg("source_id"), py::arg("geometry_id"), py::arg("properties"), + py::arg("assign") = RoleAssign::kNew, + cls_doc.AssignRole.doc_proximity_direct) + .def("AssignRole", + [](Class& self, Context* context, SourceId source_id, + GeometryId geometry_id, ProximityProperties properties, + RoleAssign assign) { + self.AssignRole( + context, source_id, geometry_id, properties, assign); + }, + py::arg("context"), py::arg("source_id"), py::arg("geometry_id"), + py::arg("properties"), py::arg("assign") = RoleAssign::kNew, + cls_doc.AssignRole.doc_proximity_context) + // - - Perception. + .def("AssignRole", + [](Class& self, SourceId source_id, GeometryId geometry_id, + PerceptionProperties properties, RoleAssign assign) { + self.AssignRole(source_id, geometry_id, properties, assign); + }, + py::arg("source_id"), py::arg("geometry_id"), py::arg("properties"), + py::arg("assign") = RoleAssign::kNew, + cls_doc.AssignRole.doc_perception_direct) + .def("AssignRole", + [](Class& self, Context* context, SourceId source_id, + GeometryId geometry_id, PerceptionProperties properties, + RoleAssign assign) { + self.AssignRole( + context, source_id, geometry_id, properties, assign); + }, + py::arg("context"), py::arg("source_id"), py::arg("geometry_id"), + py::arg("properties"), py::arg("assign") = RoleAssign::kNew, + cls_doc.AssignRole.doc_perception_context) + // - - Illustration. + .def("AssignRole", + [](Class& self, SourceId source_id, GeometryId geometry_id, + IllustrationProperties properties, RoleAssign assign) { + self.AssignRole(source_id, geometry_id, properties, assign); + }, + py::arg("source_id"), py::arg("geometry_id"), py::arg("properties"), + py::arg("assign") = RoleAssign::kNew, + cls_doc.AssignRole.doc_illustration_direct) + .def("AssignRole", + [](Class& self, Context* context, SourceId source_id, + GeometryId geometry_id, IllustrationProperties properties, + RoleAssign assign) { + self.AssignRole( + context, source_id, geometry_id, properties, assign); + }, + py::arg("context"), py::arg("source_id"), py::arg("geometry_id"), + py::arg("properties"), py::arg("assign") = RoleAssign::kNew, + cls_doc.AssignRole.doc_illustration_context) + // - End: AssignRole Overloads. + .def_static("world_frame_id", &Class::world_frame_id, + cls_doc.world_frame_id.doc); } // FramePoseVector @@ -429,6 +497,15 @@ void DoScalarIndependentDefinitions(py::module m) { .value("kIllustration", Role::kIllustration, cls_doc.kIllustration.doc) .value("kPerception", Role::kPerception, cls_doc.kPerception.doc); } + + { + constexpr auto& cls_doc = doc.RoleAssign; + using Class = RoleAssign; + py::enum_(m, "RoleAssign", cls_doc.doc) + .value("kNew", Class::kNew, cls_doc.kNew.doc) + .value("kReplace", Class::kReplace, cls_doc.kReplace.doc); + } + m.def("ConnectDrakeVisualizer", py::overload_cast*, const SceneGraph&, lcm::DrakeLcmInterface*, geometry::Role>( @@ -530,9 +607,109 @@ void DoScalarIndependentDefinitions(py::module m) { "set_pose", &Class::set_pose, py::arg("X_PG"), cls_doc.set_pose.doc) .def("shape", &Class::shape, py_reference_internal, cls_doc.shape.doc) .def("release_shape", &Class::release_shape, cls_doc.release_shape.doc) - .def("name", &Class::name, cls_doc.name.doc); + .def("name", &Class::name, cls_doc.name.doc) + .def("set_proximity_properties", &Class::set_proximity_properties, + py::arg("properties"), cls_doc.set_proximity_properties.doc) + .def("set_illustration_properties", &Class::set_illustration_properties, + py::arg("properties"), cls_doc.set_illustration_properties.doc) + .def("set_perception_properties", &Class::set_perception_properties, + py::arg("properties"), cls_doc.set_perception_properties.doc) + .def("mutable_proximity_properties", + &Class::mutable_proximity_properties, py_reference_internal, + cls_doc.mutable_proximity_properties.doc) + .def("proximity_properties", &Class::proximity_properties, + py_reference_internal, cls_doc.proximity_properties.doc) + .def("mutable_illustration_properties", + &Class::mutable_illustration_properties, py_reference_internal, + cls_doc.mutable_illustration_properties.doc) + .def("illustration_properties", &Class::illustration_properties, + py_reference_internal, cls_doc.illustration_properties.doc) + .def("mutable_perception_properties", + &Class::mutable_perception_properties, py_reference_internal, + cls_doc.mutable_perception_properties.doc) + .def("perception_properties", &Class::perception_properties, + py_reference_internal, cls_doc.perception_properties.doc); + } + + { + using Class = GeometryProperties; + constexpr auto& cls_doc = doc.GeometryProperties; + py::handle abstract_value_cls = + py::module::import("pydrake.systems.framework").attr("AbstractValue"); + py::class_(m, "GeometryProperties", cls_doc.doc) + .def("HasGroup", &Class::HasGroup, py::arg("group_name"), + cls_doc.HasGroup.doc) + .def("num_groups", &Class::num_groups, cls_doc.num_groups.doc) + .def("GetPropertiesInGroup", + [](const Class& self, const std::string& group_name) { + py::dict out; + py::object py_self = py::cast(&self, py_reference); + for (auto& [name, abstract] : + self.GetPropertiesInGroup(group_name)) { + out[name.c_str()] = + py::cast(abstract.get(), py_reference_internal, py_self); + } + return out; + }, + py::arg("group_name"), cls_doc.GetPropertiesInGroup.doc) + .def("GetGroupNames", &Class::GetGroupNames, cls_doc.GetGroupNames.doc) + .def("AddProperty", + [abstract_value_cls](Class& self, const std::string& group_name, + const std::string& name, py::object value) { + py::object abstract = abstract_value_cls.attr("Make")(value); + self.AddPropertyAbstract( + group_name, name, abstract.cast()); + }, + py::arg("group_name"), py::arg("name"), py::arg("value"), + cls_doc.AddProperty.doc) + .def("HasProperty", &Class::HasProperty, py::arg("group_name"), + py::arg("name"), cls_doc.HasProperty.doc) + .def("GetProperty", + [](const Class& self, const std::string& group_name, + const std::string& name) { + py::object abstract = py::cast( + self.GetPropertyAbstract(group_name, name), py_reference); + return abstract.attr("get_value")(); + }, + py::arg("group_name"), py::arg("name"), cls_doc.GetProperty.doc) + .def("GetPropertyOrDefault", + [](const Class& self, const std::string& group_name, + const std::string& name, py::object default_value) { + // For now, ignore typing. This is less efficient, but eh, it's + // Python. + if (self.HasProperty(group_name, name)) { + py::object py_self = py::cast(&self, py_reference); + return py_self.attr("GetProperty")(group_name, name); + } else { + return default_value; + } + }, + py::arg("group_name"), py::arg("name"), py::arg("default_value"), + cls_doc.GetPropertyOrDefault.doc) + .def_static("default_group_name", &Class::default_group_name, + cls_doc.default_group_name.doc) + .def("__str__", + [](const Class& self) { + std::stringstream ss; + ss << self; + return ss.str(); + }, + "Returns formatted string."); } + py::class_( + m, "ProximityProperties", doc.ProximityProperties.doc) + .def(py::init(), doc.ProximityProperties.ctor.doc); + py::class_( + m, "IllustrationProperties", doc.IllustrationProperties.doc) + .def(py::init(), doc.IllustrationProperties.ctor.doc); + py::class_( + m, "PerceptionProperties", doc.PerceptionProperties.doc) + .def(py::init(), doc.PerceptionProperties.ctor.doc); + m.def("MakePhongIllustrationProperties", &MakePhongIllustrationProperties, + py_reference_internal, py::arg("diffuse"), + doc.MakePhongIllustrationProperties.doc); + m.def("ReadObjToSurfaceMesh", py::overload_cast( &geometry::ReadObjToSurfaceMesh), @@ -556,7 +733,9 @@ void def_geometry_all(py::module m) { PYBIND11_MODULE(geometry, m) { PYDRAKE_PREVENT_PYTHON3_MODULE_REIMPORT(m); + py::module::import("pydrake.systems.framework"); py::module::import("pydrake.systems.lcm"); + def_geometry(m); def_geometry_render(m.def_submodule("render")); def_geometry_all(m.def_submodule("all")); diff --git a/bindings/pydrake/test/geometry_test.py b/bindings/pydrake/test/geometry_test.py index 62afab859b16..e6603c4c2145 100644 --- a/bindings/pydrake/test/geometry_test.py +++ b/bindings/pydrake/test/geometry_test.py @@ -13,7 +13,12 @@ from pydrake.lcm import DrakeMockLcm from pydrake.math import RigidTransform_ from pydrake.symbolic import Expression -from pydrake.systems.framework import DiagramBuilder_, InputPort_, OutputPort_ +from pydrake.systems.framework import ( + AbstractValue, + DiagramBuilder_, + InputPort_, + OutputPort_, +) from pydrake.systems.sensors import ( ImageRgba8U, ImageDepth32F, @@ -62,6 +67,8 @@ def test_scene_graph_api(self, T): 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() @@ -69,6 +76,28 @@ def test_scene_graph_api(self, T): 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) + def test_connect_drake_visualizer(self): # Test visualization API. # Use a mockable so that we can make a smoke test without side @@ -212,6 +241,45 @@ def test_geometry_instance_api(self): 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): + 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_render_engine_vtk_params(self): # Confirm default construction of params. diff --git a/geometry/scene_graph.h b/geometry/scene_graph.h index 945d2e799361..fc9186c687b4 100644 --- a/geometry/scene_graph.h +++ b/geometry/scene_graph.h @@ -592,7 +592,9 @@ class SceneGraph final : public systems::LeafSystem { These methods include the model- and context-modifying variants. */ //@{ - /** Assigns the proximity role to the geometry indicated by `geometry_id`. */ + /** Assigns the proximity role to the geometry indicated by `geometry_id`. + @pydrake_mkdoc_identifier{proximity_direct} + */ void AssignRole(SourceId source_id, GeometryId geometry_id, ProximityProperties properties, RoleAssign assign = RoleAssign::kNew); @@ -600,12 +602,15 @@ class SceneGraph final : public systems::LeafSystem { /** systems::Context-modifying variant of @ref AssignRole(SourceId,GeometryId,ProximityProperties) "AssignRole()" for proximity properties. Rather than modifying %SceneGraph's model, it modifies - the copy of the model stored in the provided context. */ + the copy of the model stored in the provided context. + @pydrake_mkdoc_identifier{proximity_context} + */ void AssignRole(systems::Context* context, SourceId source_id, GeometryId geometry_id, ProximityProperties properties, RoleAssign assign = RoleAssign::kNew) const; /** Assigns the perception role to the geometry indicated by `geometry_id`. + @pydrake_mkdoc_identifier{perception_direct} */ void AssignRole(SourceId source_id, GeometryId geometry_id, PerceptionProperties properties, @@ -614,12 +619,15 @@ class SceneGraph final : public systems::LeafSystem { /** systems::Context-modifying variant of @ref AssignRole(SourceId,GeometryId,PerceptionProperties) "AssignRole()" for perception properties. Rather than modifying %SceneGraph's model, it modifies - the copy of the model stored in the provided context. */ + the copy of the model stored in the provided context. + @pydrake_mkdoc_identifier{perception_context} + */ void AssignRole(systems::Context* context, SourceId source_id, GeometryId geometry_id, PerceptionProperties properties, RoleAssign assign = RoleAssign::kNew) const; /** Assigns the illustration role to the geometry indicated by `geometry_id`. + @pydrake_mkdoc_identifier{illustration_direct} */ void AssignRole(SourceId source_id, GeometryId geometry_id, IllustrationProperties properties, @@ -628,7 +636,9 @@ class SceneGraph final : public systems::LeafSystem { /** systems::Context-modifying variant of @ref AssignRole(SourceId,GeometryId,IllustrationProperties) "AssignRole()" for illustration properties. Rather than modifying %SceneGraph's model, it - modifies the copy of the model stored in the provided context. */ + modifies the copy of the model stored in the provided context. + @pydrake_mkdoc_identifier{illustration_context} + */ void AssignRole(systems::Context* context, SourceId source_id, GeometryId geometry_id, IllustrationProperties properties, RoleAssign assign = RoleAssign::kNew) const;