Skip to content

Commit

Permalink
[render] Deprecate ManipulationStation CameraProperties API (RobotLoc…
Browse files Browse the repository at this point in the history 
…omotion#14375)

* Deprecate ManipulationStation CameraProperties API

  - Deprecate the old RegisterRgbdSensor (update tests)
  - Add RegisterRgbdSensor that is compatible with RenderCamera.
    - add tests.
  - Refactor the default D415 camera (and actually *include* the full,
    documented intrinsics, differentiating between color and depth).
  - Update bindings
    - use the Class, cls_doc style to make it more compact
    - bind new methods, deprecate old.
    - update tests
  • Loading branch information
@SeanCurtis-TRI
SeanCurtis-TRI authored Nov 20, 2020
1 parent 68475b2 commit 48ec6c3
Show file tree
Hide file tree
Showing 6 changed files with 309 additions and 140 deletions.
2 changes: 2 additions & 0 deletions bindings/pydrake/examples/BUILD.bazel
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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",
],
Expand Down
208 changes: 108 additions & 100 deletions bindings/pydrake/examples/manipulation_station_py.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -47,106 +48,113 @@ PYBIND11_MODULE(manipulation_station, m) {
doc.SchunkCollisionModel.kBoxPlusFingertipSpheres.doc)
.export_values();

