final_state_cost.h
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement
4  *
5  * Copyright (c) 2020,
6  * TU Dortmund - Institute of Control Theory and Systems Engineering.
7  * All rights reserved.
8  *
9  * This program is free software: you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation, either version 3 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program. If not, see <https://www.gnu.org/licenses/>.
21  *
22  * Authors: Christoph Rösmann
23  *********************************************************************/
24 
25 #ifndef SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_FUNCTIONS_FINAL_STATE_COST_H_
26 #define SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_FUNCTIONS_FINAL_STATE_COST_H_
27 
29 
33 
34 #include <memory>
35 
36 namespace corbo {
37 
39 {
40  public:
41  using Ptr = std::shared_ptr<BaseQuadraticFinalStateCost>;
42  using ConstPtr = std::shared_ptr<const BaseQuadraticFinalStateCost>;
43 
44  virtual const Eigen::MatrixXd& getWeightQf() const = 0;
45 };
46 
48 {
49  public:
50  using Ptr = std::shared_ptr<QuadraticFinalStateCost>;
51 
52  QuadraticFinalStateCost() { _Qf_sqrt = Eigen::MatrixXd::Constant(1, 1, 1); }
53 
54  QuadraticFinalStateCost(const Eigen::Ref<const Eigen::MatrixXd>& Qf, bool lsq_form) : _lsq_form(lsq_form) { setWeightQf(Qf); }
55 
56  FinalStageCost::Ptr getInstance() const override { return std::make_shared<QuadraticFinalStateCost>(); }
57 
58  int getNonIntegralStateTermDimension(int k) const override { return _lsq_form ? _Qf.rows() : 1; }
59  bool isLsqFormNonIntegralStateTerm(int k) const override { return _lsq_form; }
60 
61  bool setWeightQf(const Eigen::Ref<const Eigen::MatrixXd>& Qf);
62  bool setWeightQf(const Eigen::DiagonalMatrix<double, -1>& Qf);
63 
64  const Eigen::MatrixXd& getWeightQf() const override { return _Qf; }
65 
66  void setLsqForm(bool lsq_form) { _lsq_form = lsq_form; }
67 
69 
71  bool single_dt, const Eigen::VectorXd& x0, StagePreprocessor::Ptr stage_preprocessor, const std::vector<double>& dts,
72  const DiscretizationGridInterface* grid) override
73  {
74  bool dimension_modified = BaseQuadraticFinalStateCost::update(n, t, xref, uref, sref, single_dt, x0, stage_preprocessor, dts, grid);
75 
76  _x_ref = &xref;
77  _zero_x_ref = _x_ref->isZero();
78 
79  return dimension_modified;
80  }
81 
82  bool checkParameters(int state_dim, int control_dim, std::stringstream* issues) const override;
83 
84 #ifdef MESSAGE_SUPPORT
85  bool fromMessage(const messages::FinalStageCost& message, std::stringstream* issues) override;
86  void toMessage(messages::FinalStageCost& message) const override;
87 #endif
88 
89  protected:
90  Eigen::MatrixXd _Qf_sqrt;
91  Eigen::MatrixXd _Qf;
94  bool _diagonal_mode = false;
95  bool _diagonal_mode_intentionally = false;
96  bool _lsq_form = true;
97 
98  const ReferenceTrajectoryInterface* _x_ref = nullptr;
99  bool _zero_x_ref = false;
100 };
102 
104 {
105  public:
106  QuadraticFinalStateCostRiccati() { _Qf_sqrt = Eigen::MatrixXd::Constant(1, 1, 1); }
107 
110  {
111  setSystemDynamics(dynamics);
112  setWeightQ(Q);
113  setWeightR(R);
114  }
115 
116  FinalStageCost::Ptr getInstance() const override { return std::make_shared<QuadraticFinalStateCostRiccati>(); }
117 
118  int getNonIntegralStateTermDimension(int k) const override { return 1; }
119 
121  {
122  _are_solved_before = false;
123  _dynamics = dynamics;
124  }
125 
126  bool setWeightQ(const Eigen::Ref<const Eigen::MatrixXd>& Q);
127  bool setWeightQ(const Eigen::DiagonalMatrix<double, -1>& Q);
128  bool setWeightR(const Eigen::Ref<const Eigen::MatrixXd>& R);
129  bool setWeightR(const Eigen::DiagonalMatrix<double, -1>& R);
130 
131  void setLsqForm(bool lsq_form) { _lsq_form = lsq_form; }
132 
133  const Eigen::MatrixXd& getWeightQf() const override { return _Qf; }
134 
136 
138  bool single_dt, const Eigen::VectorXd& x0, StagePreprocessor::Ptr stage_preprocessor, const std::vector<double>& dts,
139  const DiscretizationGridInterface* /*grid*/) override;
140 
141  bool checkParameters(int state_dim, int control_dim, std::stringstream* issues) const override;
142 
143 #ifdef MESSAGE_SUPPORT
144  bool fromMessage(const messages::FinalStageCost& message, std::stringstream* issues) override;
145  void toMessage(messages::FinalStageCost& message) const override;
146 #endif
147 
148  protected:
149  bool computeWeightQfSqrt();
150 
151  Eigen::MatrixXd _Q;
152  Eigen::MatrixXd _R;
153  bool _Q_diagonal_mode_intentionally = false;
154  bool _R_diagonal_mode_intentionally = false;
155 
156  Eigen::MatrixXd _Qf_sqrt;
157  Eigen::MatrixXd _Qf;
159  Eigen::VectorXd _steady_state_x;
160  Eigen::VectorXd _steady_state_u;
161  bool _lsq_form = true;
162  bool _are_solved_before = false; // algebraic riccati equation solved before (e.g., for linear systems solving once is sufficient)
163 
164  const ReferenceTrajectoryInterface* _x_ref = nullptr;
165  bool _zero_x_ref = false;
166 };
168 
169 } // namespace corbo
170 
171 #endif // SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_FUNCTIONS_FINAL_STATE_COST_H_
Generic interface class for discretization grids.
FinalStageCost::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
FinalStageCost::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
const Eigen::MatrixXd & getWeightQf() const override
virtual const Eigen::MatrixXd & getWeightQf() const =0
int getNonIntegralStateTermDimension(int k) const override
std::shared_ptr< const BaseQuadraticFinalStateCost > ConstPtr
bool isLsqFormNonIntegralStateTerm(int k) const override
#define FACTORY_REGISTER_FINAL_STAGE_COST(type)
SystemDynamicsInterface::Ptr _dynamics
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:192
QuadraticFinalStateCost(const Eigen::Ref< const Eigen::MatrixXd > &Qf, bool lsq_form)
void setSystemDynamics(SystemDynamicsInterface::Ptr dynamics)
Interface class for reference trajectories.
const Eigen::MatrixXd & getWeightQf() const override
void setLsqForm(bool lsq_form)
std::shared_ptr< StagePreprocessor > Ptr
std::shared_ptr< BaseQuadraticFinalStateCost > Ptr
virtual bool checkParameters(int state_dim, int control_dim, std::stringstream *issues) const
virtual bool update(int n, double t, ReferenceTrajectoryInterface &xref, ReferenceTrajectoryInterface &uref, ReferenceTrajectoryInterface *sref, bool single_dt, const Eigen::VectorXd &x0, StagePreprocessor::Ptr stage_preprocessor, const std::vector< double > &dts, const DiscretizationGridInterface *grid)
bool update(int n, double t, ReferenceTrajectoryInterface &xref, ReferenceTrajectoryInterface &uref, ReferenceTrajectoryInterface *sref, bool single_dt, const Eigen::VectorXd &x0, StagePreprocessor::Ptr stage_preprocessor, const std::vector< double > &dts, const DiscretizationGridInterface *grid) override
std::shared_ptr< FinalStageCost > Ptr
QuadraticFinalStateCostRiccati(SystemDynamicsInterface::Ptr dynamics, const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::MatrixXd > &R)
Eigen::DiagonalMatrix< double, -1 > _Qf_diag
PlainMatrixType mat * n
Definition: eigenvalues.cpp:41
void computeNonIntegralStateTerm(int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, Eigen::Ref< Eigen::VectorXd > cost) const override=0
std::shared_ptr< SystemDynamicsInterface > Ptr
int getNonIntegralStateTermDimension(int k) const override
Eigen::DiagonalMatrix< double, -1 > _Qf_diag_sqrt


control_box_rst
Author(s): Christoph Rösmann
autogenerated on Mon Feb 28 2022 22:06:51