centroidal-fwd.hpp

Go to the documentation of this file.

 
#pragma once
 
#include "aligator/modelling/dynamics/ode-abstract.hpp"
#include "aligator/modelling/contact-map.hpp"
 
#include <proxsuite-nlp/modelling/spaces/vector-space.hpp>
 
namespace aligator {
namespace dynamics {
template <typename Scalar> struct CentroidalFwdDataTpl;
 
template <typename _Scalar>
struct CentroidalFwdDynamicsTpl : ODEAbstractTpl<_Scalar> {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  using Scalar = _Scalar;
  ALIGATOR_DYNAMIC_TYPEDEFS(Scalar);
  using Base = ODEAbstractTpl<Scalar>;
  using BaseData = ContinuousDynamicsDataTpl<Scalar>;
  using ContDataAbstract = ContinuousDynamicsDataTpl<Scalar>;
  using Data = CentroidalFwdDataTpl<Scalar>;
  using Manifold = proxsuite::nlp::VectorSpaceTpl<Scalar>;
  using Matrix3s = Eigen::Matrix<Scalar, 3, 3>;
  using ContactMap = ContactMapTpl<Scalar>;
 
  using Base::nu_;
 
  Manifold space_;
  std::size_t nk_;
  double mass_;
  Vector3s gravity_;
  ContactMap contact_map_;
  int force_size_;
 
  const Manifold &space() const { return *space_; }
 
  CentroidalFwdDynamicsTpl(const Manifold &state, const double mass,
                           const Vector3s &gravity,
                           const ContactMap &contact_map, const int force_size);
 
  void forward(const ConstVectorRef &x, const ConstVectorRef &u,
               BaseData &data) const;
  void dForward(const ConstVectorRef &x, const ConstVectorRef &u,
                BaseData &data) const;
 
  shared_ptr<ContDataAbstract> createData() const;
};
 
template <typename Scalar>
struct CentroidalFwdDataTpl : ContinuousDynamicsDataTpl<Scalar> {
  using Base = ContinuousDynamicsDataTpl<Scalar>;
  using Matrix3s = Eigen::Matrix<Scalar, 3, 3>;
 
  Matrix3s Jtemp_;
 
  CentroidalFwdDataTpl(const CentroidalFwdDynamicsTpl<Scalar> *cont_dyn);
};
 
} // namespace dynamics
} // namespace aligator
 
#include "aligator/modelling/dynamics/centroidal-fwd.hxx"