py::class_<ManipulationStation<T>, Diagram<T>>(
m, "ManipulationStation", doc.ManipulationStation.doc)
.def(py::init<double>(), py::arg("time_step") = 0.002,
doc.ManipulationStation.ctor.doc)
.def("SetupManipulationClassStation",
&ManipulationStation<T>::SetupManipulationClassStation,
py::arg("collision_model") = IiwaCollisionModel::kNoCollision,
py::arg("schunk_model") = SchunkCollisionModel::kBox,
doc.ManipulationStation.SetupManipulationClassStation.doc)
.def("SetupClutterClearingStation",
&ManipulationStation<T>::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<T>::SetupPlanarIiwaStation,
py::arg("schunk_model") = SchunkCollisionModel::kBox,
doc.ManipulationStation.SetupPlanarIiwaStation.doc)
.def("AddManipulandFromFile",
&ManipulationStation<T>::AddManipulandFromFile, py::arg("model_file"),
py::arg("X_WObject"),
doc.ManipulationStation.AddManipulandFromFile.doc)
.def("RegisterIiwaControllerModel",
&ManipulationStation<T>::RegisterIiwaControllerModel,
doc.ManipulationStation.RegisterIiwaControllerModel.doc)
.def("RegisterRgbdSensor", &ManipulationStation<T>::RegisterRgbdSensor,
doc.ManipulationStation.RegisterRgbdSensor.doc)
.def("RegisterWsgControllerModel",
&ManipulationStation<T>::RegisterWsgControllerModel,
doc.ManipulationStation.RegisterWsgControllerModel.doc)
.def("Finalize", py::overload_cast<>(&ManipulationStation<T>::Finalize),
doc.ManipulationStation.Finalize.doc_0args)
.def("num_iiwa_joints", &ManipulationStation<T>::num_iiwa_joints,
doc.ManipulationStation.num_iiwa_joints.doc)
.def("get_multibody_plant", &ManipulationStation<T>::get_multibody_plant,
py_rvp::reference_internal,
doc.ManipulationStation.get_multibody_plant.doc)
.def("get_mutable_multibody_plant",
&ManipulationStation<T>::get_mutable_multibody_plant,
py_rvp::reference_internal,
doc.ManipulationStation.get_mutable_multibody_plant.doc)
.def("get_scene_graph", &ManipulationStation<T>::get_scene_graph,
py_rvp::reference_internal,
doc.ManipulationStation.get_scene_graph.doc)
.def("get_mutable_scene_graph",
&ManipulationStation<T>::get_mutable_scene_graph,
py_rvp::reference_internal,
doc.ManipulationStation.get_mutable_scene_graph.doc)
.def("get_controller_plant",
&ManipulationStation<T>::get_controller_plant,
py_rvp::reference_internal,
doc.ManipulationStation.get_controller_plant.doc)
.def("GetIiwaPosition", &ManipulationStation<T>::GetIiwaPosition,
doc.ManipulationStation.GetIiwaPosition.doc)
.def("SetIiwaPosition",
overload_cast_explicit<void, systems::Context<T>*,
const Eigen::Ref<const VectorX<T>>&>(
&ManipulationStation<T>::SetIiwaPosition),
py::arg("station_context"), py::arg("q"),
doc.ManipulationStation.SetIiwaPosition.doc_2args)
.def("GetIiwaVelocity", &ManipulationStation<T>::GetIiwaVelocity,
doc.ManipulationStation.GetIiwaVelocity.doc)
.def("SetIiwaVelocity",
overload_cast_explicit<void, systems::Context<T>*,
const Eigen::Ref<const VectorX<T>>&>(
&ManipulationStation<T>::SetIiwaVelocity),
py::arg("station_context"), py::arg("v"),
doc.ManipulationStation.SetIiwaVelocity.doc_2args)
.def("GetWsgPosition", &ManipulationStation<T>::GetWsgPosition,
doc.ManipulationStation.GetWsgPosition.doc)
.def("SetWsgPosition",
overload_cast_explicit<void, systems::Context<T>*, const T&>(
&ManipulationStation<T>::SetWsgPosition),
py::arg("station_context"), py::arg("q"),
doc.ManipulationStation.SetWsgPosition.doc_2args)
.def("GetWsgVelocity", &ManipulationStation<T>::GetWsgVelocity,
doc.ManipulationStation.GetWsgVelocity.doc)
.def("SetWsgVelocity",
overload_cast_explicit<void, systems::Context<T>*, const T&>(
&ManipulationStation<T>::SetWsgVelocity),
py::arg("station_context"), py::arg("v"),
doc.ManipulationStation.SetWsgVelocity.doc_2args)
.def("GetStaticCameraPosesInWorld",
&ManipulationStation<T>::GetStaticCameraPosesInWorld,
py_rvp::reference_internal,
doc.ManipulationStation.GetStaticCameraPosesInWorld.doc)
.def("get_camera_names", &ManipulationStation<T>::get_camera_names,
doc.ManipulationStation.get_camera_names.doc)
.def("SetWsgGains", &ManipulationStation<T>::SetWsgGains,
doc.ManipulationStation.SetWsgGains.doc)
.def("SetIiwaPositionGains",
&ManipulationStation<T>::SetIiwaPositionGains,
doc.ManipulationStation.SetIiwaPositionGains.doc)
.def("SetIiwaVelocityGains",
&ManipulationStation<T>::SetIiwaVelocityGains,
doc.ManipulationStation.SetIiwaVelocityGains.doc)
.def("SetIiwaIntegralGains",
&ManipulationStation<T>::SetIiwaIntegralGains,
doc.ManipulationStation.SetIiwaIntegralGains.doc);
{
using Class = ManipulationStation<T>;
const auto& cls_doc = doc.ManipulationStation;

py::class_<Class, Diagram<T>> cls(m, "ManipulationStation", cls_doc.doc);
cls // BR
.def(py::init<double>(), 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 std::string&, const multibody::Frame<T>&,
const math::RigidTransform<double>&,
const geometry::render::DepthCameraProperties&>(
&Class::RegisterRgbdSensor)),
cls_doc.RegisterRgbdSensor.doc_single_properties);
#pragma GCC diagnostic pop

cls // BR
.def("RegisterRgbdSensor",
py::overload_cast<const std::string&, const multibody::Frame<T>&,
const math::RigidTransform<double>&,
const geometry::render::DepthRenderCamera&>(
&Class::RegisterRgbdSensor),
cls_doc.RegisterRgbdSensor.doc_single_camera)
.def("RegisterRgbdSensor",
py::overload_cast<const std::string&, const multibody::Frame<T>&,
const math::RigidTransform<double>&,
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<void, systems::Context<T>*,
const Eigen::Ref<const VectorX<T>>&>(&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<void, systems::Context<T>*,
const Eigen::Ref<const VectorX<T>>&>(&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<void, systems::Context<T>*, 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<void, systems::Context<T>*, 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_<ManipulationStationHardwareInterface, Diagram<double>>(m,
"ManipulationStationHardwareInterface",
Expand Down
48 changes: 48 additions & 0 deletions bindings/pydrake/examples/test/manipulation_station_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,17 +2,27 @@
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,
IiwaCollisionModel,
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):
Expand Down Expand Up @@ -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)
Loading

0 comments on commit 48ec6c3

Please sign in to comment.