kinodynamics-fwd.hpp

Go to the documentation of this file.

#pragma once
 
#include "aligator/modelling/dynamics/ode-abstract.hpp"
 
#ifdef ALIGATOR_WITH_PINOCCHIO
 
#include <Eigen/LU>
#include "aligator/modelling/spaces/multibody.hpp"
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/data.hpp>
 
namespace aligator {
namespace dynamics {
template <typename Scalar> struct KinodynamicsFwdDataTpl;

template <typename _Scalar>
struct KinodynamicsFwdDynamicsTpl : 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 = KinodynamicsFwdDataTpl<Scalar>;
  using Manifold = MultibodyPhaseSpace<Scalar>;
  using Model = pinocchio::ModelTpl<Scalar>;
  using Matrix3s = Eigen::Matrix<Scalar, 3, 3>;
 
  using Base::nu_;
 
  Manifold space_;
  Model pin_model_;
  double mass_;
  Vector3s gravity_;
  int force_size_;
 
  std::vector<bool> contact_states_;
  std::vector<pinocchio::FrameIndex> contact_ids_;
 
  const Manifold &space() const { return space_; }
 
  KinodynamicsFwdDynamicsTpl(
      const Manifold &state, const Model &model, const Vector3s &gravity,
      const std::vector<bool> &contact_states,
      const std::vector<pinocchio::FrameIndex> &contact_ids,
      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 KinodynamicsFwdDataTpl : ContinuousDynamicsDataTpl<Scalar> {
  using Base = ContinuousDynamicsDataTpl<Scalar>;
  using PinData = pinocchio::DataTpl<Scalar>;
  using VectorXs = typename math_types<Scalar>::VectorXs;
  using Matrix6Xs = typename math_types<Scalar>::Matrix6Xs;
  using Matrix3Xs = typename math_types<Scalar>::Matrix3Xs;
  using Matrix3s = Eigen::Matrix<Scalar, 3, 3>;
  using Matrix6s = Eigen::Matrix<Scalar, 6, 6>;
  using Vector6s = Eigen::Matrix<Scalar, 6, 1>;
 
  PinData pin_data_;
  Matrix6Xs dh_dq_;
  Matrix6Xs dhdot_dq_;
  Matrix6Xs dhdot_dv_;
  Matrix6Xs dhdot_da_;
  Matrix6Xs temp1_;
  Matrix3Xs temp2_;
  Matrix6Xs fJf_;
  VectorXs v0_;
  VectorXs a0_;
 
  Vector6s cforces_;
  Matrix3s Jtemp_;
  Matrix6s Agu_inv_;
  Eigen::PartialPivLU<Eigen::Matrix<Scalar, 6, 6>> PivLU_;
 
  KinodynamicsFwdDataTpl(const KinodynamicsFwdDynamicsTpl<Scalar> *model);
};
 
#ifdef ALIGATOR_ENABLE_TEMPLATE_INSTANTIATION
extern template struct KinodynamicsFwdDynamicsTpl<context::Scalar>;
extern template struct KinodynamicsFwdDataTpl<context::Scalar>;
#endif
 
} // namespace dynamics
} // namespace aligator
 
#endif