aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
traj-opt-problem.hpp
Go to the documentation of this file.
1
3#pragma once
4
8
9namespace aligator {
10
64
67template <typename T1, typename T2>
68[[nodiscard]] bool assign_no_resize(const std::vector<T1> &lhs,
69 std::vector<T2> &rhs) {
70 static_assert(std::is_base_of_v<Eigen::EigenBase<T1>, T1>,
71 "T1 should be an Eigen object!");
72 static_assert(std::is_base_of_v<Eigen::EigenBase<T2>, T2>,
73 "T2 should be an Eigen object!");
74 if (lhs.size() != rhs.size())
75 return false;
76
77 const auto same_dims = [](auto &x, auto &y) {
78 return (x.cols() == y.cols()) && (x.rows() == y.rows());
79 };
80
81 for (std::size_t i = 0; i < lhs.size(); i++) {
82 if (!same_dims(lhs[i], rhs[i]))
83 return false;
84 rhs[i] = lhs[i];
85 }
86 return true;
87}
88
102template <typename _Scalar> struct TrajOptProblemTpl {
104 using Scalar = _Scalar;
105
107
116#pragma GCC diagnostic push
117#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
119#pragma GCC diagnostic pop
121 std::function<void(const Self &, std::vector<VectorXs> &)>;
122
124 xyz::polymorphic<UnaryFunction> init_constraint_;
126 std::vector<xyz::polymorphic<StageModel>> stages_;
128 xyz::polymorphic<CostAbstract> term_cost_;
132 VectorXs unone_;
133
136
138 TrajOptProblemTpl(xyz::polymorphic<UnaryFunction> init_constraint,
139 const std::vector<xyz::polymorphic<StageModel>> &stages,
140 xyz::polymorphic<CostAbstract> term_cost);
141
143 TrajOptProblemTpl(const ConstVectorRef &x0,
144 const std::vector<xyz::polymorphic<StageModel>> &stages,
145 xyz::polymorphic<CostAbstract> term_cost);
147
150
152 TrajOptProblemTpl(xyz::polymorphic<UnaryFunction> init_constraint,
153 xyz::polymorphic<CostAbstract> term_cost);
154
156 TrajOptProblemTpl(const ConstVectorRef &x0, const int nu,
157 xyz::polymorphic<Manifold> space,
158 xyz::polymorphic<CostAbstract> term_cost);
160
161 bool initCondIsStateError() const {
162 assert(init_cond_is_state_error_ == checkInitCondIsStateError());
163 return init_cond_is_state_error_;
164 }
165
167 void addStage(const xyz::polymorphic<StageModel> &stage);
168
170 ConstVectorRef getInitState() const {
171 if (!initCondIsStateError()) {
173 "Initial condition is not a StateErrorResidual.\n");
174 }
175 return static_cast<StateErrorResidual const *>(&*init_constraint_)->target_;
176 }
177
179 void setInitState(const ConstVectorRef &x0) {
180 if (!initCondIsStateError()) {
182 "Initial condition is not a StateErrorResidual.\n");
183 }
184 static_cast<StateErrorResidual *>(&*init_constraint_)->target_ = x0;
185 }
186
188 ALIGATOR_DEPRECATED void addTerminalConstraint(const StageConstraint &cstr);
190 void addTerminalConstraint(const xyz::polymorphic<StageFunction> &func,
191 const xyz::polymorphic<ConstraintSet> &set) {
192 this->term_cstrs_.pushBack(func, set);
193 }
194
196
197 [[nodiscard]] std::size_t numSteps() const;
198
200 Scalar evaluate(const std::vector<VectorXs> &xs,
201 const std::vector<VectorXs> &us, Data &prob_data,
202 std::size_t num_threads = 1) const;
203
213 void computeDerivatives(const std::vector<VectorXs> &xs,
214 const std::vector<VectorXs> &us, Data &prob_data,
215 std::size_t num_threads = 1,
216 bool compute_second_order = true) const;
217
220 void replaceStageCircular(const xyz::polymorphic<StageModel> &model);
221
222 bool checkIntegrity() const;
223
225 //
231 template <typename Callable> void setInitializationStrategy(Callable &&func) {
232 static_assert(std::is_convertible_v<Callable, InitializationStrategy>);
233 this->xs_init_strategy_ = std::forward<Callable>(func);
234 }
235
240 void initializeSolution(std::vector<VectorXs> &xs,
241 std::vector<VectorXs> &us) const {
242 xs_init_strategy_(*this, xs);
243 us_default_init(*this, us);
244 }
245
247 void initializeSolution(std::vector<VectorXs> &xs, std::vector<VectorXs> &us,
248 std::vector<VectorXs> &vs,
249 std::vector<VectorXs> &lbdas) const {
250 const size_t nsteps = numSteps();
251 initializeSolution(xs, us);
252 // initialize multipliers...
253 vs.resize(nsteps + 1);
254 lbdas.resize(nsteps + 1);
255 lbdas[0].setZero(init_constraint_->nr);
256 for (size_t i = 0; i < nsteps; i++) {
257 const StageModelTpl<Scalar> &sm = *stages_[i];
258 lbdas[i + 1].setZero(sm.ndx2());
259 vs[i].setZero(sm.nc());
260 }
261
262 if (!term_cstrs_.empty()) {
263 vs[nsteps].setZero(term_cstrs_.totalDim());
264 }
265 }
266
268 [[nodiscard]] auto initializeSolution() const {
269 std::vector<VectorXs> xs, us, vs, lbdas;
270 initializeSolution(xs, us, vs, lbdas);
271 return std::make_tuple(std::move(xs), std::move(us), std::move(vs),
272 std::move(lbdas));
273 }
274
275private:
276 InitializationStrategy xs_init_strategy_;
277
278 // Check if the initial state is a StateErrorResidual.
279 // Since this is a costly operation (dynamic_cast), we cache the result.
280 bool checkInitCondIsStateError() const;
281 bool init_cond_is_state_error_ = false;
282};
283
286template <typename Scalar>
288 std::vector<typename math_types<Scalar>::VectorXs> &xs) {
289 const std::size_t nsteps = problem.numSteps();
290 xs.resize(nsteps + 1);
291 if (problem.initCondIsStateError()) {
292 xs[0] = problem.getInitState();
293 } else {
294 if (problem.stages_.size() > 0) {
295 xs[0] = problem.stages_[0]->xspace().neutral();
296 } else {
298 "The problem should have either a StateErrorResidual as an initial "
299 "condition or at least one stage.");
300 }
301 }
302 for (std::size_t i = 0; i < nsteps; i++) {
303 const StageModelTpl<Scalar> &sm = *problem.stages_[i];
304 xs[i + 1] = sm.xspace_next().neutral();
305 }
306}
307
310template <typename Scalar>
312 std::vector<typename math_types<Scalar>::VectorXs> &us) {
313 const std::size_t nsteps = problem.numSteps();
314 us.resize(nsteps);
315 for (std::size_t i = 0; i < nsteps; i++) {
316 const StageModelTpl<Scalar> &sm = *problem.stages_[i];
317 us[i] = sm.uspace().neutral();
318 }
319}
320
330template <typename Scalar>
332 const TrajOptProblemTpl<Scalar> &problem,
333 const typename math_types<Scalar>::VectorOfVectors &xs_in,
334 const typename math_types<Scalar>::VectorOfVectors &us_in,
336 typename math_types<Scalar>::VectorOfVectors &us_out) {
337 if (xs_in.empty()) {
338 problem.initializeSolution(xs_out, us_out);
339 } else if (us_in.empty()) {
340 us_default_init(problem, us_out);
341 } else {
342 if (!assign_no_resize(xs_in, xs_out))
343 ALIGATOR_RUNTIME_ERROR("warm-start for xs has wrong size!");
344 if (!assign_no_resize(us_in, us_out))
345 ALIGATOR_RUNTIME_ERROR("warm-start for us has wrong size!");
346 }
347}
348
349namespace internal {
351template <typename Scalar>
352int problem_last_ndx_helper(const TrajOptProblemTpl<Scalar> &problem) {
353 return problem.term_cost_->ndx();
354}
355} // namespace internal
356
357#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
358extern template struct TrajOptProblemTpl<context::Scalar>;
359#endif
360} // namespace aligator
#define ALIGATOR_RUNTIME_ERROR(...)
Definition exceptions.hpp:7
Main package namespace.
bool assign_no_resize(const std::vector< T1 > &lhs, std::vector< T2 > &rhs)
Assign a vector of Eigen types into another, ensure there is no resize.
void check_initial_guess_and_assign(const TrajOptProblemTpl< Scalar > &problem, const typename math_types< Scalar >::VectorOfVectors &xs_in, const typename math_types< Scalar >::VectorOfVectors &us_in, typename math_types< Scalar >::VectorOfVectors &xs_out, typename math_types< Scalar >::VectorOfVectors &us_out)
Check the input state-control trajectory is a consistent warm-start for the output.
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.
void xs_default_init(const TrajOptProblemTpl< Scalar > &problem, std::vector< typename math_types< Scalar >::VectorXs > &xs)
Default-initialize a trajectory to the neutral states for each state space at each stage.
Base constraint set type.
Convenience class to manage a stack of constraints.
ALIGATOR_DEPRECATED void pushBack(Cstr &&el)
Stage costs for control problems.
Base class for manifolds, to use in cost funcs, solvers...
VectorXs neutral() const
Get the neutral element from the manifold (if this makes sense).
Simple struct holding together a function and set, to describe a constraint.
Class representing ternary functions .
A stage in the control problem.
const Manifold & xspace_next() const
const Manifold & uspace() const
int nc() const
Total number of constraints.
Problem data struct.
Trajectory optimization problem.
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)
Add a terminal constraint for the model.
TrajOptProblemTpl(const ConstVectorRef &x0, const std::vector< xyz::polymorphic< StageModel > > &stages, xyz::polymorphic< CostAbstract > term_cost)
Constructor for an initial value problem.
TrajOptProblemTpl(const ConstVectorRef &x0, const int nu, xyz::polymorphic< Manifold > space, xyz::polymorphic< CostAbstract > term_cost)
Constructor for an initial value problem.
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_
TrajOptProblemTpl(xyz::polymorphic< UnaryFunction > init_constraint, xyz::polymorphic< CostAbstract > term_cost)
Constructor with a given constraint function of any given type.
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
TrajOptProblemTpl(xyz::polymorphic< UnaryFunction > init_constraint, const std::vector< xyz::polymorphic< StageModel > > &stages, xyz::polymorphic< CostAbstract > term_cost)
Constructor with a given constraint function of any given type.
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.
Typedefs for math (Eigen vectors, matrices) depending on scalar type.
Definition math.hpp:122