GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/check.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 79 80 98.8%
Branches: 88 156 56.4%
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)
25 2 : model(model)
26 {
27 2 }
28
29 inline bool operator()(const bool & accumul, const boost::fusion::void_ &) const
30 {
31 return accumul;
32 }
33
34 template<typename T>
35 12 inline bool operator()(const bool & accumul, const AlgorithmCheckerBase<T> & t) const
36 {
37
2/4
✓ Branch 0 taken 6 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 12 return accumul && t.checkModel(model);
38 }
39 };
40 } // namespace internal
41
42 // Check the validity of the kinematic tree defined by parents.
43 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
44 inline bool
45 2 ParentChecker::checkModel_impl(const ModelTpl<Scalar, Options, JointCollectionTpl> & model) const
46 {
47 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
48 typedef typename Model::JointIndex JointIndex;
49
50
2/2
✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
 56 for (JointIndex j = 1; j < (JointIndex)model.njoints; ++j)
51
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 54 times.
 54 if (model.parents[j] >= j)
52 ✗ return false;
53
54 2 return true;
55 }
56
57 #if !defined(BOOST_FUSION_HAS_VARIADIC_LIST)
58 template<BOOST_PP_ENUM_PARAMS(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE, class T)>
59 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
60 bool AlgorithmCheckerList<BOOST_PP_ENUM_PARAMS(PINOCCHIO_ALGO_CHECKER_LIST_MAX_LIST_SIZE, T)>::
61 checkModel_impl(const ModelTpl<Scalar, Options, JointCollectionTpl> & model) const
62 {
63 return boost::fusion::accumulate(
64 checkerList, true, internal::AlgoFusionChecker<Scalar, Options, JointCollectionTpl>(model));
65 }
66 #else
67 template<class... T>
68 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
69 4 bool AlgorithmCheckerList<T...>::checkModel_impl(
70 const ModelTpl<Scalar, Options, JointCollectionTpl> & model) const
71 {
72 4 return boost::fusion::accumulate(
73
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 checkerList, true, internal::AlgoFusionChecker<Scalar, Options, JointCollectionTpl>(model));
74 }
75 #endif
76
77 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
78 53043 inline bool checkData(
79 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
80 const DataTpl<Scalar, Options, JointCollectionTpl> & data)
81 {
82 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
83 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
84
85 typedef typename Model::JointModel JointModel;
86
87 #define CHECK_DATA(a) \
88 if (!(a)) \
89 return false;
90
91 // TODO JMinvJt,sDUiJt are never explicitly initialized.
92 // TODO impulse_c
93 // They are not check neither
94
95
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.joints.size() == model.njoints);
96
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.a.size() == model.njoints);
97
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.a_gf.size() == model.njoints);
98
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.v.size() == model.njoints);
99
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.f.size() == model.njoints);
100
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.oMi.size() == model.njoints);
101
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.liMi.size() == model.njoints);
102
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.Ycrb.size() == model.njoints);
103
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.Yaba.size() == model.njoints);
104
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.Fcrb.size() == model.njoints);
105
18/30
✓ Branch 1 taken 53029 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 53029 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 53029 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 53029 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 53029 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 53029 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1496732 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1496732 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 1496732 times.
✓ Branch 25 taken 1496732 times.
✓ Branch 26 taken 1496732 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1496732 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 1549761 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1549761 times.
✗ Branch 35 not taken.
✓ Branch 36 taken 1496732 times.
✓ Branch 37 taken 53029 times.
✓ Branch 38 taken 1496732 times.
✓ Branch 39 taken 53029 times.
 3047187 BOOST_FOREACH (const typename Data::Matrix6x & F, data.Fcrb)
