Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/algorithm/crba.hxx	Lines:	70	70	100.0 %
Date:	2024-04-26 13:14:21	Branches:	95	207	45.9 %


//
// Copyright (c) 2015-2019 CNRS
//

#ifndef __pinocchio_crba_hxx__
#define __pinocchio_crba_hxx__

#include "pinocchio/multibody/visitor.hpp"
#include "pinocchio/spatial/act-on-set.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/check.hpp"

/// @cond DEV

namespace pinocchio
{
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  struct CrbaForwardStep
  : public fusion::JointUnaryVisitorBase< CrbaForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> >
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typedef boost::fusion::vector<const Model &,
                                  Data &,
                                  const ConfigVectorType &
                                  > ArgsType;

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

      const JointIndex & i = jmodel.id();
      jmodel.calc(jdata.derived(),q.derived());

      data.liMi[i] = model.jointPlacements[i]*jdata.M();
      data.Ycrb[i] = model.inertias[i];
    }

  };

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

    typedef boost::fusion::vector<const Model &,
				                          Data &>  ArgsType;

    template<typename JointModel>
    static void algo(const JointModelBase<JointModel> & jmodel,
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     const Model & model,
                     Data & data)
    {
      /*
       * F[1:6,i] = Y*S
       * M[i,SUBTREE] = S'*F[1:6,SUBTREE]
       * if li>0
       *   Yli += liXi Yi
       *   F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
       */

      typedef typename Model::JointIndex JointIndex;
      typedef typename Data::Matrix6x::ColsBlockXpr Block;
      const JointIndex & i = jmodel.id();

      /* F[1:6,i] = Y*S */
      //data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
      jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S();

      /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
      data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
      = jdata.S().transpose()*data.Fcrb[i].middleCols(jmodel.idx_v(),data.nvSubtree[i]);

      const JointIndex & parent = model.parents[i];
      if(parent>0)
      {
        /*   Yli += liXi Yi */
        data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);

        /*   F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */
        Block jF
        = data.Fcrb[parent].middleCols(jmodel.idx_v(),data.nvSubtree[i]);
        Block iF
        = data.Fcrb[i].middleCols(jmodel.idx_v(),data.nvSubtree[i]);
        forceSet::se3Action(data.liMi[i], iF, jF);
      }
    }
  };

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

    typedef boost::fusion::vector<const Model &,
                                  Data &,
                                  const ConfigVectorType &
                                  > ArgsType;

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

      const JointIndex & i = jmodel.id();
      jmodel.calc(jdata.derived(),q.derived());

      data.liMi[i] = model.jointPlacements[i]*jdata.M();

      const JointIndex & parent = model.parents[i];
      if (parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
      else data.oMi[i] = data.liMi[i];

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

      data.Ycrb[i] = model.inertias[i];
    }

  };

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

    typedef boost::fusion::vector<const Model &,
                                  Data &>  ArgsType;

    template<typename JointModel>
    static void algo(const JointModelBase<JointModel> & jmodel,
                     JointDataBase<typename JointModel::JointDataDerived> & jdata,
                     const Model & model,
                     Data & data)
    {
      typedef typename Model::JointIndex JointIndex;
      typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
      const JointIndex & i = jmodel.id();

      /* F[1:6,i] = Y*S */
      jdata.U() = data.Ycrb[i] * jdata.S();
      ColsBlock jF = data.Ag.template middleCols<JointModel::NV>(jmodel.idx_v());
      //        = data.Ag.middleCols(jmodel.idx_v(), jmodel.nv());

      forceSet::se3Action(data.oMi[i],jdata.U(),jF);

      /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
      data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
      = jmodel.jointCols(data.J).transpose()*data.Ag.middleCols(jmodel.idx_v(),data.nvSubtree[i]);

      const JointIndex & parent = model.parents[i];
      /*   Yli += liXi Yi */
      data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
    }
  };


  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs &
  crba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
       DataTpl<Scalar,Options,JointCollectionTpl> & data,
       const Eigen::MatrixBase<ConfigVectorType> & q)
  {
    assert(model.check(data) && "data is not consistent with model.");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");

    typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;

    typedef CrbaForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> Pass1;
    for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
    {
      Pass1::run(model.joints[i],data.joints[i],
                 typename Pass1::ArgsType(model,data,q.derived()));
    }

    typedef CrbaBackwardStep<Scalar,Options,JointCollectionTpl> Pass2;
    for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i)
    {
      Pass2::run(model.joints[i],data.joints[i],
                 typename Pass2::ArgsType(model,data));
    }

    return data.M;
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
  inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs &
  crbaMinimal(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
              DataTpl<Scalar,Options,JointCollectionTpl> & data,
              const Eigen::MatrixBase<ConfigVectorType> & q)
  {
    assert(model.check(data) && "data is not consistent with model.");
    PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");

    typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;

    typedef CrbaForwardStepMinimal<Scalar,Options,JointCollectionTpl,ConfigVectorType> Pass1;
    data.Ycrb[0].setZero();
    for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
    {
      Pass1::run(model.joints[i],data.joints[i],
                 typename Pass1::ArgsType(model,data,q.derived()));
    }

    typedef CrbaBackwardStepMinimal<Scalar,Options,JointCollectionTpl> Pass2;
    for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i)
    {
      Pass2::run(model.joints[i],data.joints[i],
                 typename Pass2::ArgsType(model,data));
    }

    // Retrieve the Centroidal Momemtum map
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
    typedef typename Data::Force Force;
    typedef Eigen::Block<typename Data::Matrix6x,3,-1> Block3x;

    data.mass[0] = data.Ycrb[0].mass();
    data.com[0] = data.Ycrb[0].lever();

    const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
    Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
    for(long i = 0; i<model.nv; ++i)
      Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);

    return data.M;
  }

  // --- CHECKER ---------------------------------------------------------------
  // --- CHECKER ---------------------------------------------------------------
  // --- CHECKER ---------------------------------------------------------------

  namespace internal
  {
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    inline bool isDescendant(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                             const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex j,
                             const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex root)
    {
      typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
      typedef typename Model::JointIndex JointIndex;

      if(j>=(JointIndex)model.njoints)  return false;
      if(j==0)                          return root==0;
      return (j==root) || isDescendant(model,model.parents[j],root);
    }
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline bool CRBAChecker::checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;

    // For CRBA, the tree must be "compact", i.e. all descendants of a node i are stored
    // immediately after i in the "parents" map, i.e. forall joint i, the interval i+1..n-1
    // can be separated in two intervals [i+1..k] and [k+1..n-1], where any [i+1..k] is a descendant
    // of i and none of [k+1..n-1] is a descendant of i.
    for(JointIndex i=1; i < (JointIndex)(model.njoints-1); ++i) // no need to check joints 0 and N-1
      {
        JointIndex k=i+1;
        while(internal::isDescendant(model,k,i)) ++k;
        for( ; int(k)<model.njoints; ++k )
          if( internal::isDescendant(model,k,i) ) return false;
      }
    return true;
  }


} // namespace pinocchio

