Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/pinocchio/algorithm/check.hxx	Lines:	77	78	98.7 %
Date:	2024-04-26 13:14:21	Branches:	89	158	56.3 %


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

#ifndef __pinocchio_check_hxx__
#define __pinocchio_check_hxx__

#include <boost/fusion/algorithm.hpp>
#include <boost/foreach.hpp>

namespace pinocchio
{
  namespace internal
  {
    // Dedicated structure for the fusion::accumulate algorithm: validate the check-algorithm
    // for all elements in a fusion list of AlgoCheckers.
    template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
    struct AlgoFusionChecker
    {
      typedef bool result_type;
      typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
      const Model & model;

      AlgoFusionChecker(const Model & model) : model(model) {}

      inline bool operator()(const bool & accumul, const boost::fusion::void_ &) const
      { return accumul; }

      template<typename T>
      inline bool operator()(const bool & accumul, const AlgorithmCheckerBase<T> & t) const
      { return accumul && t.checkModel(model); }
    };
  } // namespace internal

  // Check the validity of the kinematic tree defined by parents.
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline bool ParentChecker::checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef typename Model::JointIndex JointIndex;

    for(JointIndex j=1;j<(JointIndex)model.njoints;++j)
      if(model.parents[j]>=j)
        return false;

    return true;
  }

#if !defined(BOOST_FUSION_HAS_VARIADIC_LIST)
  template<BOOST_PP_ENUM_PARAMS(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE,class T)>
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  bool AlgorithmCheckerList<BOOST_PP_ENUM_PARAMS(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE,T)>
  ::checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const
  {
    return boost::fusion::accumulate(checkerList,
                                     true,
                                     internal::AlgoFusionChecker<Scalar,Options,JointCollectionTpl>(model));
  }
#else
  template<class ...T>
  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  bool AlgorithmCheckerList<T...>::checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const
  {
    return boost::fusion::accumulate(checkerList,
                                     true,
                                     internal::AlgoFusionChecker<Scalar,Options,JointCollectionTpl>(model));
  }
