From 3fc69b216100645a9492600d0a2070b66dfc18b7 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Fri, 13 Dec 2024 06:56:28 -0500 Subject: [PATCH] Remove LinearModelPredictiveControl Resolves #13248 --- systems/controllers/BUILD.bazel | 23 -- .../linear_model_predictive_controller.cc | 137 ---------- .../linear_model_predictive_controller.h | 117 -------- ...linear_model_predictive_controller_test.cc | 258 ------------------ 4 files changed, 535 deletions(-) delete mode 100644 systems/controllers/linear_model_predictive_controller.cc delete mode 100644 systems/controllers/linear_model_predictive_controller.h delete mode 100644 systems/controllers/test/linear_model_predictive_controller_test.cc diff --git a/systems/controllers/BUILD.bazel b/systems/controllers/BUILD.bazel index 18ed348b3462..60163de247f7 100644 --- a/systems/controllers/BUILD.bazel +++ b/systems/controllers/BUILD.bazel @@ -18,7 +18,6 @@ drake_cc_package_library( ":inverse_dynamics", ":inverse_dynamics_controller", ":joint_stiffness_controller", - ":linear_model_predictive_controller", ":linear_quadratic_regulator", ":pid_controlled_system", ":pid_controller", @@ -101,18 +100,6 @@ drake_cc_library( ], ) -drake_cc_library( - name = "linear_model_predictive_controller", - srcs = ["linear_model_predictive_controller.cc"], - hdrs = ["linear_model_predictive_controller.h"], - deps = [ - "//common/trajectories:piecewise_polynomial", - "//planning/trajectory_optimization:direct_transcription", - "//solvers:solve", - "//systems/primitives:linear_system", - ], -) - drake_cc_library( name = "linear_quadratic_regulator", srcs = ["linear_quadratic_regulator.cc"], @@ -226,16 +213,6 @@ drake_cc_googletest( ], ) -drake_cc_googletest( - name = "linear_model_predictive_controller_test", - deps = [ - ":linear_model_predictive_controller", - "//common/test_utilities:eigen_matrix_compare", - "//math:discrete_algebraic_riccati_equation", - "//systems/analysis:simulator", - ], -) - drake_cc_googletest( name = "linear_quadratic_regulator_test", data = ["//examples/acrobot:models"], diff --git a/systems/controllers/linear_model_predictive_controller.cc b/systems/controllers/linear_model_predictive_controller.cc deleted file mode 100644 index 3fd71fed5f95..000000000000 --- a/systems/controllers/linear_model_predictive_controller.cc +++ /dev/null @@ -1,137 +0,0 @@ -#include "drake/systems/controllers/linear_model_predictive_controller.h" - -#include -#include - -#include "drake/common/eigen_types.h" -#include "drake/planning/trajectory_optimization/direct_transcription.h" -#include "drake/solvers/solve.h" - -namespace drake { -namespace systems { -namespace controllers { - -using planning::trajectory_optimization::DirectTranscription; -using solvers::VectorXDecisionVariable; - -template -LinearModelPredictiveController::LinearModelPredictiveController( - std::unique_ptr> model, - std::unique_ptr> base_context, - const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R, double time_period, - double time_horizon) - : state_input_index_( - this->DeclareVectorInputPort(kUseDefaultName, Q.cols()).get_index()), - control_output_index_( - this->DeclareVectorOutputPort( - kUseDefaultName, R.cols(), - &LinearModelPredictiveController::CalcControl) - .get_index()), - model_(std::move(model)), - base_context_(std::move(base_context)), - num_states_(model_->CreateDefaultContext()->get_discrete_state(0).size()), - num_inputs_(model_->get_input_port().size()), - Q_(Q), - R_(R), - time_period_(time_period), - time_horizon_(time_horizon) { - DRAKE_DEMAND(time_period_ > 0.); - DRAKE_DEMAND(time_horizon_ > 0.); - - // Check that the model is SISO - model_->get_input_port(); - model_->get_output_port(); - - // Check that the model has discrete states belonging to a single group. - const auto model_context = model_->CreateDefaultContext(); - DRAKE_DEMAND(model_context->num_discrete_state_groups() == 1); - DRAKE_DEMAND(model_context->num_continuous_states() == 0); - DRAKE_DEMAND(model_context->num_abstract_states() == 0); - - // Check that the provided x0, u0, Q, R are consistent with the model. - DRAKE_DEMAND(num_states_ > 0 && num_inputs_ > 0); - DRAKE_DEMAND(Q.rows() == num_states_ && Q.cols() == num_states_); - DRAKE_DEMAND(R.rows() == num_inputs_ && R.cols() == num_inputs_); - - // N.B. A Cholesky decomposition exists if and only if it is positive - // semidefinite, however it turns out that Eigen's algorithm for checking this - // is incomplete: it only succeeds on *strictly* positive definite - // matrices. We exploit the fact here to check for strict - // positive-definiteness of R. - Eigen::LLT R_cholesky(R); - if (R_cholesky.info() != Eigen::Success) { - throw std::runtime_error("R must be positive definite"); - } - - // TODO(jwnimmer-tri) This seems like a misunderstood attempt at implementing - // discrete dynamics. The intent *appears* to be that SetupAndSolveQp should - // be run once every time_step. However, both because its result is NOT stored - // as state and because the output is direct-feedthrough from the input, we do - // not actually embody any kind of discrete dynamics. Anytime a user evaluates - // the output port after the input port has changed, we'll redo the QP, even - // when the current time is not an integer multiple of the step. - this->DeclarePeriodicDiscreteUpdateEvent( - time_period_, 0.0, - &LinearModelPredictiveController::DoNothingButPretendItWasSomething); - - if (base_context_ != nullptr) { - linear_model_ = Linearize(*model_, *base_context_); - } -} - -template -void LinearModelPredictiveController::CalcControl( - const Context& context, BasicVector* control) const { - const VectorX& current_state = get_state_port().Eval(context); - - const Eigen::VectorXd current_input = - SetupAndSolveQp(*base_context_, current_state); - - const VectorX input_ref = model_->get_input_port().Eval(*base_context_); - - control->SetFromVector(current_input + input_ref); - - // TODO(jadecastro) Implement the time-varying case. -} - -template -EventStatus -LinearModelPredictiveController::DoNothingButPretendItWasSomething( - const Context&, DiscreteValues*) const { - return EventStatus::Succeeded(); -} - -template -VectorX LinearModelPredictiveController::SetupAndSolveQp( - const Context& base_context, const VectorX& current_state) const { - DRAKE_DEMAND(linear_model_ != nullptr); - - const int kNumSampleTimes = - static_cast(time_horizon_ / time_period_ + 0.5); - - DirectTranscription dirtran(linear_model_.get(), *base_context_, - kNumSampleTimes); - auto& prog = dirtran.prog(); - - const auto state_error = dirtran.state(); - const auto input_error = dirtran.input(); - - dirtran.AddRunningCost(state_error.transpose() * Q_ * state_error + - input_error.transpose() * R_ * input_error); - - const VectorX state_ref = - base_context.get_discrete_state().get_vector().CopyToVector(); - prog.AddLinearConstraint(dirtran.initial_state() == - current_state - state_ref); - - const auto result = Solve(prog); - DRAKE_DEMAND(result.is_success()); - - return dirtran.GetInputSamples(result).col(0); -} - -template class LinearModelPredictiveController; - -} // namespace controllers -} // namespace systems -} // namespace drake diff --git a/systems/controllers/linear_model_predictive_controller.h b/systems/controllers/linear_model_predictive_controller.h deleted file mode 100644 index c0068f761124..000000000000 --- a/systems/controllers/linear_model_predictive_controller.h +++ /dev/null @@ -1,117 +0,0 @@ -#pragma once - -#include - -#include "drake/common/drake_copyable.h" -#include "drake/common/trajectories/piecewise_polynomial.h" -#include "drake/systems/primitives/linear_system.h" - -namespace drake { -namespace systems { -namespace controllers { - -/// Implements a basic Model Predictive Controller that linearizes the system -/// about an equilibrium condition and regulates to the same point by solving an -/// optimal control problem over a finite time horizon. In particular, MPC -/// solves, at each time step k, the following problem to find an optimal u(k) -/// as a function of x(k): -/// -/// @f[ \min_{u(k),\ldots,u(k+N),x(k+1),\ldots,x(k+N)} -/// \sum_{i=k}^{k+N} ((x(i) - xd(i))ᵀQ(x(i) - xd(i)) + -/// (u(i) - ud(i))ᵀR(u(i) - ud(i))) @f] -/// @f[ \mathrm{s.t. } x(k+1) = A(k)x(k) + B(k)u(k) @f] -/// -/// and subject to linear inequality constraints on the inputs and states, where -/// N is the horizon length, Q and R are cost matrices, and xd and ud are the -/// desired states and inputs, respectively. Note that the present -/// implementation solves the QP in whole at every time step, discarding any -/// information between steps. -/// -/// @system -/// name: LinearModelPredictiveController -/// input_ports: -/// - u0 -/// output_ports: -/// - y0 -/// @endsystem -/// -/// @tparam_double_only -/// @ingroup control_systems -template -class LinearModelPredictiveController : public LeafSystem { - public: - DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(LinearModelPredictiveController); - - // TODO(jadecastro) Implement a version that regulates to an arbitrary - // trajectory. - /// Constructor for an unconstrained MPC formulation with linearization - /// occurring about the provided base_context. Since this formulation is - /// devoid of any externally-imposed input/state constraints, the controller - /// essentially behaves the same as a finite-time LQR. - /// - /// @param model The plant model of the System to be controlled. - /// @param base_context The fixed base point about which to linearize of the - /// system and regulate the system. To be valid, @p base_context must - /// correspond to an equilibrium point of the system. - /// @param Q A symmetric positive semi-definite state cost matrix of size - /// (num_states x num_states). - /// @param R A symmetric positive definite control effort cost matrix of size - /// (num_inputs x num_inputs). - /// @param time_period The discrete time period (in seconds) at which - /// controller updates occur. - /// @param time_horizon The prediction time horizon (seconds). - /// - /// @pre model must have discrete states of dimension num_states and inputs - /// of dimension num_inputs. - /// @pre base_context must have discrete states set as appropriate for the - /// given @p model. The input must also be initialized via - /// `input_port.FixValue(base_context, u0)`, or otherwise initialized via - /// Diagram. - LinearModelPredictiveController( - std::unique_ptr> model, - std::unique_ptr> base_context, - const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R, double time_period, - double time_horizon); - // TODO(jadecastro) Get time_period directly from the plant model. - - const InputPort& get_state_port() const { - return this->get_input_port(state_input_index_); - } - const OutputPort& get_control_port() const { - return this->get_output_port(control_output_index_); - } - - private: - void CalcControl(const Context& context, BasicVector* control) const; - - EventStatus DoNothingButPretendItWasSomething(const Context&, - DiscreteValues*) const; - - // Sets up a DirectTranscription problem and solves for the current control - // input. - VectorX SetupAndSolveQp(const Context& base_context, - const VectorX& current_state) const; - - const int state_input_index_{-1}; - const int control_output_index_{-1}; - - const std::unique_ptr> model_; - // The base context that contains the reference point to regulate. - const std::unique_ptr> base_context_; - - const int num_states_{}; - const int num_inputs_{}; - - const Eigen::MatrixXd Q_; - const Eigen::MatrixXd R_; - - const double time_period_{}; - const double time_horizon_{}; - - // Description of the linearized plant model. - std::unique_ptr> linear_model_; -}; - -} // namespace controllers -} // namespace systems -} // namespace drake diff --git a/systems/controllers/test/linear_model_predictive_controller_test.cc b/systems/controllers/test/linear_model_predictive_controller_test.cc deleted file mode 100644 index 7918cfac89a7..000000000000 --- a/systems/controllers/test/linear_model_predictive_controller_test.cc +++ /dev/null @@ -1,258 +0,0 @@ -#include "drake/systems/controllers/linear_model_predictive_controller.h" - -#include - -#include "drake/common/test_utilities/eigen_matrix_compare.h" -#include "drake/math/discrete_algebraic_riccati_equation.h" -#include "drake/systems/analysis/simulator.h" -#include "drake/systems/framework/diagram.h" -#include "drake/systems/framework/diagram_builder.h" -#include "drake/systems/primitives/linear_system.h" - -namespace drake { -namespace systems { -namespace controllers { -namespace { - -using math::DiscreteAlgebraicRiccatiEquation; - -class TestMpcWithDoubleIntegrator : public ::testing::Test { - protected: - void SetUp() override { - const double kTimeStep = 0.1; // discrete time step. - const double kTimeHorizon = 10.; // Time horizon. - - // A discrete approximation of a double integrator. - Eigen::Matrix2d A; - Eigen::Vector2d B; - A << 1, 0.1, 0, 1; - B << 0.005, 0.1; - const auto C = Eigen::Matrix::Identity(); - const auto D = Eigen::Matrix::Zero(); - - std::unique_ptr> system = - std::make_unique>(A, B, C, D, kTimeStep); - - // Nominal, fixed reference condition. - const Eigen::Vector2d x0 = Eigen::Vector2d::Zero(); - const Vector1d u0 = Vector1d::Zero(); - - std::unique_ptr> system_context = - system->CreateDefaultContext(); - system->get_input_port().FixValue(system_context.get(), u0); - system_context->SetDiscreteState(0, x0); - - dut_.reset(new LinearModelPredictiveController( - std::move(system), std::move(system_context), Q_, R_, kTimeStep, - kTimeHorizon)); - - // Store another copy of the linear plant model. - system_.reset(new LinearSystem(A, B, C, D, kTimeStep)); - } - - // Cost matrices. - const Eigen::Matrix2d Q_ = Eigen::Matrix2d::Identity(); - const Vector1d R_ = Vector1d::Constant(1.); - - std::unique_ptr> dut_; - std::unique_ptr> system_; -}; - -TEST_F(TestMpcWithDoubleIntegrator, TestAgainstInfiniteHorizonSolution) { - const double kTolerance = 1e-5; - - const Eigen::Matrix2d A = system_->A(); - const Eigen::Matrix B = system_->B(); - - // Analytical solution to the LQR problem. - const Eigen::Matrix2d S = DiscreteAlgebraicRiccatiEquation(A, B, Q_, R_); - const Eigen::Matrix K = - -(R_ + B.transpose() * S * B).inverse() * (B.transpose() * S * A); - - const Eigen::Matrix x0 = Eigen::Vector2d::Ones(); - - auto context = dut_->CreateDefaultContext(); - dut_->get_input_port(0).FixValue(context.get(), x0); - std::unique_ptr> output = dut_->AllocateOutput(); - - dut_->CalcOutput(*context, output.get()); - - EXPECT_TRUE(CompareMatrices(K * x0, output->get_vector_data(0)->get_value(), - kTolerance)); -} - -namespace { - -// A discrete-time cubic polynomial system. -template -class CubicPolynomialSystem final : public LeafSystem { - public: - explicit CubicPolynomialSystem(double time_step) - : LeafSystem(SystemTypeTag{}), - time_step_(time_step) { - this->DeclareInputPort(kUseDefaultName, kVectorValued, 1); - this->DeclareVectorOutputPort(kUseDefaultName, 2, - &CubicPolynomialSystem::OutputState, - {this->all_state_ticket()}); - this->DeclareDiscreteState(2); - this->DeclarePeriodicDiscreteUpdateEvent( - time_step, 0.0, &CubicPolynomialSystem::CalcDiscreteUpdate); - } - - template - CubicPolynomialSystem(const CubicPolynomialSystem& other) - : CubicPolynomialSystem(other.time_step_) {} - - private: - template friend class CubicPolynomialSystem; - - // x1(k+1) = u(k) - // x2(k+1) = -x1³(k) - void CalcDiscreteUpdate( - const Context& context, - DiscreteValues* next_state) const { - using std::pow; - const T& x1 = context.get_discrete_state(0).get_value()[0]; - const T& u = this->get_input_port(0).Eval(context)[0]; - next_state->set_value(0, Vector2{u, pow(x1, 3.)}); - } - - void OutputState(const systems::Context& context, - BasicVector* output) const { - output->set_value(context.get_discrete_state(0).get_value()); - } - - const double time_step_{0.}; -}; - -} // namespace - -class TestMpcWithCubicSystem : public ::testing::Test { - protected: - const System& GetSystemByName(std::string name, - const Diagram& diagram) { - const System* result{nullptr}; - for (const System* system : diagram.GetSystems()) { - if (system->get_name() == name) result = system; - } - return *result; - } - - void MakeTimeInvariantMpcController() { - EXPECT_NE(nullptr, system_); - auto context = system_->CreateDefaultContext(); - - // Set nominal input to zero. - system_->get_input_port(0).FixValue(context.get(), 0.); - - // Set the nominal state. - BasicVector& x = - context->get_mutable_discrete_state().get_mutable_vector(); - x.SetFromVector(Eigen::Vector2d::Zero()); // Fixed point is zero. - - dut_.reset(new LinearModelPredictiveController( - std::move(system_), std::move(context), Q_, R_, time_step_, - time_horizon_)); - } - - void MakeControlledSystem(bool is_time_varying) { - EXPECT_FALSE(is_time_varying); // TODO(jadecastro) Introduce tests for the - // time-varying case. - EXPECT_EQ(nullptr, diagram_); - - system_.reset(new CubicPolynomialSystem(time_step_)); - EXPECT_FALSE(system_->HasAnyDirectFeedthrough()); - - DiagramBuilder builder; - auto cubic_system = builder.AddSystem(time_step_); - cubic_system->set_name("cubic_system"); - - MakeTimeInvariantMpcController(); - EXPECT_NE(nullptr, dut_); - auto controller = builder.AddSystem(std::move(dut_)); - controller->set_name("controller"); - - builder.Connect(cubic_system->get_output_port(0), - controller->get_state_port()); - builder.Connect(controller->get_control_port(), - cubic_system->get_input_port(0)); - - diagram_ = builder.Build(); - } - - void Simulate() { - EXPECT_NE(nullptr, diagram_); - EXPECT_EQ(nullptr, simulator_); - - simulator_.reset(new Simulator(*diagram_)); - - const auto& cubic_system = GetSystemByName("cubic_system", *diagram_); - Context& cubic_system_context = - diagram_->GetMutableSubsystemContext( - cubic_system, &simulator_->get_mutable_context()); - BasicVector& x0 = - cubic_system_context.get_mutable_discrete_state().get_mutable_vector(); - - // Set an initial condition near the fixed point. - x0.SetFromVector(10. * Eigen::Vector2d::Ones()); - - simulator_->set_target_realtime_rate(1.); - simulator_->Initialize(); - simulator_->AdvanceTo(time_horizon_); - } - - const double time_step_ = 0.1; - const double time_horizon_ = 0.2; - - // Set up the quadratic cost matrices. - const Eigen::Matrix2d Q_ = Eigen::Matrix2d::Identity(); - const Vector1d R_ = Vector1d::Constant(1.); - - std::unique_ptr> simulator_; - - private: - std::unique_ptr> dut_; - std::unique_ptr> system_; - std::unique_ptr> diagram_; -}; - -TEST_F(TestMpcWithCubicSystem, TimeInvariantMpc) { - const double kTolerance = 1e-10; - MakeControlledSystem(false /* is NOT time-varying */); - Simulate(); - - // Result should be deadbeat; expect convergence to within a tiny tolerance in - // one step. - Eigen::Vector2d result = - simulator_->get_mutable_context().get_discrete_state(0).get_value(); - EXPECT_TRUE(CompareMatrices(result, Eigen::Vector2d::Zero(), kTolerance)); -} - -GTEST_TEST(TestMpcConstructor, ThrowIfRNotStrictlyPositiveDefinite) { - const auto A = Eigen::Matrix::Identity(); - const auto B = Eigen::Matrix::Identity(); - const auto C = Eigen::Matrix::Identity(); - const auto D = Eigen::Matrix::Zero(); - - std::unique_ptr> system = - std::make_unique>(A, B, C, D, 1.); - - std::unique_ptr> context = system->CreateDefaultContext(); - system->get_input_port().FixValue(context.get(), Eigen::Vector2d::Zero()); - - const Eigen::Matrix2d Q = Eigen::Matrix2d::Identity(); - - // Provide a positive-definite matrix (but not strict). - Eigen::Matrix2d R = Eigen::Matrix2d::Identity(); - R(0, 0) = 0.; - - // Expect the constructor to throw since R is not strictly positive definite. - EXPECT_THROW(LinearModelPredictiveController( - std::move(system), std::move(context), Q, R, 1., 1.), - std::runtime_error); -} - -} // namespace -} // namespace controllers -} // namespace systems -} // namespace drake