67template <
typename T1,
typename T2>
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())
77 const auto same_dims = [](
auto &x,
auto &y) {
78 return (x.cols() == y.cols()) && (x.rows() == y.rows());
81 for (std::size_t i = 0; i < lhs.size(); i++) {
82 if (!same_dims(lhs[i], rhs[i]))
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> &)>;
126 std::vector<xyz::polymorphic<StageModel>>
stages_;
139 const std::vector<xyz::polymorphic<StageModel>> &stages,
140 xyz::polymorphic<CostAbstract> term_cost);
144 const std::vector<xyz::polymorphic<StageModel>> &stages,
145 xyz::polymorphic<CostAbstract> term_cost);
153 xyz::polymorphic<CostAbstract> term_cost);
157 xyz::polymorphic<Manifold> space,
158 xyz::polymorphic<CostAbstract> term_cost);
162 assert(init_cond_is_state_error_ == checkInitCondIsStateError());
163 return init_cond_is_state_error_;
167 void addStage(
const xyz::polymorphic<StageModel> &stage);
173 "Initial condition is not a StateErrorResidual.\n");
182 "Initial condition is not a StateErrorResidual.\n");
191 const xyz::polymorphic<ConstraintSet> &set) {
192 this->term_cstrs_.
pushBack(func, set);
201 const std::vector<VectorXs> &us,
Data &prob_data,
202 std::size_t num_threads = 1)
const;
214 const std::vector<VectorXs> &us,
Data &prob_data,
215 std::size_t num_threads = 1,
216 bool compute_second_order =
true)
const;
232 static_assert(std::is_convertible_v<Callable, InitializationStrategy>);
233 this->xs_init_strategy_ = std::forward<Callable>(func);
241 std::vector<VectorXs> &us)
const {
242 xs_init_strategy_(*
this, xs);
248 std::vector<VectorXs> &vs,
249 std::vector<VectorXs> &lbdas)
const {
253 vs.resize(nsteps + 1);
254 lbdas.resize(nsteps + 1);
256 for (
size_t i = 0; i < nsteps; i++) {
258 lbdas[i + 1].setZero(sm.
ndx2());
259 vs[i].setZero(sm.
nc());
269 std::vector<VectorXs> xs, us, vs, lbdas;
271 return std::make_tuple(std::move(xs), std::move(us), std::move(vs),
280 bool checkInitCondIsStateError()
const;
281 bool init_cond_is_state_error_ =
false;
286template <
typename Scalar>
289 const std::size_t nsteps = problem.
numSteps();
290 xs.resize(nsteps + 1);
294 if (problem.
stages_.size() > 0) {
295 xs[0] = problem.
stages_[0]->xspace().neutral();
298 "The problem should have either a StateErrorResidual as an initial "
299 "condition or at least one stage.");
302 for (std::size_t i = 0; i < nsteps; i++) {
310template <
typename Scalar>
313 const std::size_t nsteps = problem.
numSteps();
315 for (std::size_t i = 0; i < nsteps; i++) {
330template <
typename Scalar>
339 }
else if (us_in.empty()) {
351template <
typename Scalar>
352int problem_last_ndx_helper(
const TrajOptProblemTpl<Scalar> &problem) {
353 return problem.term_cost_->ndx();
357#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#define ALIGATOR_RUNTIME_ERROR(...)
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.
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_
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_
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_
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
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.