Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/algorithm/center-of-mass.hxx	Lines:	179	184	97.3 %
Date:	2024-04-26 13:14:21	Branches:	179	410	43.7 %


//
// Copyright (c) 2015-2020 CNRS INRIA
//

#ifndef __pinocchio_algorithm_center_of_mass_hxx__
#define __pinocchio_algorithm_center_of_mass_hxx__

#include "pinocchio/algorithm/check.hpp"
#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/jacobian.hpp"

/// @cond DEV

namespace pinocchio
{
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline Scalar computeTotalMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
  {
    Scalar m = Scalar(0);
    for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
    {
      m += model.inertias[i].mass();
    }
    return m;
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline Scalar computeTotalMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                 DataTpl<Scalar,Options,JointCollectionTpl> & data)
  {
    data.mass[0] = computeTotalMass(model);
    return data.mass[0];
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline void computeSubtreeMasses(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                   DataTpl<Scalar,Options,JointCollectionTpl> & data)
  {
    data.mass[0] = Scalar(0);

    // Forward Step
    for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
    {
      data.mass[i] = model.inertias[i].mass();
    }

    // Backward Step
    for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i)
    {
      const JointIndex & parent = model.parents[i];
      data.mass[parent] += data.mass[i];
    }
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
  centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
               DataTpl<Scalar,Options,JointCollectionTpl> & data,
               const Eigen::MatrixBase<ConfigVectorType> & q,
               const bool computeSubtreeComs)
  {
    forwardKinematics(model,data,q.derived());

    centerOfMass(model,data,POSITION,computeSubtreeComs);
    return data.com[0];
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
  centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
               DataTpl<Scalar,Options,JointCollectionTpl> & data,
               const Eigen::MatrixBase<ConfigVectorType> & q,
               const Eigen::MatrixBase<TangentVectorType> & v,
               const bool computeSubtreeComs)
  {
    forwardKinematics(model,data,q.derived(),v.derived());

    centerOfMass(model,data,VELOCITY,computeSubtreeComs);
    return data.com[0];
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
  centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
               DataTpl<Scalar,Options,JointCollectionTpl> & data,
               const Eigen::MatrixBase<ConfigVectorType> & q,
               const Eigen::MatrixBase<TangentVectorType1> & v,
               const Eigen::MatrixBase<TangentVectorType2> & a,
               const bool computeSubtreeComs)
  {
    forwardKinematics(model,data,q,v,a);

    centerOfMass(model,data,ACCELERATION,computeSubtreeComs);
    return data.com[0];
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
  centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
               DataTpl<Scalar,Options,JointCollectionTpl> & data,
               KinematicLevel kinematic_level,
               const bool computeSubtreeComs)
  {
    assert(model.check(data) && "data is not consistent with model.");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(kinematic_level >= 0 && kinematic_level <= 2);

    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typedef typename Model::JointIndex JointIndex;

    typedef typename Data::SE3 SE3;
    typedef typename Data::Motion Motion;
    typedef typename Data::Inertia Inertia;

    const bool do_position = (kinematic_level >= POSITION);
    const bool do_velocity = (kinematic_level >= VELOCITY);
    const bool do_acceleration = (kinematic_level >= ACCELERATION);

    data.mass[0] = 0;
    if(do_position)
      data.com[0].setZero();
    if(do_velocity)
      data.vcom[0].setZero();
    if(do_acceleration)
      data.acom[0].setZero();

    // Forward Step
    for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
    {
      const typename Inertia::Scalar  & mass = model.inertias[i].mass();
      const typename SE3::Vector3 & lever = model.inertias[i].lever();

      const Motion & v = data.v[i];
      const Motion & a = data.a[i];

      data.mass[i] = mass;

      if(do_position)
        data.com[i].noalias()  = mass * lever;

      if(do_velocity)
        data.vcom[i].noalias() = mass * (v.angular().cross(lever) + v.linear());

      if(do_acceleration)
        data.acom[i].noalias() = mass * (a.angular().cross(lever) + a.linear())
                     + v.angular().cross(data.vcom[i]); // take into accound the coriolis part of the acceleration
    }

    // Backward Step
    for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i)
    {
      const JointIndex & parent = model.parents[i];
      const SE3 & liMi = data.liMi[i];

      data.mass[parent] += data.mass[i];

      if(do_position)
        data.com[parent] += (liMi.rotation()*data.com[i]
                         + data.mass[i] * liMi.translation());

      if(do_velocity)
        data.vcom[parent] += liMi.rotation()*data.vcom[i];

      if(do_acceleration)
        data.acom[parent] += liMi.rotation()*data.acom[i];

      if(computeSubtreeComs)
      {
        if(do_position)
          data.com[i] /= data.mass[i];
        if(do_velocity)
          data.vcom[i] /= data.mass[i];
        if(do_acceleration)
          data.acom[i] /= data.mass[i];
      }
    }

