Adds simple tutorial on the systems framework (RobotLocomotion#12009)

* Adds simple tutorial on the systems framework ported (and updated) from the underactuated notes appendix.
YinjieLin · Sep 12, 2019 · 264cd5f · 264cd5f
1 parent b474529
commit 264cd5f
Show file tree

Hide file tree

Showing 9 changed files with 460 additions and 9 deletions.
diff --git a/bindings/pydrake/examples/pendulum_py.cc b/bindings/pydrake/examples/pendulum_py.cc
@@ -33,7 +33,14 @@ PYBIND11_MODULE(pendulum, m) {
 
   py::class_<PendulumPlant<T>, LeafSystem<T>>(
       m, "PendulumPlant", doc.PendulumPlant.doc)
-      .def(py::init<>(), doc.PendulumPlant.ctor.doc);
+      .def(py::init<>(), doc.PendulumPlant.ctor.doc)
+      .def("get_input_port", &System<T>::get_input_port, py_reference_internal,
+          py::arg("port_index"),
+          pydrake_doc.drake.systems.System.get_input_port.doc)
+      .def("get_input_port", &PendulumPlant<T>::get_input_port,
+          py_reference_internal, doc.PendulumPlant.get_input_port.doc)
+      .def("get_state_output_port", &PendulumPlant<T>::get_state_output_port,
+          py_reference_internal, doc.PendulumPlant.get_state_output_port.doc);
 
   // TODO(russt): Remove custom bindings once #8096 is resolved.
   py::class_<PendulumInput<T>, BasicVector<T>>(

diff --git a/bindings/pydrake/examples/test/pendulum_test.py b/bindings/pydrake/examples/test/pendulum_test.py
@@ -14,6 +14,12 @@
 
 
 class TestPendulum(unittest.TestCase):
+    def test_plant(self):
+        pendulum = PendulumPlant()
+        self.assertEqual(pendulum.get_input_port(), pendulum.get_input_port(0))
+        self.assertEqual(pendulum.get_state_output_port(),
+                         pendulum.get_output_port(0))
+
     def test_input(self):
         input = PendulumInput()
         input.set_tau(1.)

diff --git a/bindings/pydrake/systems/primitives_py.cc b/bindings/pydrake/systems/primitives_py.cc
@@ -83,8 +83,25 @@ PYBIND11_MODULE(primitives, m) {
         .def("y0",
             overload_cast_explicit<const VectorXd&>(&AffineSystem<T>::y0),
             doc.AffineSystem.y0.doc)
+        // Wrap a few methods from the TimeVaryingAffineSystem parent class.
+        // TODO(russt): Move to TimeVaryingAffineSystem if/when that class is
+        // wrapped.
+        .def("get_input_port", &TimeVaryingAffineSystem<T>::get_input_port,
+            py_reference_internal,
+            doc.TimeVaryingAffineSystem.get_input_port.doc)
+        .def("get_output_port", &TimeVaryingAffineSystem<T>::get_output_port,
+            py_reference_internal,
+            doc.TimeVaryingAffineSystem.get_output_port.doc)
         .def("time_period", &AffineSystem<T>::time_period,
-            doc.TimeVaryingAffineSystem.time_period.doc);
+            doc.TimeVaryingAffineSystem.time_period.doc)
+        // Need to specifically redeclare the System to have both overloads
+        // available.
+        .def("get_input_port", &System<T>::get_input_port,
+            py_reference_internal, py::arg("port_index"),
+            pydrake_doc.drake.systems.System.get_input_port.doc)
+        .def("get_output_port", &System<T>::get_output_port,
+            py_reference_internal, py::arg("port_index"),
+            pydrake_doc.drake.systems.System.get_output_port.doc);
 
     DefineTemplateClassWithDefault<ConstantValueSource<T>, LeafSystem<T>>(
         m, "ConstantValueSource", GetPyParam<T>(), doc.ConstantValueSource.doc)

diff --git a/bindings/pydrake/systems/test/primitives_test.py b/bindings/pydrake/systems/test/primitives_test.py
@@ -183,6 +183,8 @@ def test_linear_affine_system(self):
         self.assertFalse(IsObservable(system))
 
         system = AffineSystem(A, B, f0, C, D, y0, .1)
+        self.assertEqual(system.get_input_port(0), system.get_input_port())
+        self.assertEqual(system.get_output_port(0), system.get_output_port())
         context = system.CreateDefaultContext()
         self.assertEqual(system.get_input_port(0).size(), 1)
         self.assertEqual(context.get_discrete_state_vector().size(), 2)

diff --git a/systems/controllers/pid_controller.cc b/systems/controllers/pid_controller.cc
@@ -39,10 +39,10 @@ PidController<T>::PidController(const MatrixX<double>& state_projection,
       state_projection_(state_projection),
       output_projection_(output_projection) {
   if (kp_.size() != kd_.size() || kd_.size() != ki_.size()) {
-    throw std::logic_error("Gains must have equal length: |Kp| = " +
-                           std::to_string(kp_.size()) + ", |Ki| = " +
-                           std::to_string(ki_.size()) + ", |Kd| = " +
-                           std::to_string(kd_.size()));
+    throw std::logic_error(
+        "Gains must have equal length: |Kp| = " + std::to_string(kp_.size()) +
+        ", |Ki| = " + std::to_string(ki_.size()) +
+        ", |Kd| = " + std::to_string(kd_.size()));
   }
   if (state_projection_.rows() != 2 * num_controlled_q_) {
     throw std::logic_error(
@@ -60,15 +60,19 @@ PidController<T>::PidController(const MatrixX<double>& state_projection,
   this->DeclareContinuousState(num_controlled_q_);
 
   output_index_control_ =
-      this->DeclareVectorOutputPort(BasicVector<T>(output_projection_.rows()),
+      this->DeclareVectorOutputPort("control",
+                                    BasicVector<T>(output_projection_.rows()),
                                     &PidController<T>::CalcControl)
           .get_index();
 
   input_index_state_ =
-      this->DeclareInputPort(kVectorValued, num_full_state_).get_index();
+      this->DeclareInputPort("estimated_state", kVectorValued, num_full_state_)
+          .get_index();
 
   input_index_desired_state_ =
-      this->DeclareInputPort(kVectorValued, 2 * num_controlled_q_).get_index();
+      this->DeclareInputPort("desired_state", kVectorValued,
+                             2 * num_controlled_q_)
+          .get_index();
 }
 
 template <typename T>

diff --git a/systems/controllers/pid_controller.h b/systems/controllers/pid_controller.h
@@ -26,6 +26,11 @@ namespace controllers {
  * </pre>
  * where `P_y` is the output projection matrix.
  *
+ * @system{PidController,
+ *    @input_port{estimated_state, desired_state},
+ *    @output_port{control}
+ * }
+ *
  * This system has one continuous state, which is the integral of position
  * error, two input ports: estimated state `x_in` and desired state `x_d`, and
  * one output port `y`. Note that this class assumes `|q_c| = |v_c|` and

diff --git a/systems/controllers/test/pid_controller_test.cc b/systems/controllers/test/pid_controller_test.cc
@@ -51,6 +51,15 @@ class PidControllerTest : public ::testing::Test {
   Vector3d error_rate_signal_{1.3, 0.9, 3.14};
 };
 
+TEST_F(PidControllerTest, PortNames) {
+  EXPECT_EQ(controller_.GetInputPort("estimated_state").get_index(),
+            controller_.get_input_port_estimated_state().get_index());
+  EXPECT_EQ(controller_.GetInputPort("desired_state").get_index(),
+            controller_.get_input_port_desired_state().get_index());
+  EXPECT_EQ(controller_.GetOutputPort("control").get_index(),
+            controller_.get_output_port_control().get_index());
+}
+
 // Tests getter methods for controller constants.
 TEST_F(PidControllerTest, Getters) {
   ASSERT_EQ(kp_, controller_.get_Kp_vector());

diff --git a/tutorials/BUILD.bazel b/tutorials/BUILD.bazel
@@ -25,4 +25,12 @@ drake_jupyter_py_binary(
     ],
 )
 
+drake_jupyter_py_binary(
+    name = "dynamical_systems",
+    add_test_rule = 1,
+    deps = [
+        "//bindings/pydrake",
+    ],
+)
+
 add_lint_tests()