dynamics.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/fwd.hpp"
#include "aligator/third-party/polymorphic_cxx14.h"
 
namespace aligator {
template <typename _Scalar> struct DynamicsModelTpl {
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using Data = DynamicsDataTpl<Scalar>;
  using Manifold = ManifoldAbstractTpl<Scalar>;

  xyz::polymorphic<Manifold> space_;
  xyz::polymorphic<Manifold> space_next_;
  const int ndx1;
  const int nu;
  const int ndx2;

  const Manifold &space() const { return *space_; }
  const Manifold &space_next() const { return *space_next_; }
  virtual bool isExplicit() const { return false; }
 
  inline int nx1() const { return space_->nx(); }
  inline int nx2() const { return space_next_->nx(); }

  DynamicsModelTpl(xyz::polymorphic<Manifold> space, const int nu);

  DynamicsModelTpl(xyz::polymorphic<Manifold> space, const int nu,
                   xyz::polymorphic<Manifold> space_next);
 
  virtual void evaluate(const ConstVectorRef &x, const ConstVectorRef &u,
                        const ConstVectorRef &xn, Data &) const = 0;
 
  virtual void computeJacobians(const ConstVectorRef &x,
                                const ConstVectorRef &u,
                                const ConstVectorRef &xn, Data &) const = 0;
 
  virtual void computeVectorHessianProducts(const ConstVectorRef &x,
                                            const ConstVectorRef &u,
                                            const ConstVectorRef &xn,
                                            const ConstVectorRef &lbda,
                                            Data &data) const;
 
  virtual shared_ptr<Data> createData() const;
 
  virtual ~DynamicsModelTpl() = default;
};
 
template <typename _Scalar> struct DynamicsDataTpl {
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
 
  const int ndx1;
  const int nu;
  const int ndx2;
  const int nvar = ndx1 + nu + ndx2;

  VectorXs value_;
  VectorRef valref_;
  MatrixXs jac_buffer_;
  MatrixRef Jx_;
  MatrixRef Ju_;
  MatrixRef Jy_;
 
  /* Vector-Hessian product buffers */
 
  MatrixXs Hxx_;
  MatrixXs Hxu_;
  MatrixXs Hxy_;
  MatrixXs Huu_;
  MatrixXs Huy_;
  MatrixXs Hyy_;
 
  explicit DynamicsDataTpl(const DynamicsModelTpl<Scalar> &model);
  DynamicsDataTpl(const int ndx1, const int nu, const int ndx2);
  virtual ~DynamicsDataTpl() = default;
};
 
} // namespace aligator
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
#include "aligator/core/dynamics.txx"
#endif