106 {
107
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1496732 times.
 1497072 CHECK_DATA(F.cols() == model.nv);
108 }
109
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.iMf.size() == model.njoints);
110
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.iMf.size() == model.njoints);
111
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.com.size() == model.njoints);
112
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.vcom.size() == model.njoints);
113
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.acom.size() == model.njoints);
114
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.mass.size() == model.njoints);
115
116
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.tau.size() == model.nv);
117
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.nle.size() == model.nv);
118
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.ddq.size() == model.nv);
119
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.u.size() == model.nv);
120
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.M.rows() == model.nv);
121
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.M.cols() == model.nv);
122
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.Ag.cols() == model.nv);
123
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.U.cols() == model.nv);
124
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.U.rows() == model.nv);
125
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.D.size() == model.nv);
126
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.tmp.size() >= model.nv);
127
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.J.cols() == model.nv);
128
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.Jcom.cols() == model.nv);
129
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.torque_residual.size() == model.nv);
130
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.dq_after.size() == model.nv);
131 // CHECK_DATA( data.impulse_c.size()== model.nv );
132
133
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.kinematic_hessians.dimension(0) == 6);
134 #if EIGEN_VERSION_AT_LEAST(3, 2, 90) && !EIGEN_VERSION_AT_LEAST(3, 2, 93)
135 CHECK_DATA(data.kinematic_hessians.dimension(1) == std::max(1, model.nv));
136 CHECK_DATA(data.kinematic_hessians.dimension(2) == std::max(1, model.nv));
137 #else
138
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.kinematic_hessians.dimension(1) == model.nv);
139
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA(data.kinematic_hessians.dimension(2) == model.nv);
140 #endif
141
142
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.oMf.size() == model.nframes);
143
144
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.lastChild.size() == model.njoints);
145
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.nvSubtree.size() == model.njoints);
146
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.parents_fromRow.size() == model.nv);
147
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53029 times.
 53043 CHECK_DATA((int)data.nvSubtree_fromRow.size() == model.nv);
148
149
2/2
✓ Branch 0 taken 1443703 times.
✓ Branch 1 taken 53029 times.
 1497072 for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
150 {
151 1444029 const typename Model::JointModel & jmodel = model.joints[joint_id];
152
153
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1443703 times.
 1444029 CHECK_DATA(model.nqs[joint_id] == jmodel.nq());
154
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1443703 times.
 1444029 CHECK_DATA(model.idx_qs[joint_id] == jmodel.idx_q());
155
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1443703 times.
 1444029 CHECK_DATA(model.nvs[joint_id] == jmodel.nv());
156
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1443703 times.
 1444029 CHECK_DATA(model.idx_vs[joint_id] == jmodel.idx_v());
157 }
158
159
2/2
✓ Branch 0 taken 1443703 times.
✓ Branch 1 taken 53029 times.
 1497072 for (JointIndex j = 1; int(j) < model.njoints; ++j)
160 {
161 1444029 JointIndex c = (JointIndex)data.lastChild[j];
162
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1443703 times.
 1444029 CHECK_DATA((int)c < model.njoints);
163 1444029 int nv = model.joints[j].nv();
164
2/2
✓ Branch 0 taken 5868454 times.
✓ Branch 1 taken 1443703 times.
 7313815 for (JointIndex d = j + 1; d <= c; ++d) // explore all descendant
165 {
166
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 5868454 times.
 5869786 CHECK_DATA(model.parents[d] >= j);
167 5869786 nv += model.joints[d].nv();
168 }
169
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1443703 times.
 1444029 CHECK_DATA(nv == data.nvSubtree[j]);
170
171
2/2
✓ Branch 0 taken 13808192 times.
✓ Branch 1 taken 1443703 times.
 15255101 for (JointIndex d = c + 1; (int)d < model.njoints; ++d)
172
4/6
✓ Branch 1 taken 12438387 times.
✓ Branch 2 taken 1369805 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 12438387 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 13808192 times.
 13811072 CHECK_DATA((model.parents[d] < j) || (model.parents[d] > c));
173
174 1444029 int row = model.joints[j].idx_v();
175
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1443703 times.
 1444029 CHECK_DATA(data.nvSubtree[j] == data.nvSubtree_fromRow[(size_t)row]);
176
177 1444029 const JointModel & jparent = model.joints[model.parents[j]];
178
2/2
✓ Branch 0 taken 53027 times.
✓ Branch 1 taken 1390676 times.
 1444029 if (row == 0)
179 {
180
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 53027 times.
 53041 CHECK_DATA(data.parents_fromRow[(size_t)row] == -1);
181 }
182 else
183 {
184
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1390676 times.
 1390988 CHECK_DATA(jparent.idx_v() + jparent.nv() - 1 == data.parents_fromRow[(size_t)row]);
185 }
186 }
187
188 #undef CHECK_DATA
189 53043 return true;
190 }
191
192 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
193 53028 inline bool ModelTpl<Scalar, Options, JointCollectionTpl>::check(
194 const DataTpl<Scalar, Options, JointCollectionTpl> & data) const
195 {
196 53028 return checkData(*this, data);
197 }
198
199 } // namespace pinocchio
200
201 #endif // ifndef __pinocchio_check_hxx__
202