suggested modifications

bindings/pydrake/systems/controllers_py.cc

@@ -9,6 +9,7 @@
 #include "drake/bindings/pydrake/symbolic_types_pybind.h"
 #include "drake/multibody/plant/multibody_plant.h"
 #include "drake/systems/controllers/dynamic_programming.h"
+#include "drake/systems/controllers/finite_horizon_linear_quadratic_regulator.h"
 #include "drake/systems/controllers/inverse_dynamics.h"
 #include "drake/systems/controllers/inverse_dynamics_controller.h"
 #include "drake/systems/controllers/linear_quadratic_regulator.h"
@@ -18,6 +19,18 @@
 namespace drake {
 namespace pydrake {
 
+template <typename PyClass, typename Class, typename T>
+void DefReadOnlyUniquePtr(PyClass* cls, const char* name,
+    std::unique_ptr<T> Class::*member, const char* doc) {
+  auto getter = py::cpp_function(
+      [member](const Class* self) {
+        const std::unique_ptr<T>& value = self->*member;
+        return value.get();
+      },
+      py_reference_internal);
+  cls->def_property_readonly(name, getter, doc);
+}
+
 PYBIND11_MODULE(controllers, m) {
   // NOLINTNEXTLINE(build/namespaces): Emulate placement in namespace.
   using namespace drake::systems::controllers;
@@ -32,6 +45,7 @@ PYBIND11_MODULE(controllers, m) {
   py::module::import("pydrake.symbolic");
   py::module::import("pydrake.systems.framework");
   py::module::import("pydrake.systems.primitives");
+  py::module::import("pydrake.trajectories");
 
   py::class_<DynamicProgrammingOptions::PeriodicBoundaryCondition>(m,
       "PeriodicBoundaryCondition",
@@ -242,6 +256,43 @@ PYBIND11_MODULE(controllers, m) {
       py::arg("system"), py::arg("context"), py::arg("Q"), py::arg("R"),
       py::arg("N") = Eigen::Matrix<double, 0, 0>::Zero(),
       py::arg("input_port_index") = 0, doc.LinearQuadraticRegulator.doc_6args);
+
+  py::class_<FiniteHorizonLinearQuadraticRegulatorOptions>(m,
+      "FiniteHorizonLinearQuadraticRegulatorOptions",
+      doc.FiniteHorizonLinearQuadraticRegulatorOptions.doc)
+      .def(py::init<>(),
+          doc.FiniteHorizonLinearQuadraticRegulatorOptions.ctor.doc)
+      .def_readwrite("Qf", &FiniteHorizonLinearQuadraticRegulatorOptions::Qf,
+          doc.FiniteHorizonLinearQuadraticRegulatorOptions.Qf.doc)
+      .def_readwrite("x0", &FiniteHorizonLinearQuadraticRegulatorOptions::x0,
+          doc.FiniteHorizonLinearQuadraticRegulatorOptions.x0.doc)
+      .def_readwrite("u0", &FiniteHorizonLinearQuadraticRegulatorOptions::u0,
+          doc.FiniteHorizonLinearQuadraticRegulatorOptions.Qf.doc)
+      .def_readwrite("input_port_index",
+          &FiniteHorizonLinearQuadraticRegulatorOptions::input_port_index,
+          doc.FiniteHorizonLinearQuadraticRegulatorOptions.input_port_index
+              .doc);
+
+  {
+    using Class = FiniteHorizonLinearQuadraticRegulatorResult;
+    constexpr auto& cls_doc = doc.FiniteHorizonLinearQuadraticRegulatorResult;
+    // This class should avoid having its members be writeable (#8904). If this
+    // is to be made constructable by Python, it should clone the arguments to
+    // avoid lifetime issues.
+    py::class_<Class> cls(
+        m, "FiniteHorizonLinearQuadraticRegulatorResult", cls_doc.doc);
+    DefReadOnlyUniquePtr(&cls, "x0", &Class::x0, cls_doc.x0.doc);
+    DefReadOnlyUniquePtr(&cls, "u0", &Class::u0, cls_doc.u0.doc);
+    DefReadOnlyUniquePtr(&cls, "K", &Class::K, cls_doc.K.doc);
+    DefReadOnlyUniquePtr(&cls, "S", &Class::S, cls_doc.S.doc);
+  }
+
+  m.def("FiniteHorizonLinearQuadraticRegulator",
+      &FiniteHorizonLinearQuadraticRegulator, py::arg("system"),
+      py::arg("context"), py::arg("t0"), py::arg("tf"), py::arg("Q"),
+      py::arg("R"),
+      py::arg("options") = FiniteHorizonLinearQuadraticRegulatorOptions(),
+      doc.FiniteHorizonLinearQuadraticRegulator.doc);
 }
 
 }  // namespace pydrake

bindings/pydrake/systems/test/controllers_test.py

@@ -10,15 +10,23 @@
 from pydrake.multibody.parsing import Parser
 from pydrake.systems.analysis import Simulator
 from pydrake.systems.controllers import (
-    DiscreteTimeLinearQuadraticRegulator, DynamicProgrammingOptions,
+    DiscreteTimeLinearQuadraticRegulator,
+    DynamicProgrammingOptions,
+    FiniteHorizonLinearQuadraticRegulator,
+    FiniteHorizonLinearQuadraticRegulatorOptions,
+    FiniteHorizonLinearQuadraticRegulatorResult,
     FittedValueIteration,
-    InverseDynamicsController, InverseDynamics,
+    InverseDynamicsController,
+    InverseDynamics,
     LinearQuadraticRegulator,
     LinearProgrammingApproximateDynamicProgramming,
-    PeriodicBoundaryCondition, PidControlledSystem, PidController
+    PeriodicBoundaryCondition,
+    PidControlledSystem,
+    PidController,
 )
 from pydrake.systems.framework import InputPortSelection
 from pydrake.systems.primitives import Integrator, LinearSystem
+from pydrake.trajectories import Trajectory
 
 
 class TestControllers(unittest.TestCase):
@@ -260,3 +268,44 @@ def test_discrete_time_linear_quadratic_regulator(self):
         (K, S) = DiscreteTimeLinearQuadraticRegulator(A, B, Q, R)
         self.assertEqual(K.shape, (1, 2))
         self.assertEqual(S.shape, (2, 2))
+
+    def test_finite_horizon_linear_quadratic_regulator(self):
+        A = np.array([[0, 1], [0, 0]])
+        B = np.array([[0], [1]])
+        C = np.identity(2)
+        D = np.array([[0], [0]])
+        double_integrator = LinearSystem(A, B, C, D)
+
+        Q = np.identity(2)
+        R = np.identity(1)
+
+        options = FiniteHorizonLinearQuadraticRegulatorOptions()
+        options.Qf = Q
+        self.assertIsNone(options.x0)
+        self.assertIsNone(options.u0)
+        self.assertEqual(options.input_port_index,
+                         InputPortSelection.kUseFirstInputIfItExists)
+
+        context = double_integrator.CreateDefaultContext()
+        context.FixInputPort(0, [0.0])
+
+        result = FiniteHorizonLinearQuadraticRegulator(
+            system=double_integrator,
+            context=context,
+            t0=0,
+            tf=0.1,
+            Q=Q,
+            R=R,
+            options=options)
+
+        self.assertIsInstance(result,
+                              FiniteHorizonLinearQuadraticRegulatorResult)
+
+        self.assertIsInstance(result.x0, Trajectory)
+        self.assertEqual(result.x0.value(0).shape, (2, 1))
+        self.assertIsInstance(result.u0, Trajectory)
+        self.assertEqual(result.u0.value(0).shape, (1, 1))
+        self.assertIsInstance(result.K, Trajectory)
+        self.assertEqual(result.K.value(0).shape, (1, 2))
+        self.assertIsInstance(result.S, Trajectory)
+        self.assertEqual(result.S.value(0).shape, (2, 2))

systems/controllers/finite_horizon_linear_quadratic_regulator.cc

@@ -6,6 +6,7 @@
 #include <vector>
 
 #include "drake/common/drake_assert.h"
+#include "drake/common/is_approx_equal_abstol.h"
 #include "drake/math/autodiff.h"
 #include "drake/math/autodiff_gradient.h"
 #include "drake/math/matrix_util.h"
@@ -31,8 +32,7 @@ class RiccatiSystem : public LeafSystem<double> {
   RiccatiSystem(const System<double>& system, const Context<double>& context,
                 const Eigen::Ref<const Eigen::MatrixXd>& Q,
                 const Eigen::Ref<const Eigen::MatrixXd>& R,
-                const Trajectory<double>& x0,
-                const Trajectory<double>& u0,
+                const Trajectory<double>& x0, const Trajectory<double>& u0,
                 const FiniteHorizonLinearQuadraticRegulatorOptions& options)
       : system_(System<double>::ToAutoDiffXd(system)),
         input_port_(
@@ -81,15 +81,20 @@ class RiccatiSystem : public LeafSystem<double> {
     context_->SetTime(system_time);
 
     // Get (time-varying) linearization of the plant.
-    auto autodiff_args = math::initializeAutoDiffTuple(
-        x0_.value(system_time), u0_.value(system_time));
+    auto autodiff_args = math::initializeAutoDiffTuple(x0_.value(system_time),
+                                                       u0_.value(system_time));
     context_->SetContinuousState(std::get<0>(autodiff_args));
     input_port_->FixValue(context_.get(), std::get<1>(autodiff_args));
 
     const VectorX<AutoDiffXd> autodiff_xdot0 =
         system_->EvalTimeDerivatives(*context_).CopyToVector();
 
-    const Eigen::VectorXd xdot0 = math::autoDiffToValueMatrix(autodiff_xdot0);
+    // Note: This version assumes that the nominal trajectory is a feasible
+    // trajectory of the system.
+    // TODO(russt): Implement the affine terms, or add a check here to make sure
+    // that math::autoDiffToValueMatrix(autodiff_xdot0) ~=
+    // x0_.EvalDerivative(system_time).  Implementing he check requires #13144.
+
     const Eigen::MatrixXd AB = math::autoDiffToGradientMatrix(autodiff_xdot0);
     const Eigen::Ref<const Eigen::MatrixXd>& A = AB.leftCols(num_states_);
     const Eigen::Ref<const Eigen::MatrixXd>& B = AB.rightCols(num_inputs_);
@@ -170,7 +175,7 @@ FiniteHorizonLinearQuadraticRegulator(
   DRAKE_DEMAND(system.num_input_ports() > 0);
   DRAKE_DEMAND(tf > t0);
   const int num_states = context.num_total_states();
-  std::unique_ptr<Trajectory<double>> x0;
+  std::unique_ptr<PiecewisePolynomial<double>> x0;
   if (options.x0) {
     DRAKE_DEMAND(options.x0->start_time() <= t0);
     DRAKE_DEMAND(options.x0->end_time() >= tf);
@@ -180,7 +185,7 @@ FiniteHorizonLinearQuadraticRegulator(
         context.get_continuous_state_vector().CopyToVector());
   }
 
-  std::unique_ptr<Trajectory<double>> u0;
+  std::unique_ptr<PiecewisePolynomial<double>> u0;
   if (options.u0) {
     DRAKE_DEMAND(options.u0->start_time() <= t0);
     DRAKE_DEMAND(options.u0->end_time() >= tf);
@@ -218,10 +223,15 @@ FiniteHorizonLinearQuadraticRegulator(
   simulator.AdvanceTo(-t0);
 
   FiniteHorizonLinearQuadraticRegulatorResult result;
-  result.S = std::move(*(integrator.StopDenseIntegration()));
-  result.S.ReverseTime();
-  result.S.Reshape(num_states, num_states);
-  result.K = riccati.MakeKTrajectory(result.S);
+  std::unique_ptr<PiecewisePolynomial<double>> S =
+      integrator.StopDenseIntegration();
+  S->ReverseTime();
+  S->Reshape(num_states, num_states);
+  result.K = std::make_unique<PiecewisePolynomial<double>>(
+      riccati.MakeKTrajectory(*S));
+  result.x0 = options.x0 ? options.x0->Clone() : std::move(x0);
+  result.u0 = options.u0 ? options.u0->Clone() : std::move(u0);
+  result.S = std::move(S);
 
   return result;
 }

systems/controllers/finite_horizon_linear_quadratic_regulator.h

@@ -16,6 +16,8 @@ A structure to facilitate passing the myriad of optional arguments to the
 FiniteHorizonLinearQuadraticRegulator algorithms.
 */
 struct FiniteHorizonLinearQuadraticRegulatorOptions {
+  FiniteHorizonLinearQuadraticRegulatorOptions() = default;
+
   /**
   A num_states x num_states positive semi-definite matrix which specified the
   cost at the final time. If unset, then Qf will be set to the zero matrix.
@@ -52,8 +54,10 @@ results. The finite-horizon cost-to-go is given by (x-x0(t))'*S(t)*(x-x0(t)) and
 the optimal controller is given by u-u0(t) = -K(t)*(x-x0(t)).
 */
 struct FiniteHorizonLinearQuadraticRegulatorResult {
-  trajectories::PiecewisePolynomial<double> K;
-  trajectories::PiecewisePolynomial<double> S;
+  std::unique_ptr<trajectories::Trajectory<double>> x0;
+  std::unique_ptr<trajectories::Trajectory<double>> u0;
+  std::unique_ptr<trajectories::Trajectory<double>> K;
+  std::unique_ptr<trajectories::Trajectory<double>> S;
 };
 
 // TODO(russt): Add support for difference-equation systems.
@@ -73,10 +77,10 @@ optimal cost-to-go for the finite-horizon linear quadratic regulator:
 @f]
 
 where A(t), B(t), and c(t) are taken from the gradients of the continuous-time
-dynamics ẋ = f(t,x,u), as A(t) = dfdx(t, x0(t), u0(t)), B(t) = dfdu(t,
-x0(t), u0(t)), and c(t) = f(t, x0(t), u0(t)).  x0(t) and u0(t) can be specified
-in @p options, otherwise are taken to be constant trajectories with values
-given by @p context.
+dynamics ẋ = f(t,x,u), as A(t) = dfdx(t, x0(t), u0(t)), B(t) = dfdu(t, x0(t),
+u0(t)), and c(t) = f(t, x0(t), u0(t)).  x0(t) and u0(t) can be specified in @p
+options, otherwise are taken to be constant trajectories with values given by @p
+context.  The current implementation assumes that ẋ0(t) = f(t, x0(t), u0(t)).
 
 @param system a System<double> representing the plant.
 @param context a Context<double> used to pass the default input, state, and

systems/controllers/test/finite_horizon_linear_quadratic_regulator_test.cc

@@ -39,22 +39,26 @@ GTEST_TEST(FiniteHorizonLQRTest, InfiniteHorizonTest) {
   FiniteHorizonLinearQuadraticRegulatorResult result =
       FiniteHorizonLinearQuadraticRegulator(sys, *context, t0, tf, Q, R,
                                             options);
-  EXPECT_TRUE(result.S.start_time() == t0);
-  EXPECT_TRUE(result.S.end_time() == tf);
+  EXPECT_TRUE(result.S->start_time() == t0);
+  EXPECT_TRUE(result.S->end_time() == tf);
   // Confirm that it's initialized to zero.
-  EXPECT_TRUE(result.S.value(tf).isZero(1e-12));
+  EXPECT_TRUE(result.S->value(tf).isZero(1e-12));
   // Confirm that it converges to the solution.
-  EXPECT_TRUE(CompareMatrices(result.S.value(t0), lqr_result.S, 1e-5));
-  EXPECT_TRUE(result.K.start_time() == t0);
-  EXPECT_TRUE(result.K.end_time() == tf);
-  EXPECT_TRUE(CompareMatrices(result.K.value(t0), lqr_result.K, 1e-5));
+  EXPECT_TRUE(CompareMatrices(result.S->value(t0), lqr_result.S, 1e-5));
+  EXPECT_TRUE(result.K->start_time() == t0);
+  EXPECT_TRUE(result.K->end_time() == tf);
+  EXPECT_TRUE(CompareMatrices(result.K->value(t0), lqr_result.K, 1e-5));
+  EXPECT_TRUE(CompareMatrices(result.x0->value(t0), Eigen::Vector2d::Zero()));
+  EXPECT_TRUE(CompareMatrices(result.x0->value(tf), Eigen::Vector2d::Zero()));
+  EXPECT_TRUE(CompareMatrices(result.u0->value(t0), Vector1d::Zero()));
+  EXPECT_TRUE(CompareMatrices(result.u0->value(tf), Vector1d::Zero()));
 
   // Test that it stays at the fixed-point if initialized at the fixed point.
   options.Qf = lqr_result.S;
   result = FiniteHorizonLinearQuadraticRegulator(sys, *context, t0, t0 + 0.1, Q,
                                                  R, options);
-  EXPECT_TRUE(CompareMatrices(result.S.value(t0), lqr_result.S, 1e-12));
-  EXPECT_TRUE(CompareMatrices(result.K.value(t0), lqr_result.K, 1e-12));
+  EXPECT_TRUE(CompareMatrices(result.S->value(t0), lqr_result.S, 1e-12));
+  EXPECT_TRUE(CompareMatrices(result.K->value(t0), lqr_result.K, 1e-12));
 }
 
 // Verify that we can stabilize a non-zero fixed-point specified via the
@@ -97,9 +101,13 @@ GTEST_TEST(FiniteHorizonLQRTest, NominalTrajectoryTest) {
         FiniteHorizonLinearQuadraticRegulator(*system, *context, t0, tf,
                                               Q, R, options);
     EXPECT_TRUE(
-        CompareMatrices(result.S.value(t0), result.S.value(tf), 1e-12));
-    EXPECT_TRUE(CompareMatrices(result.S.value(t0), lqr_result.S, 1e-12));
-    EXPECT_TRUE(CompareMatrices(result.K.value(t0), lqr_result.K, 1e-12));
+        CompareMatrices(result.S->value(t0), result.S->value(tf), 1e-12));
+    EXPECT_TRUE(CompareMatrices(result.S->value(t0), lqr_result.S, 1e-12));
+    EXPECT_TRUE(CompareMatrices(result.K->value(t0), lqr_result.K, 1e-12));
+    EXPECT_TRUE(CompareMatrices(result.x0->value(t0), x0v));
+    EXPECT_TRUE(CompareMatrices(result.x0->value(tf), x0v));
+    EXPECT_TRUE(CompareMatrices(result.u0->value(t0), u0v));
+    EXPECT_TRUE(CompareMatrices(result.u0->value(tf), u0v));
   }
 }