manipulation setters take Context* as the first argument

Resolves RobotLocomotion#9741

bindings/pydrake/examples/BUILD.bazel

@@ -61,6 +61,7 @@ drake_pybind_library(
     name = "manipulation_station_py",
     cc_deps = [
         "//bindings/pydrake:documentation_pybind",
+        "//bindings/pydrake/util:deprecation_pybind",
     ],
     cc_srcs = ["manipulation_station_py.cc"],
     package_info = PACKAGE_INFO,

bindings/pydrake/examples/manipulation_station_py.cc

@@ -4,6 +4,7 @@
 
 #include "drake/bindings/pydrake/documentation_pybind.h"
 #include "drake/bindings/pydrake/pydrake_pybind.h"
+#include "drake/bindings/pydrake/util/deprecation_pybind.h"
 #include "drake/examples/manipulation_station/manipulation_station.h"
 #include "drake/examples/manipulation_station/manipulation_station_hardware_interface.h"  // noqa
 
@@ -69,20 +70,68 @@ PYBIND11_MODULE(manipulation_station, m) {
           doc.ManipulationStation.get_controller_plant.doc)
       .def("GetIiwaPosition", &ManipulationStation<T>::GetIiwaPosition,
           doc.ManipulationStation.GetIiwaPosition.doc)
-      .def("SetIiwaPosition", &ManipulationStation<T>::SetIiwaPosition,
-          doc.ManipulationStation.SetIiwaPosition.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_station_context_q)
+      .def("SetIiwaPosition",
+          [](ManipulationStation<T>* self,
+              const Eigen::Ref<const VectorX<T>>& q,
+              systems::Context<T>* context) {
+            WarnDeprecated(
+                "SetIiwaPosition(q, context) is deprecated.  Please use "
+                "(context, q) instead.");
+            self->SetIiwaPosition(context, q);
+          })
       .def("GetIiwaVelocity", &ManipulationStation<T>::GetIiwaVelocity,
           doc.ManipulationStation.GetIiwaVelocity.doc)
-      .def("SetIiwaVelocity", &ManipulationStation<T>::SetIiwaVelocity,
-          doc.ManipulationStation.SetIiwaVelocity.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_station_context_v)
+      .def("SetIiwaVelocity",
+          [](ManipulationStation<T>* self,
+              const Eigen::Ref<const VectorX<T>>& v,
+              systems::Context<T>* context) {
+            WarnDeprecated(
+                "SetIiwaVelocity(v, context) is deprecated.  Please use "
+                "(context, v) instead.");
+            self->SetIiwaVelocity(context, v);
+          })
       .def("GetWsgPosition", &ManipulationStation<T>::GetWsgPosition,
           doc.ManipulationStation.GetWsgPosition.doc)
-      .def("SetWsgPosition", &ManipulationStation<T>::SetWsgPosition,
-          doc.ManipulationStation.SetWsgPosition.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_station_context_q)
+      .def("SetWsgPosition",
+          [](ManipulationStation<T>* self, const T& q,
+              systems::Context<T>* context) {
+            WarnDeprecated(
+                "SetWsgPosition(q, context) is deprecated.  Please use "
+                "(context, q) instead.");
+            self->SetWsgPosition(context, q);
+          })
       .def("GetWsgVelocity", &ManipulationStation<T>::GetWsgVelocity,
           doc.ManipulationStation.GetWsgVelocity.doc)
-      .def("SetWsgVelocity", &ManipulationStation<T>::SetWsgVelocity,
-          doc.ManipulationStation.SetWsgVelocity.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_station_context_v)
+      .def("SetWsgVelocity",
+          [](ManipulationStation<T>* self, const T& v,
+              systems::Context<T>* context) {
+            WarnDeprecated(
+                "SetWsgVelocity(v, context) is deprecated.  Please use "
+                "(context, v) instead.");
+            self->SetWsgVelocity(context, v);
+          })
       .def("GetStaticCameraPosesInWorld",
           &ManipulationStation<T>::GetStaticCameraPosesInWorld,
           py_reference_internal,

bindings/pydrake/examples/test/manipulation_station_test.py

@@ -34,16 +34,16 @@ def test_manipulation_station(self):
         context = station.CreateDefaultContext()
         q = np.linspace(0.04, 0.6, num=7)
         v = np.linspace(-2.3, 0.5, num=7)
-        station.SetIiwaPosition(q, context)
+        station.SetIiwaPosition(context, q)
         np.testing.assert_array_equal(q, station.GetIiwaPosition(context))
-        station.SetIiwaVelocity(v, context)
+        station.SetIiwaVelocity(context, v)
         np.testing.assert_array_equal(v, station.GetIiwaVelocity(context))
 
         q = 4.23
         v = 8.51
-        station.SetWsgPosition(q, context)
+        station.SetWsgPosition(context, q)
         self.assertEqual(q, station.GetWsgPosition(context))
-        station.SetWsgVelocity(v, context)
+        station.SetWsgVelocity(context, v)
         self.assertEqual(v, station.GetWsgVelocity(context))
 
         station.GetStaticCameraPosesInWorld()["0"]

examples/manipulation_station/end_effector_teleop.py

@@ -335,12 +335,12 @@ def CopyPositionOut(self, context, output):
     # Set the initial positions of the IIWA to a comfortable configuration
     # inside the workspace of the station.
     q0 = [0, 0.6, 0, -1.75, 0, 1.0, 0]
-    station.SetIiwaPosition(q0, station_context)
-    station.SetIiwaVelocity(np.zeros(7), station_context)
+    station.SetIiwaPosition(station_context, q0)
+    station.SetIiwaVelocity(station_context, np.zeros(7))
 
     # Set the initial configuration of the gripper to open.
-    station.SetWsgPosition(0.1, station_context)
-    station.SetWsgVelocity(0, station_context)
+    station.SetWsgPosition(station_context, 0.1)
+    station.SetWsgVelocity(station_context, 0)
 
     # Place the object in the middle of the workspace.
     X_WObject = Isometry3.Identity()

examples/manipulation_station/joint_teleop.py

@@ -94,12 +94,12 @@
     # Set the initial positions of the IIWA to a comfortable configuration
     # inside the workspace of the station.
     q0 = [0, 0.6, 0, -1.75, 0, 1.0, 0]
-    station.SetIiwaPosition(q0, station_context)
-    station.SetIiwaVelocity(np.zeros(7), station_context)
+    station.SetIiwaPosition(station_context, q0)
+    station.SetIiwaVelocity(station_context, np.zeros(7))
 
     # Set the initial configuration of the gripper to open.
-    station.SetWsgPosition(0.1, station_context)
-    station.SetWsgVelocity(0, station_context)
+    station.SetWsgPosition(station_context, 0.1)
+    station.SetWsgVelocity(station_context, 0)
 
     # Place the object in the middle of the workspace.
     X_WObject = Isometry3.Identity()

examples/manipulation_station/manipulation_station.cc

@@ -523,8 +523,8 @@ VectorX<T> ManipulationStation<T>::GetIiwaPosition(
 
 template <typename T>
 void ManipulationStation<T>::SetIiwaPosition(
-    const Eigen::Ref<const drake::VectorX<T>>& q,
-    drake::systems::Context<T>* station_context) const {
+    drake::systems::Context<T>* station_context,
+    const Eigen::Ref<const drake::VectorX<T>>& q) const {
   const int num_iiwa_positions =
       plant_->num_positions(iiwa_model_.model_instance);
   DRAKE_DEMAND(station_context != nullptr);
@@ -553,8 +553,8 @@ VectorX<T> ManipulationStation<T>::GetIiwaVelocity(
 
 template <typename T>
 void ManipulationStation<T>::SetIiwaVelocity(
-    const Eigen::Ref<const drake::VectorX<T>>& v,
-    drake::systems::Context<T>* station_context) const {
+    drake::systems::Context<T>* station_context,
+    const Eigen::Ref<const drake::VectorX<T>>& v) const {
   const int num_iiwa_velocities =
       plant_->num_velocities(iiwa_model_.model_instance);
   DRAKE_DEMAND(station_context != nullptr);
@@ -600,7 +600,7 @@ T ManipulationStation<T>::GetWsgVelocity(
 
 template <typename T>
 void ManipulationStation<T>::SetWsgPosition(
-    const T& q, drake::systems::Context<T>* station_context) const {
+    drake::systems::Context<T>* station_context, const T& q) const {
   auto& plant_context =
       this->GetMutableSubsystemContext(*plant_, station_context);
 
@@ -624,7 +624,7 @@ void ManipulationStation<T>::SetWsgPosition(
 
 template <typename T>
 void ManipulationStation<T>::SetWsgVelocity(
-    const T& v, drake::systems::Context<T>* station_context) const {
+    drake::systems::Context<T>* station_context, const T& v) const {
   auto& plant_context =
       this->GetMutableSubsystemContext(*plant_, station_context);
 

examples/manipulation_station/manipulation_station.h

@@ -301,17 +301,31 @@ class ManipulationStation : public systems::Diagram<T> {
   /// Convenience method for setting all of the joint angles of the Kuka IIWA.
   /// Also sets the position history in the velocity command generator.
   /// @p q must have size num_iiwa_joints().
+  void SetIiwaPosition(systems::Context<T>* station_context,
+                       const Eigen::Ref<const VectorX<T>>& q) const;
+
+  DRAKE_DEPRECATED("Prefer the version with the Context as the first argument."
+                   "This method will be deleted after 2019-04-01.")
   void SetIiwaPosition(const Eigen::Ref<const VectorX<T>>& q,
-                       systems::Context<T>* station_context) const;
+                       systems::Context<T>* station_context) const {
+    SetIiwaPosition(station_context, q);
+  }
 
   /// Convenience method for getting all of the joint velocities of the Kuka
   // IIWA.  This does not include the gripper.
   VectorX<T> GetIiwaVelocity(const systems::Context<T>& station_context) const;
 
   /// Convenience method for setting all of the joint velocities of the Kuka
   /// IIWA. @v must have size num_iiwa_joints().
+  void SetIiwaVelocity(systems::Context<T>* station_context,
+                       const Eigen::Ref<const VectorX<T>>& v) const;
+
+  DRAKE_DEPRECATED("Prefer the version with the Context as the first argument."
+                   "This method will be deleted after 2019-04-01.")
   void SetIiwaVelocity(const Eigen::Ref<const VectorX<T>>& v,
-                       systems::Context<T>* station_context) const;
+                       systems::Context<T>* station_context) const {
+    SetIiwaVelocity(station_context, v);
+  }
 
   /// Convenience method for getting the position of the Schunk WSG. Note
   /// that the WSG position is the signed distance between the two fingers
@@ -325,10 +339,22 @@ class ManipulationStation : public systems::Diagram<T> {
   /// sets the position history in the velocity interpolator.  Note that the
   /// WSG position is the signed distance between the two fingers (not the
   /// state of the fingers individually).
-  void SetWsgPosition(const T& q, systems::Context<T>* station_context) const;
+  void SetWsgPosition(systems::Context<T>* station_context, const T& q) const;
+
+  DRAKE_DEPRECATED("Prefer the version with the Context as the first argument."
+                   "This method will be deleted after 2019-04-01.")
+  void SetWsgPosition(const T& q, systems::Context<T>* station_context) const {
+    SetWsgPosition(station_context, q);
+  }
 
   /// Convenience method for setting the velocity of the Schunk WSG.
-  void SetWsgVelocity(const T& v, systems::Context<T>* station_context) const;
+  void SetWsgVelocity(systems::Context<T>* station_context, const T& v) const;
+
+  DRAKE_DEPRECATED("Prefer the version with the Context as the first argument."
+                   "This method will be deleted after 2019-04-01.")
+  void SetWsgVelocity(const T& v, systems::Context<T>* station_context) const {
+    SetWsgVelocity(station_context, v);
+  }
 
   /// Returns a map from camera name to X_WCameraBody for all the static
   /// (rigidly attached to the world body) cameras that have been registered.

examples/manipulation_station/mock_station_simulation.cc

@@ -151,9 +151,9 @@ int do_main(int argc, char* argv[]) {
   q0 << 0, 0.6, 0, -1.75, 0, 1.0, 0;
   iiwa_command->set_initial_position(
       &diagram->GetMutableSubsystemContext(*iiwa_command, &context), q0);
-  station->SetIiwaPosition(q0, &station_context);
+  station->SetIiwaPosition(&station_context, q0);
   const VectorXd qdot0 = VectorXd::Zero(7);
-  station->SetIiwaVelocity(qdot0, &station_context);
+  station->SetIiwaVelocity(&station_context, qdot0);
 
   // Place the object in the center of the table in front of the robot.
   Eigen::Isometry3d pose = Eigen::Isometry3d::Identity();

examples/manipulation_station/proof_of_life.cc

@@ -81,9 +81,9 @@ int do_main(int argc, char* argv[]) {
   const int kNumDofIiwa = 7;
   VectorXd q0(kNumDofIiwa);
   q0 << 0, 0.6, 0, -1.75, 0, 1.0, 0;
-  station->SetIiwaPosition(q0, &station_context);
+  station->SetIiwaPosition(&station_context, q0);
   const VectorXd qdot0 = VectorXd::Zero(kNumDofIiwa);
-  station->SetIiwaVelocity(qdot0, &station_context);
+  station->SetIiwaVelocity(&station_context, qdot0);
 
   // Position command should hold the arm at the initial state.
   station_context.FixInputPort(

examples/manipulation_station/test/manipulation_station_test.cc

@@ -41,7 +41,7 @@ GTEST_TEST(ManipulationStationTest, CheckPlantBasics) {
            tau_ff = VectorXd::LinSpaced(7, 3.1, 3.7);
 
   // Set positions and read them back out, multiple ways.
-  station.SetIiwaPosition(q, context.get());
+  station.SetIiwaPosition(context.get(), q);
   EXPECT_TRUE(CompareMatrices(q, station.GetIiwaPosition(*context)));
   EXPECT_TRUE(CompareMatrices(q, station.GetOutputPort("iiwa_position_measured")
                                      .Eval<BasicVector<double>>(*context)
@@ -54,7 +54,7 @@ GTEST_TEST(ManipulationStationTest, CheckPlantBasics) {
   }
 
   // Set velocities and read them back out, multiple ways.
-  station.SetIiwaVelocity(v, context.get());
+  station.SetIiwaVelocity(context.get(), v);
   EXPECT_TRUE(CompareMatrices(v, station.GetIiwaVelocity(*context)));
   EXPECT_TRUE(
       CompareMatrices(v, station.GetOutputPort("iiwa_velocity_estimated")
@@ -215,10 +215,10 @@ GTEST_TEST(ManipulationStationTest, CheckWsg) {
   const double q = 0.023;
   const double v = 0.12;
 
-  station.SetWsgPosition(q, context.get());
+  station.SetWsgPosition(context.get(), q);
   EXPECT_EQ(station.GetWsgPosition(*context), q);
 
-  station.SetWsgVelocity(v, context.get());
+  station.SetWsgVelocity(context.get(), v);
   EXPECT_EQ(station.GetWsgVelocity(*context), v);
 
   EXPECT_TRUE(CompareMatrices(station.GetOutputPort("wsg_state_measured")