diff --git a/bindings/pydrake/geometry_py.cc b/bindings/pydrake/geometry_py.cc index f9f23abb4a8e..5b11af6dccf6 100644 --- a/bindings/pydrake/geometry_py.cc +++ b/bindings/pydrake/geometry_py.cc @@ -32,8 +32,27 @@ namespace drake { namespace pydrake { namespace { + +using Eigen::Vector3d; +using geometry::GeometryId; +using geometry::PerceptionProperties; +using geometry::Shape; +using geometry::render::CameraProperties; +using geometry::render::ColorRenderCamera; +using geometry::render::DepthCameraProperties; +using geometry::render::DepthRenderCamera; +using geometry::render::RenderEngine; +using math::RigidTransformd; using systems::Context; using systems::LeafSystem; +using systems::sensors::CameraInfo; +using systems::sensors::ColorD; +using systems::sensors::ColorI; +using systems::sensors::Image; +using systems::sensors::ImageDepth32F; +using systems::sensors::ImageLabel16I; +using systems::sensors::ImageRgba8U; +using systems::sensors::PixelType; template void BindIdentifier(py::module m, const std::string& name, const char* id_doc) { @@ -54,17 +73,176 @@ void BindIdentifier(py::module m, const std::string& name, const char* id_doc) { }); } +class PyRenderEngine : public py::wrapper { + public: + using Base = RenderEngine; + using BaseWrapper = py::wrapper; + PyRenderEngine() : BaseWrapper() {} + + void UpdateViewpoint(RigidTransformd const& X_WR) override { + PYBIND11_OVERLOAD_PURE(void, Base, UpdateViewpoint, X_WR); + } + + void RenderColorImage(CameraProperties const& camera, bool show_window, + ImageRgba8U* color_image_out) const override { + PYBIND11_OVERLOAD_PURE( + void, Base, RenderColorImage, camera, show_window, color_image_out); + } + + void RenderDepthImage(DepthCameraProperties const& camera, + ImageDepth32F* depth_image_out) const override { + PYBIND11_OVERLOAD_PURE( + void, Base, RenderDepthImage, camera, depth_image_out); + } + + void RenderLabelImage(CameraProperties const& camera, bool show_window, + ImageLabel16I* label_image_out) const override { + PYBIND11_OVERLOAD_PURE( + void, Base, RenderLabelImage, camera, show_window, label_image_out); + } + + bool DoRegisterVisual(GeometryId id, Shape const& shape, + PerceptionProperties const& properties, + RigidTransformd const& X_WG) override { + PYBIND11_OVERLOAD_PURE( + bool, Base, DoRegisterVisual, id, shape, properties, X_WG); + } + + void DoUpdateVisualPose(GeometryId id, RigidTransformd const& X_WG) override { + PYBIND11_OVERLOAD_PURE(void, Base, DoUpdateVisualPose, id, X_WG); + } + + bool DoRemoveGeometry(GeometryId id) override { + PYBIND11_OVERLOAD_PURE(bool, Base, DoRemoveGeometry, id); + } + + std::unique_ptr DoClone() const override { + PYBIND11_OVERLOAD_PURE(std::unique_ptr, Base, DoClone); + } + + void DoRenderColorImage(ColorRenderCamera const& camera, + ImageRgba8U* color_image_out) const override { + PYBIND11_OVERLOAD(void, Base, DoRenderColorImage, camera, color_image_out); + } + + void DoRenderDepthImage(DepthRenderCamera const& camera, + ImageDepth32F* depth_image_out) const override { + PYBIND11_OVERLOAD(void, Base, DoRenderDepthImage, camera, depth_image_out); + } + + void DoRenderLabelImage(ColorRenderCamera const& camera, + ImageLabel16I* label_image_out) const override { + PYBIND11_OVERLOAD(void, Base, DoRenderLabelImage, camera, label_image_out); + } + + void SetDefaultLightPosition(Vector3d const& X_DL) override { + PYBIND11_OVERLOAD(void, Base, SetDefaultLightPosition, X_DL); + } + + using Base::DoClone; + using Base::DoRegisterVisual; + using Base::DoRemoveGeometry; + using Base::DoRenderColorImage; + using Base::DoRenderDepthImage; + using Base::DoRenderLabelImage; + using Base::DoUpdateVisualPose; + using Base::GetColorDFromLabel; + using Base::GetColorIFromLabel; + using Base::GetRenderLabelOrThrow; + using Base::LabelFromColor; + using Base::SetDefaultLightPosition; + + template + static void ThrowIfInvalid(const systems::sensors::CameraInfo& intrinsics, + const ImageType* image, const char* image_type) { + return Base::ThrowIfInvalid(intrinsics, image, image_type); + } +}; + void def_geometry_render(py::module m) { // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. using namespace drake; // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. + using namespace drake::geometry; + // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace. using namespace drake::geometry::render; m.doc() = "Local bindings for `drake::geometry::render`"; constexpr auto& doc = pydrake_doc.drake.geometry.render; + { + using Class = ClippingRange; + const auto& cls_doc = doc.ClippingRange; + py::class_(m, "ClippingRange", cls_doc.doc) + .def(py::init(), cls_doc.ctor.doc) + .def(py::init(), cls_doc.near.doc, cls_doc.far.doc) + .def("far", static_cast(&Class::far), + cls_doc.far.doc) + .def("near", static_cast(&Class::near), + cls_doc.near.doc); + } + { + using Class = ColorRenderCamera; + const auto& cls_doc = doc.ColorRenderCamera; + py::class_ cls(m, "ColorRenderCamera", cls_doc.doc); + cls // BR + .def(py::init(), cls_doc.ctor.doc) + .def(py::init(), cls_doc.core.doc, + cls_doc.show_window.doc) + .def("core", static_cast( + &Class::core)) + .def("show_window", + static_cast(&Class::show_window)); + DefCopyAndDeepCopy(&cls); + } + { + using Class = DepthRange; + py::class_ cls(m, "DepthRange"); + cls // BR + .def(py::init()) + .def(py::init()) + .def("max_depth", + static_cast(&Class::max_depth)) + .def("min_depth", + static_cast(&Class::min_depth)); + DefCopyAndDeepCopy(&cls); + } + { + using Class = DepthRenderCamera; + py::class_ cls(m, "DepthRenderCamera"); + cls // BR + .def(py::init()) + .def(py::init()) + .def("core", static_cast( + &Class::core)) + .def("depth_range", static_cast( + &Class::depth_range)); + DefCopyAndDeepCopy(&cls); + } + { + using Class = RenderCameraCore; + py::class_ cls(m, "RenderCameraCore"); + cls // BR + .def(py::init()) + .def(py::init<::std::string, CameraInfo, ClippingRange, + RigidTransformd>()) + .def("clipping", static_cast( + &Class::clipping)) + .def("intrinsics", static_cast( + &Class::intrinsics)) + .def("renderer_name", + static_cast<::std::string const& (Class::*)() const>( + &Class::renderer_name)) + .def("sensor_pose_in_camera_body", + static_cast( + &Class::sensor_pose_in_camera_body)); + DefCopyAndDeepCopy(&cls); + } + + // TODO(eric.cousineau): Deprecate these. { using Class = CameraProperties; - py::class_(m, "CameraProperties", doc.CameraProperties.doc) + py::class_ cls(m, "CameraProperties", doc.CameraProperties.doc); + cls // BR .def(py::init(), py::arg("width"), py::arg("height"), py::arg("fov_y"), py::arg("renderer_name"), doc.CameraProperties.ctor.doc) @@ -74,12 +252,14 @@ void def_geometry_render(py::module m) { .def_readwrite("fov_y", &Class::fov_y, doc.CameraProperties.fov_y.doc) .def_readwrite("renderer_name", &Class::renderer_name, doc.CameraProperties.renderer_name.doc); + DefCopyAndDeepCopy(&cls); } { using Class = DepthCameraProperties; - py::class_( - m, "DepthCameraProperties", doc.DepthCameraProperties.doc) + py::class_ cls( + m, "DepthCameraProperties", doc.DepthCameraProperties.doc); + cls // BR .def(py::init(), py::arg("width"), py::arg("height"), py::arg("fov_y"), py::arg("renderer_name"), py::arg("z_near"), py::arg("z_far"), @@ -88,14 +268,126 @@ void def_geometry_render(py::module m) { "z_near", &Class::z_near, doc.DepthCameraProperties.z_near.doc) .def_readwrite( "z_far", &Class::z_far, doc.DepthCameraProperties.z_far.doc); + DefCopyAndDeepCopy(&cls); } { - // TODO(SeanCurtis-TRI): Expose the full public API after the RenderIndex - // and GeometryIndex classes go away (everything will become GeometryId - // centric). using Class = RenderEngine; - py::class_(m, "RenderEngine"); + const auto& cls_doc = doc.RenderEngine; + py::class_(m, "RenderEngine") + .def(py::init<>(), cls_doc.ctor.doc) + .def("Clone", + static_cast<::std::unique_ptr (Class::*)() const>( + &Class::Clone), + cls_doc.Clone.doc) + .def("RegisterVisual", + static_cast( + &Class::RegisterVisual), + py::arg("id"), py::arg("shape"), py::arg("properties"), + py::arg("X_WG"), py::arg("needs_updates") = true, + cls_doc.RegisterVisual.doc) + .def("RemoveGeometry", + static_cast(&Class::RemoveGeometry), + py::arg("id"), cls_doc.RemoveGeometry.doc) + .def("has_geometry", + static_cast( + &Class::has_geometry), + py::arg("id"), cls_doc.has_geometry.doc) + .def("UpdateViewpoint", + static_cast( + &Class::UpdateViewpoint), + py::arg("X_WR"), cls_doc.UpdateViewpoint.doc) + .def("RenderColorImage", + static_cast(&Class::RenderColorImage), + py::arg("camera"), py::arg("color_image_out"), + cls_doc.RenderColorImage.doc_2args) + .def("RenderDepthImage", + static_cast(&Class::RenderDepthImage), + py::arg("camera"), py::arg("depth_image_out"), + cls_doc.RenderDepthImage.doc_was_unable_to_choose_unambiguous_names) + .def("RenderLabelImage", + static_cast(&Class::RenderLabelImage), + py::arg("camera"), py::arg("label_image_out"), + cls_doc.RenderLabelImage.doc_2args) + .def("default_render_label", + static_cast( + &Class::default_render_label), + cls_doc.default_render_label.doc) + .def("DoRegisterVisual", + static_cast( + &PyRenderEngine::DoRegisterVisual), + py::arg("id"), py::arg("shape"), py::arg("properties"), + py::arg("X_WG"), cls_doc.DoRegisterVisual.doc) + .def("DoUpdateVisualPose", + static_cast( + &PyRenderEngine::DoUpdateVisualPose), + py::arg("id"), py::arg("X_WG"), cls_doc.DoUpdateVisualPose.doc) + .def("DoRemoveGeometry", + static_cast( + &PyRenderEngine::DoRemoveGeometry), + py::arg("id"), cls_doc.DoRemoveGeometry.doc) + .def("DoClone", + static_cast<::std::unique_ptr (Class::*)() const>( + &PyRenderEngine::DoClone), + cls_doc.DoClone.doc) + .def("DoRenderColorImage", + static_cast(&PyRenderEngine::DoRenderColorImage), + py::arg("camera"), py::arg("color_image_out"), + cls_doc.DoRenderColorImage.doc) + .def("DoRenderDepthImage", + static_cast(&PyRenderEngine::DoRenderDepthImage), + py::arg("camera"), py::arg("depth_image_out"), + cls_doc.DoRenderLabelImage.doc) + .def("DoRenderLabelImage", + static_cast(&PyRenderEngine::DoRenderLabelImage), + py::arg("camera"), py::arg("label_image_out"), + cls_doc.DoRenderLabelImage.doc) + .def("GetRenderLabelOrThrow", + static_cast(&PyRenderEngine::GetRenderLabelOrThrow), + py::arg("properties"), cls_doc.GetRenderLabelOrThrow.doc) + .def_static("LabelFromColor", + static_cast( + &PyRenderEngine::LabelFromColor), + py::arg("color"), cls_doc.LabelFromColor.doc) + .def_static("GetColorIFromLabel", + static_cast( + &PyRenderEngine::GetColorIFromLabel), + py::arg("label"), cls_doc.GetColorIFromLabel.doc) + .def_static("GetColorDFromLabel", + static_cast( + &PyRenderEngine::GetColorDFromLabel), + py::arg("label"), cls_doc.GetColorDFromLabel.doc) + .def("SetDefaultLightPosition", + static_cast( + &PyRenderEngine::SetDefaultLightPosition), + py::arg("X_DL"), cls_doc.SetDefaultLightPosition.doc) + .def_static("ThrowIfInvalid", + static_cast const*, char const*)>( + &PyRenderEngine::ThrowIfInvalid>), + py::arg("intrinsics"), py::arg("image"), py::arg("image_type"), + cls_doc.ThrowIfInvalid.doc) + .def_static("ThrowIfInvalid", + static_cast const*, char const*)>( + &PyRenderEngine::ThrowIfInvalid>), + py::arg("intrinsics"), py::arg("image"), py::arg("image_type"), + cls_doc.ThrowIfInvalid.doc) + .def_static("ThrowIfInvalid", + static_cast const*, char const*)>( + &PyRenderEngine::ThrowIfInvalid>), + py::arg("intrinsics"), py::arg("image"), py::arg("image_type"), + cls_doc.ThrowIfInvalid.doc); } py::class_( @@ -589,7 +881,8 @@ void DoScalarIndependentDefinitions(py::module m) { { using Class = Rgba; constexpr auto& cls_doc = doc.Rgba; - py::class_(m, "Rgba", cls_doc.doc) + py::class_ cls(m, "Rgba", cls_doc.doc); + cls // BR .def(py::init(), py::arg("r"), py::arg("g"), py::arg("b"), py::arg("a") = 1., cls_doc.ctor.doc) .def("r", &Class::r, cls_doc.r.doc) @@ -604,6 +897,7 @@ void DoScalarIndependentDefinitions(py::module m) { return py::str("Rgba(r={}, g={}, b={}, a={})") .format(self.r(), self.g(), self.b(), self.a()); }); + DefCopyAndDeepCopy(&cls); AddValueInstantiation(m); } @@ -707,19 +1001,22 @@ void DoScalarIndependentDefinitions(py::module m) { { using Class = GeometryFrame; constexpr auto& cls_doc = doc.GeometryFrame; - py::class_(m, "GeometryFrame", cls_doc.doc) + py::class_ cls(m, "GeometryFrame", cls_doc.doc); + cls // BR .def(py::init(), py::arg("frame_name"), py::arg("frame_group_id") = 0, cls_doc.ctor.doc) .def("id", &Class::id, cls_doc.id.doc) .def("name", &Class::name, cls_doc.name.doc) .def("frame_group", &Class::frame_group, cls_doc.frame_group.doc); + DefCopyAndDeepCopy(&cls); } // GeometryInstance { using Class = GeometryInstance; constexpr auto& cls_doc = doc.GeometryInstance; - py::class_(m, "GeometryInstance", cls_doc.doc) + py::class_ cls(m, "GeometryInstance", cls_doc.doc); + cls // BR .def(py::init&, std::unique_ptr, const std::string&>(), py::arg("X_PG"), py::arg("shape"), py::arg("name"), @@ -754,6 +1051,7 @@ void DoScalarIndependentDefinitions(py::module m) { cls_doc.mutable_perception_properties.doc) .def("perception_properties", &Class::perception_properties, py_rvp::reference_internal, cls_doc.perception_properties.doc); + DefCopyAndDeepCopy(&cls); } { diff --git a/bindings/pydrake/test/geometry_test.py b/bindings/pydrake/test/geometry_test.py index c57de44e0837..9474a7badcd8 100644 --- a/bindings/pydrake/test/geometry_test.py +++ b/bindings/pydrake/test/geometry_test.py @@ -2,34 +2,33 @@ import pydrake.geometry._testing as mut_testing import copy -import sys import unittest -import warnings from math import pi import numpy as np from drake import lcmt_viewer_load_robot, lcmt_viewer_draw -from pydrake.autodiffutils import AutoDiffXd from pydrake.common import FindResourceOrThrow from pydrake.common.test_utilities import numpy_compare from pydrake.common.value import AbstractValue, Value from pydrake.lcm import DrakeLcm, Subscriber -from pydrake.math import RigidTransform_ -from pydrake.symbolic import Expression +from pydrake.math import RigidTransform, RigidTransform_ from pydrake.systems.analysis import ( Simulator_, ) from pydrake.systems.framework import ( + DiagramBuilder, DiagramBuilder_, InputPort_, OutputPort_, ) from pydrake.systems.sensors import ( ImageRgba8U, + ImageDepth16U, ImageDepth32F, - ImageLabel16I - ) + ImageLabel16I, + RgbdSensor, +) class TestGeometry(unittest.TestCase): @@ -43,7 +42,7 @@ def test_scene_graph_api(self, T): 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( + scene_graph.RegisterFrame( source_id=global_source, parent_id=global_frame, frame=mut.GeometryFrame("anchored_frame")) global_geometry = scene_graph.RegisterGeometry( @@ -51,12 +50,12 @@ def test_scene_graph_api(self, T): geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](), shape=mut.Sphere(1.), name="sphere1")) - global_geometry_2 = scene_graph.RegisterGeometry( + scene_graph.RegisterGeometry( source_id=global_source, geometry_id=global_geometry, geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](), shape=mut.Sphere(1.), name="sphere2")) - anchored_geometry = scene_graph.RegisterAnchoredGeometry( + scene_graph.RegisterAnchoredGeometry( source_id=global_source, geometry=mut.GeometryInstance(X_PG=RigidTransform_[float](), shape=mut.Sphere(1.), @@ -169,7 +168,6 @@ def test_connect_drake_visualizer(self): Simulator = Simulator_[T] lcm = DrakeLcm() role = mut.Role.kIllustration - test_prefix = "TEST_PREFIX_" def normal(builder, scene_graph): mut.ConnectDrakeVisualizer( @@ -629,3 +627,133 @@ def test_value_instantiations(self, T): Value[mut.QueryObject_[T]] Value[mut.Rgba] Value[mut.render.RenderLabel] + + def test_render_engine_api(self): + class DummyRenderEngine(mut.render.RenderEngine): + """Mirror of C++ DummyRenderEngine.""" + + # See comment below above the first assignment to `current_engine`. + 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.simple_color_count = 0 + self.simple_depth_count = 0 + self.simple_label_count = 0 + self.color_props = None + self.depth_props = None + self.label_props = None + + def UpdateViewpoint(self, X_WC): + DummyRenderEngine.latest_instance = self + self.X_WC = X_WC + + def DoRenderColorImage(self, camera, color_image_out): + DummyRenderEngine.latest_instance = self + self.simple_color_count += 1 + self.color_props = camera + + def DoRenderDepthImage(self, camera, depth_image_out): + DummyRenderEngine.latest_instance = self + self.simple_depth_count += 1 + self.depth_props = camera + + def DoRenderLabelImage(self, camera, label_image_out): + DummyRenderEngine.latest_instance = self + self.simple_label_count += 1 + self.label_props = camera + + 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.simple_color_count = copy.copy(self.simple_color_count) + new.simple_depth_count = copy.copy(self.simple_depth_count) + new.simple_label_count = copy.copy(self.simple_label_count) + new.color_props = copy.copy(self.color_props) + new.depth_props = copy.copy(self.depth_props) + new.label_props = copy.copy(self.label_props) + return new + + 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(), + properties=mut.render.DepthCameraProperties( + width=640, height=480, fov_y=np.pi/4, + renderer_name="renderer", z_near=0.1, z_far=5.0, + ), + camera_poses=RgbdSensor.CameraPoses(), + show_window=False, + )) + 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.simple_color_count, 1) + + image = sensor.depth_image_32F_output_port().Eval(sensor_context) + self.assertIsInstance(image, ImageDepth32F) + self.assertEqual(current_engine.simple_depth_count, 1) + + image = sensor.depth_image_16U_output_port().Eval(sensor_context) + self.assertIsInstance(image, ImageDepth16U) + self.assertEqual(current_engine.simple_depth_count, 2) + + image = sensor.label_image_output_port().Eval(sensor_context) + self.assertIsInstance(image, ImageLabel16I) + self.assertEqual(current_engine.simple_label_count, 1) + + # TODO(eric, duy): Test more properties.