46 if (Qf.isDiagonal(1e-10))
return setWeightQf(Qf.diagonal().asDiagonal());
99 cost.noalias() = x_k.transpose() *
_Qf_diag * x_k;
101 cost.noalias() = x_k.transpose() *
_Qf * x_k;
107 cost.noalias() = xd.transpose() *
_Qf_diag * xd;
109 cost.noalias() = xd.transpose() *
_Qf * xd;
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;
130 if (
_Qf.rows() != state_dim ||
_Qf.cols() != state_dim)
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;
143 #ifdef MESSAGE_SUPPORT 144 bool QuadraticFinalStateCost::fromMessage(
const messages::FinalStageCost& message, std::stringstream* issues)
154 *issues <<
"QuadraticFinalStateCost: cannot set diagonal weight matrix Qf.\n";
160 int p =
std::sqrt(message.quadratic_state_cost().qf_size());
162 if (p * p != message.quadratic_state_cost().qf_size())
164 *issues <<
"QuadraticFinalStateCost: weight matrix Qf is not square.\n";
171 *issues <<
"QuadraticFinalStateCost: weight matrix Qf is not positive definite.\n";
177 _lsq_form = message.quadratic_state_cost().lsq_form();
182 void QuadraticFinalStateCost::toMessage(messages::FinalStageCost& message)
const 188 message.mutable_quadratic_state_cost()->mutable_qf()->Resize(Qfdiag.size(), 0);
191 message.mutable_quadratic_state_cost()->set_diagonal_only(
true);
196 message.mutable_quadratic_state_cost()->mutable_qf()->Resize(Qf.rows() * Qf.cols(), 0);
200 message.mutable_quadratic_state_cost()->set_diagonal_only(
false);
204 message.mutable_quadratic_state_cost()->set_lsq_form(
_lsq_form);
212 _Q_diagonal_mode_intentionally =
false;
220 _Q_diagonal_mode_intentionally =
true;
231 _R_diagonal_mode_intentionally =
false;
239 _R_diagonal_mode_intentionally =
true;
264 if (!_are_solved_before || !_dynamics->isLinear())
267 if (_steady_state_x.size() == 0 || _steady_state_u.size() == 0 || _steady_state_x != steady_state_x || _steady_state_u != steady_state_u)
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);
275 assert(_R.rows() == _dynamics->getInputDimension());
276 assert(_R.cols() == _dynamics->getInputDimension());
278 _Qf.resize(_Q.rows(), _Q.cols());
283 PRINT_WARNING(
"QuadraticFinalStateCostRiccati::update(): the linearized model is not fully controllable.");
287 if (_dynamics->isContinuousTime())
291 PRINT_ERROR(
"QuadraticFinalStateCostRiccati::update(): continuous-time algebraic riccati solver failed. Setting Qf = Q.");
299 PRINT_ERROR(
"QuadraticFinalStateCostRiccati::update(): discrete-time algebraic riccati solver failed. Setting Qf = Q.");
307 "QuadraticFinalStateCostRiccati::update(): Cholesky solution on Qf failed. Since lsq_mode is on, setting Qf_sqrt = Q_sqrt.");
309 computeWeightQfSqrt();
311 _steady_state_x = steady_state_x;
312 _steady_state_u = steady_state_u;
313 _are_solved_before =
true;
316 return dimension_modified;
339 cost.noalias() = x_k.transpose() *
_Qf * x_k;
344 cost.noalias() = xd.transpose() *
_Qf * xd;
354 if (issues) *issues <<
"QuadraticFinalStateCostRiccati: No system model specified. Cannot solve algebraic riccati equation." << std::endl;
358 if (_dynamics->getStateDimension() != state_dim)
361 *issues <<
"QuadraticFinalStateCostRiccati: State dimension of the specified sytem model (" << _dynamics->getStateDimension()
362 <<
") does not match state vector dimension (" << state_dim <<
")." << std::endl;
366 if (_dynamics->getInputDimension() != control_dim)
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;
374 if (_Q.rows() != state_dim || _Q.cols() != state_dim)
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;
383 if (_R.rows() != control_dim || _R.cols() != control_dim)
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;
395 #ifdef MESSAGE_SUPPORT 396 bool QuadraticFinalStateCostRiccati::fromMessage(
const messages::FinalStageCost& message, std::stringstream* issues)
398 const messages::QuadraticFinalStateCostRiccati& msg = message.quadratic_state_cost_riccati();
400 _Q_diagonal_mode_intentionally = msg.q_diagonal_only();
401 _R_diagonal_mode_intentionally = msg.r_diagonal_only();
404 if (!msg.has_system_dynamics())
406 if (issues) *issues <<
"QuadraticFinalStateCostRiccati: no system model specified.\n";
411 util::get_oneof_field_type_expand_isolated(msg.system_dynamics(),
"system_dynamics", type,
false, 1);
416 dynamics->fromMessage(msg.system_dynamics(), issues);
417 setSystemDynamics(dynamics);
421 if (issues) *issues <<
"QuadraticFinalStateCostRiccati: unknown system model specified.\n";
426 if (_Q_diagonal_mode_intentionally)
430 *issues <<
"QuadraticFinalStateCostRiccati: cannot set diagonal weight matrix Q.\n";
438 if (p * p != msg.q_size())
440 *issues <<
"QuadraticFinalStateCostRiccati: weight matrix Q is not square.\n";
446 *issues <<
"QuadraticFinalStateCostRiccati: weight matrix Q is not positive definite.\n";
452 if (_R_diagonal_mode_intentionally)
456 *issues <<
"QuadraticFinalStateCostRiccati: cannot set diagonal weight matrix R.\n";
464 if (q * q != msg.r_size())
466 *issues <<
"QuadraticFinalStateCostRiccati: weight matrix R is not square.\n";
472 *issues <<
"QuadraticFinalStateCostRiccati: weight matrix R is not positive definite.\n";
483 void QuadraticFinalStateCostRiccati::toMessage(messages::FinalStageCost& message)
const 485 messages::QuadraticFinalStateCostRiccati* msg = message.mutable_quadratic_state_cost_riccati();
488 if (_dynamics) _dynamics->toMessage(*msg->mutable_system_dynamics());
491 if (_Q_diagonal_mode_intentionally)
493 Eigen::VectorXd Qdiag = _Q.diagonal();
494 msg->mutable_q()->Resize(Qdiag.size(), 0);
497 msg->set_q_diagonal_only(
true);
501 Eigen::MatrixXd Q = _Q;
502 msg->mutable_q()->Resize(Q.rows() * Q.cols(), 0);
505 msg->set_q_diagonal_only(
false);
509 if (_R_diagonal_mode_intentionally)
511 Eigen::VectorXd Rdiag = _R.diagonal();
512 msg->mutable_r()->Resize(Rdiag.size(), 0);
515 msg->set_r_diagonal_only(
true);
519 Eigen::MatrixXd R = _R;
520 msg->mutable_r()->Resize(R.rows() * R.cols(), 0);
523 msg->set_r_diagonal_only(
false);
527 message.mutable_quadratic_state_cost()->set_lsq_form(
_lsq_form);
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.
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
bool computeWeightQfSqrt()
A matrix or vector expression mapping an existing array of data.
Generic interface class for discretization grids.
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
bool _diagonal_mode_intentionally
bool is_square(const Eigen::MatrixBase< Derived > &matrix)
Determine if a given matrix is square.
Representation of time stamps.
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'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.
const ReferenceTrajectoryInterface * _x_ref
EIGEN_DEVICE_FUNC const Scalar & q
ComputationInfo info() const
Reports whether previous computation was successful.
static Factory & instance()
< Retrieve static instance of the factory
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.
virtual bool isZero() const
Interface class for reference trajectories.
bool checkParameters(int state_dim, int control_dim, std::stringstream *issues) const override
Traits::MatrixU matrixU() const
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.
The matrix class, also used for vectors and row-vectors.
std::shared_ptr< SystemDynamicsInterface > Ptr
#define PRINT_ERROR(msg)
Print msg-stream as error msg.
Eigen::DiagonalMatrix< double, -1 > _Qf_diag_sqrt