#endif

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline bool checkData(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
                        const DataTpl<Scalar,Options,JointCollectionTpl> & data)
  {
    typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
    typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;

    typedef typename Model::JointModel JointModel;

#define CHECK_DATA(a)  if(!(a)) return false;

    // TODO JMinvJt,sDUiJt are never explicitly initialized.
    // TODO impulse_c
    // They are not check neither

    CHECK_DATA( (int)data.joints.size()   == model.njoints );
    CHECK_DATA( (int)data.a.size()        == model.njoints );
    CHECK_DATA( (int)data.a_gf.size()     == model.njoints );
    CHECK_DATA( (int)data.v.size()        == model.njoints );
    CHECK_DATA( (int)data.f.size()        == model.njoints );
    CHECK_DATA( (int)data.oMi.size()      == model.njoints );
    CHECK_DATA( (int)data.liMi.size()     == model.njoints );
    CHECK_DATA( (int)data.Ycrb.size()     == model.njoints );
    CHECK_DATA( (int)data.Yaba.size()     == model.njoints );
    CHECK_DATA( (int)data.Fcrb.size()     == model.njoints );
    BOOST_FOREACH(const typename Data::Matrix6x & F,data.Fcrb)
    {
      CHECK_DATA( F.cols() == model.nv );
    }
    CHECK_DATA( (int)data.iMf.size()      == model.njoints );
    CHECK_DATA( (int)data.iMf.size()      == model.njoints );
    CHECK_DATA( (int)data.com.size()      == model.njoints );
    CHECK_DATA( (int)data.vcom.size()     == model.njoints );
    CHECK_DATA( (int)data.acom.size()     == model.njoints );
    CHECK_DATA( (int)data.mass.size()     == model.njoints );

    CHECK_DATA( data.tau.size()      == model.nv );
    CHECK_DATA( data.nle.size()      == model.nv );
    CHECK_DATA( data.ddq.size()      == model.nv );
    CHECK_DATA( data.u.size()        == model.nv );
    CHECK_DATA( data.M.rows()        == model.nv );
    CHECK_DATA( data.M.cols()        == model.nv );
    CHECK_DATA( data.Ag.cols()       == model.nv );
    CHECK_DATA( data.U.cols()        == model.nv );
    CHECK_DATA( data.U.rows()        == model.nv );
    CHECK_DATA( data.D.size()        == model.nv );
    CHECK_DATA( data.tmp.size()      == model.nv );
    CHECK_DATA( data.J.cols()        == model.nv );
    CHECK_DATA( data.Jcom.cols()     == model.nv );
    CHECK_DATA( data.torque_residual.size() == model.nv );
    CHECK_DATA( data.dq_after.size() == model.nv );
    //CHECK_DATA( data.impulse_c.size()== model.nv );

    CHECK_DATA( data.kinematic_hessians.dimension(0) == 6);
#if EIGEN_VERSION_AT_LEAST(3,2,90) && !EIGEN_VERSION_AT_LEAST(3,2,93)
    CHECK_DATA( data.kinematic_hessians.dimension(1) == std::max(1,model.nv));
    CHECK_DATA( data.kinematic_hessians.dimension(2) == std::max(1,model.nv));
#else
    CHECK_DATA( data.kinematic_hessians.dimension(1) == model.nv);
    CHECK_DATA( data.kinematic_hessians.dimension(2) == model.nv);
#endif

    CHECK_DATA( (int)data.oMf.size()      == model.nframes );

    CHECK_DATA( (int)data.lastChild.size()         == model.njoints );
    CHECK_DATA( (int)data.nvSubtree.size()         == model.njoints );
    CHECK_DATA( (int)data.parents_fromRow.size()   == model.nv );
    CHECK_DATA( (int)data.nvSubtree_fromRow.size() == model.nv );

    for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
    {
      const typename Model::JointModel & jmodel = model.joints[joint_id];

      CHECK_DATA(model.nqs[joint_id] == jmodel.nq());
      CHECK_DATA(model.idx_qs[joint_id] == jmodel.idx_q());
      CHECK_DATA(model.nvs[joint_id] == jmodel.nv());
      CHECK_DATA(model.idx_vs[joint_id] == jmodel.idx_v());
    }

    for( JointIndex j=1;int(j)<model.njoints;++j )
    {
      JointIndex c = (JointIndex)data.lastChild[j];
      CHECK_DATA((int)c<model.njoints);
      int nv=model.joints[j].nv();
      for( JointIndex d=j+1;d<=c;++d ) // explore all descendant
      {
        CHECK_DATA( model.parents[d]>=j );
        nv+=model.joints[d].nv();
      }
      CHECK_DATA(nv==data.nvSubtree[j]);

      for( JointIndex d=c+1;(int)d<model.njoints;++d)
        CHECK_DATA( (model.parents[d]<j)||(model.parents[d]>c) );

      int row = model.joints[j].idx_v();
      CHECK_DATA(data.nvSubtree[j] == data.nvSubtree_fromRow[(size_t)row]);

      const JointModel & jparent = model.joints[model.parents[j]];
      if(row==0) { CHECK_DATA(data.parents_fromRow[(size_t)row]==-1); }
      else       { CHECK_DATA(jparent.idx_v()+jparent.nv()-1 == data.parents_fromRow[(size_t)row]); }
    }

#undef CHECK_DATA
    return true;
  }

  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
  inline bool ModelTpl<Scalar,Options,JointCollectionTpl>::
  check(const DataTpl<Scalar,Options,JointCollectionTpl> & data) const
  { return checkData(*this,data); }


} // namespace pinocchio

