GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 |
33562 |
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 |
✗✓ | 33562 |
CHECK_DATA( (int)data.joints.size() == model.njoints ); |
86 |
✗✓ | 33562 |
CHECK_DATA( (int)data.a.size() == model.njoints ); |
87 |
✗✓ | 33562 |
CHECK_DATA( (int)data.a_gf.size() == model.njoints ); |
88 |
✗✓ | 33562 |
CHECK_DATA( (int)data.v.size() == model.njoints ); |
89 |
✗✓ | 33562 |
CHECK_DATA( (int)data.f.size() == model.njoints ); |
90 |
✗✓ | 33562 |
CHECK_DATA( (int)data.oMi.size() == model.njoints ); |
91 |
✗✓ | 33562 |
CHECK_DATA( (int)data.liMi.size() == model.njoints ); |
92 |
✗✓ | 33562 |
CHECK_DATA( (int)data.Ycrb.size() == model.njoints ); |
93 |
✗✓ | 33562 |
CHECK_DATA( (int)data.Yaba.size() == model.njoints ); |
94 |
✗✓ | 33562 |
CHECK_DATA( (int)data.Fcrb.size() == model.njoints ); |
95 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✓✓✓ ✓✗✓✗ ✓✓✓✗ ✓✗ |
1886248 |
BOOST_FOREACH(const typename Data::Matrix6x & F,data.Fcrb) |
96 |
{ |
||
97 |
✓✗✗✓ |
926343 |
CHECK_DATA( F.cols() == model.nv ); |
98 |
} |
||
99 |
✗✓ | 33562 |
CHECK_DATA( (int)data.iMf.size() == model.njoints ); |
100 |
✗✓ | 33562 |
CHECK_DATA( (int)data.iMf.size() == model.njoints ); |
101 |
✗✓ | 33562 |
CHECK_DATA( (int)data.com.size() == model.njoints ); |
102 |
✗✓ | 33562 |
CHECK_DATA( (int)data.vcom.size() == model.njoints ); |
103 |
✗✓ | 33562 |
CHECK_DATA( (int)data.acom.size() == model.njoints ); |
104 |
✗✓ | 33562 |
CHECK_DATA( (int)data.mass.size() == model.njoints ); |
105 |
|||
106 |
✗✓ | 33562 |
CHECK_DATA( data.tau.size() == model.nv ); |
107 |
✗✓ | 33562 |
CHECK_DATA( data.nle.size() == model.nv ); |
108 |
✗✓ | 33562 |
CHECK_DATA( data.ddq.size() == model.nv ); |
109 |
✗✓ | 33562 |
CHECK_DATA( data.u.size() == model.nv ); |
110 |
✗✓ | 33562 |
CHECK_DATA( data.M.rows() == model.nv ); |
111 |
✗✓ | 33562 |
CHECK_DATA( data.M.cols() == model.nv ); |
112 |
✗✓ | 33562 |
CHECK_DATA( data.Ag.cols() == model.nv ); |
113 |
✗✓ | 33562 |
CHECK_DATA( data.U.cols() == model.nv ); |
114 |
✗✓ | 33562 |
CHECK_DATA( data.U.rows() == model.nv ); |
115 |
✗✓ | 33562 |
CHECK_DATA( data.D.size() == model.nv ); |
116 |
✗✓ | 33562 |
CHECK_DATA( data.tmp.size() == model.nv ); |
117 |
✗✓ | 33562 |
CHECK_DATA( data.J.cols() == model.nv ); |
118 |
✗✓ | 33562 |
CHECK_DATA( data.Jcom.cols() == model.nv ); |
119 |
✗✓ | 33562 |
CHECK_DATA( data.torque_residual.size() == model.nv ); |
120 |
✗✓ | 33562 |
CHECK_DATA( data.dq_after.size() == model.nv ); |
121 |
//CHECK_DATA( data.impulse_c.size()== model.nv ); |
||
122 |
|||
123 |
✗✓ | 33562 |
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 |
✗✓ | 33562 |
CHECK_DATA( data.kinematic_hessians.dimension(1) == model.nv); |
129 |
✗✓ | 33562 |
CHECK_DATA( data.kinematic_hessians.dimension(2) == model.nv); |
130 |
#endif |
||
131 |
|||
132 |
✗✓ | 33562 |
CHECK_DATA( (int)data.oMf.size() == model.nframes ); |
133 |
|||
134 |
✗✓ | 33562 |
CHECK_DATA( (int)data.lastChild.size() == model.njoints ); |
135 |
✗✓ | 33562 |
CHECK_DATA( (int)data.nvSubtree.size() == model.njoints ); |
136 |
✗✓ | 33562 |
CHECK_DATA( (int)data.parents_fromRow.size() == model.nv ); |
137 |
✗✓ | 33562 |
CHECK_DATA( (int)data.nvSubtree_fromRow.size() == model.nv ); |
138 |
|||
139 |
✓✓ | 926343 |
for(JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
140 |
{ |
||
141 |
892781 |
const typename Model::JointModel & jmodel = model.joints[joint_id]; |
|
142 |
|||
143 |
✗✓ | 892781 |
CHECK_DATA(model.nqs[joint_id] == jmodel.nq()); |
144 |
✗✓ | 892781 |
CHECK_DATA(model.idx_qs[joint_id] == jmodel.idx_q()); |
145 |
✗✓ | 892781 |
CHECK_DATA(model.nvs[joint_id] == jmodel.nv()); |
146 |
✗✓ | 892781 |
CHECK_DATA(model.idx_vs[joint_id] == jmodel.idx_v()); |
147 |
} |
||
148 |
|||
149 |
✓✓ | 926343 |
for( JointIndex j=1;int(j)<model.njoints;++j ) |
150 |
{ |
||
151 |
892781 |
JointIndex c = (JointIndex)data.lastChild[j]; |
|
152 |
✗✓ | 892781 |
CHECK_DATA((int)c<model.njoints); |
153 |
892781 |
int nv=model.joints[j].nv(); |
|
154 |
✓✓ | 4558819 |
for( JointIndex d=j+1;d<=c;++d ) // explore all descendant |
155 |
{ |
||
156 |
✗✓ | 3666038 |
CHECK_DATA( model.parents[d]>=j ); |
157 |
3666038 |
nv+=model.joints[d].nv(); |
|
158 |
} |
||
159 |
✗✓ | 892781 |
CHECK_DATA(nv==data.nvSubtree[j]); |
160 |
|||
161 |
✓✓ | 8813299 |
for( JointIndex d=c+1;(int)d<model.njoints;++d) |
162 |
✓✓✗✓ ✗✓ |
7920518 |
CHECK_DATA( (model.parents[d]<j)||(model.parents[d]>c) ); |
163 |
|||
164 |
892781 |
int row = model.joints[j].idx_v(); |
|
165 |
✗✓ | 892781 |
CHECK_DATA(data.nvSubtree[j] == data.nvSubtree_fromRow[(size_t)row]); |
166 |
|||
167 |
892781 |
const JointModel & jparent = model.joints[model.parents[j]]; |
|
168 |
✓✓✗✓ |
892781 |
if(row==0) { CHECK_DATA(data.parents_fromRow[(size_t)row]==-1); } |
169 |
✗✓ | 859220 |
else { CHECK_DATA(jparent.idx_v()+jparent.nv()-1 == data.parents_fromRow[(size_t)row]); } |
170 |
} |
||
171 |
|||
172 |
#undef CHECK_DATA |
||
173 |
33562 |
return true; |
|
174 |
} |
||
175 |
|||
176 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
177 |
33561 |
inline bool ModelTpl<Scalar,Options,JointCollectionTpl>:: |
|
178 |
check(const DataTpl<Scalar,Options,JointCollectionTpl> & data) const |
||
179 |
33561 |
{ return checkData(*this,data); } |
|
180 |
|||
181 |
|||
182 |
} // namespace pinocchio |
||
183 |
|||
184 |
#endif // ifndef __pinocchio_check_hxx__ |
Generated by: GCOVR (Version 4.2) |