25 #ifndef SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_FUNCTIONS_FINAL_STATE_CONSTRAINTS_H_ 26 #define SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_FUNCTIONS_FINAL_STATE_CONSTRAINTS_H_ 41 using Ptr = std::shared_ptr<TerminalBall>;
42 using ConstPtr = std::shared_ptr<const TerminalBall>;
80 #ifdef MESSAGE_SUPPORT 81 bool fromMessage(
const messages::FinalStageConstraints& message, std::stringstream* issues)
override;
82 void toMessage(messages::FinalStageConstraints& message)
const override;
101 using Ptr = std::shared_ptr<TerminalBallInheritFromCost>;
102 using ConstPtr = std::shared_ptr<const TerminalBallInheritFromCost>;
121 #ifdef MESSAGE_SUPPORT 122 bool fromMessage(
const messages::FinalStageConstraints& message, std::stringstream* issues)
override;
123 void toMessage(messages::FinalStageConstraints& message)
const override;
133 using Ptr = std::shared_ptr<TerminalEqualityConstraint>;
134 using ConstPtr = std::shared_ptr<const TerminalEqualityConstraint>;
147 const Eigen::VectorXd&
getXRef()
const {
return _xref; }
151 assert(x_k.size() == _xref.size());
152 assert(cost.size() == _xref.size());
165 if (state_dim != _xref.size())
168 *issues <<
"TerminalEqualityConstraint: Dimension of xref (" << _xref.size() <<
") does not coincide with state dimension (" 169 << state_dim <<
")." << std::endl;
174 #ifdef MESSAGE_SUPPORT 175 bool fromMessage(
const messages::TerminalEqualityConstraint& message, std::stringstream* issues)
180 void toMessage(messages::TerminalEqualityConstraint& message)
const 182 message.mutable_x_ref()->Resize(_xref.size(), 0);
186 bool fromMessage(
const messages::FinalStageConstraints& message, std::stringstream* issues)
override 188 return fromMessage(message.terminal_equality_constraint(), issues);
190 void toMessage(messages::FinalStageConstraints& message)
const override { toMessage(*message.mutable_terminal_equality_constraint()); }
201 using Ptr = std::shared_ptr<TerminalPartialEqualityConstraint>;
202 using ConstPtr = std::shared_ptr<const TerminalPartialEqualityConstraint>;
207 setActiveComponents(active_components);
218 _active_components = active_components;
219 _num_active = _active_components.count();
225 setActiveComponents(xfixed);
227 const Eigen::VectorXd&
getXRef()
const {
return _xref; }
232 assert(x_k.size() == _xref.size());
233 assert(cost.size() == _num_active);
234 assert(_active_components.size() == _xref.size());
235 assert(_num_active == _active_components.count());
237 for (
int i = 0; i < _active_components.size(); ++i)
239 if (_active_components[i])
241 cost[idx] = x_k[i] - _xref[i];
245 assert(idx == _num_active);
257 if (state_dim != _xref.size())
260 *issues <<
"TerminalEqualityConstraint: Dimension of xref (" << _xref.size() <<
") does not coincide with state dimension (" 261 << state_dim <<
")." << std::endl;
263 if (state_dim != _active_components.size())
266 *issues <<
"TerminalEqualityConstraint: Dimension of active_components (" << _active_components.size()
267 <<
") does not coincide with state dimension (" << state_dim <<
")." << std::endl;
272 #ifdef MESSAGE_SUPPORT 273 bool fromMessage(
const messages::TerminalPartialEqualityConstraint& message, std::stringstream* issues)
280 void toMessage(messages::TerminalPartialEqualityConstraint& message)
const 282 message.mutable_x_ref()->Resize(_xref.size(), 0);
286 if (_active_components.size() > 0)
288 message.mutable_active_components()->Resize(_active_components.size(),
false);
289 Eigen::Map<
Eigen::Matrix<bool, -1, 1>>(message.mutable_active_components()->mutable_data(), _active_components.size()) =
294 bool fromMessage(
const messages::FinalStageConstraints& message, std::stringstream* issues)
override 296 return fromMessage(message.terminal_partial_eq_constr(), issues);
298 void toMessage(messages::FinalStageConstraints& message)
const override { toMessage(*message.mutable_terminal_partial_eq_constr()); }
309 #endif // SRC_OPTIMAL_CONTROL_INCLUDE_CORBO_OPTIMAL_CONTROL_FUNCTIONS_FINAL_STATE_CONSTRAINTS_H_ const ReferenceTrajectoryInterface * _x_ref
bool checkParameters(int state_dim, int control_dim, FinalStageCost::ConstPtr final_stage_cost, std::stringstream *issues) const override
const Eigen::VectorXd & getXRef() const
const Eigen::Matrix< bool, -1, 1 > & getXFixed() const
int getNonIntegralStateTermDimension(int k) const override
#define FACTORY_REGISTER_FINAL_STAGE_CONSTRAINT(type)
int getNonIntegralStateTermDimension(int k) const override
A matrix or vector expression mapping an existing array of data.
bool checkParameters(int state_dim, int control_dim, FinalStageCost::ConstPtr final_stage_cost, std::stringstream *issues) const override
Generic interface class for discretization grids.
void computeNonIntegralStateTerm(int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, Eigen::Ref< Eigen::VectorXd > cost) const override
std::shared_ptr< const FinalStageCost > ConstPtr
std::shared_ptr< const TerminalEqualityConstraint > ConstPtr
std::shared_ptr< const TerminalBall > ConstPtr
void setGamma(double gamma)
TerminalPartialEqualityConstraint(const Eigen::Ref< const Eigen::Matrix< bool, -1, 1 >> &active_components)
bool isEqualityConstraint() const override
bool _diagonal_mode_intentionally
bool update(int n, double t, ReferenceTrajectoryInterface &xref, ReferenceTrajectoryInterface &uref, ReferenceTrajectoryInterface *sref, bool single_dt, const Eigen::VectorXd &x0, FinalStageCost::ConstPtr final_stage_cost, StagePreprocessor::Ptr stage_preprocessor, const std::vector< double > &dts, const DiscretizationGridInterface *) override
const Eigen::VectorXd & getXRef() const
std::shared_ptr< FinalStageConstraint > Ptr
void computeNonIntegralStateTerm(int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, Eigen::Ref< Eigen::VectorXd > cost) const override
bool isEqualityConstraint() const override
std::shared_ptr< TerminalBall > Ptr
int getNonIntegralStateTermDimension(int k) const override
bool update(int n, double t, ReferenceTrajectoryInterface &xref, ReferenceTrajectoryInterface &uref, ReferenceTrajectoryInterface *sref, bool single_dt, const Eigen::VectorXd &x0, FinalStageCost::ConstPtr final_stage_cost, StagePreprocessor::Ptr stage_preprocessor, const std::vector< double > &dts, const DiscretizationGridInterface *) override
bool checkParameters(int state_dim, int control_dim, FinalStageCost::ConstPtr final_stage_cost, std::stringstream *issues) const override
TerminalEqualityConstraint(const Eigen::Ref< const Eigen::VectorXd > &xref)
A matrix or vector expression mapping an existing expression.
bool isEqualityConstraint() const override
virtual bool isZero() const
void setXRef(const Eigen::Ref< const Eigen::VectorXd > &xref)
bool update(int n, double t, ReferenceTrajectoryInterface &xref, ReferenceTrajectoryInterface &uref, ReferenceTrajectoryInterface *sref, bool single_dt, const Eigen::VectorXd &x0, FinalStageCost::ConstPtr final_stage_cost, StagePreprocessor::Ptr stage_preprocessor, const std::vector< double > &dts, const DiscretizationGridInterface *) override
int getNonIntegralStateTermDimension(int k) const override
FinalStageConstraint::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
void setActiveComponents(const Eigen::Ref< const Eigen::Matrix< bool, -1, 1 >> &active_components)
Interface class for reference trajectories.
void setXRef(const Eigen::Ref< const Eigen::VectorXd > &xref, const Eigen::Ref< const Eigen::Matrix< bool, -1, 1 >> &xfixed)
const Eigen::MatrixXd & getWeightS() const
FinalStageConstraint::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
std::shared_ptr< StagePreprocessor > Ptr
FinalStageConstraint::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
FinalStageConstraint::Ptr getInstance() const override
Return a newly created shared instance of the implemented class.
bool setWeightS(const Eigen::Ref< const Eigen::MatrixXd > &S)
TerminalBall(const Eigen::Ref< const Eigen::MatrixXd > &S, double gamma)
std::shared_ptr< TerminalEqualityConstraint > Ptr
Eigen::Matrix< bool, -1, 1 > _active_components
Eigen::DiagonalMatrix< double, -1 > _S_diag
void computeNonIntegralStateTerm(int k, const Eigen::Ref< const Eigen::VectorXd > &x_k, Eigen::Ref< Eigen::VectorXd > cost) const override