#endif // ifndef __pinocchio_check_hxx__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2016-2020 CNRS INRIA
3			//
4
5			#ifndef __pinocchio_check_hxx__
6			#define __pinocchio_check_hxx__
7
8			#include <boost/fusion/algorithm.hpp>
9			#include <boost/foreach.hpp>
10
11			namespace pinocchio
12			{
13			namespace internal
14			{
15			// Dedicated structure for the fusion::accumulate algorithm: validate the check-algorithm
16			// for all elements in a fusion list of AlgoCheckers.
17			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
18			struct AlgoFusionChecker
19			{
20			typedef bool result_type;
21			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
22			const Model & model;
23
24		2	AlgoFusionChecker(const Model & model) : model(model) {}
25
26			inline bool operator()(const bool & accumul, const boost::fusion::void_ &) const
27			{ return accumul; }
28
29			template<typename T>
30		12	inline bool operator()(const bool & accumul, const AlgorithmCheckerBase<T> & t) const
31	✓✗✓✗	12	{ return accumul && t.checkModel(model); }
32			};
33			} // namespace internal
34
35			// Check the validity of the kinematic tree defined by parents.
36			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
37		2	inline bool ParentChecker::checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const
38			{
39			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
40			typedef typename Model::JointIndex JointIndex;
41
42	✓✓	56	for(JointIndex j=1;j<(JointIndex)model.njoints;++j)
43	✗✓	54	if(model.parents[j]>=j)
44			return false;
45
46		2	return true;
47			}
48
49			#if !defined(BOOST_FUSION_HAS_VARIADIC_LIST)
50			template<BOOST_PP_ENUM_PARAMS(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE,class T)>
51			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
52			bool AlgorithmCheckerList<BOOST_PP_ENUM_PARAMS(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE,T)>
53			::checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const
54			{
55			return boost::fusion::accumulate(checkerList,
56			true,
57			internal::AlgoFusionChecker<Scalar,Options,JointCollectionTpl>(model));
58			}
59			#else
60			template<class ...T>
61			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
62		4	bool AlgorithmCheckerList<T...>::checkModel_impl(const ModelTpl<Scalar,Options,JointCollectionTpl> & model) const
63			{
64		4	return boost::fusion::accumulate(checkerList,
65	✓✗	8	true,
66		8	internal::AlgoFusionChecker<Scalar,Options,JointCollectionTpl>(model));
67			}
68			#endif
69
70			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
71		33569	inline bool checkData(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
72			const DataTpl<Scalar,Options,JointCollectionTpl> & data)
73			{
74			typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
75			typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
76
77			typedef typename Model::JointModel JointModel;
78
79			#define CHECK_DATA(a) if(!(a)) return false;
80
81			// TODO JMinvJt,sDUiJt are never explicitly initialized.
82			// TODO impulse_c
83			// They are not check neither
84
85	✗✓	33569	CHECK_DATA( (int)data.joints.size() == model.njoints );
86	✗✓	33569	CHECK_DATA( (int)data.a.size() == model.njoints );
87	✗✓	33569	CHECK_DATA( (int)data.a_gf.size() == model.njoints );
88	✗✓	33569	CHECK_DATA( (int)data.v.size() == model.njoints );
89	✗✓	33569	CHECK_DATA( (int)data.f.size() == model.njoints );
90	✗✓	33569	CHECK_DATA( (int)data.oMi.size() == model.njoints );
91	✗✓	33569	CHECK_DATA( (int)data.liMi.size() == model.njoints );
92	✗✓	33569	CHECK_DATA( (int)data.Ycrb.size() == model.njoints );
93	✗✓	33569	CHECK_DATA( (int)data.Yaba.size() == model.njoints );
94	✗✓	33569	CHECK_DATA( (int)data.Fcrb.size() == model.njoints );
95	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗	1886657	BOOST_FOREACH(const typename Data::Matrix6x & F,data.Fcrb)
96			{
97	✓✗✗✓	926544	CHECK_DATA( F.cols() == model.nv );
98			}
99	✗✓	33569	CHECK_DATA( (int)data.iMf.size() == model.njoints );
100	✗✓	33569	CHECK_DATA( (int)data.iMf.size() == model.njoints );
101	✗✓	33569	CHECK_DATA( (int)data.com.size() == model.njoints );
102	✗✓	33569	CHECK_DATA( (int)data.vcom.size() == model.njoints );
103	✗✓	33569	CHECK_DATA( (int)data.acom.size() == model.njoints );
104	✗✓	33569	CHECK_DATA( (int)data.mass.size() == model.njoints );
105
106	✗✓	33569	CHECK_DATA( data.tau.size() == model.nv );
107	✗✓	33569	CHECK_DATA( data.nle.size() == model.nv );
108	✗✓	33569	CHECK_DATA( data.ddq.size() == model.nv );
109	✗✓	33569	CHECK_DATA( data.u.size() == model.nv );
110	✗✓	33569	CHECK_DATA( data.M.rows() == model.nv );
111	✗✓	33569	CHECK_DATA( data.M.cols() == model.nv );
112	✗✓	33569	CHECK_DATA( data.Ag.cols() == model.nv );
113	✗✓	33569	CHECK_DATA( data.U.cols() == model.nv );
114	✗✓	33569	CHECK_DATA( data.U.rows() == model.nv );
115	✗✓	33569	CHECK_DATA( data.D.size() == model.nv );
116	✗✓	33569	CHECK_DATA( data.tmp.size() == model.nv );
117	✗✓	33569	CHECK_DATA( data.J.cols() == model.nv );
118	✗✓	33569	CHECK_DATA( data.Jcom.cols() == model.nv );
119	✗✓	33569	CHECK_DATA( data.torque_residual.size() == model.nv );
120	✗✓	33569	CHECK_DATA( data.dq_after.size() == model.nv );
121			//CHECK_DATA( data.impulse_c.size()== model.nv );
122
123	✗✓	33569	CHECK_DATA( data.kinematic_hessians.dimension(0) == 6);
124			#if EIGEN_VERSION_AT_LEAST(3,2,90) && !EIGEN_VERSION_AT_LEAST(3,2,93)
125			CHECK_DATA( data.kinematic_hessians.dimension(1) == std::max(1,model.nv));
126			CHECK_DATA( data.kinematic_hessians.dimension(2) == std::max(1,model.nv));
127			#else
128	✗✓	33569	CHECK_DATA( data.kinematic_hessians.dimension(1) == model.nv);
129	✗✓	33569	CHECK_DATA( data.kinematic_hessians.dimension(2) == model.nv);
130			#endif
131
132	✗✓	33569	CHECK_DATA( (int)data.oMf.size() == model.nframes );
133
134	✗✓	33569	CHECK_DATA( (int)data.lastChild.size() == model.njoints );
135	✗✓	33569	CHECK_DATA( (int)data.nvSubtree.size() == model.njoints );
136	✗✓	33569	CHECK_DATA( (int)data.parents_fromRow.size() == model.nv );
137	✗✓	33569	CHECK_DATA( (int)data.nvSubtree_fromRow.size() == model.nv );
138
139	✓✓	926544	for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
140			{
141		892975	const typename Model::JointModel & jmodel = model.joints[joint_id];
142
143	✗✓	892975	CHECK_DATA(model.nqs[joint_id] == jmodel.nq());
144	✗✓	892975	CHECK_DATA(model.idx_qs[joint_id] == jmodel.idx_q());
145	✗✓	892975	CHECK_DATA(model.nvs[joint_id] == jmodel.nv());
146	✗✓	892975	CHECK_DATA(model.idx_vs[joint_id] == jmodel.idx_v());
147			}
148
149	✓✓	926544	for( JointIndex j=1;int(j)<model.njoints;++j )
150			{
151		892975	JointIndex c = (JointIndex)data.lastChild[j];
152	✗✓	892975	CHECK_DATA((int)c<model.njoints);
153		892975	int nv=model.joints[j].nv();
154	✓✓	4559835	for( JointIndex d=j+1;d<=c;++d ) // explore all descendant
155			{
156	✗✓	3666860	CHECK_DATA( model.parents[d]>=j );
157		3666860	nv+=model.joints[d].nv();
158			}
159	✗✓	892975	CHECK_DATA(nv==data.nvSubtree[j]);
160
161	✓✓	8815263	for( JointIndex d=c+1;(int)d<model.njoints;++d)
162	✓✓✗✓ ✗✓	7922288	CHECK_DATA( (model.parents[d]<j)\|\|(model.parents[d]>c) );
163
164		892975	int row = model.joints[j].idx_v();
165	✗✓	892975	CHECK_DATA(data.nvSubtree[j] == data.nvSubtree_fromRow[(size_t)row]);
166
167		892975	const JointModel & jparent = model.joints[model.parents[j]];
168	✓✓✗✓	892975	if(row==0) { CHECK_DATA(data.parents_fromRow[(size_t)row]==-1); }
169	✗✓	859407	else { CHECK_DATA(jparent.idx_v()+jparent.nv()-1 == data.parents_fromRow[(size_t)row]); }
170			}
171
172			#undef CHECK_DATA
173		33569	return true;
174			}
175
176			template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
177		33568	inline bool ModelTpl<Scalar,Options,JointCollectionTpl>::
178			check(const DataTpl<Scalar,Options,JointCollectionTpl> & data) const
179		33568	{ return checkData(*this,data); }
180
181
182			} // namespace pinocchio
183
184			#endif // ifndef __pinocchio_check_hxx__