continuous-dynamics-abstract.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/fwd.hpp"
#include "aligator/modelling/dynamics/fwd.hpp"
#include "aligator/core/manifold-base.hpp"
#include "aligator/third-party/polymorphic_cxx14.h"
 
namespace aligator {
namespace dynamics {

template <typename _Scalar> struct ContinuousDynamicsAbstractTpl {
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using Manifold = ManifoldAbstractTpl<Scalar>;
  using ManifoldPtr = xyz::polymorphic<Manifold>;
  using Data = ContinuousDynamicsDataTpl<Scalar>;

  ManifoldPtr space_;
  const int nu_;
 
  inline int ndx() const { return space_->ndx(); }
  inline int nu() const { return nu_; }

  inline const Manifold &space() const { return *space_; }
 
  ContinuousDynamicsAbstractTpl(ManifoldPtr space, const int nu);
 
  virtual ~ContinuousDynamicsAbstractTpl() = default;

  virtual void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
                        const ConstVectorRef &xdot, Data &data) const = 0;

  virtual void computeJacobians(const ConstVectorRef &x,
                                const ConstVectorRef &u,
                                const ConstVectorRef &xdot,
                                Data &data) const = 0;

  virtual shared_ptr<Data> createData() const;
};

template <typename _Scalar> struct ContinuousDynamicsDataTpl {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);

  VectorXs value_;
  MatrixXs Jx_;
  MatrixXs Ju_;
  MatrixXs Jxdot_;
  VectorXs xdot_;
 
  ConstMatrixRef Jx() const { return Jx_; }
  ConstMatrixRef Ju() const { return Ju_; }
  MatrixRef Jx() { return Jx_; }
  MatrixRef Ju() { return Ju_; }
 
  ContinuousDynamicsDataTpl(const int ndx, const int nu);
 
  // marks this type as polymorphic; required for Boost.Python
  virtual ~ContinuousDynamicsDataTpl() = default;
};
 
} // namespace dynamics
} // namespace aligator
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#include "aligator/modelling/dynamics/continuous-dynamics-abstract.txx"
#endif