integrator-rk2.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/modelling/dynamics/integrator-explicit.hpp"
 
namespace aligator {
namespace dynamics {
template <typename Scalar> struct IntegratorRK2DataTpl;

template <typename _Scalar>
struct IntegratorRK2Tpl : ExplicitIntegratorAbstractTpl<_Scalar> {
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using Base = ExplicitIntegratorAbstractTpl<Scalar>;
  using BaseData = ExplicitDynamicsDataTpl<Scalar>;
  using Data = IntegratorRK2DataTpl<Scalar>;
  using ODEType = typename Base::ODEType;
  using Base::space_next_;
 
  Scalar timestep_;
 
  IntegratorRK2Tpl(const xyz::polymorphic<ODEType> &cont_dynamics,
                   const Scalar timestep);
  void forward(const ConstVectorRef &x, const ConstVectorRef &u,
               BaseData &data) const;
  void dForward(const ConstVectorRef &x, const ConstVectorRef &u,
                BaseData &data) const;
 
  shared_ptr<DynamicsDataTpl<Scalar>> createData() const {
    return std::make_shared<Data>(this);
  }
 
protected:
  Scalar dt_2_ = 0.5 * timestep_;
};
 
template <typename Scalar>
struct IntegratorRK2DataTpl : ExplicitIntegratorDataTpl<Scalar> {
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using Base = ExplicitIntegratorDataTpl<Scalar>;
  using ODEData = ContinuousDynamicsDataTpl<Scalar>;
  shared_ptr<ODEData> continuous_data2;
 
  VectorXs x1_;
  VectorXs dx1_;
 
  explicit IntegratorRK2DataTpl(const IntegratorRK2Tpl<Scalar> *integrator);
 
  using Base::dx_;
  using Base::Jtmp_xnext;
  using Base::Ju_;
  using Base::Jx_;
  using Base::xnext_;
};
 
} // namespace dynamics
} // namespace aligator
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#include "aligator/modelling/dynamics/integrator-rk2.txx"
#endif