diff --git a/bindings/pydrake/examples/BUILD.bazel b/bindings/pydrake/examples/BUILD.bazel index e56d6271065d..43d3f446e44b 100644 --- a/bindings/pydrake/examples/BUILD.bazel +++ b/bindings/pydrake/examples/BUILD.bazel @@ -62,6 +62,7 @@ drake_pybind_library( name = "manipulation_station_py", cc_deps = [ "//bindings/pydrake:documentation_pybind", + "//bindings/pydrake/common:deprecation_pybind", ], cc_srcs = ["manipulation_station_py.cc"], package_info = PACKAGE_INFO, @@ -174,6 +175,7 @@ drake_py_unittest( name = "manipulation_station_test", deps = [ ":manipulation_station_py", + "//bindings/pydrake/common/test_utilities", "//bindings/pydrake/multibody:parsing_py", "//bindings/pydrake/multibody:plant_py", ], diff --git a/bindings/pydrake/examples/manipulation_station_py.cc b/bindings/pydrake/examples/manipulation_station_py.cc index 7a3eccc009e8..9abdae616849 100644 --- a/bindings/pydrake/examples/manipulation_station_py.cc +++ b/bindings/pydrake/examples/manipulation_station_py.cc @@ -2,6 +2,7 @@ #include "pybind11/pybind11.h" #include "pybind11/stl.h" +#include "drake/bindings/pydrake/common/deprecation_pybind.h" #include "drake/bindings/pydrake/documentation_pybind.h" #include "drake/bindings/pydrake/pydrake_pybind.h" #include "drake/examples/manipulation_station/manipulation_station.h" @@ -47,106 +48,113 @@ PYBIND11_MODULE(manipulation_station, m) { doc.SchunkCollisionModel.kBoxPlusFingertipSpheres.doc) .export_values(); - py::class_, Diagram>( - m, "ManipulationStation", doc.ManipulationStation.doc) - .def(py::init(), py::arg("time_step") = 0.002, - doc.ManipulationStation.ctor.doc) - .def("SetupManipulationClassStation", - &ManipulationStation::SetupManipulationClassStation, - py::arg("collision_model") = IiwaCollisionModel::kNoCollision, - py::arg("schunk_model") = SchunkCollisionModel::kBox, - doc.ManipulationStation.SetupManipulationClassStation.doc) - .def("SetupClutterClearingStation", - &ManipulationStation::SetupClutterClearingStation, - py::arg("X_WCameraBody") = std::nullopt, - py::arg("collision_model") = IiwaCollisionModel::kNoCollision, - py::arg("schunk_model") = SchunkCollisionModel::kBox, - doc.ManipulationStation.SetupClutterClearingStation.doc) - .def("SetupPlanarIiwaStation", - &ManipulationStation::SetupPlanarIiwaStation, - py::arg("schunk_model") = SchunkCollisionModel::kBox, - doc.ManipulationStation.SetupPlanarIiwaStation.doc) - .def("AddManipulandFromFile", - &ManipulationStation::AddManipulandFromFile, py::arg("model_file"), - py::arg("X_WObject"), - doc.ManipulationStation.AddManipulandFromFile.doc) - .def("RegisterIiwaControllerModel", - &ManipulationStation::RegisterIiwaControllerModel, - doc.ManipulationStation.RegisterIiwaControllerModel.doc) - .def("RegisterRgbdSensor", &ManipulationStation::RegisterRgbdSensor, - doc.ManipulationStation.RegisterRgbdSensor.doc) - .def("RegisterWsgControllerModel", - &ManipulationStation::RegisterWsgControllerModel, - doc.ManipulationStation.RegisterWsgControllerModel.doc) - .def("Finalize", py::overload_cast<>(&ManipulationStation::Finalize), - doc.ManipulationStation.Finalize.doc_0args) - .def("num_iiwa_joints", &ManipulationStation::num_iiwa_joints, - doc.ManipulationStation.num_iiwa_joints.doc) - .def("get_multibody_plant", &ManipulationStation::get_multibody_plant, - py_rvp::reference_internal, - doc.ManipulationStation.get_multibody_plant.doc) - .def("get_mutable_multibody_plant", - &ManipulationStation::get_mutable_multibody_plant, - py_rvp::reference_internal, - doc.ManipulationStation.get_mutable_multibody_plant.doc) - .def("get_scene_graph", &ManipulationStation::get_scene_graph, - py_rvp::reference_internal, - doc.ManipulationStation.get_scene_graph.doc) - .def("get_mutable_scene_graph", - &ManipulationStation::get_mutable_scene_graph, - py_rvp::reference_internal, - doc.ManipulationStation.get_mutable_scene_graph.doc) - .def("get_controller_plant", - &ManipulationStation::get_controller_plant, - py_rvp::reference_internal, - doc.ManipulationStation.get_controller_plant.doc) - .def("GetIiwaPosition", &ManipulationStation::GetIiwaPosition, - doc.ManipulationStation.GetIiwaPosition.doc) - .def("SetIiwaPosition", - overload_cast_explicit*, - const Eigen::Ref>&>( - &ManipulationStation::SetIiwaPosition), - py::arg("station_context"), py::arg("q"), - doc.ManipulationStation.SetIiwaPosition.doc_2args) - .def("GetIiwaVelocity", &ManipulationStation::GetIiwaVelocity, - doc.ManipulationStation.GetIiwaVelocity.doc) - .def("SetIiwaVelocity", - overload_cast_explicit*, - const Eigen::Ref>&>( - &ManipulationStation::SetIiwaVelocity), - py::arg("station_context"), py::arg("v"), - doc.ManipulationStation.SetIiwaVelocity.doc_2args) - .def("GetWsgPosition", &ManipulationStation::GetWsgPosition, - doc.ManipulationStation.GetWsgPosition.doc) - .def("SetWsgPosition", - overload_cast_explicit*, const T&>( - &ManipulationStation::SetWsgPosition), - py::arg("station_context"), py::arg("q"), - doc.ManipulationStation.SetWsgPosition.doc_2args) - .def("GetWsgVelocity", &ManipulationStation::GetWsgVelocity, - doc.ManipulationStation.GetWsgVelocity.doc) - .def("SetWsgVelocity", - overload_cast_explicit*, const T&>( - &ManipulationStation::SetWsgVelocity), - py::arg("station_context"), py::arg("v"), - doc.ManipulationStation.SetWsgVelocity.doc_2args) - .def("GetStaticCameraPosesInWorld", - &ManipulationStation::GetStaticCameraPosesInWorld, - py_rvp::reference_internal, - doc.ManipulationStation.GetStaticCameraPosesInWorld.doc) - .def("get_camera_names", &ManipulationStation::get_camera_names, - doc.ManipulationStation.get_camera_names.doc) - .def("SetWsgGains", &ManipulationStation::SetWsgGains, - doc.ManipulationStation.SetWsgGains.doc) - .def("SetIiwaPositionGains", - &ManipulationStation::SetIiwaPositionGains, - doc.ManipulationStation.SetIiwaPositionGains.doc) - .def("SetIiwaVelocityGains", - &ManipulationStation::SetIiwaVelocityGains, - doc.ManipulationStation.SetIiwaVelocityGains.doc) - .def("SetIiwaIntegralGains", - &ManipulationStation::SetIiwaIntegralGains, - doc.ManipulationStation.SetIiwaIntegralGains.doc); + { + using Class = ManipulationStation; + const auto& cls_doc = doc.ManipulationStation; + + py::class_> cls(m, "ManipulationStation", cls_doc.doc); + cls // BR + .def(py::init(), py::arg("time_step") = 0.002, cls_doc.ctor.doc) + .def("SetupManipulationClassStation", + &Class::SetupManipulationClassStation, + py::arg("collision_model") = IiwaCollisionModel::kNoCollision, + py::arg("schunk_model") = SchunkCollisionModel::kBox, + cls_doc.SetupManipulationClassStation.doc) + .def("SetupClutterClearingStation", &Class::SetupClutterClearingStation, + py::arg("X_WCameraBody") = std::nullopt, + py::arg("collision_model") = IiwaCollisionModel::kNoCollision, + py::arg("schunk_model") = SchunkCollisionModel::kBox, + cls_doc.SetupClutterClearingStation.doc) + .def("SetupPlanarIiwaStation", &Class::SetupPlanarIiwaStation, + py::arg("schunk_model") = SchunkCollisionModel::kBox, + cls_doc.SetupPlanarIiwaStation.doc) + .def("AddManipulandFromFile", &Class::AddManipulandFromFile, + py::arg("model_file"), py::arg("X_WObject"), + cls_doc.AddManipulandFromFile.doc) + .def("RegisterIiwaControllerModel", &Class::RegisterIiwaControllerModel, + cls_doc.RegisterIiwaControllerModel.doc); + +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + cls.def("RegisterRgbdSensor", + WrapDeprecated(cls_doc.RegisterRgbdSensor.doc_single_properties, + py::overload_cast&, + const math::RigidTransform&, + const geometry::render::DepthCameraProperties&>( + &Class::RegisterRgbdSensor)), + cls_doc.RegisterRgbdSensor.doc_single_properties); +#pragma GCC diagnostic pop + + cls // BR + .def("RegisterRgbdSensor", + py::overload_cast&, + const math::RigidTransform&, + const geometry::render::DepthRenderCamera&>( + &Class::RegisterRgbdSensor), + cls_doc.RegisterRgbdSensor.doc_single_camera) + .def("RegisterRgbdSensor", + py::overload_cast&, + const math::RigidTransform&, + const geometry::render::ColorRenderCamera&, + const geometry::render::DepthRenderCamera&>( + &Class::RegisterRgbdSensor), + cls_doc.RegisterRgbdSensor.doc_dual_camera) + .def("RegisterWsgControllerModel", &Class::RegisterWsgControllerModel, + cls_doc.RegisterWsgControllerModel.doc) + .def("Finalize", py::overload_cast<>(&Class::Finalize), + cls_doc.Finalize.doc_0args) + .def("num_iiwa_joints", &Class::num_iiwa_joints, + cls_doc.num_iiwa_joints.doc) + .def("get_multibody_plant", &Class::get_multibody_plant, + py_rvp::reference_internal, cls_doc.get_multibody_plant.doc) + .def("get_mutable_multibody_plant", &Class::get_mutable_multibody_plant, + py_rvp::reference_internal, cls_doc.get_mutable_multibody_plant.doc) + .def("get_scene_graph", &Class::get_scene_graph, + py_rvp::reference_internal, cls_doc.get_scene_graph.doc) + .def("get_mutable_scene_graph", &Class::get_mutable_scene_graph, + py_rvp::reference_internal, cls_doc.get_mutable_scene_graph.doc) + .def("get_controller_plant", &Class::get_controller_plant, + py_rvp::reference_internal, cls_doc.get_controller_plant.doc) + .def("GetIiwaPosition", &Class::GetIiwaPosition, + cls_doc.GetIiwaPosition.doc) + .def("SetIiwaPosition", + overload_cast_explicit*, + const Eigen::Ref>&>(&Class::SetIiwaPosition), + py::arg("station_context"), py::arg("q"), + cls_doc.SetIiwaPosition.doc_2args) + .def("GetIiwaVelocity", &Class::GetIiwaVelocity, + cls_doc.GetIiwaVelocity.doc) + .def("SetIiwaVelocity", + overload_cast_explicit*, + const Eigen::Ref>&>(&Class::SetIiwaVelocity), + py::arg("station_context"), py::arg("v"), + cls_doc.SetIiwaVelocity.doc_2args) + .def("GetWsgPosition", &Class::GetWsgPosition, + cls_doc.GetWsgPosition.doc) + .def("SetWsgPosition", + overload_cast_explicit*, const T&>( + &Class::SetWsgPosition), + py::arg("station_context"), py::arg("q"), + cls_doc.SetWsgPosition.doc_2args) + .def("GetWsgVelocity", &Class::GetWsgVelocity, + cls_doc.GetWsgVelocity.doc) + .def("SetWsgVelocity", + overload_cast_explicit*, const T&>( + &Class::SetWsgVelocity), + py::arg("station_context"), py::arg("v"), + cls_doc.SetWsgVelocity.doc_2args) + .def("GetStaticCameraPosesInWorld", &Class::GetStaticCameraPosesInWorld, + py_rvp::reference_internal, cls_doc.GetStaticCameraPosesInWorld.doc) + .def("get_camera_names", &Class::get_camera_names, + cls_doc.get_camera_names.doc) + .def("SetWsgGains", &Class::SetWsgGains, cls_doc.SetWsgGains.doc) + .def("SetIiwaPositionGains", &Class::SetIiwaPositionGains, + cls_doc.SetIiwaPositionGains.doc) + .def("SetIiwaVelocityGains", &Class::SetIiwaVelocityGains, + cls_doc.SetIiwaVelocityGains.doc) + .def("SetIiwaIntegralGains", &Class::SetIiwaIntegralGains, + cls_doc.SetIiwaIntegralGains.doc); + } py::class_>(m, "ManipulationStationHardwareInterface", diff --git a/bindings/pydrake/examples/test/manipulation_station_test.py b/bindings/pydrake/examples/test/manipulation_station_test.py index 719b2fd488f7..5b31d19535c8 100644 --- a/bindings/pydrake/examples/test/manipulation_station_test.py +++ b/bindings/pydrake/examples/test/manipulation_station_test.py @@ -2,6 +2,7 @@ import numpy as np from pydrake.common import FindResourceOrThrow +from pydrake.common.test_utilities.deprecation import catch_drake_warnings from pydrake.examples.manipulation_station import ( CreateClutterClearingYcbObjectList, CreateManipulationClassYcbObjectList, @@ -9,10 +10,19 @@ ManipulationStation, ManipulationStationHardwareInterface ) +from pydrake.geometry.render import ( + ClippingRange, + ColorRenderCamera, + DepthCameraProperties, + DepthRange, + DepthRenderCamera, + RenderCameraCore, +) from pydrake.math import RigidTransform, RollPitchYaw from pydrake.multibody.plant import MultibodyPlant from pydrake.multibody.tree import ModelInstanceIndex from pydrake.multibody.parsing import Parser +from pydrake.systems.sensors import CameraInfo class TestManipulationStation(unittest.TestCase): @@ -167,3 +177,41 @@ def test_ycb_object_creation(self): ycb_objects = CreateManipulationClassYcbObjectList() self.assertEqual(len(ycb_objects), 5) + + def test_rgbd_sensor_registration(self): + X_PC = RigidTransform(p=[1, 2, 3]) + station = ManipulationStation(time_step=0.001) + station.SetupManipulationClassStation() + plant = station.get_multibody_plant() + color_camera = ColorRenderCamera( + RenderCameraCore("renderer", CameraInfo(10, 10, np.pi/4), + ClippingRange(0.1, 10.0), RigidTransform()), + False) + depth_camera = DepthRenderCamera(color_camera.core(), + DepthRange(0.1, 9.5)) + station.RegisterRgbdSensor("single_sensor", plant.world_frame(), X_PC, + depth_camera) + station.RegisterRgbdSensor("dual_sensor", plant.world_frame(), X_PC, + color_camera, depth_camera) + station.Finalize() + camera_poses = station.GetStaticCameraPosesInWorld() + # The three default cameras plus the two just added. + self.assertEqual(len(camera_poses), 5) + self.assertTrue("single_sensor" in camera_poses) + self.assertTrue("dual_sensor" in camera_poses) + + def test_rgbd_sensor_registration_deprecated(self): + X_PC = RigidTransform(p=[1, 2, 3]) + station = ManipulationStation(time_step=0.001) + station.SetupManipulationClassStation() + plant = station.get_multibody_plant() + properties = DepthCameraProperties(10, 10, np.pi / 4, "renderer", + 0.01, 5.0) + with catch_drake_warnings(expected_count=1): + station.RegisterRgbdSensor("deprecated", plant.world_frame(), X_PC, + properties) + station.Finalize() + camera_poses = station.GetStaticCameraPosesInWorld() + # The three default cameras plus the one just added. + self.assertEqual(len(camera_poses), 4) + self.assertTrue("deprecated" in camera_poses) diff --git a/examples/manipulation_station/manipulation_station.cc b/examples/manipulation_station/manipulation_station.cc index 7a99f0ef0098..adbcea1688a8 100644 --- a/examples/manipulation_station/manipulation_station.cc +++ b/examples/manipulation_station/manipulation_station.cc @@ -151,6 +151,41 @@ multibody::ModelInstanceIndex AddAndWeldModelFrom( return new_model; } +std::pair +MakeD415CameraModel(const std::string& renderer_name) { + // Typical D415 intrinsics for 848 x 480 resolution, note that rgb and + // depth are slightly different (in both intrinsics and relative to the + // camera body frame). + // RGB: + // - w: 848, h: 480, fx: 616.285, fy: 615.778, ppx: 405.418, ppy: 232.864 + // DEPTH: + // - w: 848, h: 480, fx: 645.138, fy: 645.138, ppx: 420.789, ppy: 239.13 + const int kHeight = 480; + const int kWidth = 848; + + // To pose the two sensors relative to the camera body, we'll assume X_BC = I, + // and select a representative value for X_CD drawn from calibration to define + // X_BD. + geometry::render::ColorRenderCamera color_camera{ + {renderer_name, + {kWidth, kHeight, 616.285, 615.778, 405.418, 232.864} /* intrinsics */, + {0.01, 3.0} /* clipping_range */, + {} /* X_BC */}, + false}; + const RigidTransformd X_BD( + RotationMatrix(RollPitchYaw( + -0.19 * M_PI / 180, -0.016 * M_PI / 180, -0.03 * M_PI / 180)), + Vector3d(0.015, -0.00019, -0.0001)); + geometry::render::DepthRenderCamera depth_camera{ + {renderer_name, + {kWidth, kHeight, 645.138, 645.138, 420.789, 239.13} /* intrinsics */, + {0.01, 3.0} /* clipping_range */, + X_BD}, + {0.1, 2.0} /* depth_range */}; + return {color_camera, depth_camera}; +} + } // namespace internal template @@ -214,27 +249,14 @@ void ManipulationStation::SetupClutterClearingStation( // Add the camera. { - // Typical D415 intrinsics for 848 x 480 resolution, note that rgb and - // depth are slightly different. And we are not able to model that at the - // moment. - // RGB: - // - w: 848, h: 480, fx: 616.285, fy: 615.778, ppx: 405.418, ppy: 232.864 - // DEPTH: - // - w: 848, h: 480, fx: 645.138, fy: 645.138, ppx: 420.789, ppy: 239.13 - // For this camera, we are going to assume that fx = fy, and we can compute - // fov_y by: fy = height / 2 / tan(fov_y / 2) - const double kFocalY = 645.; - const int kHeight = 480; - const int kWidth = 848; - const double fov_y = std::atan(kHeight / 2. / kFocalY) * 2; - geometry::render::DepthCameraProperties camera_properties( - kWidth, kHeight, fov_y, default_renderer_name_, 0.1, 2.0); + const auto& [color_camera, depth_camera] = + internal::MakeD415CameraModel(default_renderer_name_); RegisterRgbdSensor("0", plant_->world_frame(), X_WCameraBody.value_or(math::RigidTransform( math::RollPitchYaw(-0.3, 0.8, 1.5), Eigen::Vector3d(0, -1.5, 1.5))), - camera_properties); + color_camera, depth_camera); } AddDefaultIiwa(collision_model); @@ -290,24 +312,11 @@ void ManipulationStation::SetupManipulationClassStation( { std::map> camera_poses; internal::get_camera_poses(&camera_poses); - // Typical D415 intrinsics for 848 x 480 resolution, note that rgb and - // depth are slightly different. And we are not able to model that at the - // moment. - // RGB: - // - w: 848, h: 480, fx: 616.285, fy: 615.778, ppx: 405.418, ppy: 232.864 - // DEPTH: - // - w: 848, h: 480, fx: 645.138, fy: 645.138, ppx: 420.789, ppy: 239.13 - // For this camera, we are going to assume that fx = fy, and we can compute - // fov_y by: fy = height / 2 / tan(fov_y / 2) - const double kFocalY = 645.; - const int kHeight = 480; - const int kWidth = 848; - const double fov_y = std::atan(kHeight / 2. / kFocalY) * 2; - geometry::render::DepthCameraProperties camera_properties( - kWidth, kHeight, fov_y, default_renderer_name_, 0.1, 2.0); + const auto& [color_camera, depth_camera] = + internal::MakeD415CameraModel(default_renderer_name_); for (const auto& camera_pair : camera_poses) { RegisterRgbdSensor(camera_pair.first, plant_->world_frame(), - camera_pair.second, camera_properties); + camera_pair.second, color_camera, depth_camera); } } } @@ -685,9 +694,8 @@ void ManipulationStation::Finalize( MakeRenderEngineVtk(RenderEngineVtkParams())); } - for (const auto& info_pair : camera_information_) { - std::string camera_name = "camera_" + info_pair.first; - const CameraInformation& info = info_pair.second; + for (const auto& [name, info] : camera_information_) { + std::string camera_name = "camera_" + name; const std::optional parent_body_id = plant_->GetBodyFrameIdIfExists(info.parent_frame->body().index()); @@ -696,7 +704,7 @@ void ManipulationStation::Finalize( info.parent_frame->GetFixedPoseInBodyFrame() * info.X_PC; auto camera = builder.template AddSystem( - parent_body_id.value(), X_PC, info.properties); + parent_body_id.value(), X_PC, info.color_camera, info.depth_camera); builder.Connect(scene_graph_->get_query_output_port(), camera->query_object_input_port()); @@ -823,6 +831,9 @@ void ManipulationStation::SetWsgVelocity( velocities); } +// TODO(SeanCurtis-TRI) This method does not deserve the snake_case name. +// See https://drake.mit.edu/styleguide/cppguide.html#Function_Names +// Deprecate and rename. template std::vector ManipulationStation::get_camera_names() const { std::vector names; @@ -878,15 +889,40 @@ void ManipulationStation::RegisterWsgControllerModel( wsg_model_.model_instance = wsg_instance; } +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" template void ManipulationStation::RegisterRgbdSensor( const std::string& name, const multibody::Frame& parent_frame, const RigidTransform& X_PC, const geometry::render::DepthCameraProperties& properties) { + RegisterRgbdSensor(name, parent_frame, X_PC, + geometry::render::DepthRenderCamera(properties)); +} +#pragma GCC diagnostic pop + +template +void ManipulationStation::RegisterRgbdSensor( + const std::string& name, const multibody::Frame& parent_frame, + const RigidTransform& X_PC, + const geometry::render::DepthRenderCamera& depth_camera) { + RegisterRgbdSensor( + name, parent_frame, X_PC, + geometry::render::ColorRenderCamera(depth_camera.core(), false), + depth_camera); +} + +template +void ManipulationStation::RegisterRgbdSensor( + const std::string& name, const multibody::Frame& parent_frame, + const RigidTransform& X_PC, + const geometry::render::ColorRenderCamera& color_camera, + const geometry::render::DepthRenderCamera& depth_camera) { CameraInformation info; info.parent_frame = &parent_frame; info.X_PC = X_PC; - info.properties = properties; + info.depth_camera = depth_camera; + info.color_camera = color_camera; camera_information_[name] = info; } diff --git a/examples/manipulation_station/manipulation_station.h b/examples/manipulation_station/manipulation_station.h index 468dd8b85b73..8fbbf93b405b 100644 --- a/examples/manipulation_station/manipulation_station.h +++ b/examples/manipulation_station/manipulation_station.h @@ -260,7 +260,9 @@ class ManipulationStation : public systems::Diagram { const multibody::Frame& child_frame, const math::RigidTransform& X_PC); - /// Registers a RGBD sensor. Must be called before Finalize(). +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + /// Registers an RGBD sensor. Must be called before Finalize(). /// @param name Name for the camera. /// @param parent_frame The parent frame (frame P). The body that /// @p parent_frame is attached to must have a corresponding @@ -269,10 +271,41 @@ class ManipulationStation : public systems::Diagram { /// see systems::sensors:::RgbdSensor for descriptions about how the /// camera body, RGB, and depth image frames are related. /// @param properties Properties for the RGBD camera. + /// @pydrake_mkdoc_identifier{single_properties} + DRAKE_DEPRECATED("2021-03-01", + "CameraProperties are being deprecated. Please use the " + "DepthRenderCamera variant.") void RegisterRgbdSensor( const std::string& name, const multibody::Frame& parent_frame, const math::RigidTransform& X_PCameraBody, const geometry::render::DepthCameraProperties& properties); +#pragma GCC diagnostic pop + + /// Registers an RGBD sensor. Must be called before Finalize(). + /// @param name Name for the camera. + /// @param parent_frame The parent frame (frame P). The body that + /// @p parent_frame is attached to must have a corresponding + /// geometry::FrameId. Otherwise, an exception will be thrown in Finalize(). + /// @param X_PCameraBody Transformation between frame P and the camera body. + /// see systems::sensors:::RgbdSensor for descriptions about how the + /// camera body, RGB, and depth image frames are related. + /// @param depth_camera Specification for the RGBD camera. The color render + /// camera is inferred from the depth_camera. The color camera will share the + /// RenderCameraCore and be configured to *not* show its window. + /// @pydrake_mkdoc_identifier{single_camera} + void RegisterRgbdSensor( + const std::string& name, const multibody::Frame& parent_frame, + const math::RigidTransform& X_PCameraBody, + const geometry::render::DepthRenderCamera& depth_camera); + + /// Registers an RGBD sensor with uniquely characterized color/label and + /// depth cameras. + /// @pydrake_mkdoc_identifier{dual_camera} + void RegisterRgbdSensor( + const std::string& name, const multibody::Frame& parent_frame, + const math::RigidTransform& X_PCameraBody, + const geometry::render::ColorRenderCamera& color_camera, + const geometry::render::DepthRenderCamera& depth_camera); /// Adds a single object for the robot to manipulate /// @note Must be called before Finalize(). @@ -466,8 +499,14 @@ class ManipulationStation : public systems::Diagram { struct CameraInformation { const multibody::Frame* parent_frame{}; math::RigidTransform X_PC{math::RigidTransform::Identity()}; - geometry::render::DepthCameraProperties properties{ - 0, 0, 0, default_renderer_name_, 0, 0}; + geometry::render::ColorRenderCamera color_camera{ + {"", {2, 2, M_PI}, {0.1, 10}, {}}, // RenderCameraCore + false, // show_window + }; + geometry::render::DepthRenderCamera depth_camera{ + {"", {2, 2, M_PI}, {0.1, 10}, {}}, // RenderCameraCore + {0.1, 0.2}, // DepthRange + }; }; // Assumes iiwa_model_info_ and wsg_model_info_ have already being populated. diff --git a/examples/manipulation_station/test/manipulation_station_test.cc b/examples/manipulation_station/test/manipulation_station_test.cc index 3b215804f3be..e91e7721aef5 100644 --- a/examples/manipulation_station/test/manipulation_station_test.cc +++ b/examples/manipulation_station/test/manipulation_station_test.cc @@ -360,6 +360,39 @@ GTEST_TEST(ManipulationStationTest, RegisterRgbdCameraTest) { multibody::MultibodyPlant& plant = dut.get_mutable_multibody_plant(); + geometry::render::DepthRenderCamera depth_camera{ + {dut.default_renderer_name(), {640, 480, M_PI_4}, {0.05, 3.0}, {}}, + {0.1, 2.0}}; + + const Eigen::Translation3d X_WF0(0, 0, 0.2); + const Eigen::Translation3d X_F0C0(0.3, 0.2, 0.0); + const auto& frame0 = + plant.AddFrame(std::make_unique>( + "frame0", plant.world_frame(), X_WF0)); + dut.RegisterRgbdSensor("camera0", frame0, X_F0C0, depth_camera); + + const Eigen::Translation3d X_F0F1(0, -0.1, 0.2); + const Eigen::Translation3d X_F1C1(-0.2, 0.2, 0.33); + const auto& frame1 = + plant.AddFrame(std::make_unique>( + "frame1", frame0, X_F0F1)); + dut.RegisterRgbdSensor("camera1", frame1, X_F1C1, depth_camera); + + std::map> camera_poses = + dut.GetStaticCameraPosesInWorld(); + + EXPECT_EQ(camera_poses.size(), 2); + EXPECT_TRUE(camera_poses.at("camera0").IsExactlyEqualTo(X_WF0 * X_F0C0)); + EXPECT_TRUE( + camera_poses.at("camera1").IsExactlyEqualTo(X_WF0 * X_F0F1 * X_F1C1)); + } + + { + // Repeat the above test for the deprecated API. + ManipulationStation dut; + multibody::MultibodyPlant& plant = + dut.get_mutable_multibody_plant(); + geometry::render::DepthCameraProperties camera_properties( 640, 480, M_PI_4, dut.default_renderer_name(), 0.1, 2.0); @@ -368,6 +401,8 @@ GTEST_TEST(ManipulationStationTest, RegisterRgbdCameraTest) { const auto& frame0 = plant.AddFrame(std::make_unique>( "frame0", plant.world_frame(), X_WF0)); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" dut.RegisterRgbdSensor("camera0", frame0, X_F0C0, camera_properties); const Eigen::Translation3d X_F0F1(0, -0.1, 0.2); @@ -376,6 +411,7 @@ GTEST_TEST(ManipulationStationTest, RegisterRgbdCameraTest) { plant.AddFrame(std::make_unique>( "frame1", frame0, X_F0F1)); dut.RegisterRgbdSensor("camera1", frame1, X_F1C1, camera_properties); +#pragma GCC diagnostic pop std::map> camera_poses = dut.GetStaticCameraPosesInWorld();