aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
stage-model.hpp
Go to the documentation of this file.
1
3#pragma once
4
7
8namespace aligator {
9using xyz::polymorphic;
10
11#define ALIGATOR_CHECK_DERIVED_CLASS(Base, Derived) \
12 static_assert((std::is_base_of_v<Base, Derived>), \
13 "Failed check for derived class.")
14
22template <typename _Scalar> struct StageModelTpl {
23public:
24 using Scalar = _Scalar;
26
28 using PolyManifold = polymorphic<Manifold>;
30 using PolyDynamics = polymorphic<Dynamics>;
31 using PolyFunction = polymorphic<StageFunctionTpl<Scalar>>;
32 using PolyConstraintSet = polymorphic<ConstraintSetTpl<Scalar>>;
34 using PolyCost = polymorphic<Cost>;
35#pragma GCC diagnostic push
36#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
38#pragma GCC diagnostic pop
40
53
55 template <typename U> U *getCost() {
57 return dynamic_cast<U *>(&*cost_);
58 }
59
61 template <typename U> const U *getCost() const {
63 return dynamic_cast<const U *>(&*cost_);
64 }
65
67 template <typename U> U *getDynamics() {
69 return dynamic_cast<U *>(&*dynamics_);
70 }
71
73 template <typename U> const U *getDynamics() const {
75 return dynamic_cast<const U *>(&*dynamics_);
76 }
77
81 virtual ~StageModelTpl() = default;
82
83 const Manifold &xspace() const { return *xspace_; }
84 const Manifold &uspace() const { return *uspace_; }
85 const Manifold &xspace_next() const { return *xspace_next_; }
86
87 const Cost &cost() const { return *cost_; }
91 virtual bool hasDynModel() const { return true; }
92
93 int nx1() const { return xspace_->nx(); }
94 int ndx1() const { return xspace_->ndx(); }
95 int nu() const { return uspace_->ndx(); }
96 int nx2() const { return xspace_next_->nx(); }
97 int ndx2() const { return xspace_next_->ndx(); }
99 int nc() const { return (int)constraints_.totalDim(); }
100
102 std::size_t numConstraints() const { return constraints_.size(); }
103
105 int numDual() const { return ndx2() + nc(); }
106
108 template <typename Cstr, typename = std::enable_if_t<std::is_same_v<
109 std::decay_t<Cstr>, StageConstraint>>>
110 ALIGATOR_DEPRECATED void addConstraint(Cstr &&cstr);
111
114 void addConstraint(const PolyFunction &func,
115 const PolyConstraintSet &cstr_set);
116
117 /* Evaluate costs, constraints, ... */
118
121 virtual void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
122 Data &data) const;
123
125 virtual void computeFirstOrderDerivatives(const ConstVectorRef &x,
126 const ConstVectorRef &u,
127 Data &data) const;
128
130 virtual void computeSecondOrderDerivatives(const ConstVectorRef &x,
131 const ConstVectorRef &u,
132 Data &data) const;
133
135 virtual shared_ptr<Data> createData() const;
136};
137
138template <typename Scalar>
139template <typename Cstr, typename>
141 const int c_nu = cstr.func->nu;
142 if (c_nu != this->nu()) {
144 "Function has the wrong dimension for u: got {:d}, expected {:d}", c_nu,
145 this->nu());
146 }
147 constraints_.pushBack(std::forward<Cstr>(cstr));
148}
149
150#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
151extern template struct StageModelTpl<context::Scalar>;
152#endif
153
154} // namespace aligator
155
156template <typename Scalar>
157struct fmt::formatter<aligator::StageModelTpl<Scalar>> {
158 constexpr auto parse(format_parse_context &ctx) const
159 -> decltype(ctx.begin()) {
160 return ctx.end();
161 }
162
163 auto format(const aligator::StageModelTpl<Scalar> &stage,
164 format_context &ctx) const -> decltype(ctx.out()) {
165 if (stage.ndx1() == stage.ndx2()) {
166 return fmt::format_to(ctx.out(),
167 "StageModel {{"
168 "\n ndx: {:d},"
169 "\n nu: {:d},"
170 "\n nc: {:d}, [{:d} constraints]"
171 "\n}}",
172 stage.ndx1(), stage.nu(), stage.nc(),
173 stage.numConstraints());
174 } else {
175 return fmt::format_to(ctx.out(),
176 "StageModel {{"
177 "\n ndx1: {:d},"
178 "\n nu: {:d},"
179 "\n nc: {:d}, [{:d} constraints]"
180 "\n ndx2: {:d},"
181 "\n}}",
182 stage.ndx1(), stage.nu(), stage.nc(),
183 stage.numConstraints(), stage.ndx2());
184 }
185 }
186};
187
188namespace aligator {
189template <typename Scalar>
190std::ostream &operator<<(std::ostream &oss, const StageModelTpl<Scalar> &sm) {
191 return oss << fmt::format("{}", sm);
192}
193} // namespace aligator
Defines the constraint object and constraint stack manager for this library.
#define ALIGATOR_RUNTIME_ERROR(...)
Definition exceptions.hpp:7
Base definitions for ternary functions.
Namespace for modelling system dynamics.
Main package namespace.
std::ostream & operator<<(std::ostream &oss, const StageFunctionDataTpl< T > &self)
#define ALIGATOR_CHECK_DERIVED_CLASS(Base, Derived)
Convenience class to manage a stack of constraints.
Stage costs for control problems.
Explicit forward dynamics model .
Base class for manifolds, to use in cost funcs, solvers...
Simple struct holding together a function and set, to describe a constraint.
Data struct for stage models StageModelTpl.
A stage in the control problem.
virtual ~StageModelTpl()=default
const U * getCost() const
CostAbstractTpl< Scalar > Cost
virtual void computeFirstOrderDerivatives(const ConstVectorRef &x, const ConstVectorRef &u, Data &data) const
Compute the first-order derivatives of the StageModelTpl.
StageConstraintTpl< Scalar > StageConstraint
U * getDynamics()
Get a pointer to an expected concrete type for the dynamics class.
virtual bool hasDynModel() const
virtual void computeSecondOrderDerivatives(const ConstVectorRef &x, const ConstVectorRef &u, Data &data) const
Compute the second-order derivatives of the StageModelTpl.
const Manifold & xspace_next() const
polymorphic< Manifold > PolyManifold
ALIGATOR_DEPRECATED void addConstraint(Cstr &&cstr)
Add a constraint to the stage.
ManifoldAbstractTpl< Scalar > Manifold
ConstraintStackTpl< Scalar > constraints_
virtual void evaluate(const ConstVectorRef &x, const ConstVectorRef &u, Data &data) const
Evaluate all the functions (cost, dynamics, constraints) at this node.
polymorphic< ConstraintSetTpl< Scalar > > PolyConstraintSet
ExplicitDynamicsModelTpl< Scalar > Dynamics
StageModelTpl(const PolyCost &cost, const PolyDynamics &dynamics)
virtual shared_ptr< Data > createData() const
Create a StageData object.
const Manifold & uspace() const
StageDataTpl< Scalar > Data
int numDual() const
Number of dual variables, i.e. Lagrange multipliers.
void addConstraint(const PolyFunction &func, const PolyConstraintSet &cstr_set)
Add a constraint to the stage.
const U * getDynamics() const
U * getCost()
Get a pointer to an expected concrete type for the cost function.
polymorphic< Dynamics > PolyDynamics
std::size_t numConstraints() const
Number of constraint objects.
const Manifold & xspace() const
polymorphic< StageFunctionTpl< Scalar > > PolyFunction
int nc() const
Total number of constraints.