integrator-midpoint.hpp

Go to the documentation of this file.

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

template <typename _Scalar>
struct IntegratorMidpointTpl : IntegratorAbstractTpl<_Scalar> {
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using Base = IntegratorAbstractTpl<Scalar>;
  using ContinuousDynamics = ContinuousDynamicsAbstractTpl<Scalar>;
  using Manifold = ManifoldAbstractTpl<Scalar>;
  using Data = IntegratorMidpointDataTpl<Scalar>;
  using BaseData = DynamicsDataTpl<Scalar>;
 
  Scalar timestep_;
 
  IntegratorMidpointTpl(
      const xyz::polymorphic<ContinuousDynamics> &cont_dynamics,
      const Scalar timestep);
 
  void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
                const ConstVectorRef &y, BaseData &data) const;
 
  void computeJacobians(const ConstVectorRef &x, const ConstVectorRef &u,
                        const ConstVectorRef &y, BaseData &data) const;
 
  shared_ptr<BaseData> createData() const;
};
 
template <typename _Scalar>
struct IntegratorMidpointDataTpl : IntegratorDataTpl<_Scalar> {
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using Base = IntegratorDataTpl<Scalar>;
 
  VectorXs x1_;
  VectorXs dx1_;
  MatrixXs J_v_0;
  MatrixXs J_v_1;
  MatrixXs Jtm0;
  MatrixXs Jtm1;
 
  explicit IntegratorMidpointDataTpl(
      const IntegratorMidpointTpl<Scalar> &integrator)
      : Base(integrator)
      , x1_(integrator.space().neutral())
      , dx1_(this->ndx1)
      , J_v_0(this->ndx1, this->ndx1)
      , J_v_1(this->ndx1, this->ndx1)
      , Jtm0(this->ndx1, this->ndx1)
      , Jtm1(this->ndx1, this->ndx1) {
    x1_.setZero();
    dx1_.setZero();
    J_v_0.setZero();
    J_v_1.setZero();
    Jtm0.setZero();
    Jtm1.setZero();
  }
};
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
extern template struct IntegratorMidpointTpl<context::Scalar>;
extern template struct IntegratorMidpointDataTpl<context::Scalar>;
#endif
 
} // namespace dynamics
} // namespace aligator