final_state_cost.cpp
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 
27 
32 
34 
35 #include <cmath>
36 
37 namespace corbo {
38 
40 {
42 
43  if (!is_square(Qf)) return false;
44 
45  // check if we have a diagonal matrix
46  if (Qf.isDiagonal(1e-10)) return setWeightQf(Qf.diagonal().asDiagonal());
47 
48  _diagonal_mode = false;
49 
50  _Qf = Qf; // also store original Qf
51  if (Qf.isZero())
52  {
53  _Qf_sqrt.setZero();
54  return true;
55  }
57  if (cholesky.info() == Eigen::NumericalIssue) return false;
58  _Qf_sqrt = cholesky.matrixU();
59  return true;
60 }
61 
63 {
65  _diagonal_mode = true;
66  _Qf_diag = Qf;
67  _Qf_diag_sqrt = Qf.diagonal().cwiseSqrt().asDiagonal();
69  return true;
70 }
71 
73 {
74  assert(cost.size() == getNonIntegralStateTermDimension(k));
75 
76  if (_lsq_form)
77  {
78  if (_zero_x_ref)
79  {
80  if (_diagonal_mode)
81  cost.noalias() = _Qf_diag_sqrt * x_k;
82  else
83  cost.noalias() = _Qf_sqrt * x_k;
84  }
85  else
86  {
87  Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k);
88  if (_diagonal_mode)
89  cost.noalias() = _Qf_diag_sqrt * xd;
90  else
91  cost.noalias() = _Qf_sqrt * xd;
92  }
93  }
94  else
95  {
96  if (_zero_x_ref)
97  {
98  if (_diagonal_mode)
99  cost.noalias() = x_k.transpose() * _Qf_diag * x_k;
100  else
101  cost.noalias() = x_k.transpose() * _Qf * x_k;
102  }
103  else
104  {
105  Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k);
106  if (_diagonal_mode)
107  cost.noalias() = xd.transpose() * _Qf_diag * xd;
108  else
109  cost.noalias() = xd.transpose() * _Qf * xd;
110  }
111  }
112 }
113 
114 bool QuadraticFinalStateCost::checkParameters(int state_dim, int control_dim, std::stringstream* issues) const
115 {
116  bool success = true;
117 
119  {
120  if (_Qf_diag.diagonal().size() != state_dim)
121  {
122  if (issues)
123  *issues << "QuadraticFinalStateCost: Diagonal matrix dimension of Qf (" << _Qf_diag.diagonal().size()
124  << ") does not match state vector dimension (" << state_dim << "); Please specify diagonal elements only." << std::endl;
125  success = false;
126  }
127  }
128  else
129  {
130  if (_Qf.rows() != state_dim || _Qf.cols() != state_dim)
131  {
132  if (issues)
133  *issues << "QuadraticFinalStateCost: Matrix dimension of Qf (" << _Qf.rows() << "x" << _Qf.cols()
134  << ") does not match state vector dimension (" << state_dim << "); Please specify " << (state_dim * state_dim)
135  << " elements (Row-Major)." << std::endl;
136  success = false;
137  }
138  }
139 
140  return success;
141 }
142 
143 #ifdef MESSAGE_SUPPORT
144 bool QuadraticFinalStateCost::fromMessage(const messages::FinalStageCost& message, std::stringstream* issues)
145 {
146  _diagonal_mode = message.quadratic_state_cost().diagonal_only();
147 
148  if (_diagonal_mode)
149  {
150  if (!setWeightQf(
151  Eigen::Map<const Eigen::Matrix<double, -1, 1>>(message.quadratic_state_cost().qf().data(), message.quadratic_state_cost().qf_size())
152  .asDiagonal()))
153  {
154  *issues << "QuadraticFinalStateCost: cannot set diagonal weight matrix Qf.\n";
155  return false;
156  }
157  }
158  else
159  {
160  int p = std::sqrt(message.quadratic_state_cost().qf_size());
161 
162  if (p * p != message.quadratic_state_cost().qf_size())
163  {
164  *issues << "QuadraticFinalStateCost: weight matrix Qf is not square.\n";
165  return false;
166  }
167 
168  if (!setWeightQf(Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(message.quadratic_state_cost().qf().data(), p, p)) &&
169  issues)
170  {
171  *issues << "QuadraticFinalStateCost: weight matrix Qf is not positive definite.\n";
172  return false;
173  }
174  }
175 
176  // lsq form
177  _lsq_form = message.quadratic_state_cost().lsq_form();
178 
179  return true;
180 }
181 
182 void QuadraticFinalStateCost::toMessage(messages::FinalStageCost& message) const
183 {
184  // weight matrix Q
186  {
187  Eigen::VectorXd Qfdiag = _Qf_diag.diagonal();
188  message.mutable_quadratic_state_cost()->mutable_qf()->Resize(Qfdiag.size(), 0);
189  Eigen::Map<Eigen::VectorXd>(message.mutable_quadratic_state_cost()->mutable_qf()->mutable_data(), Qfdiag.size()) = Qfdiag;
190 
191  message.mutable_quadratic_state_cost()->set_diagonal_only(true);
192  }
193  else
194  {
195  Eigen::MatrixXd Qf = _Qf_sqrt.adjoint() * _Qf_sqrt;
196  message.mutable_quadratic_state_cost()->mutable_qf()->Resize(Qf.rows() * Qf.cols(), 0);
197  Eigen::Map<Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(message.mutable_quadratic_state_cost()->mutable_qf()->mutable_data(), Qf.rows(),
198  Qf.cols()) = Qf;
199 
200  message.mutable_quadratic_state_cost()->set_diagonal_only(false);
201  }
202 
203  // lsq_form
204  message.mutable_quadratic_state_cost()->set_lsq_form(_lsq_form);
205 }
206 #endif
207 
209 {
210  if (!is_square(Q)) return false;
211  if (!is_positive_definite(Q)) return false;
212  _Q_diagonal_mode_intentionally = false;
213 
214  _Q = Q;
215  return true;
216 }
217 
219 {
220  _Q_diagonal_mode_intentionally = true;
221 
222  _Q = Q.toDenseMatrix();
223  if (!is_positive_definite(_Q)) return false;
224  return true;
225 }
226 
228 {
229  if (!is_square(R)) return false;
230  if (!is_positive_definite(R)) return false;
231  _R_diagonal_mode_intentionally = false;
232 
233  _R = R;
234  return true;
235 }
236 
238 {
239  _R_diagonal_mode_intentionally = true;
240 
241  _R = R.toDenseMatrix();
242  if (!is_positive_definite(_R)) return false;
243  return true;
244 }
245 
247  ReferenceTrajectoryInterface* sref, bool single_dt, const Eigen::VectorXd& x0,
248  StagePreprocessor::Ptr stage_preprocessor, const std::vector<double>& dts,
249  const DiscretizationGridInterface* grid)
250 {
251  bool dimension_modified = BaseQuadraticFinalStateCost::update(n, t, xref, uref, sref, single_dt, x0, stage_preprocessor, dts, grid);
252 
253  // store reference trajectory
254  _x_ref = &xref;
255 
257 
258  assert(_dynamics);
259 
260  Time t_ref(t);
261  Eigen::VectorXd steady_state_x = xref.getNextSteadyState(t_ref);
262  Eigen::VectorXd steady_state_u = uref.getNextSteadyState(t_ref);
263 
264  if (!_are_solved_before || !_dynamics->isLinear())
265  {
266  // don't solve if steady steade has not changed
267  if (_steady_state_x.size() == 0 || _steady_state_u.size() == 0 || _steady_state_x != steady_state_x || _steady_state_u != steady_state_u)
268  {
269  Eigen::MatrixXd A(_dynamics->getStateDimension(), _dynamics->getStateDimension());
270  Eigen::MatrixXd B(_dynamics->getStateDimension(), _dynamics->getInputDimension());
271  _dynamics->getLinearA(steady_state_x, steady_state_u, A);
272  _dynamics->getLinearB(steady_state_x, steady_state_u, B);
273 
274  assert(have_equal_size(_Q, A));
275  assert(_R.rows() == _dynamics->getInputDimension());
276  assert(_R.cols() == _dynamics->getInputDimension());
277 
278  _Qf.resize(_Q.rows(), _Q.cols());
279 
280 #ifndef NDEBUG
282  {
283  PRINT_WARNING("QuadraticFinalStateCostRiccati::update(): the linearized model is not fully controllable.");
284  }
285 #endif
286 
287  if (_dynamics->isContinuousTime())
288  {
289  if (!AlgebraicRiccatiContinuous::solve(A, B, _Q, _R, _Qf))
290  {
291  PRINT_ERROR("QuadraticFinalStateCostRiccati::update(): continuous-time algebraic riccati solver failed. Setting Qf = Q.");
292  _Qf = _Q;
293  }
294  }
295  else
296  {
297  if (!AlgebraicRiccatiDiscrete::solve(A, B, _Q, _R, _Qf))
298  {
299  PRINT_ERROR("QuadraticFinalStateCostRiccati::update(): discrete-time algebraic riccati solver failed. Setting Qf = Q.");
300  _Qf = _Q;
301  }
302  }
303  // update internals
304  if (!computeWeightQfSqrt() && _lsq_form)
305  {
306  PRINT_ERROR(
307  "QuadraticFinalStateCostRiccati::update(): Cholesky solution on Qf failed. Since lsq_mode is on, setting Qf_sqrt = Q_sqrt.");
308  _Qf = _Q;
309  computeWeightQfSqrt();
310  }
311  _steady_state_x = steady_state_x;
312  _steady_state_u = steady_state_u;
313  _are_solved_before = true;
314  }
315  }
316  return dimension_modified;
317 }
318 
320 {
321  if (_Qf.isZero())
322  {
323  _Qf_sqrt.setZero();
324  return true;
325  }
327  if (cholesky.info() == Eigen::NumericalIssue) return false;
328  _Qf_sqrt = cholesky.matrixU();
329  return true;
330 }
331 
333  Eigen::Ref<Eigen::VectorXd> cost) const
334 {
335  assert(cost.size() == getNonIntegralStateTermDimension(k));
336 
337  if (_zero_x_ref)
338  {
339  cost.noalias() = x_k.transpose() * _Qf * x_k;
340  }
341  else
342  {
343  Eigen::VectorXd xd = x_k - _x_ref->getReferenceCached(k);
344  cost.noalias() = xd.transpose() * _Qf * xd;
345  }
346 }
347 
348 bool QuadraticFinalStateCostRiccati::checkParameters(int state_dim, int control_dim, std::stringstream* issues) const
349 {
350  bool success = true;
351 
352  if (!_dynamics)
353  {
354  if (issues) *issues << "QuadraticFinalStateCostRiccati: No system model specified. Cannot solve algebraic riccati equation." << std::endl;
355  return false;
356  }
357 
358  if (_dynamics->getStateDimension() != state_dim)
359  {
360  if (issues)
361  *issues << "QuadraticFinalStateCostRiccati: State dimension of the specified sytem model (" << _dynamics->getStateDimension()
362  << ") does not match state vector dimension (" << state_dim << ")." << std::endl;
363  success = false;
364  }
365 
366  if (_dynamics->getInputDimension() != control_dim)
367  {
368  if (issues)
369  *issues << "QuadraticFinalStateCostRiccati: Control input dimension of the specified sytem model (" << _dynamics->getStateDimension()
370  << ") does not match control input vector dimension (" << control_dim << ")." << std::endl;
371  success = false;
372  }
373 
374  if (_Q.rows() != state_dim || _Q.cols() != state_dim)
375  {
376  if (issues)
377  *issues << "QuadraticFinalStateCostRiccati: Matrix dimension of Q (" << _Q.rows() << "x" << _Q.cols()
378  << ") does not match state vector dimension (" << state_dim << "); Please specify " << (state_dim * state_dim)
379  << " elements (Row-Major)." << std::endl;
380  success = false;
381  }
382 
383  if (_R.rows() != control_dim || _R.cols() != control_dim)
384  {
385  if (issues)
386  *issues << "QuadraticFinalStateCostRiccati: Matrix dimension of R (" << _R.rows() << "x" << _R.cols()
387  << ") does not match control input vector dimension (" << control_dim << "); Please specify " << (control_dim * control_dim)
388  << " elements (Row-Major)." << std::endl;
389  success = false;
390  }
391 
392  return success;
393 }
394 
395 #ifdef MESSAGE_SUPPORT
396 bool QuadraticFinalStateCostRiccati::fromMessage(const messages::FinalStageCost& message, std::stringstream* issues)
397 {
398  const messages::QuadraticFinalStateCostRiccati& msg = message.quadratic_state_cost_riccati();
399 
400  _Q_diagonal_mode_intentionally = msg.q_diagonal_only();
401  _R_diagonal_mode_intentionally = msg.r_diagonal_only();
402 
403  // system dynamics
404  if (!msg.has_system_dynamics())
405  {
406  if (issues) *issues << "QuadraticFinalStateCostRiccati: no system model specified.\n";
407  return false;
408  }
409  // construct object
410  std::string type;
411  util::get_oneof_field_type_expand_isolated(msg.system_dynamics(), "system_dynamics", type, false, 1);
413  // import parameters
414  if (dynamics)
415  {
416  dynamics->fromMessage(msg.system_dynamics(), issues);
417  setSystemDynamics(dynamics);
418  }
419  else
420  {
421  if (issues) *issues << "QuadraticFinalStateCostRiccati: unknown system model specified.\n";
422  return false;
423  }
424 
425  // Weight matrix Q
426  if (_Q_diagonal_mode_intentionally)
427  {
428  if (!setWeightQ(Eigen::Map<const Eigen::Matrix<double, -1, 1>>(msg.q().data(), msg.q_size()).asDiagonal()))
429  {
430  *issues << "QuadraticFinalStateCostRiccati: cannot set diagonal weight matrix Q.\n";
431  return false;
432  }
433  }
434  else
435  {
436  int p = std::sqrt(msg.q_size());
437 
438  if (p * p != msg.q_size())
439  {
440  *issues << "QuadraticFinalStateCostRiccati: weight matrix Q is not square.\n";
441  return false;
442  }
443 
444  if (!setWeightQ(Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(msg.q().data(), p, p)) && issues)
445  {
446  *issues << "QuadraticFinalStateCostRiccati: weight matrix Q is not positive definite.\n";
447  return false;
448  }
449  }
450 
451  // Weight matrix R
452  if (_R_diagonal_mode_intentionally)
453  {
454  if (!setWeightR(Eigen::Map<const Eigen::Matrix<double, -1, 1>>(msg.r().data(), msg.r_size()).asDiagonal()))
455  {
456  *issues << "QuadraticFinalStateCostRiccati: cannot set diagonal weight matrix R.\n";
457  return false;
458  }
459  }
460  else
461  {
462  double q = std::sqrt(msg.r_size());
463 
464  if (q * q != msg.r_size())
465  {
466  *issues << "QuadraticFinalStateCostRiccati: weight matrix R is not square.\n";
467  return false;
468  }
469 
470  if (!setWeightR(Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(msg.r().data(), q, q)) && issues)
471  {
472  *issues << "QuadraticFinalStateCostRiccati: weight matrix R is not positive definite.\n";
473  return false;
474  }
475  }
476 
477  // lsq form
478  _lsq_form = msg.lsq_form();
479 
480  return true;
481 }
482 
483 void QuadraticFinalStateCostRiccati::toMessage(messages::FinalStageCost& message) const
484 {
485  messages::QuadraticFinalStateCostRiccati* msg = message.mutable_quadratic_state_cost_riccati();
486 
487  // system model
488  if (_dynamics) _dynamics->toMessage(*msg->mutable_system_dynamics());
489 
490  // weight matrix Q
491  if (_Q_diagonal_mode_intentionally)
492  {
493  Eigen::VectorXd Qdiag = _Q.diagonal();
494  msg->mutable_q()->Resize(Qdiag.size(), 0);
495  Eigen::Map<Eigen::VectorXd>(msg->mutable_q()->mutable_data(), Qdiag.size()) = Qdiag;
496 
497  msg->set_q_diagonal_only(true);
498  }
499  else
500  {
501  Eigen::MatrixXd Q = _Q; // Q = _Q_sqrt.adjoint() * _Q_sqrt;
502  msg->mutable_q()->Resize(Q.rows() * Q.cols(), 0);
503  Eigen::Map<Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(msg->mutable_q()->mutable_data(), Q.rows(), Q.cols()) = Q;
504 
505  msg->set_q_diagonal_only(false);
506  }
507 
508  // weight matrix R
509  if (_R_diagonal_mode_intentionally)
510  {
511  Eigen::VectorXd Rdiag = _R.diagonal();
512  msg->mutable_r()->Resize(Rdiag.size(), 0);
513  Eigen::Map<Eigen::VectorXd>(msg->mutable_r()->mutable_data(), Rdiag.size()) = Rdiag;
514 
515  msg->set_r_diagonal_only(true);
516  }
517  else
518  {
519  Eigen::MatrixXd R = _R; // R = _R_sqrt.adjoint() * _R_sqrt;
520  msg->mutable_r()->Resize(R.rows() * R.cols(), 0);
521  Eigen::Map<Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(msg->mutable_r()->mutable_data(), R.rows(), R.cols()) = R;
522 
523  msg->set_r_diagonal_only(false);
524  }
525 
526  // lsq_form
527  message.mutable_quadratic_state_cost()->set_lsq_form(_lsq_form);
528 }
529 #endif
530 
531 } // namespace corbo
static bool solve(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::MatrixXd > &B, const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::MatrixXd > &R, Eigen::MatrixXd &X, Eigen::MatrixXd *G=nullptr)
Solve continuous-time algebraic Riccati equation.
bool have_equal_size(const Eigen::MatrixBase< DerivedA > &matrix1, const Eigen::MatrixBase< DerivedB > &matrix2)
Determine if two matrices exhibit equal sizes/dimensions.
#define PRINT_WARNING(msg)
Print msg-stream.
Definition: console.h:145
static bool checkLinearTimeInvariantSystem(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::MatrixXd > &B, int *rank=nullptr)
Check controllability of linear time invariant system.
EIGEN_DEVICE_FUNC const DiagonalVectorType & diagonal() const
A matrix or vector expression mapping an existing array of data.
Definition: Map.h:94
Generic interface class for discretization grids.
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
bool is_square(const Eigen::MatrixBase< Derived > &matrix)
Determine if a given matrix is square.
Representation of time stamps.
Definition: time.h:251
bool checkParameters(int state_dim, int control_dim, std::stringstream *issues) const override
virtual const OutputVector & getReferenceCached(int k) const =0
bool is_positive_definite(const Eigen::MatrixBase< Derived > &matrix)
Determine if a given matrix is positive definiteThe current implementation performs Eigen&#39;s Cholesky ...
MatrixType A(a, *n, *n, *lda)
int getNonIntegralStateTermDimension(int k) const override
void computeNonIntegralStateTerm(int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, Eigen::Ref< Eigen::VectorXd > cost) const override
bool setWeightQ(const Eigen::Ref< const Eigen::MatrixXd > &Q)
Standard Cholesky decomposition (LL^T) of a matrix and associated features.
Definition: LLT.h:56
const ReferenceTrajectoryInterface * _x_ref
EIGEN_DEVICE_FUNC const Scalar & q
ComputationInfo info() const
Reports whether previous computation was successful.
Definition: LLT.h:186
static Factory & instance()
< Retrieve static instance of the factory
Definition: factory.h:72
static bool solve(const Eigen::Ref< const Eigen::MatrixXd > &A, const Eigen::Ref< const Eigen::MatrixXd > &B, const Eigen::Ref< const Eigen::MatrixXd > &Q, const Eigen::Ref< const Eigen::MatrixXd > &R, Eigen::MatrixXd &X, Eigen::MatrixXd *G=nullptr)
Solve discrete-time algebraic Riccati equation.
bool setWeightQf(const Eigen::Ref< const Eigen::MatrixXd > &Qf)
bool setWeightR(const Eigen::Ref< const Eigen::MatrixXd > &R)
A matrix or vector expression mapping an existing expression.
Definition: Ref.h:192
Interface class for reference trajectories.
bool checkParameters(int state_dim, int control_dim, std::stringstream *issues) const override
Traits::MatrixU matrixU() const
Definition: LLT.h:119
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 *) override
std::shared_ptr< StagePreprocessor > Ptr
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)
virtual const OutputVector & getNextSteadyState(const Time &t)=0
EIGEN_DEVICE_FUNC DenseMatrixType toDenseMatrix() const
MatrixType B(b, *n, *nrhs, *ldb)
Eigen::DiagonalMatrix< double, -1 > _Qf_diag
void computeNonIntegralStateTerm(int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, Eigen::Ref< Eigen::VectorXd > cost) const override
std::shared_ptr< Derived > create(const std::string &name, bool print_error=true) const
Create a shared instance of the desired object.
Definition: factory.h:93
The matrix class, also used for vectors and row-vectors.
Definition: Matrix.h:178
PlainMatrixType mat * n
Definition: eigenvalues.cpp:41
std::shared_ptr< SystemDynamicsInterface > Ptr
#define PRINT_ERROR(msg)
Print msg-stream as error msg.
Definition: console.h:173
Eigen::DiagonalMatrix< double, -1 > _Qf_diag_sqrt


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