From 1f5395f936ab2bf44c82b4b1a818698eeb63df43 Mon Sep 17 00:00:00 2001 From: Russ Tedrake <russt@mit.edu> Date: Tue, 21 Apr 2020 12:52:25 -0400 Subject: [PATCH 1/2] controllers: add python bindings for finite-horizon LQR as discussed on slack, this uses a work-around to binding the std::unique_ptr member variables in FiniteLQRResult. also includes a few small improvements to finite-horizon LQR, in preparation for the supporting discrete-time systems. --- bindings/pydrake/systems/controllers_py.cc | 79 +++++++++++++++++++ .../pydrake/systems/test/controllers_test.py | 57 ++++++++++++- ...nite_horizon_linear_quadratic_regulator.cc | 32 +++++--- ...inite_horizon_linear_quadratic_regulator.h | 16 ++-- ...horizon_linear_quadratic_regulator_test.cc | 32 +++++--- 5 files changed, 184 insertions(+), 32 deletions(-) diff --git a/bindings/pydrake/systems/controllers_py.cc b/bindings/pydrake/systems/controllers_py.cc index a65c5cdee4e9..8ecbdc0bf8be 100644 --- a/bindings/pydrake/systems/controllers_py.cc +++ b/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" @@ -32,6 +33,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 +244,83 @@ 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); + + py::class_<FiniteHorizonLinearQuadraticRegulatorResult>(m, + "FiniteHorizonLinearQuadraticRegulatorResult", + doc.FiniteHorizonLinearQuadraticRegulatorResult.doc) + // Note: Use these as a workaround for .def_readwrite not working with + // unique_ptr (yet). + .def("x0", + [](FiniteHorizonLinearQuadraticRegulatorResult* self) { + return self->x0.get(); + }, + py_reference_internal, + doc.FiniteHorizonLinearQuadraticRegulatorResult.x0.doc) + .def("x0", + [](FiniteHorizonLinearQuadraticRegulatorResult* self, double t) { + DRAKE_DEMAND(self->x0 != nullptr); + return self->x0->value(t); + }, + py::arg("t"), doc.FiniteHorizonLinearQuadraticRegulatorResult.x0.doc) + .def("u0", + [](FiniteHorizonLinearQuadraticRegulatorResult* self) { + return self->u0.get(); + }, + py_reference_internal, + doc.FiniteHorizonLinearQuadraticRegulatorResult.u0.doc) + .def("u0", + [](FiniteHorizonLinearQuadraticRegulatorResult* self, double t) { + DRAKE_DEMAND(self->u0 != nullptr); + return self->u0->value(t); + }, + py::arg("t"), doc.FiniteHorizonLinearQuadraticRegulatorResult.u0.doc) + .def("K", + [](FiniteHorizonLinearQuadraticRegulatorResult* self) { + return self->K.get(); + }, + py_reference_internal, + doc.FiniteHorizonLinearQuadraticRegulatorResult.K.doc) + .def("K", + [](FiniteHorizonLinearQuadraticRegulatorResult* self, double t) { + DRAKE_DEMAND(self->K != nullptr); + return self->K->value(t); + }, + py::arg("t"), doc.FiniteHorizonLinearQuadraticRegulatorResult.K.doc) + .def("S", + [](FiniteHorizonLinearQuadraticRegulatorResult* self) { + return self->S.get(); + }, + py_reference_internal, + doc.FiniteHorizonLinearQuadraticRegulatorResult.S.doc) + .def("S", + [](FiniteHorizonLinearQuadraticRegulatorResult* self, double t) { + DRAKE_DEMAND(self->S != nullptr); + return self->S->value(t); + }, + py::arg("t"), doc.FiniteHorizonLinearQuadraticRegulatorResult.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 diff --git a/bindings/pydrake/systems/test/controllers_test.py b/bindings/pydrake/systems/test/controllers_test.py index 0a4edf7fb6a0..d4987824bfa8 100644 --- a/bindings/pydrake/systems/test/controllers_test.py +++ b/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,46 @@ 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(0).shape, (2, 1)) + self.assertIsInstance(result.u0(), Trajectory) + self.assertEqual(result.u0(0).shape, (1, 1)) + self.assertIsInstance(result.K(), Trajectory) + self.assertEqual(result.K().value(0).shape, (1, 2)) + self.assertEqual(result.K(0).shape, (1, 2)) + self.assertIsInstance(result.S(), Trajectory) + self.assertEqual(result.S().value(0).shape, (2, 2)) + self.assertEqual(result.S(0).shape, (2, 2)) diff --git a/systems/controllers/finite_horizon_linear_quadratic_regulator.cc b/systems/controllers/finite_horizon_linear_quadratic_regulator.cc index c22932b914ba..6979ef86d434 100644 --- a/systems/controllers/finite_horizon_linear_quadratic_regulator.cc +++ b/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; } diff --git a/systems/controllers/finite_horizon_linear_quadratic_regulator.h b/systems/controllers/finite_horizon_linear_quadratic_regulator.h index aa734d3094e5..7826ed9f47fd 100644 --- a/systems/controllers/finite_horizon_linear_quadratic_regulator.h +++ b/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 diff --git a/systems/controllers/test/finite_horizon_linear_quadratic_regulator_test.cc b/systems/controllers/test/finite_horizon_linear_quadratic_regulator_test.cc index 2b5327668a4f..3a2c1f02f815 100644 --- a/systems/controllers/test/finite_horizon_linear_quadratic_regulator_test.cc +++ b/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)); } } From df99690719054b0a3d397f81ae1ad05d05316451 Mon Sep 17 00:00:00 2001 From: Eric Cousineau <eric.cousineau@tri.global> Date: Mon, 27 Apr 2020 21:59:30 -0400 Subject: [PATCH 2/2] suggested modifications --- bindings/pydrake/systems/controllers_py.cc | 78 ++++++------------- .../pydrake/systems/test/controllers_test.py | 18 ++--- 2 files changed, 33 insertions(+), 63 deletions(-) diff --git a/bindings/pydrake/systems/controllers_py.cc b/bindings/pydrake/systems/controllers_py.cc index 8ecbdc0bf8be..af9a0ccf43c3 100644 --- a/bindings/pydrake/systems/controllers_py.cc +++ b/bindings/pydrake/systems/controllers_py.cc @@ -19,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; @@ -261,59 +273,19 @@ PYBIND11_MODULE(controllers, m) { doc.FiniteHorizonLinearQuadraticRegulatorOptions.input_port_index .doc); - py::class_<FiniteHorizonLinearQuadraticRegulatorResult>(m, - "FiniteHorizonLinearQuadraticRegulatorResult", - doc.FiniteHorizonLinearQuadraticRegulatorResult.doc) - // Note: Use these as a workaround for .def_readwrite not working with - // unique_ptr (yet). - .def("x0", - [](FiniteHorizonLinearQuadraticRegulatorResult* self) { - return self->x0.get(); - }, - py_reference_internal, - doc.FiniteHorizonLinearQuadraticRegulatorResult.x0.doc) - .def("x0", - [](FiniteHorizonLinearQuadraticRegulatorResult* self, double t) { - DRAKE_DEMAND(self->x0 != nullptr); - return self->x0->value(t); - }, - py::arg("t"), doc.FiniteHorizonLinearQuadraticRegulatorResult.x0.doc) - .def("u0", - [](FiniteHorizonLinearQuadraticRegulatorResult* self) { - return self->u0.get(); - }, - py_reference_internal, - doc.FiniteHorizonLinearQuadraticRegulatorResult.u0.doc) - .def("u0", - [](FiniteHorizonLinearQuadraticRegulatorResult* self, double t) { - DRAKE_DEMAND(self->u0 != nullptr); - return self->u0->value(t); - }, - py::arg("t"), doc.FiniteHorizonLinearQuadraticRegulatorResult.u0.doc) - .def("K", - [](FiniteHorizonLinearQuadraticRegulatorResult* self) { - return self->K.get(); - }, - py_reference_internal, - doc.FiniteHorizonLinearQuadraticRegulatorResult.K.doc) - .def("K", - [](FiniteHorizonLinearQuadraticRegulatorResult* self, double t) { - DRAKE_DEMAND(self->K != nullptr); - return self->K->value(t); - }, - py::arg("t"), doc.FiniteHorizonLinearQuadraticRegulatorResult.K.doc) - .def("S", - [](FiniteHorizonLinearQuadraticRegulatorResult* self) { - return self->S.get(); - }, - py_reference_internal, - doc.FiniteHorizonLinearQuadraticRegulatorResult.S.doc) - .def("S", - [](FiniteHorizonLinearQuadraticRegulatorResult* self, double t) { - DRAKE_DEMAND(self->S != nullptr); - return self->S->value(t); - }, - py::arg("t"), doc.FiniteHorizonLinearQuadraticRegulatorResult.S.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"), diff --git a/bindings/pydrake/systems/test/controllers_test.py b/bindings/pydrake/systems/test/controllers_test.py index d4987824bfa8..5bee5f4fe458 100644 --- a/bindings/pydrake/systems/test/controllers_test.py +++ b/bindings/pydrake/systems/test/controllers_test.py @@ -301,13 +301,11 @@ def test_finite_horizon_linear_quadratic_regulator(self): self.assertIsInstance(result, FiniteHorizonLinearQuadraticRegulatorResult) - self.assertIsInstance(result.x0(), Trajectory) - self.assertEqual(result.x0(0).shape, (2, 1)) - self.assertIsInstance(result.u0(), Trajectory) - self.assertEqual(result.u0(0).shape, (1, 1)) - self.assertIsInstance(result.K(), Trajectory) - self.assertEqual(result.K().value(0).shape, (1, 2)) - self.assertEqual(result.K(0).shape, (1, 2)) - self.assertIsInstance(result.S(), Trajectory) - self.assertEqual(result.S().value(0).shape, (2, 2)) - self.assertEqual(result.S(0).shape, (2, 2)) + 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))