GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/check-data.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 91 91 100.0%
Branches: 90 156 57.7%
Line Branch Exec Source
1 //
2 // Copyright (c) 2025 INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_check_data_hxx__
6 #define __pinocchio_algorithm_check_data_hxx__
7
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
10
11 namespace pinocchio
12 {
13
14 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
15 59644 inline bool checkData(
16 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
17 const DataTpl<Scalar, Options, JointCollectionTpl> & data)
18 {
19 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
20 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
21
22 typedef typename Model::JointModel JointModel;
23
24 #define CHECK_DATA(a) \
25 if (!(a)) \
26 return false;
27
28 // TODO JMinvJt,sDUiJt are never explicitly initialized.
29 // TODO impulse_c
30 // They are not check neither
31
32
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.joints.size() == model.njoints);
33
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.a.size() == model.njoints);
34
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.a_gf.size() == model.njoints);
35
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.v.size() == model.njoints);
36
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.f.size() == model.njoints);
37
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.oMi.size() == model.njoints);
38
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.liMi.size() == model.njoints);
39
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.Ycrb.size() == model.njoints);
40
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.Yaba.size() == model.njoints);
41
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.Fcrb.size() == model.njoints);
42
2/2
✓ Branch 5 taken 1680655 times.
✓ Branch 6 taken 59622 times.
 1740887 for (const typename Data::Matrix6x & F : data.Fcrb)
43 {
44
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1680655 times.
 1681243 CHECK_DATA(F.cols() == model.nv);
45 }
46
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.iMf.size() == model.njoints);
47
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.iMf.size() == model.njoints);
48
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.com.size() == model.njoints);
49
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.vcom.size() == model.njoints);
50
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.acom.size() == model.njoints);
51
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.mass.size() == model.njoints);
52
53
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.tau.size() == model.nv);
54
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.nle.size() == model.nv);
55
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.ddq.size() == model.nv);
56
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.u.size() == model.nv);
57
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.M.rows() == model.nv);
58
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.M.cols() == model.nv);
59
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.Ag.cols() == model.nv);
60
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.U.cols() == model.nv);
61
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.U.rows() == model.nv);
62
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.D.size() == model.nv);
63
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.tmp.size() >= model.nv);
64
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.J.cols() == model.nvExtended);
65
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.Jcom.cols() == model.nv);
66
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.torque_residual.size() == model.nv);
67
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.dq_after.size() == model.nv);
68 // CHECK_DATA( data.impulse_c.size()== model.nv );
69
70
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.kinematic_hessians.dimension(0) == 6);
71 #if EIGEN_VERSION_AT_LEAST(3, 2, 90) && !EIGEN_VERSION_AT_LEAST(3, 2, 93)
72 CHECK_DATA(data.kinematic_hessians.dimension(1) == std::max(1, model.nv));
73 CHECK_DATA(data.kinematic_hessians.dimension(2) == std::max(1, model.nv));
74 #else
75
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.kinematic_hessians.dimension(1) == model.nv);
76
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA(data.kinematic_hessians.dimension(2) == model.nv);
77 #endif
78
79
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.oMf.size() == model.nframes);
80
81
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.lastChild.size() == model.njoints);
82
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.nvSubtree.size() == model.njoints);
83
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.parents_fromRow.size() == model.nvExtended);
84
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.mimic_parents_fromRow.size() == model.nvExtended);
85
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.non_mimic_parents_fromRow.size() == model.nvExtended);
86
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.idx_vExtended_to_idx_v_fromRow.size() == model.nvExtended);
87
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.nvSubtree_fromRow.size() == model.nvExtended);
88
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.start_idx_v_fromRow.size() == model.nvExtended);
89
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59622 times.
 59644 CHECK_DATA((int)data.end_idx_v_fromRow.size() == model.nvExtended);
90
91
2/2
✓ Branch 0 taken 1621033 times.
✓ Branch 1 taken 59622 times.
 1681243 for (JointIndex joint_id = 1; joint_id < (JointIndex)model.njoints; ++joint_id)
92 {
93 1621599 const typename Model::JointModel & jmodel = model.joints[joint_id];
94
95
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1621033 times.
 1621599 CHECK_DATA(model.nqs[joint_id] == jmodel.nq());
96
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1621033 times.
 1621599 CHECK_DATA(model.idx_qs[joint_id] == jmodel.idx_q());
97
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1621033 times.
 1621599 CHECK_DATA(model.nvs[joint_id] == jmodel.nv());
98
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1621033 times.
 1621599 CHECK_DATA(model.idx_vs[joint_id] == jmodel.idx_v());
99
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1621033 times.
 1621599 CHECK_DATA(model.nvExtendeds[joint_id] == jmodel.nvExtended());
100
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1621033 times.
 1621599 CHECK_DATA(model.idx_vExtendeds[joint_id] == jmodel.idx_vExtended());
101 }
102
2/2
✓ Branch 0 taken 1621033 times.
✓ Branch 1 taken 59622 times.
 1681243 for (JointIndex j = 1; int(j) < model.njoints; ++j)
