FD.h

Go to the documentation of this file.

/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
// includes
// std
#include <vector>
 
// Eigen
#include <Eigen/Core>
 
// SpaceVecAlg
#include <rbdyn/config.hh>
 
#include <SpaceVecAlg/SpaceVecAlg>
 
namespace rbd
{
class MultiBody;
struct MultiBodyConfig;
 
class RBDYN_DLLAPI ForwardDynamics
{
public:
  ForwardDynamics() {}
  ForwardDynamics(const MultiBody & mb);
 
  void forwardDynamics(const MultiBody & mb, MultiBodyConfig & mbc);
 
  void computeHIr(const MultiBody & mb);
 
  void computeH(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  void computeC(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  const Eigen::MatrixXd & H() const
  {
    return H_;
  }
 
  const Eigen::MatrixXd & HIr() const
  {
    return HIr_;
  }
 
  const Eigen::VectorXd & C() const
  {
    return C_;
  }
 
  const std::vector<sva::RBInertiad> & inertiaSubTree() const
  {
    return I_st_;
  }
 
  // safe version for python binding
 
  void sForwardDynamics(const MultiBody & mb, MultiBodyConfig & mbc);
 
  void sComputeH(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  void sComputeC(const MultiBody & mb, const MultiBodyConfig & mbc);
 
private:
  Eigen::MatrixXd H_;
  Eigen::VectorXd C_;
 
  // H computation
  std::vector<sva::RBInertiad> I_st_;
  std::vector<Eigen::Matrix<double, 6, Eigen::Dynamic>> F_;
  Eigen::MatrixXd HIr_;
 
  // C computation
  std::vector<sva::MotionVecd> acc_;
  std::vector<sva::ForceVecd> f_;
 
  // torque computation
  Eigen::VectorXd tmpFd_;
 
  std::vector<int> dofPos_;
 
  Eigen::LDLT<Eigen::MatrixXd> ldlt_;
};
 
} // namespace rbd