CoM.h

Go to the documentation of this file.

/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
// std
#include <vector>
 
// Eigen
#include <Eigen/Core>
 
// RBDyn
#include <rbdyn/config.hh>
 
#include "Jacobian.h"
 
namespace rbd
{
class MultiBody;
struct MultiBodyConfig;
 
RBDYN_DLLAPI Eigen::Vector3d computeCoM(const MultiBody & mb, const MultiBodyConfig & mbc);
 
RBDYN_DLLAPI Eigen::Vector3d computeCoMVelocity(const MultiBody & mb, const MultiBodyConfig & mbc);
 
RBDYN_DLLAPI Eigen::Vector3d computeCoMAcceleration(const MultiBody & mb, const MultiBodyConfig & mbc);
 
class RBDYN_DLLAPI CoMJacobianDummy
{
public:
  CoMJacobianDummy();
 
  CoMJacobianDummy(const MultiBody & mb);
  CoMJacobianDummy(const MultiBody & mb, std::vector<double> weight);
 
  ~CoMJacobianDummy();
 
  const Eigen::MatrixXd & jacobian(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  inline const Eigen::MatrixXd & jacobian() const noexcept
  {
    return jac_;
  }
 
  const Eigen::MatrixXd & jacobianDot(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  inline const Eigen::MatrixXd & jacobianDot() const noexcept
  {
    return jacDot_;
  }
 
  // safe version for python binding
 
  const Eigen::MatrixXd & sJacobian(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  const Eigen::MatrixXd & sJacobianDot(const MultiBody & mb, const MultiBodyConfig & mbc);
 
private:
  void init(const rbd::MultiBody & mb);
 
private:
  Eigen::MatrixXd jac_;
  Eigen::MatrixXd jacDot_;
  Eigen::MatrixXd jacFull_;
 
  std::vector<Jacobian> jacVec_;
  double totalMass_;
  std::vector<double> bodiesWeight_;
};
 
class RBDYN_DLLAPI CoMJacobian
{
public:
  CoMJacobian();
 
  CoMJacobian(const MultiBody & mb);
  CoMJacobian(const MultiBody & mb, std::vector<double> weight);
 
  void updateInertialParameters(const MultiBody & mb);
 
  const std::vector<double> & weight() const;
 
  void weight(const MultiBody & mb, std::vector<double> w);
 
  const Eigen::MatrixXd & jacobian(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  inline const Eigen::MatrixXd & jacobian() const noexcept
  {
    return jac_;
  }
 
  const Eigen::MatrixXd & jacobianDot(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  inline const Eigen::MatrixXd & jacobianDot() const noexcept
  {
    return jacDot_;
  }
 
  Eigen::Vector3d velocity(const MultiBody & mb, const MultiBodyConfig & mbc) const;
 
  Eigen::Vector3d normalAcceleration(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  Eigen::Vector3d normalAcceleration(const MultiBody & mb,
                                     const MultiBodyConfig & mbc,
                                     const std::vector<sva::MotionVecd> & normalAccB) const;
 
  // safe version for python binding
 
  void sUpdateInertialParameters(const MultiBody & mb);
 
  void sWeight(const MultiBody & mb, std::vector<double> w);
 
  const Eigen::MatrixXd & sJacobian(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  const Eigen::MatrixXd & sJacobianDot(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  Eigen::Vector3d sVelocity(const MultiBody & mb, const MultiBodyConfig & mbc) const;
 
  Eigen::Vector3d sNormalAcceleration(const MultiBody & mb, const MultiBodyConfig & mbc);
 
  Eigen::Vector3d sNormalAcceleration(const MultiBody & mb,
                                      const MultiBodyConfig & mbc,
                                      const std::vector<sva::MotionVecd> & normalAccB) const;
 
private:
  void init(const rbd::MultiBody & mb);
 
private:
  Eigen::MatrixXd jac_;
  Eigen::MatrixXd jacDot_;
 
  std::vector<double> bodiesCoeff_;
 
  std::vector<sva::PTransformd> bodiesCoM_;
  std::vector<std::vector<int>> jointsSubBodies_;
 
  // jacobian, jacobianDot computation buffer
  std::vector<sva::PTransformd> bodiesCoMWorld_;
  std::vector<sva::MotionVecd> bodiesCoMVelB_;
  // store normal acceleration of each bodies when calling normal acceleration
  std::vector<sva::MotionVecd> normalAcc_;
 
  std::vector<double> weight_;
};
 
// safe version for python binding
 
RBDYN_DLLAPI Eigen::Vector3d sComputeCoM(const MultiBody & mb, const MultiBodyConfig & mbc);
 
RBDYN_DLLAPI Eigen::Vector3d sComputeCoMVelocity(const MultiBody & mb, const MultiBodyConfig & mbc);
 
RBDYN_DLLAPI Eigen::Vector3d sComputeCoMAcceleration(const MultiBody & mb, const MultiBodyConfig & mbc);
 
} // namespace rbd