/// @endcond

#endif // ifndef __pinocchio_crba_hxx__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2015-2019 CNRS
3			//
4
5			#ifndef __pinocchio_crba_hxx__
6			#define __pinocchio_crba_hxx__
7
8			#include "pinocchio/multibody/visitor.hpp"
9			#include "pinocchio/spatial/act-on-set.hpp"
10			#include "pinocchio/algorithm/kinematics.hpp"
11			#include "pinocchio/algorithm/check.hpp"
12
13			/// @cond DEV
14
15			namespace pinocchio
16			{
17			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
18			struct CrbaForwardStep
19			: public fusion::JointUnaryVisitorBase< CrbaForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> >
20			{
21			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
22			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
23
24			typedef boost::fusion::vector<const Model &,
25			Data &,
26			const ConfigVectorType &
27			> ArgsType;
28
29			template<typename JointModel>
30		1892	static void algo(const JointModelBase<JointModel> & jmodel,
31			JointDataBase<typename JointModel::JointDataDerived> & jdata,
32			const Model & model,
33			Data & data,
34			const Eigen::MatrixBase<ConfigVectorType> & q)
35			{
36			typedef typename Model::JointIndex JointIndex;
37
38		1892	const JointIndex & i = jmodel.id();
39		1892	jmodel.calc(jdata.derived(),q.derived());
40
41	✓✗	1892	data.liMi[i] = model.jointPlacements[i]*jdata.M();
42		1892	data.Ycrb[i] = model.inertias[i];
43			}
44
45			};
46
47			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
48			struct CrbaBackwardStep
49			: public fusion::JointUnaryVisitorBase< CrbaBackwardStep<Scalar,Options,JointCollectionTpl> >
50			{
51			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
52			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
53
54			typedef boost::fusion::vector<const Model &,
55			Data &> ArgsType;
56
57			template<typename JointModel>
58		1892	static void algo(const JointModelBase<JointModel> & jmodel,
59			JointDataBase<typename JointModel::JointDataDerived> & jdata,
60			const Model & model,
61			Data & data)
62			{
63			/*
64			* F[1:6,i] = Y*S
65			* M[i,SUBTREE] = S'*F[1:6,SUBTREE]
66			* if li>0
67			* Yli += liXi Yi
68			* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
69			*/
70
71			typedef typename Model::JointIndex JointIndex;
72			typedef typename Data::Matrix6x::ColsBlockXpr Block;
73		1892	const JointIndex & i = jmodel.id();
74
75			/* F[1:6,i] = YS /
76			//data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
77	✓✗✓✗	1892	jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S();
78
79			/* M[i,SUBTREE] = S'F[1:6,SUBTREE] /
80	✓✗✓✗ ✓✗✓✗	1892	data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
81	✓✗✓✓ ✗✓✓✗ ✓✓✗✓ ✗	3784	= jdata.S().transpose()*data.Fcrb[i].middleCols(jmodel.idx_v(),data.nvSubtree[i]);
82
83		1892	const JointIndex & parent = model.parents[i];
84	✓✓	1892	if(parent>0)
85			{
86			/* Yli += liXi Yi */
87	✓✗✓✗	1784	data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
88
89			/* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */
90	✓✗✓✗	1784	Block jF
91		1784	= data.Fcrb[parent].middleCols(jmodel.idx_v(),data.nvSubtree[i]);
92	✓✗✓✗	1784	Block iF
93		1784	= data.Fcrb[i].middleCols(jmodel.idx_v(),data.nvSubtree[i]);
94	✓✗	1784	forceSet::se3Action(data.liMi[i], iF, jF);
95			}
96			}
97			};
98
99			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
100			struct CrbaForwardStepMinimal
101			: public fusion::JointUnaryVisitorBase< CrbaForwardStepMinimal<Scalar,Options,JointCollectionTpl,ConfigVectorType> >
102			{
103			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
104			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
105
106			typedef boost::fusion::vector<const Model &,
107			Data &,
108			const ConfigVectorType &
109			> ArgsType;
110
111			template<typename JointModel>
112		216	static void algo(const JointModelBase<JointModel> & jmodel,
113			JointDataBase<typename JointModel::JointDataDerived> & jdata,
114			const Model & model,
115			Data & data,
116			const Eigen::MatrixBase<ConfigVectorType> & q)
117			{
118			typedef typename Model::JointIndex JointIndex;
119
120		216	const JointIndex & i = jmodel.id();
121		216	jmodel.calc(jdata.derived(),q.derived());
122
123	✓✗	216	data.liMi[i] = model.jointPlacements[i]*jdata.M();
124
125		216	const JointIndex & parent = model.parents[i];
126	✓✓	216	if (parent>0) data.oMi[i] = data.oMi[parent]*data.liMi[i];
127		8	else data.oMi[i] = data.liMi[i];
128
129	✓✗✓✗	216	jmodel.jointCols(data.J) = data.oMi[i].act(jdata.S());
130
131		216	data.Ycrb[i] = model.inertias[i];
132			}
133
134			};
135
136			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
137			struct CrbaBackwardStepMinimal
138			: public fusion::JointUnaryVisitorBase< CrbaBackwardStepMinimal<Scalar,Options,JointCollectionTpl> >
139			{
140			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
141			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
142
143			typedef boost::fusion::vector<const Model &,
144			Data &> ArgsType;
145
146			template<typename JointModel>
147		216	static void algo(const JointModelBase<JointModel> & jmodel,
148			JointDataBase<typename JointModel::JointDataDerived> & jdata,
149			const Model & model,
150			Data & data)
151			{
152			typedef typename Model::JointIndex JointIndex;
153			typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
154	✓✗	216	const JointIndex & i = jmodel.id();
155
156			/* F[1:6,i] = YS /
157	✓✗✓✗ ✓✗	216	jdata.U() = data.Ycrb[i] * jdata.S();
158	✓✗✓✗	216	ColsBlock jF = data.Ag.template middleCols<JointModel::NV>(jmodel.idx_v());
159			// = data.Ag.middleCols(jmodel.idx_v(), jmodel.nv());
160
161	✓✗✓✗	216	forceSet::se3Action(data.oMi[i],jdata.U(),jF);
162
163			/* M[i,SUBTREE] = S'F[1:6,SUBTREE] /
164	✓✗✓✗ ✓✗✓✗	216	data.M.block(jmodel.idx_v(),jmodel.idx_v(),jmodel.nv(),data.nvSubtree[i]).noalias()
165	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	432	= jmodel.jointCols(data.J).transpose()*data.Ag.middleCols(jmodel.idx_v(),data.nvSubtree[i]);
166
167		216	const JointIndex & parent = model.parents[i];
168			/* Yli += liXi Yi */
169	✓✗✓✗	216	data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
170			}
171			};
172
173
174			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
175			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs &
176		54	crba(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
177			DataTpl<Scalar,Options,JointCollectionTpl> & data,
178			const Eigen::MatrixBase<ConfigVectorType> & q)
179			{
180	✓✗	54	assert(model.check(data) && "data is not consistent with model.");
181	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	54	PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
182
183			typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
184
185			typedef CrbaForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType> Pass1;
186	✓✓	1000	for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
187			{
188	✓✗	946	Pass1::run(model.joints[i],data.joints[i],
189			typename Pass1::ArgsType(model,data,q.derived()));
190			}
191
192			typedef CrbaBackwardStep<Scalar,Options,JointCollectionTpl> Pass2;
193	✓✓	1000	for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i)
194			{
195	✓✗	946	Pass2::run(model.joints[i],data.joints[i],
196			typename Pass2::ArgsType(model,data));
197			}
198
199		54	return data.M;
200			}
201
202			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType>
203			inline const typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs &
204		4	crbaMinimal(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
205			DataTpl<Scalar,Options,JointCollectionTpl> & data,
206			const Eigen::MatrixBase<ConfigVectorType> & q)
207			{
208	✓✗✓✗	4	assert(model.check(data) && "data is not consistent with model.");
209	✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗	4	PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq, "The configuration vector is not of right size");
210
211			typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex JointIndex;
212
213			typedef CrbaForwardStepMinimal<Scalar,Options,JointCollectionTpl,ConfigVectorType> Pass1;
214	✓✗	4	data.Ycrb[0].setZero();
215	✓✓	112	for(JointIndex i=1; i<(JointIndex)(model.njoints); ++i)
216			{
217	✓✗✓✗	108	Pass1::run(model.joints[i],data.joints[i],
218			typename Pass1::ArgsType(model,data,q.derived()));
219			}
220
221			typedef CrbaBackwardStepMinimal<Scalar,Options,JointCollectionTpl> Pass2;
222	✓✓	112	for(JointIndex i=(JointIndex)(model.njoints-1); i>0; --i)
223			{
224	✓✗✓✗	108	Pass2::run(model.joints[i],data.joints[i],
225			typename Pass2::ArgsType(model,data));
226			}
227
228			// Retrieve the Centroidal Momemtum map
229			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
230			typedef typename Data::Force Force;
231			typedef Eigen::Block<typename Data::Matrix6x,3,-1> Block3x;
232
233		4	data.mass[0] = data.Ycrb[0].mass();
234	✓✗	4	data.com[0] = data.Ycrb[0].lever();
235
236	✓✗	4	const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
237	✓✗	4	Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
238	✓✓	132	for(long i = 0; i<model.nv; ++i)
239	✓✗✓✗ ✓✗✓✗	128	Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
240
241		4	return data.M;
242			}
243
244			// --- CHECKER ---------------------------------------------------------------
245			// --- CHECKER ---------------------------------------------------------------
246			// --- CHECKER ---------------------------------------------------------------
247
248			namespace internal
249			{
250			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
251		6495	inline bool isDescendant(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
252			const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex j,
253			const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex root)
254			{
255			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
256			typedef typename Model::JointIndex JointIndex;
257
258	✓✓	6495	if(j>=(JointIndex)model.njoints) return false;
259	✓✓	6471	if(j==0) return root==0;
260	✓✓✓✓	5697	return (j==root) \|\| isDescendant(model,model.parents[j],root);
261			}
262			}
263
264			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
265		3	inline bool CRBAChecker::checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const
266			{
267			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
268			typedef typename Model::JointIndex JointIndex;
269
270			// For CRBA, the tree must be "compact", i.e. all descendants of a node i are stored
271			// immediately after i in the "parents" map, i.e. forall joint i, the interval i+1..n-1
272			// can be separated in two intervals [i+1..k] and [k+1..n-1], where any [i+1..k] is a descendant
273			// of i and none of [k+1..n-1] is a descendant of i.
274	✓✓	81	for(JointIndex i=1; i < (JointIndex)(model.njoints-1); ++i) // no need to check joints 0 and N-1
275			{
276		78	JointIndex k=i+1;
277	✓✓	411	while(internal::isDescendant(model,k,i)) ++k;
278	✓✓	798	for( ; int(k)<model.njoints; ++k )
279	✗✓	720	if( internal::isDescendant(model,k,i) ) return false;
280			}
281		3	return true;
282			}
283
284
285			} // namespace pinocchio
286
287			/// @endcond
288
289			#endif // ifndef __pinocchio_crba_hxx__