@brief Addition dynamics. @details It maps \f$(x,u)\f$ to \f$ x + u \f$.
| 25 | /// @brief Addition dynamics. |
| 26 | /// @details It maps \f$(x,u)\f$ to \f$ x + u \f$. |
| 27 | struct MyModel : ExplicitDynamicsModelTpl<double> { |
| 28 | using Manifold = ManifoldAbstractTpl<double>; |
| 29 | using ManifoldPtr = xyz::polymorphic<Manifold>; |
| 30 | using ExplicitData = ExplicitDynamicsDataTpl<double>; |
| 31 | explicit MyModel(const ManifoldPtr &space) |
| 32 | : ExplicitDynamicsModelTpl<double>(space, space->ndx()) {} |
| 33 | |
| 34 | void forward(const ConstVectorRef &x, const ConstVectorRef &u, |
| 35 | ExplicitData &data) const { |
| 36 | space_next().integrate(x, u, data.xnext_); |
| 37 | } |
| 38 | |
| 39 | void dForward(const ConstVectorRef &x, const ConstVectorRef &u, |
| 40 | ExplicitData &data) const { |
| 41 | space_next().Jintegrate(x, u, data.Jx(), 0); |
| 42 | space_next().Jintegrate(x, u, data.Ju(), 1); |
| 43 | } |
| 44 | }; |
| 45 | |
| 46 | struct MyCost : CostAbstractTpl<double> { |
| 47 | using CostAbstractTpl<double>::CostAbstractTpl; |