Directory: | ./ |
---|---|
File: | include/pinocchio/algorithm/check.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 67 | 79 | 84.8% |
Branches: | 82 | 156 | 52.6% |
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 | ✗ | AlgoFusionChecker(const Model & model) | |
25 | ✗ | : model(model) | |
26 | { | ||
27 | } | ||
28 | |||
29 | inline bool operator()(const bool & accumul, const boost::fusion::void_ &) const | ||
30 | { | ||
31 | return accumul; | ||
32 | } | ||
33 | |||
34 | template<typename T> | ||
35 | ✗ | inline bool operator()(const bool & accumul, const AlgorithmCheckerBase<T> & t) const | |
36 | { | ||
37 | ✗ | 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 | ✗ | 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 | ✗ | for (JointIndex j = 1; j < (JointIndex)model.njoints; ++j) | |
51 | ✗ | if (model.parents[j] >= j) | |
52 | ✗ | return false; | |
53 | |||
54 | ✗ | 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 | ✗ | bool AlgorithmCheckerList<T...>::checkModel_impl( | |
70 | const ModelTpl<Scalar, Options, JointCollectionTpl> & model) const | ||
71 | { | ||
72 | ✗ | return boost::fusion::accumulate( | |
73 | ✗ | checkerList, true, internal::AlgoFusionChecker<Scalar, Options, JointCollectionTpl>(model)); | |
74 | } | ||
75 | #endif | ||
76 | |||
77 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
78 | 42592 | 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 42592 times.
|
42592 | CHECK_DATA((int)data.joints.size() == model.njoints); |
96 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.a.size() == model.njoints); |
97 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.a_gf.size() == model.njoints); |
98 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.v.size() == model.njoints); |
99 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.f.size() == model.njoints); |
100 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.oMi.size() == model.njoints); |
101 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.liMi.size() == model.njoints); |
102 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.Ycrb.size() == model.njoints); |
103 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.Yaba.size() == model.njoints); |
104 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.Fcrb.size() == model.njoints); |
105 |
18/30✓ Branch 1 taken 42592 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 42592 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 42592 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 42592 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 42592 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 42592 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1223111 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1223111 times.
✗ Branch 23 not taken.
✓ Branch 24 taken 1223111 times.
✓ Branch 25 taken 1223111 times.
✓ Branch 26 taken 1223111 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 1223111 times.
✗ Branch 30 not taken.
✓ Branch 31 taken 1265703 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 1265703 times.
✗ Branch 35 not taken.
✓ Branch 36 taken 1223111 times.
✓ Branch 37 taken 42592 times.
✓ Branch 38 taken 1223111 times.
✓ Branch 39 taken 42592 times.
|
2488814 | BOOST_FOREACH (const typename Data::Matrix6x & F, data.Fcrb) |
106 | { | ||
107 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1223111 times.
|
1223111 | CHECK_DATA(F.cols() == model.nv); |
108 | } | ||
109 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.iMf.size() == model.njoints); |
110 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.iMf.size() == model.njoints); |
111 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.com.size() == model.njoints); |
112 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.vcom.size() == model.njoints); |
113 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.acom.size() == model.njoints); |
114 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.mass.size() == model.njoints); |
115 | |||
116 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA(data.tau.size() == model.nv); |
117 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA(data.nle.size() == model.nv); |
118 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA(data.ddq.size() == model.nv); |
119 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA(data.u.size() == model.nv); |
120 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA(data.M.rows() == model.nv); |
121 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA(data.M.cols() == model.nv); |
122 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA(data.Ag.cols() == model.nv); |
123 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA(data.U.cols() == model.nv); |
124 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA(data.U.rows() == model.nv); |
125 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA(data.D.size() == model.nv); |
126 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA(data.tmp.size() >= model.nv); |
127 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA(data.J.cols() == model.nv); |
128 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA(data.Jcom.cols() == model.nv); |
129 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA(data.torque_residual.size() == model.nv); |
130 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | 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 42592 times.
|
42592 | 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 42592 times.
|
42592 | CHECK_DATA(data.kinematic_hessians.dimension(1) == model.nv); |
139 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA(data.kinematic_hessians.dimension(2) == model.nv); |
140 | #endif | ||
141 | |||
142 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.oMf.size() == model.nframes); |
143 | |||
144 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.lastChild.size() == model.njoints); |
145 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.nvSubtree.size() == model.njoints); |
146 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.parents_fromRow.size() == model.nv); |
147 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42592 times.
|
42592 | CHECK_DATA((int)data.nvSubtree_fromRow.size() == model.nv); |
148 | |||
149 |
2/2✓ Branch 0 taken 1180519 times.
✓ Branch 1 taken 42592 times.
|
1223111 | for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id) |
150 | { | ||
151 | 1180519 | const typename Model::JointModel & jmodel = model.joints[joint_id]; | |
152 | |||
153 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 1180519 times.
|
1180519 | CHECK_DATA(model.nqs[joint_id] == jmodel.nq()); |
154 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 1180519 times.
|
1180519 | CHECK_DATA(model.idx_qs[joint_id] == jmodel.idx_q()); |
155 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 1180519 times.
|
1180519 | CHECK_DATA(model.nvs[joint_id] == jmodel.nv()); |
156 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 1180519 times.
|
1180519 | CHECK_DATA(model.idx_vs[joint_id] == jmodel.idx_v()); |
157 | } | ||
158 | |||
159 |
2/2✓ Branch 0 taken 1180519 times.
✓ Branch 1 taken 42592 times.
|
1223111 | for (JointIndex j = 1; int(j) < model.njoints; ++j) |
160 | { | ||
161 | 1180519 | JointIndex c = (JointIndex)data.lastChild[j]; | |
162 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 1180519 times.
|
1180519 | CHECK_DATA((int)c < model.njoints); |
163 | 1180519 | int nv = model.joints[j].nv(); | |
164 |
2/2✓ Branch 0 taken 4791346 times.
✓ Branch 1 taken 1180519 times.
|
5971865 | for (JointIndex d = j + 1; d <= c; ++d) // explore all descendant |
165 | { | ||
166 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 4791346 times.
|
4791346 | CHECK_DATA(model.parents[d] >= j); |
167 | 4791346 | nv += model.joints[d].nv(); | |
168 | } | ||
169 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 1180519 times.
|
1180519 | CHECK_DATA(nv == data.nvSubtree[j]); |
170 | |||
171 |
2/2✓ Branch 0 taken 11487623 times.
✓ Branch 1 taken 1180519 times.
|
12668142 | for (JointIndex d = c + 1; (int)d < model.njoints; ++d) |
172 |
4/6✓ Branch 1 taken 10349680 times.
✓ Branch 2 taken 1137943 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 10349680 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 11487623 times.
|
11487623 | CHECK_DATA((model.parents[d] < j) || (model.parents[d] > c)); |
173 | |||
174 | 1180519 | int row = model.joints[j].idx_v(); | |
175 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 1180519 times.
|
1180519 | CHECK_DATA(data.nvSubtree[j] == data.nvSubtree_fromRow[(size_t)row]); |
176 | |||
177 | 1180519 | const JointModel & jparent = model.joints[model.parents[j]]; | |
178 |
2/2✓ Branch 0 taken 42591 times.
✓ Branch 1 taken 1137928 times.
|
1180519 | if (row == 0) |
179 | { | ||
180 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 42591 times.
|
42591 | CHECK_DATA(data.parents_fromRow[(size_t)row] == -1); |
181 | } | ||
182 | else | ||
183 | { | ||
184 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 1137928 times.
|
1137928 | CHECK_DATA(jparent.idx_v() + jparent.nv() - 1 == data.parents_fromRow[(size_t)row]); |
185 | } | ||
186 | } | ||
187 | |||
188 | #undef CHECK_DATA | ||
189 | 42592 | return true; | |
190 | } | ||
191 | |||
192 | template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl> | ||
193 | 42592 | inline bool ModelTpl<Scalar, Options, JointCollectionTpl>::check( | |
194 | const DataTpl<Scalar, Options, JointCollectionTpl> & data) const | ||
195 | { | ||
196 | 42592 | return checkData(*this, data); | |
197 | } | ||
198 | |||
199 | } // namespace pinocchio | ||
200 | |||
201 | #endif // ifndef __pinocchio_check_hxx__ | ||
202 |