Momentum.h

Go to the documentation of this file.

/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
// include
#include <vector>
// SpaceVecAlg
#include <rbdyn/config.hh>
 
#include <SpaceVecAlg/SpaceVecAlg>
 
#include <vector>
 
namespace rbd
{
class MultiBody;
struct MultiBodyConfig;
struct Block;
using Blocks = std::vector<Block>;
class Jacobian;
 
RBDYN_DLLAPI sva::ForceVecd computeCentroidalMomentum(const MultiBody & mb,
                                                      const MultiBodyConfig & mbc,
                                                      const Eigen::Vector3d & com);
 
RBDYN_DLLAPI sva::ForceVecd computeCentroidalMomentumDot(const MultiBody & mb,
                                                         const MultiBodyConfig & mbc,
                                                         const Eigen::Vector3d & com,
                                                         const Eigen::Vector3d & comDot);
 
class RBDYN_DLLAPI CentroidalMomentumMatrix
{
public:
  CentroidalMomentumMatrix();
 
  CentroidalMomentumMatrix(const MultiBody & mb);
  CentroidalMomentumMatrix(const MultiBody & mb, std::vector<double> weight);
 
public:
  void computeMatrix(const MultiBody & mb, const MultiBodyConfig & mbc, const Eigen::Vector3d & com);
 
  void computeMatrixDot(const MultiBody & mb,
                        const MultiBodyConfig & mbc,
                        const Eigen::Vector3d & com,
                        const Eigen::Vector3d & comDot);
 
  void computeMatrixAndMatrixDot(const MultiBody & mb,
                                 const MultiBodyConfig & mbc,
                                 const Eigen::Vector3d & com,
                                 const Eigen::Vector3d & comDot);
 
  const Eigen::MatrixXd & matrix() const;
 
  const Eigen::MatrixXd & matrixDot() const;
 
  sva::ForceVecd momentum(const MultiBody & mb, const MultiBodyConfig & mbc, const Eigen::Vector3d & com) const;
 
  sva::ForceVecd normalMomentumDot(const MultiBody & mb,
                                   const MultiBodyConfig & mbc,
                                   const Eigen::Vector3d & com,
                                   const Eigen::Vector3d & comDot);
 
  sva::ForceVecd normalMomentumDot(const MultiBody & mb,
                                   const MultiBodyConfig & mbc,
                                   const Eigen::Vector3d & com,
                                   const Eigen::Vector3d & comDot,
                                   const std::vector<sva::MotionVecd> & normalAccB) const;
 
  // safe version for python binding
 
  void sComputeMatrix(const MultiBody & mb, const MultiBodyConfig & mbc, const Eigen::Vector3d & com);
 
  void sComputeMatrixDot(const MultiBody & mb,
                         const MultiBodyConfig & mbc,
                         const Eigen::Vector3d & com,
                         const Eigen::Vector3d & comDot);
 
  void sComputeMatrixAndMatrixDot(const MultiBody & mb,
                                  const MultiBodyConfig & mbc,
                                  const Eigen::Vector3d & com,
                                  const Eigen::Vector3d & comDot);
 
  sva::ForceVecd sMomentum(const MultiBody & mb, const MultiBodyConfig & mbc, const Eigen::Vector3d & com) const;
 
  sva::ForceVecd sNormalMomentumDot(const MultiBody & mb,
                                    const MultiBodyConfig & mbc,
                                    const Eigen::Vector3d & com,
                                    const Eigen::Vector3d & comDot);
 
  sva::ForceVecd sNormalMomentumDot(const MultiBody & mb,
                                    const MultiBodyConfig & mbc,
                                    const Eigen::Vector3d & com,
                                    const Eigen::Vector3d & comDot,
                                    const std::vector<sva::MotionVecd> & normalAccB) const;
 
private:
  void init(const rbd::MultiBody & mb);
 
private:
  Eigen::MatrixXd cmMat_;
  Eigen::MatrixXd cmMatDot_;
 
  std::vector<Jacobian> jacVec_;
  std::vector<Blocks> blocksVec_;
  std::vector<Eigen::MatrixXd> jacWork_;
  std::vector<double> bodiesWeight_;
  std::vector<sva::MotionVecd> normalAcc_;
};
 
// safe version for python binding
 
RBDYN_DLLAPI sva::ForceVecd sComputeCentroidalMomentum(const MultiBody & mb,
                                                       const MultiBodyConfig & mbc,
                                                       const Eigen::Vector3d & com);
 
RBDYN_DLLAPI sva::ForceVecd sComputeCentroidalMomentumDot(const MultiBody & mb,
                                                          const MultiBodyConfig & mbc,
                                                          const Eigen::Vector3d & com,
                                                          const Eigen::Vector3d & comDot);
 
} // namespace rbd