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_ 41 using Ptr = std::shared_ptr<BaseQuadraticFinalStateCost>;
42 using ConstPtr = std::shared_ptr<const BaseQuadraticFinalStateCost>;
44 virtual const Eigen::MatrixXd&
getWeightQf()
const = 0;
50 using Ptr = std::shared_ptr<QuadraticFinalStateCost>;
64 const Eigen::MatrixXd&
getWeightQf()
const override {
return _Qf; }
71 bool single_dt,
const Eigen::VectorXd& x0,
StagePreprocessor::Ptr stage_preprocessor,
const std::vector<double>& dts,
77 _zero_x_ref = _x_ref->
isZero();
79 return dimension_modified;
82 bool checkParameters(
int state_dim,
int control_dim, std::stringstream* issues)
const override;
84 #ifdef MESSAGE_SUPPORT 85 bool fromMessage(
const messages::FinalStageCost& message, std::stringstream* issues)
override;
86 void toMessage(messages::FinalStageCost& message)
const override;
94 bool _diagonal_mode =
false;
95 bool _diagonal_mode_intentionally =
false;
96 bool _lsq_form =
true;
99 bool _zero_x_ref =
false;
111 setSystemDynamics(dynamics);
122 _are_solved_before =
false;
123 _dynamics = dynamics;
133 const Eigen::MatrixXd&
getWeightQf()
const override {
return _Qf; }
138 bool single_dt,
const Eigen::VectorXd& x0,
StagePreprocessor::Ptr stage_preprocessor,
const std::vector<double>& dts,
141 bool checkParameters(
int state_dim,
int control_dim, std::stringstream* issues)
const override;
143 #ifdef MESSAGE_SUPPORT 144 bool fromMessage(
const messages::FinalStageCost& message, std::stringstream* issues)
override;
145 void toMessage(messages::FinalStageCost& message)
const override;
149 bool computeWeightQfSqrt();
153 bool _Q_diagonal_mode_intentionally =
false;
154 bool _R_diagonal_mode_intentionally =
false;
161 bool _lsq_form =
true;
162 bool _are_solved_before =
false;
165 bool _zero_x_ref =
false;
171 #endif // SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_FUNCTIONS_FINAL_STATE_COST_H_ Eigen::VectorXd _steady_state_x
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
Eigen::VectorXd _steady_state_u
QuadraticFinalStateCostRiccati()
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.
QuadraticFinalStateCost(const Eigen::Ref< const Eigen::MatrixXd > &Qf, bool lsq_form)
virtual bool isZero() const
void setSystemDynamics(SystemDynamicsInterface::Ptr dynamics)
Interface class for reference trajectories.
const Eigen::MatrixXd & getWeightQf() const override
QuadraticFinalStateCost()
void setLsqForm(bool lsq_form)
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
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