37#pragma GCC diagnostic push
38#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
40#pragma GCC diagnostic pop
42 std::function<void(
const Self &, std::vector<VectorXs> &)>;
101 std::vector<xyz::polymorphic<StageModel>>
stages_;
113 const std::vector<xyz::polymorphic<StageModel>> &stages,
114 xyz::polymorphic<CostAbstract> term_cost);
119 const std::vector<xyz::polymorphic<StageModel>> &stages,
120 xyz::polymorphic<CostAbstract> term_cost);
126 xyz::polymorphic<CostAbstract> term_cost);
131 xyz::polymorphic<Manifold> space,
132 xyz::polymorphic<CostAbstract> term_cost);
135 assert(init_cond_is_state_error_ == checkInitCondIsStateError());
136 return init_cond_is_state_error_;
140 void addStage(
const xyz::polymorphic<StageModel> &stage);
146 "Initial condition is not a StateErrorResidual.\n");
155 "Initial condition is not a StateErrorResidual.\n");
163 const xyz::polymorphic<ConstraintSet> &set) {
164 this->term_cstrs_.
pushBack(func, set);
173 const std::vector<VectorXs> &us,
Data &prob_data,
174 std::size_t num_threads = 1)
const;
186 const std::vector<VectorXs> &us,
Data &prob_data,
187 std::size_t num_threads = 1,
188 bool compute_second_order =
true)
const;
204 static_assert(std::is_convertible_v<Callable, InitializationStrategy>);
205 this->xs_init_strategy_ = std::forward<Callable>(func);
213 std::vector<VectorXs> &us)
const {
214 xs_init_strategy_(*
this, xs);
220 std::vector<VectorXs> &vs,
221 std::vector<VectorXs> &lbdas)
const {
225 vs.resize(nsteps + 1);
226 lbdas.resize(nsteps + 1);
228 for (
size_t i = 0; i < nsteps; i++) {
230 lbdas[i + 1].setZero(sm.
ndx2());
231 vs[i].setZero(sm.
nc());
241 std::vector<VectorXs> xs, us, vs, lbdas;
243 return std::make_tuple(std::move(xs), std::move(us), std::move(vs),
252 bool checkInitCondIsStateError()
const;
253 bool init_cond_is_state_error_ =
false;
257template <
typename Scalar>
258auto problem_last_state_space_helper(
const TrajOptProblemTpl<Scalar> &problem) {
259 return problem.term_cost_->space;
263template <
typename Scalar>
264int problem_last_ndx_helper(
const TrajOptProblemTpl<Scalar> &problem) {
265 return problem_last_state_space_helper(problem)->ndx();
271#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
272#include "aligator/core/traj-opt-problem.txx"
#define ALIGATOR_RUNTIME_ERROR(...)
TrajOptProblemTpl(const ConstVectorRef &x0, const std::vector< xyz::polymorphic< StageModel > > &stages, xyz::polymorphic< CostAbstract > term_cost)
Constructor for an initial value problem.
TrajOptProblemTpl(xyz::polymorphic< UnaryFunction > init_constraint, const std::vector< xyz::polymorphic< StageModel > > &stages, xyz::polymorphic< CostAbstract > term_cost)
TrajOptProblemTpl(const ConstVectorRef &x0, const int nu, xyz::polymorphic< Manifold > space, xyz::polymorphic< CostAbstract > term_cost)
Constructor for an initial value problem.
TrajOptProblemTpl(xyz::polymorphic< UnaryFunction > init_constraint, xyz::polymorphic< CostAbstract > term_cost)
void us_default_init(const TrajOptProblemTpl< Scalar > &problem, std::vector< typename math_types< Scalar >::VectorXs > &us)
Default-initialize a controls trajectory from the neutral element of each control space.
Convenience class to manage a stack of constraints.
ALIGATOR_DEPRECATED void pushBack(Cstr &&el)
Stage costs for control problems.
Simple struct holding together a function and set, to describe a constraint.
Class representing ternary functions .
A stage in the control problem.
int nc() const
Total number of constraints.
void addStage(const xyz::polymorphic< StageModel > &stage)
Add a stage to the control problem.
void addTerminalConstraint(const xyz::polymorphic< StageFunction > &func, const xyz::polymorphic< ConstraintSet > &set)
ConstraintSetTpl< Scalar > ConstraintSet
ConstraintStackTpl< Scalar > term_cstrs_
bool initCondIsStateError() const
StageFunctionTpl< Scalar > StageFunction
void setInitState(const ConstVectorRef &x0)
Set initial state constraint.
TrajOptDataTpl< Scalar > Data
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
void computeDerivatives(const std::vector< VectorXs > &xs, const std::vector< VectorXs > &us, Data &prob_data, std::size_t num_threads=1, bool compute_second_order=true) const
Rollout the problem derivatives, stage per stage.
void initializeSolution(std::vector< VectorXs > &xs, std::vector< VectorXs > &us) const
Execute the initialization strategy to generate an initial candidate solution to the problem.
StageConstraintTpl< Scalar > StageConstraint
xyz::polymorphic< UnaryFunction > init_constraint_
void setInitializationStrategy(Callable &&func)
Set a function to initialize the state trajectory.
UnaryFunctionTpl< Scalar > UnaryFunction
void removeTerminalConstraints()
Remove all terminal constraints.
xyz::polymorphic< CostAbstract > term_cost_
CostAbstractTpl< Scalar > CostAbstract
Scalar evaluate(const std::vector< VectorXs > &xs, const std::vector< VectorXs > &us, Data &prob_data, std::size_t num_threads=1) const
Rollout the problem costs, constraints, dynamics, stage per stage.
bool checkIntegrity() const
void replaceStageCircular(const xyz::polymorphic< StageModel > &model)
Pop out the first StageModel and replace by the supplied one; updates the supplied problem data (Traj...
ALIGATOR_DEPRECATED void addTerminalConstraint(const StageConstraint &cstr)
Add a terminal constraint for the model.
ConstVectorRef getInitState() const
Get initial state constraint.
auto initializeSolution() const
Execute the initialization strategy to generate an initial candidate solution to the problem.
StageModelTpl< Scalar > StageModel
std::vector< xyz::polymorphic< StageModel > > stages_
StateErrorResidualTpl< Scalar > StateErrorResidual
std::function< void(const Self &, std::vector< VectorXs > &)> InitializationStrategy
std::size_t numSteps() const
ManifoldAbstractTpl< Scalar > Manifold
void initializeSolution(std::vector< VectorXs > &xs, std::vector< VectorXs > &us, std::vector< VectorXs > &vs, std::vector< VectorXs > &lbdas) const
Execute the initialization strategy to generate an initial candidate solution to the problem.
Represents unary functions of the form , with no control (or next-state) arguments.