11#define ALIGATOR_CHECK_DERIVED_CLASS(Base, Derived) \
12 static_assert((std::is_base_of_v<Base, Derived>), \
13 "Failed check for derived class.")
35#pragma GCC diagnostic push
36#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
38#pragma GCC diagnostic pop
57 return dynamic_cast<U *
>(&*
cost_);
61 template <
typename U>
const U *
getCost()
const {
63 return dynamic_cast<const U *
>(&*
cost_);
75 return dynamic_cast<const U *
>(&*
dynamics_);
108 template <
typename Cstr,
typename = std::enable_if_t<std::is_same_v<
121 virtual void evaluate(
const ConstVectorRef &x,
const ConstVectorRef &u,
126 const ConstVectorRef &u,
131 const ConstVectorRef &u,
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,
150#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
156template <
typename Scalar>
157struct fmt::formatter<aligator::StageModelTpl<Scalar>> {
158 constexpr auto parse(format_parse_context &ctx)
const
159 ->
decltype(ctx.begin()) {
164 format_context &ctx)
const ->
decltype(ctx.out()) {
165 if (stage.ndx1() == stage.ndx2()) {
166 return fmt::format_to(ctx.out(),
170 "\n nc: {:d}, [{:d} constraints]"
172 stage.ndx1(), stage.nu(), stage.nc(),
173 stage.numConstraints());
175 return fmt::format_to(ctx.out(),
179 "\n nc: {:d}, [{:d} constraints]"
182 stage.ndx1(), stage.nu(), stage.nc(),
183 stage.numConstraints(), stage.ndx2());
189template <
typename Scalar>
191 return oss << fmt::format(
"{}", sm);
Defines the constraint object and constraint stack manager for this library.
#define ALIGATOR_RUNTIME_ERROR(...)
Base definitions for ternary functions.
Namespace for modelling system dynamics.
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
PolyManifold xspace_next_
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
ALIGATOR_DYNAMIC_TYPEDEFS(Scalar)
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
const Cost & cost() const
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.
polymorphic< Cost > PolyCost
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.