aligator  0.16.0
A versatile and efficient C++ library for real-time constrained trajectory optimization.
Loading...
Searching...
No Matches
integrator-rk2.hpp
Go to the documentation of this file.
1
2#pragma once
3
5
6namespace aligator {
7namespace dynamics {
8template <typename Scalar> struct IntegratorRK2DataTpl;
9
18template <typename _Scalar>
20 using Scalar = _Scalar;
25 using ODEType = typename Base::ODEType;
26
28
29 IntegratorRK2Tpl(const xyz::polymorphic<ODEType> &cont_dynamics,
30 const Scalar timestep);
31 void forward(const ConstVectorRef &x, const ConstVectorRef &u,
32 BaseData &data) const;
33 void dForward(const ConstVectorRef &x, const ConstVectorRef &u,
34 BaseData &data) const;
35
36 shared_ptr<BaseData> createData() const {
37 return std::make_shared<Data>(*this);
38 }
39};
40
41template <typename Scalar>
46 shared_ptr<ODEData> continuous_data2;
47
48 VectorXs x1_;
49 VectorXs dx1_;
50
52 : Base(integrator)
53 , x1_(integrator.space_next().neutral())
54 , dx1_(this->ndx1) {
55 continuous_data2 = integrator.ode_->createData();
56 dx1_.setZero();
57 }
58
59 using Base::dx_;
60 using Base::Jtmp_xnext;
61 using Base::Ju;
62 using Base::Jx;
63 using Base::xnext_;
64};
65
66#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
67extern template struct IntegratorRK2Tpl<context::Scalar>;
68extern template struct IntegratorRK2DataTpl<context::Scalar>;
69#endif
70} // namespace dynamics
71} // namespace aligator
Base definitions for explicit integrators.
Namespace for modelling system dynamics.
Main package namespace.
Specific data struct for explicit dynamics ExplicitDynamicsModelTpl.
Data struct for ContinuousDynamicsAbstractTpl.
ExplicitIntegratorAbstractTpl(const xyz::polymorphic< ODEType > &cont_dynamics)
IntegratorRK2DataTpl(const IntegratorRK2Tpl< Scalar > &integrator)
ContinuousDynamicsDataTpl< Scalar > ODEData
ExplicitIntegratorDataTpl< Scalar > Base
Second-order Runge-Kutta integrator.
shared_ptr< BaseData > createData() const
IntegratorRK2Tpl(const xyz::polymorphic< ODEType > &cont_dynamics, const Scalar timestep)
IntegratorRK2DataTpl< Scalar > Data
ExplicitIntegratorAbstractTpl< Scalar > Base
ExplicitDynamicsDataTpl< Scalar > BaseData
void dForward(const ConstVectorRef &x, const ConstVectorRef &u, BaseData &data) const
Compute the Jacobians of the forward dynamics.
void forward(const ConstVectorRef &x, const ConstVectorRef &u, BaseData &data) const
Evaluate the forward discrete dynamics.