    if(do_position)
      data.com[0] /= data.mass[0];
    if(do_velocity)
      data.vcom[0] /= data.mass[0];
    if(do_acceleration)
      data.acom[0] /= data.mass[0];

    return data.com[0];
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
  getComFromCrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                 DataTpl<Scalar,Options,JointCollectionTpl> & data)
  {
    PINOCCHIO_UNUSED_VARIABLE(model);

    assert(model.check(data) && "data is not consistent with model.");
    return data.com[0] = data.liMi[1].act(data.Ycrb[1].lever());
  }

  /* --- JACOBIAN ---------------------------------------------------------- */
  /* --- JACOBIAN ---------------------------------------------------------- */
  /* --- JACOBIAN ---------------------------------------------------------- */

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix3x>
  struct JacobianCenterOfMassBackwardStep
  : public fusion::JointUnaryVisitorBase< JacobianCenterOfMassBackwardStep<Scalar,Options,JointCollectionTpl,Matrix3x> >
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typedef boost::fusion::vector<const Model &,
                                  Data &,
                                  const Eigen::MatrixBase<Matrix3x> &,
                                  const bool &
                                  > ArgsType;

    template<typename JointModel>
    static void algo(const JointModelBase<JointModel> & jmodel,
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     const Model & model,
                     Data & data,
                     const Eigen::MatrixBase<Matrix3x> & Jcom,
                     const bool & computeSubtreeComs)
    {
      const JointIndex & i      = (JointIndex) jmodel.id();
      const JointIndex & parent = model.parents[i];

      data.com[parent]  += data.com[i];
      data.mass[parent] += data.mass[i];

      typedef typename Data::Matrix6x Matrix6x;
      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;

      Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x,Jcom);

      ColBlock Jcols = jmodel.jointCols(data.J);
      Jcols = data.oMi[i].act(jdata.S());

      for(Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id)
      {
        jmodel.jointCols(Jcom_).col(col_id)
        = data.mass[i] * Jcols.col(col_id).template segment<3>(Motion::LINEAR)
        - data.com[i].cross(Jcols.col(col_id).template segment<3>(Motion::ANGULAR));
      }

      if(computeSubtreeComs)
        data.com[i] /= data.mass[i];
    }

  };

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
  jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                       DataTpl<Scalar,Options,JointCollectionTpl> & data,
                       const Eigen::MatrixBase<ConfigVectorType> & q,
                       const bool computeSubtreeComs)
  {
    forwardKinematics(model, data, q);
    return jacobianCenterOfMass(model, data, computeSubtreeComs);
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
  jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                       DataTpl<Scalar,Options,JointCollectionTpl> & data,
                       const bool computeSubtreeComs)
  {
    assert(model.check(data) && "data is not consistent with model.");

    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typedef typename Model::JointIndex JointIndex;

    typedef typename Data::Matrix3x Matrix3x;
    typedef typename Data::SE3 SE3;
    typedef typename Data::Inertia Inertia;

    data.com[0].setZero();
    data.mass[0] = Scalar(0);

    for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
    {
      const typename Inertia::Scalar & mass = model.inertias[i].mass();
      const typename SE3::Vector3 & lever = model.inertias[i].lever();

      data.mass[i] = mass;
      data.com[i].noalias() = mass*data.oMi[i].act(lever);
    }

    // Backward step
    typedef JacobianCenterOfMassBackwardStep<Scalar,Options,JointCollectionTpl,Matrix3x> Pass2;
    for(JointIndex i= (JointIndex) (model.njoints-1); i>0; --i)
    {
      Pass2::run(model.joints[i],data.joints[i],
                 typename Pass2::ArgsType(model,data,data.Jcom,computeSubtreeComs));
    }

    data.com[0] /= data.mass[0];
    data.Jcom /=  data.mass[0];

    return data.Jcom;
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix3x>
  struct JacobianSubtreeCenterOfMassBackwardStep
  : public fusion::JointUnaryVisitorBase< JacobianSubtreeCenterOfMassBackwardStep<Scalar,Options,JointCollectionTpl,Matrix3x> >
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typedef boost::fusion::vector<const Model &,
    Data &,
    const JointIndex &,
    const Eigen::MatrixBase<Matrix3x> &
    > ArgsType;

    template<typename JointModel>
    static void algo(const JointModelBase<JointModel> & jmodel,
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     const Model & model,
                     Data & data,
                     const JointIndex & subtree_root_id,
                     const Eigen::MatrixBase<Matrix3x> & Jcom)
    {
      PINOCCHIO_UNUSED_VARIABLE(model);

      const JointIndex & i      = (JointIndex) jmodel.id();

      typedef typename Data::Matrix6x Matrix6x;
      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;

      Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x,Jcom);

      ColBlock Jcols = jmodel.jointCols(data.J);
      Jcols = data.oMi[i].act(jdata.S());

      for(Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id)
      {
        jmodel.jointCols(Jcom_).col(col_id)
        = Jcols.col(col_id).template segment<3>(Motion::LINEAR)
        - data.com[subtree_root_id].cross(Jcols.col(col_id).template segment<3>(Motion::ANGULAR));
      }
    }

  };

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix3xLike>
  inline void
  jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                              DataTpl<Scalar,Options,JointCollectionTpl> & data,
                              const Eigen::MatrixBase<ConfigVectorType> & q,
                              const JointIndex & rootSubtreeId,
                              const Eigen::MatrixBase<Matrix3xLike> & res)
  {
    forwardKinematics(model, data, q);
    jacobianSubtreeCenterOfMass(model, data, rootSubtreeId, res);
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix3xLike>
  inline void
  jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                              DataTpl<Scalar,Options,JointCollectionTpl> & data,
                              const JointIndex & rootSubtreeId,
                              const Eigen::MatrixBase<Matrix3xLike> & res)
  {
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;

    assert(model.check(data) && "data is not consistent with model.");
    PINOCCHIO_CHECK_INPUT_ARGUMENT((int)rootSubtreeId < model.njoints, "Invalid joint id.");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), 3, "the resulting matrix does not have the right size.");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(res.cols(), model.nv, "the resulting matrix does not have the right size.");

    Matrix3xLike & Jcom_subtree = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xLike,res);

    typedef typename Data::SE3 SE3;
    typedef typename Data::Inertia Inertia;

    typedef typename Model::IndexVector IndexVector;
    const IndexVector & subtree = model.subtrees[rootSubtreeId];

    const bool computeSubtreeComs = true;

    if(rootSubtreeId == 0)
    {
      data.mass[0] = 0;
      data.com[0].setZero();
    }

    for(size_t k = 0; k < subtree.size(); ++k)
    {
      const JointIndex joint_id = subtree[k];
      const typename Inertia::Scalar & mass = model.inertias[joint_id].mass();
      const typename SE3::Vector3 & lever = model.inertias[joint_id].lever();

      data.mass[joint_id] = mass;
      data.com[joint_id] = mass*data.oMi[joint_id].act(lever);
    }

    // Backward step
    typedef JacobianCenterOfMassBackwardStep<Scalar,Options,JointCollectionTpl,Matrix3xLike> Pass2;
    for(Eigen::DenseIndex k = (Eigen::DenseIndex)subtree.size() - 1; k >= 0; --k)
    {
      const JointIndex joint_id = subtree[(size_t)k];
      Pass2::run(model.joints[joint_id],data.joints[joint_id],
                 typename Pass2::ArgsType(model,data,Jcom_subtree,computeSubtreeComs));
    }

    PINOCCHIO_CHECK_INPUT_ARGUMENT(data.mass[rootSubtreeId] > 0., "The mass of the subtree is not positive.");
    const Scalar mass_inv_subtree = Scalar(1)/data.mass[rootSubtreeId];
    typename Data::Vector3 & com_subtree = data.com[rootSubtreeId];
    if(!computeSubtreeComs)
      com_subtree *= mass_inv_subtree;

    if(rootSubtreeId == 0)
    {
      Jcom_subtree *= mass_inv_subtree;
      return; // skip the rest
    }

    const int idx_v = model.joints[rootSubtreeId].idx_v();
    const int nv_subtree = data.nvSubtree[rootSubtreeId];

    Jcom_subtree.middleCols(idx_v,nv_subtree) *= mass_inv_subtree;

    // Second backward step
    typedef JacobianSubtreeCenterOfMassBackwardStep<Scalar,Options,JointCollectionTpl,Matrix3xLike> Pass3;
    for(JointIndex parent = model.parents[rootSubtreeId];
        parent > 0;
        parent = model.parents[parent])
    {
      Pass3::run(model.joints[parent],data.joints[parent],
                 typename Pass3::ArgsType(model,data,rootSubtreeId,Jcom_subtree));
    }
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix3xLike>
  inline void
  getJacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                                 const DataTpl<Scalar,Options,JointCollectionTpl> & data,
                                 const JointIndex & rootSubtreeId,
                                 const Eigen::MatrixBase<Matrix3xLike> & res)
  {
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    assert(model.check(data) && "data is not consistent with model.");
    PINOCCHIO_CHECK_INPUT_ARGUMENT(((int)rootSubtreeId < model.njoints), "Invalid joint id.");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), 3, "the resulting matrix does not have the right size.");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(res.cols(), model.nv, "the resulting matrix does not have the right size.");

    Matrix3xLike & Jcom_subtree = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xLike,res);

    if(rootSubtreeId == 0)
    {
      Jcom_subtree = data.Jcom;
      return;
    }

    const int idx_v = model.joints[rootSubtreeId].idx_v();
    const int nv_subtree = data.nvSubtree[rootSubtreeId];

    const Scalar mass_ratio = data.mass[0] / data.mass[rootSubtreeId];
    Jcom_subtree.middleCols(idx_v,nv_subtree)
    = mass_ratio * data.Jcom.middleCols(idx_v,nv_subtree);

    const typename Data::Vector3 & com_subtree = data.com[rootSubtreeId];

    for(int parent = data.parents_fromRow[(size_t)idx_v];
        parent >= 0;
        parent = data.parents_fromRow[(size_t)parent])
    {
      typename Data::Matrix6x::ConstColXpr Jcol = data.J.col(parent);
      Jcom_subtree.col(parent).noalias() = Jcol.template segment<3>(Motion::LINEAR) - com_subtree.cross(Jcol.template segment<3>(Motion::ANGULAR));
    }
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
  getJacobianComFromCrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                         DataTpl<Scalar,Options,JointCollectionTpl> & data)
  {
    PINOCCHIO_UNUSED_VARIABLE(model);
    assert(model.check(data) && "data is not consistent with model.");

    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
    typedef typename Data::SE3 SE3;

    const SE3 & oM1 = data.liMi[1];

    // Extract the total mass of the system.
    data.mass[0] = data.M(0,0);

    // As the 6 first rows of M*a are a wrench, we just need to multiply by the
    // relative rotation between the first joint and the world
    const typename SE3::Matrix3 oR1_over_m (oM1.rotation() / data.M(0,0));

    // I don't know why, but the colwise multiplication is much more faster
    // than the direct Eigen multiplication
    for(long k=0; k<model.nv;++k)
      data.Jcom.col(k).noalias() = oR1_over_m * data.M.template topRows<3>().col(k);

    return data.Jcom;
  }

} // namespace pinocchio

