aligator  0.12.0
A primal-dual augmented Lagrangian-type solver for nonlinear trajectory optimization.
 
Loading...
Searching...
No Matches
traj-opt-problem.hpp
Go to the documentation of this file.
1
3#pragma once
4
7
8namespace aligator {
9
23template <typename _Scalar> struct TrajOptProblemTpl {
25 using Scalar = _Scalar;
26
28
33 using Manifold = ManifoldAbstractTpl<Scalar>;
35 using ConstraintSet = ConstraintSetTpl<Scalar>;
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> &)>;
43
97
99 xyz::polymorphic<UnaryFunction> init_constraint_;
101 std::vector<xyz::polymorphic<StageModel>> stages_;
103 xyz::polymorphic<CostAbstract> term_cost_;
107 VectorXs unone_;
108
110
112 TrajOptProblemTpl(xyz::polymorphic<UnaryFunction> init_constraint,
113 const std::vector<xyz::polymorphic<StageModel>> &stages,
114 xyz::polymorphic<CostAbstract> term_cost);
115
118 TrajOptProblemTpl(const ConstVectorRef &x0,
119 const std::vector<xyz::polymorphic<StageModel>> &stages,
120 xyz::polymorphic<CostAbstract> term_cost);
121
123
125 TrajOptProblemTpl(xyz::polymorphic<UnaryFunction> init_constraint,
126 xyz::polymorphic<CostAbstract> term_cost);
127
130 TrajOptProblemTpl(const ConstVectorRef &x0, const int nu,
131 xyz::polymorphic<Manifold> space,
132 xyz::polymorphic<CostAbstract> term_cost);
133
134 bool initCondIsStateError() const {
135 assert(init_cond_is_state_error_ == checkInitCondIsStateError());
136 return init_cond_is_state_error_;
137 }
138
140 void addStage(const xyz::polymorphic<StageModel> &stage);
141
143 ConstVectorRef getInitState() const {
144 if (!initCondIsStateError()) {
146 "Initial condition is not a StateErrorResidual.\n");
147 }
148 return static_cast<StateErrorResidual const *>(&*init_constraint_)->target_;
149 }
150
152 void setInitState(const ConstVectorRef &x0) {
153 if (!initCondIsStateError()) {
155 "Initial condition is not a StateErrorResidual.\n");
156 }
157 static_cast<StateErrorResidual *>(&*init_constraint_)->target_ = x0;
158 }
159
161 ALIGATOR_DEPRECATED void addTerminalConstraint(const StageConstraint &cstr);
162 void addTerminalConstraint(const xyz::polymorphic<StageFunction> &func,
163 const xyz::polymorphic<ConstraintSet> &set) {
164 this->term_cstrs_.pushBack(func, set);
165 }
166
168
169 [[nodiscard]] std::size_t numSteps() const;
170
172 Scalar evaluate(const std::vector<VectorXs> &xs,
173 const std::vector<VectorXs> &us, Data &prob_data,
174 std::size_t num_threads = 1) const;
175
185 void computeDerivatives(const std::vector<VectorXs> &xs,
186 const std::vector<VectorXs> &us, Data &prob_data,
187 std::size_t num_threads = 1,
188 bool compute_second_order = true) const;
189
192 void replaceStageCircular(const xyz::polymorphic<StageModel> &model);
193
194 bool checkIntegrity() const;
195
197 //
203 template <typename Callable> void setInitializationStrategy(Callable &&func) {
204 static_assert(std::is_convertible_v<Callable, InitializationStrategy>);
205 this->xs_init_strategy_ = std::forward<Callable>(func);
206 }
207
212 void initializeSolution(std::vector<VectorXs> &xs,
213 std::vector<VectorXs> &us) const {
214 xs_init_strategy_(*this, xs);
215 us_default_init(*this, us);
216 }
217
219 void initializeSolution(std::vector<VectorXs> &xs, std::vector<VectorXs> &us,
220 std::vector<VectorXs> &vs,
221 std::vector<VectorXs> &lbdas) const {
222 const size_t nsteps = numSteps();
223 initializeSolution(xs, us);
224 // initialize multipliers...
225 vs.resize(nsteps + 1);
226 lbdas.resize(nsteps + 1);
227 lbdas[0].setZero(init_constraint_->nr);
228 for (size_t i = 0; i < nsteps; i++) {
229 const StageModelTpl<Scalar> &sm = *stages_[i];
230 lbdas[i + 1].setZero(sm.ndx2());
231 vs[i].setZero(sm.nc());
232 }
233
234 if (!term_cstrs_.empty()) {
235 vs[nsteps].setZero(term_cstrs_.totalDim());
236 }
237 }
238
240 [[nodiscard]] auto initializeSolution() const {
241 std::vector<VectorXs> xs, us, vs, lbdas;
242 initializeSolution(xs, us, vs, lbdas);
243 return std::make_tuple(std::move(xs), std::move(us), std::move(vs),
244 std::move(lbdas));
245 }
246
247private:
248 InitializationStrategy xs_init_strategy_;
249
250 // Check if the initial state is a StateErrorResidual.
251 // Since this is a costly operation (dynamic_cast), we cache the result.
252 bool checkInitCondIsStateError() const;
253 bool init_cond_is_state_error_ = false;
254};
255
256namespace internal {
257template <typename Scalar>
258auto problem_last_state_space_helper(const TrajOptProblemTpl<Scalar> &problem) {
259 return problem.term_cost_->space;
260}
261
263template <typename Scalar>
264int problem_last_ndx_helper(const TrajOptProblemTpl<Scalar> &problem) {
265 return problem_last_state_space_helper(problem)->ndx();
266}
267} // namespace internal
268
269} // namespace aligator
270
271#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
272#include "aligator/core/traj-opt-problem.txx"
273#endif
#define ALIGATOR_RUNTIME_ERROR(...)
Definition exceptions.hpp:7
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)
Main package namespace.
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.
Problem data struct.
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_
StageFunctionTpl< Scalar > StageFunction
void setInitState(const ConstVectorRef &x0)
Set initial state constraint.
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_
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.
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.
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.