103 {
104 1621599 JointIndex c = (JointIndex)data.lastChild[j];
105
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1621033 times.
 1621599 CHECK_DATA((int)c < model.njoints);
106 int nv;
107 // For mimic, since in nvSubtree we're using the idx_vExtended, we need to do the same here
108
3/4
✓ Branch 1 taken 1621033 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7529 times.
✓ Branch 5 taken 1613504 times.
 1621599 if (boost::get<JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>(&model.joints[j]))
109 7534 nv = 0;
110 else
111 {
112 1614065 nv = model.joints[j].nv();
113
2/2
✓ Branch 0 taken 6569136 times.
✓ Branch 1 taken 1613504 times.
 8185502 for (JointIndex d = j + 1; d <= c; ++d) // explore all descendant
114 {
115
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6569136 times.
 6571437 CHECK_DATA(model.parents[d] >= j);
116
117 6571437 nv += model.joints[d].nv();
118 }
119 }
120
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1621033 times.
 1621599 CHECK_DATA(nv == data.nvSubtree[j]);
121
122
2/2
✓ Branch 0 taken 15385250 times.
✓ Branch 1 taken 1621033 times.
 17012267 for (JointIndex d = c + 1; (int)d < model.njoints; ++d)
123
4/6
✓ Branch 1 taken 13857785 times.
✓ Branch 2 taken 1527465 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 13857785 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 15385250 times.
 15390668 CHECK_DATA((model.parents[d] < j) || (model.parents[d] > c));
124
125
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 1621033 times.
 1621599 CHECK_DATA(
126 data.nvSubtree[j] == data.nvSubtree_fromRow[(size_t)model.joints[j].idx_vExtended()]);
127
128 1621599 int row = model.joints[j].idx_vExtended();
129 1621599 const JointModel & jparent = model.joints[model.parents[j]];
130
2/2
✓ Branch 0 taken 59620 times.
✓ Branch 1 taken 1561413 times.
 1621599 if (row == 0)
131 {
132
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59620 times.
 59642 CHECK_DATA(data.parents_fromRow[(size_t)row] == -1);
133
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59620 times.
 59642 CHECK_DATA(data.mimic_parents_fromRow[(size_t)row] == -1);
134
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 59620 times.
 59642 CHECK_DATA(data.non_mimic_parents_fromRow[(size_t)row] == -1);
135 }
136 else
137 {
138
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1561413 times.
 1561957 CHECK_DATA(
139 jparent.idx_vExtended() + jparent.nvExtended() - 1 == data.parents_fromRow[(size_t)row]);
140
2/2
✓ Branch 1 taken 6529 times.
✓ Branch 2 taken 1554884 times.
 1561957 if (boost::get<JointModelMimicTpl<Scalar, Options, JointCollectionTpl>>(&jparent))
141 {
142
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 6529 times.
 6532 CHECK_DATA(data.parents_fromRow[(size_t)row] == data.mimic_parents_fromRow[(size_t)row]);
143 }
144 else
145 {
146
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1554884 times.
 1555425 CHECK_DATA(
147 data.parents_fromRow[(size_t)row] == data.non_mimic_parents_fromRow[(size_t)row]);
148 }
149 }
150 }
151
152
2/2
✓ Branch 1 taken 5665 times.
✓ Branch 2 taken 53957 times.
 59644 if (model.mimicking_joints.size() != 0)
153 {
154
2/2
✓ Branch 1 taken 7529 times.
✓ Branch 2 taken 5665 times.
 13204 for (size_t k = 0; k < model.mimicking_joints.size(); k++)
155 {
156 // Check the mimic_subtree_joint
157 7534 const auto & mimicking_sub = model.subtrees[model.mimicking_joints[k]];
158 7534 size_t j = 1;
159 7534 JointIndex id_subtree = 0;
160
2/2
✓ Branch 1 taken 6529 times.
✓ Branch 2 taken 1000 times.
 7534 for (; j < mimicking_sub.size(); j++)
161 {
162
1/2
✓ Branch 2 taken 6529 times.
✗ Branch 3 not taken.
 6532 if (model.nvs[mimicking_sub[j]] != 0)
163 {
164 6532 id_subtree = mimicking_sub[j];
165 }
166 6532 break;
167 }
168
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 7529 times.
 7534 CHECK_DATA(id_subtree == data.mimic_subtree_joint[k]);
169 }
170 }
171
172 #undef CHECK_DATA
173 59644 return true;
174 }
175
176 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
177 59621 inline bool ModelTpl<Scalar, Options, JointCollectionTpl>::check(
178 const DataTpl<Scalar, Options, JointCollectionTpl> & data) const
179 {
180 59621 return checkData(*this, data);
181 }
182
183 } // namespace pinocchio
184
185 #endif // __pinocchio_algorithm_check_data_hxx__
186