/// @endcond

#endif // ifndef __pinocchio_algorithm_center_of_mass_hxx__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2020 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_algorithm_center_of_mass_hxx__
6			#define __pinocchio_algorithm_center_of_mass_hxx__
7
8			#include "pinocchio/algorithm/check.hpp"
9			#include "pinocchio/multibody/visitor.hpp"
10			#include "pinocchio/algorithm/kinematics.hpp"
11			#include "pinocchio/algorithm/jacobian.hpp"
12
13			/// @cond DEV
14
15			namespace pinocchio
16			{
17			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
18		2	inline Scalar computeTotalMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model)
19			{
20		2	Scalar m = Scalar(0);
21	✓✓	56	for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
22			{
23		54	m += model.inertias[i].mass();
24			}
25		2	return m;
26			}
27
28			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
29		1	inline Scalar computeTotalMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
30			DataTpl<Scalar,Options,JointCollectionTpl> & data)
31			{
32		1	data.mass[0] = computeTotalMass(model);
33		1	return data.mass[0];
34			}
35
36			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
37		1	inline void computeSubtreeMasses(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
38			DataTpl<Scalar,Options,JointCollectionTpl> & data)
39			{
40		1	data.mass[0] = Scalar(0);
41
42			// Forward Step
43	✓✓	28	for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
44			{
45		27	data.mass[i] = model.inertias[i].mass();
46			}
47
48			// Backward Step
49	✓✓	28	for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i)
50			{
51		27	const JointIndex & parent = model.parents[i];
52		27	data.mass[parent] += data.mass[i];
53			}
54		1	}
55
56			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
57			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
58		899	centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
59			DataTpl<Scalar,Options,JointCollectionTpl> & data,
60			const Eigen::MatrixBase<ConfigVectorType> & q,
61			const bool computeSubtreeComs)
62			{
63		899	forwardKinematics(model,data,q.derived());
64
65		899	centerOfMass(model,data,POSITION,computeSubtreeComs);
66		899	return data.com[0];
67			}
68
69			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType>
70			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
71		44	centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
72			DataTpl<Scalar,Options,JointCollectionTpl> & data,
73			const Eigen::MatrixBase<ConfigVectorType> & q,
74			const Eigen::MatrixBase<TangentVectorType> & v,
75			const bool computeSubtreeComs)
76			{
77		44	forwardKinematics(model,data,q.derived(),v.derived());
78
79		44	centerOfMass(model,data,VELOCITY,computeSubtreeComs);
80		44	return data.com[0];
81			}
82
83			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2>
84			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
85		3	centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
86			DataTpl<Scalar,Options,JointCollectionTpl> & data,
87			const Eigen::MatrixBase<ConfigVectorType> & q,
88			const Eigen::MatrixBase<TangentVectorType1> & v,
89			const Eigen::MatrixBase<TangentVectorType2> & a,
90			const bool computeSubtreeComs)
91			{
92		3	forwardKinematics(model,data,q,v,a);
93
94		3	centerOfMass(model,data,ACCELERATION,computeSubtreeComs);
95		3	return data.com[0];
96			}
97
98			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
99			const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
100		946	centerOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
101			DataTpl<Scalar,Options,JointCollectionTpl> & data,
102			KinematicLevel kinematic_level,
103			const bool computeSubtreeComs)
104			{
105	✓✗	946	assert(model.check(data) && "data is not consistent with model.");
106	✓✗✗✓ ✗✗	946	PINOCCHIO_CHECK_INPUT_ARGUMENT(kinematic_level >= 0 && kinematic_level <= 2);
107
108			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
109			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
110
111			typedef typename Model::JointIndex JointIndex;
112
113			typedef typename Data::SE3 SE3;
114			typedef typename Data::Motion Motion;
115			typedef typename Data::Inertia Inertia;
116
117		946	const bool do_position = (kinematic_level >= POSITION);
118		946	const bool do_velocity = (kinematic_level >= VELOCITY);
119		946	const bool do_acceleration = (kinematic_level >= ACCELERATION);
120
121		946	data.mass[0] = 0;
122	✓✗	946	if(do_position)
123		946	data.com[0].setZero();
124	✓✓	946	if(do_velocity)
125		47	data.vcom[0].setZero();
126	✓✓	946	if(do_acceleration)
127		3	data.acom[0].setZero();
128
129			// Forward Step
130	✓✓	26574	for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
131			{
132		25628	const typename Inertia::Scalar & mass = model.inertias[i].mass();
133		25628	const typename SE3::Vector3 & lever = model.inertias[i].lever();
134
135		25628	const Motion & v = data.v[i];
136		25628	const Motion & a = data.a[i];
137
138		25628	data.mass[i] = mass;
139
140	✓✗	25628	if(do_position)
141	✓✗✓✗	25628	data.com[i].noalias() = mass * lever;
142
143	✓✓	25628	if(do_velocity)
144	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	1343	data.vcom[i].noalias() = mass * (v.angular().cross(lever) + v.linear());
145
146	✓✓	25628	if(do_acceleration)
147	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	164	data.acom[i].noalias() = mass * (a.angular().cross(lever) + a.linear())
148		82	+ v.angular().cross(data.vcom[i]); // take into accound the coriolis part of the acceleration
149			}
150
151			// Backward Step
152	✓✓	26574	for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i)
153			{
154		25628	const JointIndex & parent = model.parents[i];
155		25628	const SE3 & liMi = data.liMi[i];
156
157		25628	data.mass[parent] += data.mass[i];
158
159	✓✗	25628	if(do_position)
160	✓✗✓✗ ✓✗✓✗	25628	data.com[parent] += (liMi.rotation()*data.com[i]
161		25628	+ data.mass[i] * liMi.translation());
162
163	✓✓	25628	if(do_velocity)
164	✓✗	1343	data.vcom[parent] += liMi.rotation()*data.vcom[i];
165
166	✓✓	25628	if(do_acceleration)
167	✓✗	82	data.acom[parent] += liMi.rotation()*data.acom[i];
168
169	✓✓	25628	if(computeSubtreeComs)
170			{
171	✓✗	25600	if(do_position)
172		25600	data.com[i] /= data.mass[i];
173	✓✓	25600	if(do_velocity)
174		1315	data.vcom[i] /= data.mass[i];
175	✓✓	25600	if(do_acceleration)
176		82	data.acom[i] /= data.mass[i];
177			}
178			}
179
180	✓✗	946	if(do_position)
181		946	data.com[0] /= data.mass[0];
182	✓✓	946	if(do_velocity)
183		47	data.vcom[0] /= data.mass[0];
184	✓✓	946	if(do_acceleration)
185		3	data.acom[0] /= data.mass[0];
186
187		946	return data.com[0];
188			}
189
190			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
191			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Vector3 &
192		3	getComFromCrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
193			DataTpl<Scalar,Options,JointCollectionTpl> & data)
194			{
195			PINOCCHIO_UNUSED_VARIABLE(model);
196
197	✓✗	3	assert(model.check(data) && "data is not consistent with model.");
198		3	return data.com[0] = data.liMi[1].act(data.Ycrb[1].lever());
199			}
200
201			/* --- JACOBIAN ---------------------------------------------------------- */
202			/* --- JACOBIAN ---------------------------------------------------------- */
203			/* --- JACOBIAN ---------------------------------------------------------- */
204
205			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix3x>
206			struct JacobianCenterOfMassBackwardStep
207			: public fusion::JointUnaryVisitorBase< JacobianCenterOfMassBackwardStep<Scalar,Options,JointCollectionTpl,Matrix3x> >
208			{
209			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
210			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
211
212			typedef boost::fusion::vector<const Model &,
213			Data &,
214			const Eigen::MatrixBase<Matrix3x> &,
215			const bool &
216			> ArgsType;
217
218			template<typename JointModel>
219		546	static void algo(const JointModelBase<JointModel> & jmodel,
220			JointDataBase<typename JointModel::JointDataDerived> & jdata,
221			const Model & model,
222			Data & data,
223			const Eigen::MatrixBase<Matrix3x> & Jcom,
224			const bool & computeSubtreeComs)
225			{
226	✓✗	546	const JointIndex & i = (JointIndex) jmodel.id();
227		546	const JointIndex & parent = model.parents[i];
228
229	✓✗	546	data.com[parent] += data.com[i];
230		546	data.mass[parent] += data.mass[i];
231
232			typedef typename Data::Matrix6x Matrix6x;
233			typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
234
235		546	Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x,Jcom);
236
237	✓✗	546	ColBlock Jcols = jmodel.jointCols(data.J);
238	✓✗✓✗ ✓✗	546	Jcols = data.oMi[i].act(jdata.S());
239
240	✓✗✓✓	1152	for(Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id)
241			{
242			jmodel.jointCols(Jcom_).col(col_id)
243	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	606	= data.mass[i] * Jcols.col(col_id).template segment<3>(Motion::LINEAR)
244		606	- data.com[i].cross(Jcols.col(col_id).template segment<3>(Motion::ANGULAR));
245			}
246
247	✓✗	546	if(computeSubtreeComs)
248	✓✗	546	data.com[i] /= data.mass[i];
249			}
250
251			};
252
253			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
254			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
255		4	jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
256			DataTpl<Scalar,Options,JointCollectionTpl> & data,
257			const Eigen::MatrixBase<ConfigVectorType> & q,
258			const bool computeSubtreeComs)
259			{
260		4	forwardKinematics(model, data, q);
261		4	return jacobianCenterOfMass(model, data, computeSubtreeComs);
262			}
263
264			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
265			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
266		4	jacobianCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
267			DataTpl<Scalar,Options,JointCollectionTpl> & data,
268			const bool computeSubtreeComs)
269			{
270	✓✗	4	assert(model.check(data) && "data is not consistent with model.");
271
272			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
273			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
274
275			typedef typename Model::JointIndex JointIndex;
276
277			typedef typename Data::Matrix3x Matrix3x;
278			typedef typename Data::SE3 SE3;
279			typedef typename Data::Inertia Inertia;
280
281		4	data.com[0].setZero();
282		4	data.mass[0] = Scalar(0);
283
284	✓✓	112	for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
285			{
286		108	const typename Inertia::Scalar & mass = model.inertias[i].mass();
287		108	const typename SE3::Vector3 & lever = model.inertias[i].lever();
288
289		108	data.mass[i] = mass;
290	✓✗✓✗ ✓✗	108	data.com[i].noalias() = mass*data.oMi[i].act(lever);
291			}
292
293			// Backward step
294			typedef JacobianCenterOfMassBackwardStep<Scalar,Options,JointCollectionTpl,Matrix3x> Pass2;
295	✓✓	112	for(JointIndex i= (JointIndex) (model.njoints-1); i>0; --i)
296			{
297	✓✗	108	Pass2::run(model.joints[i],data.joints[i],
298		108	typename Pass2::ArgsType(model,data,data.Jcom,computeSubtreeComs));
299			}
300
301		4	data.com[0] /= data.mass[0];
302		4	data.Jcom /= data.mass[0];
303
304		4	return data.Jcom;
305			}
306
307			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix3x>
308			struct JacobianSubtreeCenterOfMassBackwardStep
309			: public fusion::JointUnaryVisitorBase< JacobianSubtreeCenterOfMassBackwardStep<Scalar,Options,JointCollectionTpl,Matrix3x> >
310			{
311			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
312			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
313
314			typedef boost::fusion::vector<const Model &,
315			Data &,
316			const JointIndex &,
317			const Eigen::MatrixBase<Matrix3x> &
318			> ArgsType;
319
320			template<typename JointModel>
321		222	static void algo(const JointModelBase<JointModel> & jmodel,
322			JointDataBase<typename JointModel::JointDataDerived> & jdata,
323			const Model & model,
324			Data & data,
325			const JointIndex & subtree_root_id,
326			const Eigen::MatrixBase<Matrix3x> & Jcom)
327			{
328			PINOCCHIO_UNUSED_VARIABLE(model);
329
330	✓✗	222	const JointIndex & i = (JointIndex) jmodel.id();
331
332			typedef typename Data::Matrix6x Matrix6x;
333			typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
334
335		222	Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x,Jcom);
336
337	✓✗	222	ColBlock Jcols = jmodel.jointCols(data.J);
338	✓✗✓✗ ✓✗	222	Jcols = data.oMi[i].act(jdata.S());
339
340	✓✗✓✓	704	for(Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id)
341			{
342			jmodel.jointCols(Jcom_).col(col_id)
343	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	482	= Jcols.col(col_id).template segment<3>(Motion::LINEAR)
344		482	- data.com[subtree_root_id].cross(Jcols.col(col_id).template segment<3>(Motion::ANGULAR));
345			}
346			}
347
348			};
349
350			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix3xLike>
351			inline void
352			jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
353			DataTpl<Scalar,Options,JointCollectionTpl> & data,
354			const Eigen::MatrixBase<ConfigVectorType> & q,
355			const JointIndex & rootSubtreeId,
356			const Eigen::MatrixBase<Matrix3xLike> & res)
357			{
358			forwardKinematics(model, data, q);
359			jacobianSubtreeCenterOfMass(model, data, rootSubtreeId, res);
360			}
361
362			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix3xLike>
363			inline void
364		28	jacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
365			DataTpl<Scalar,Options,JointCollectionTpl> & data,
366			const JointIndex & rootSubtreeId,
367			const Eigen::MatrixBase<Matrix3xLike> & res)
368			{
369			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
370			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
371
372	✓✗✓✗	28	assert(model.check(data) && "data is not consistent with model.");
373	✗✓✗✗	28	PINOCCHIO_CHECK_INPUT_ARGUMENT((int)rootSubtreeId < model.njoints, "Invalid joint id.");
374	✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗	28	PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), 3, "the resulting matrix does not have the right size.");
375	✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗	28	PINOCCHIO_CHECK_ARGUMENT_SIZE(res.cols(), model.nv, "the resulting matrix does not have the right size.");
376
377		28	Matrix3xLike & Jcom_subtree = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xLike,res);
378
379			typedef typename Data::SE3 SE3;
380			typedef typename Data::Inertia Inertia;
381
382			typedef typename Model::IndexVector IndexVector;
383		28	const IndexVector & subtree = model.subtrees[rootSubtreeId];
384
385		28	const bool computeSubtreeComs = true;
386
387	✓✓	28	if(rootSubtreeId == 0)
388			{
389		1	data.mass[0] = 0;
390	✓✗	1	data.com[0].setZero();
391			}
392
393	✓✓	193	for(size_t k = 0; k < subtree.size(); ++k)
394			{
395		165	const JointIndex joint_id = subtree[k];
396		165	const typename Inertia::Scalar & mass = model.inertias[joint_id].mass();
397		165	const typename SE3::Vector3 & lever = model.inertias[joint_id].lever();
398
399		165	data.mass[joint_id] = mass;
400	✓✗✓✗ ✓✗	165	data.com[joint_id] = mass*data.oMi[joint_id].act(lever);
401			}
402
403			// Backward step
404			typedef JacobianCenterOfMassBackwardStep<Scalar,Options,JointCollectionTpl,Matrix3xLike> Pass2;
405	✓✓	193	for(Eigen::DenseIndex k = (Eigen::DenseIndex)subtree.size() - 1; k >= 0; --k)
406			{
407		165	const JointIndex joint_id = subtree[(size_t)k];
408	✓✗✓✗	165	Pass2::run(model.joints[joint_id],data.joints[joint_id],
409			typename Pass2::ArgsType(model,data,Jcom_subtree,computeSubtreeComs));
410			}
411
412	✗✓✗✗	28	PINOCCHIO_CHECK_INPUT_ARGUMENT(data.mass[rootSubtreeId] > 0., "The mass of the subtree is not positive.");
413		28	const Scalar mass_inv_subtree = Scalar(1)/data.mass[rootSubtreeId];
414		28	typename Data::Vector3 & com_subtree = data.com[rootSubtreeId];
415			if(!computeSubtreeComs)
416			com_subtree *= mass_inv_subtree;
417
418	✓✓	28	if(rootSubtreeId == 0)
419			{
420	✓✗	1	Jcom_subtree *= mass_inv_subtree;
421		1	return; // skip the rest
422			}
423
424	✓✗	27	const int idx_v = model.joints[rootSubtreeId].idx_v();
425		27	const int nv_subtree = data.nvSubtree[rootSubtreeId];
426
427	✓✗✓✗	27	Jcom_subtree.middleCols(idx_v,nv_subtree) *= mass_inv_subtree;
428
429			// Second backward step
430			typedef JacobianSubtreeCenterOfMassBackwardStep<Scalar,Options,JointCollectionTpl,Matrix3xLike> Pass3;
431		138	for(JointIndex parent = model.parents[rootSubtreeId];
432	✓✓	138	parent > 0;
433		111	parent = model.parents[parent])
434			{
435	✓✗✓✗	111	Pass3::run(model.joints[parent],data.joints[parent],
436			typename Pass3::ArgsType(model,data,rootSubtreeId,Jcom_subtree));
437			}
438			}
439
440			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix3xLike>
441			inline void
442		27	getJacobianSubtreeCenterOfMass(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
443			const DataTpl<Scalar,Options,JointCollectionTpl> & data,
444			const JointIndex & rootSubtreeId,
445			const Eigen::MatrixBase<Matrix3xLike> & res)
446			{
447			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
448
449	✓✗✓✗	27	assert(model.check(data) && "data is not consistent with model.");
450	✗✓✗✗	27	PINOCCHIO_CHECK_INPUT_ARGUMENT(((int)rootSubtreeId < model.njoints), "Invalid joint id.");
451	✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗	27	PINOCCHIO_CHECK_ARGUMENT_SIZE(res.rows(), 3, "the resulting matrix does not have the right size.");
452	✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗	27	PINOCCHIO_CHECK_ARGUMENT_SIZE(res.cols(), model.nv, "the resulting matrix does not have the right size.");
453
454		27	Matrix3xLike & Jcom_subtree = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xLike,res);
455
456	✗✓	27	if(rootSubtreeId == 0)
457			{
458			Jcom_subtree = data.Jcom;
459			return;
460			}
461
462	✓✗	27	const int idx_v = model.joints[rootSubtreeId].idx_v();
463		27	const int nv_subtree = data.nvSubtree[rootSubtreeId];
464
465		27	const Scalar mass_ratio = data.mass[0] / data.mass[rootSubtreeId];
466			Jcom_subtree.middleCols(idx_v,nv_subtree)
467	✓✗✓✗ ✓✗✓✗	27	= mass_ratio * data.Jcom.middleCols(idx_v,nv_subtree);
468
469		27	const typename Data::Vector3 & com_subtree = data.com[rootSubtreeId];
470
471		268	for(int parent = data.parents_fromRow[(size_t)idx_v];
472	✓✓	268	parent >= 0;
473		241	parent = data.parents_fromRow[(size_t)parent])
474			{
475	✓✗	241	typename Data::Matrix6x::ConstColXpr Jcol = data.J.col(parent);
476	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	241	Jcom_subtree.col(parent).noalias() = Jcol.template segment<3>(Motion::LINEAR) - com_subtree.cross(Jcol.template segment<3>(Motion::ANGULAR));
477			}
478			}
479
480			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
481			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x &
482		7	getJacobianComFromCrba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
483			DataTpl<Scalar,Options,JointCollectionTpl> & data)
484			{
485			PINOCCHIO_UNUSED_VARIABLE(model);
486	✓✗✓✗	7	assert(model.check(data) && "data is not consistent with model.");
487
488			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
489			typedef typename Data::SE3 SE3;
490
491		7	const SE3 & oM1 = data.liMi[1];
492
493			// Extract the total mass of the system.
494	✓✗	7	data.mass[0] = data.M(0,0);
495
496			// As the 6 first rows of M*a are a wrench, we just need to multiply by the
497			// relative rotation between the first joint and the world
498	✓✗✓✗ ✓✗✓✗	7	const typename SE3::Matrix3 oR1_over_m (oM1.rotation() / data.M(0,0));
499
500			// I don't know why, but the colwise multiplication is much more faster
501			// than the direct Eigen multiplication
502	✓✓	231	for(long k=0; k<model.nv;++k)
503	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	224	data.Jcom.col(k).noalias() = oR1_over_m * data.M.template topRows<3>().col(k);
504
505		7	return data.Jcom;
506			}
507
508			} // namespace pinocchio
509
510			/// @endcond
511
512			#endif // ifndef __pinocchio_algorithm_center_of_mass_hxx__