GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/crba.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 186 191 97.4%
Branches: 251 493 50.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2021 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_crba_hxx__
6 #define __pinocchio_crba_hxx__
7
8 #include "pinocchio/multibody/visitor.hpp"
9 #include "pinocchio/spatial/act-on-set.hpp"
10 #include "pinocchio/algorithm/kinematics.hpp"
11 #include "pinocchio/algorithm/check.hpp"
12 #include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
13
14 /// @cond DEV
15
16 namespace pinocchio
17 {
18 namespace impl
19 {
20 template<
21 typename Scalar,
22 int Options,
23 template<typename, int> class JointCollectionTpl,
24 typename ConfigVectorType>
25 struct CrbaWorldConventionForwardStep
26 : public fusion::JointUnaryVisitorBase<
27 CrbaWorldConventionForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType>>
28 {
29 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
30 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
31
32 typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &> ArgsType;
33
34 template<typename JointModel>
35 4882 static void algo(
36 const JointModelBase<JointModel> & jmodel,
37 JointDataBase<typename JointModel::JointDataDerived> & jdata,
38 const Model & model,
39 Data & data,
40 const Eigen::MatrixBase<ConfigVectorType> & q)
41 {
42 typedef typename Model::JointIndex JointIndex;
43
44
1/2
✓ Branch 1 taken 2441 times.
✗ Branch 2 not taken.
 4882 const JointIndex & i = jmodel.id();
45
1/2
✓ Branch 3 taken 2441 times.
✗ Branch 4 not taken.
 4882 jmodel.calc(jdata.derived(), q.derived());
46
47
6/10
✓ Branch 1 taken 2441 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2319 times.
✓ Branch 5 taken 122 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2319 times.
✓ Branch 9 taken 122 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2319 times.
✗ Branch 13 not taken.
 4882 data.liMi[i] = model.jointPlacements[i] * jdata.M();
48
49 4882 const JointIndex & parent = model.parents[i];
50
2/2
✓ Branch 0 taken 2323 times.
✓ Branch 1 taken 118 times.
 4882 if (parent > 0)
51
2/4
✓ Branch 3 taken 2323 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 2323 times.
✗ Branch 8 not taken.
 4646 data.oMi[i] = data.oMi[parent] * data.liMi[i];
52 else
53
1/2
✓ Branch 3 taken 118 times.
✗ Branch 4 not taken.
 236 data.oMi[i] = data.liMi[i];
54
55
4/8
✓ Branch 2 taken 2441 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2441 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2441 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2441 times.
✗ Branch 12 not taken.
 4882 jmodel.jointExtendedModelCols(data.J) = data.oMi[i].act(jdata.S());
56
57
2/4
✓ Branch 3 taken 2441 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 2441 times.
✗ Branch 8 not taken.
 4882 data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
58 }
59 };
60
61 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
62 struct CrbaWorldConventionBackwardStep
63 : public fusion::JointUnaryVisitorBase<
64 CrbaWorldConventionBackwardStep<Scalar, Options, JointCollectionTpl>>
65 {
66 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
67 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
68 typedef JointModelMimicTpl<Scalar, Options, JointCollectionTpl> JointModelMimic;
69
70 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
71
72 template<typename JointModel>
73 4882 static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data)
74 {
75 4882 algo_impl(jmodel, model, data);
76 }
77
78 template<typename JointModel>
79 static void
80 4848 algo_impl(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data)
81 {
82 typedef typename Model::JointIndex JointIndex;
83 typedef
84 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
85 ColsBlock;
86
1/2
✓ Branch 1 taken 2424 times.
✗ Branch 2 not taken.
 4848 const JointIndex & i = jmodel.id();
87
88 // Centroidal momentum map
89
1/2
✓ Branch 1 taken 2424 times.
✗ Branch 2 not taken.
 4848 ColsBlock Ag_cols = jmodel.jointCols(data.Ag);
90
1/2
✓ Branch 1 taken 2424 times.
✗ Branch 2 not taken.
 4848 ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J);
91
1/2
✓ Branch 2 taken 2424 times.
✗ Branch 3 not taken.
 4848 motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols);
92
93 // Joint Space Inertia Matrix
94
6/12
✓ Branch 2 taken 2424 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2424 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2424 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2424 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2424 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2424 times.
✗ Branch 18 not taken.
 4848 data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() =
95
4/8
✓ Branch 2 taken 2424 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2424 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2424 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2424 times.
✗ Branch 12 not taken.
 4848 J_cols.transpose() * data.Ag.middleCols(jmodel.idx_v(), data.nvSubtree[i]);
96
97 4848 const JointIndex & parent = model.parents[i];
98
1/2
✓ Branch 3 taken 2424 times.
✗ Branch 4 not taken.
 4848 data.oYcrb[parent] += data.oYcrb[i];
99 }
100
101 static void
102 17 algo_impl(const JointModelBase<JointModelMimic> & jmodel, const Model & model, Data & data)
103 {
104 typedef typename Model::JointIndex JointIndex;
105
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 const JointIndex & i = jmodel.id();
106
107 17 const JointIndex & parent = model.parents[i];
108
1/2
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
 17 data.oYcrb[parent] += data.oYcrb[i];
109 17 }
110 };
111
112 // In case of a tree with a mimic joint, as seen above, the backward pass, is truncated, in
113 // order not to replace the mimicked joint(s) info in the matrix. This step allows for the
114 // mimicking joint(s) contribution to be added. It is done in 3 steps:
115 // - First we compute the mimicking joint(s) Force matrix
116 // - Then we compute the "old" row of the mimicking joint in the matrix, by getting the subtree
117 // of mimicking joint
118 // - Since here only the upper part of the matrix is computed, we need to go over the support of
119 // the mimicking joint, and compute how the force matrix of the mimicking joint is affecting
120 // each joint ATTENTION : the last loop is spilt in 2, to only fill the upper part of the matrix
121 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
122 struct CrbaWorldConventionMimicStep
123 : public fusion::JointUnaryVisitorBase<
124 CrbaWorldConventionMimicStep<Scalar, Options, JointCollectionTpl>>
125 {
126 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
127 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
128 typedef JointModelMimicTpl<Scalar, Options, JointCollectionTpl> JointModelMimic;
129
130 typedef boost::fusion::vector<const Model &, Data &, const size_t &> ArgsType;
131
132 template<typename JointModel>
133 34 static void algo(
134 const JointModelBase<JointModel> & jmodel,
135 const Model & model,
136 Data & data,
137 const size_t & mims_id)
138 {
139 34 algo_impl(jmodel, model, data, mims_id);
140 }
141
142 template<typename JointModel>
143 static void
144 ✗ algo_impl(const JointModelBase<JointModel> &, const Model &, Data &, const size_t &)
145 {
146 }
147
148 17 static void algo_impl(
149 const JointModelBase<JointModelMimic> & jmodel,
150 const Model & model,
151 Data & data,
152 const size_t & mims_id)
153 {
154 typedef JointModelMimicTpl<Scalar, Options, JointCollectionTpl> JointModel;
155
156
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 JointIndex mimicking_id = jmodel.id();
157
1/2
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
 17 JointIndex mimicked_id = jmodel.derived().jmodel().id();
158
159 Eigen::Ref<Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options, 6, JointModel::MaxNVMimicked>>
160
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 J_cols = jmodel.jointExtendedModelCols(data.J);
161 17 Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options, 6, JointModel::MaxNVMimicked> Ag_sec(
162
2/4
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
 17 6, jmodel.nvExtended());
163
164
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 17 motionSet::inertiaAction(data.oYcrb[mimicking_id], J_cols, Ag_sec);
165
166 // Compute mimicking terms that were previously in the diagonal
167
5/10
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17 times.
✗ Branch 14 not taken.
 17 data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nvExtended(), jmodel.nvExtended())
168
4/8
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17 times.
✗ Branch 11 not taken.
 34 .noalias() += J_cols.transpose() * Ag_sec;
169
170 // Compute for the "subtree" of the mimicking
171 17 JointIndex sub_mimic_id = data.mimic_subtree_joint[mims_id];
172
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 2 times.
 17 if (sub_mimic_id != 0)
173
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 jmodel.jointRows(data.M)
174
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 .middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id])
175
2/4
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
 15 .noalias() +=
176
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 J_cols.transpose()
177
2/4
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 15 times.
✗ Branch 7 not taken.
 45 * data.Ag.middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]);
178
179 17 const auto & supports = model.supports[mimicking_id];
180 17 size_t j = supports.size() - 2;
181 Eigen::Matrix<
182 Scalar, Eigen::Dynamic, Eigen::Dynamic, Options, JointModel::MaxNVMimicked,
183 JointModel::MaxNVMimicked>
184
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 temp_JAG;
185
3/4
✓ Branch 3 taken 30 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 13 times.
✓ Branch 6 taken 17 times.
 30 for (; model.idx_vs[supports[j]] >= jmodel.idx_v(); j--)
186 {
187 13 int sup_idx_v = model.idx_vs[supports[j]];
188 13 int sup_nv = model.nvs[supports[j]];
189 13 int sup_idx_vExtended = model.idx_vExtendeds[supports[j]];
190 13 int sup_nvExtended = model.nvExtendeds[supports[j]];
191
192
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
 13 temp_JAG.noalias() =
193
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
 13 data.J.middleCols(sup_idx_vExtended, sup_nvExtended).transpose() * Ag_sec;
194
5/10
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 13 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 13 times.
✗ Branch 14 not taken.
 13 data.M.block(jmodel.idx_v(), sup_idx_v, jmodel.nvExtended(), sup_nv).noalias() +=
195 temp_JAG;
196
2/2
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 4 times.
 13 if (supports[j] == mimicked_id)
197
5/10
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9 times.
✗ Branch 14 not taken.
 9 data.M.block(jmodel.idx_v(), sup_idx_v, jmodel.nvExtended(), sup_nv).noalias() +=
198 temp_JAG;
199 }
200
201
2/2
✓ Branch 0 taken 39 times.
✓ Branch 1 taken 17 times.
 56 for (; j > 0; j--)
202 {
203 39 int sup_idx_v = model.idx_vs[supports[j]];
204 39 int sup_nv = model.nvs[supports[j]];
205 39 int sup_idx_vExtended = model.idx_vExtendeds[supports[j]];
206 39 int sup_nvExtended = model.nvExtendeds[supports[j]];
207
208
5/10
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 39 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 39 times.
✗ Branch 14 not taken.
 39 data.M.block(sup_idx_v, jmodel.idx_v(), sup_nv, jmodel.nvExtended()).noalias() +=
209
3/6
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39 times.
✗ Branch 8 not taken.
 78 data.J.middleCols(sup_idx_vExtended, sup_nvExtended).transpose() * Ag_sec;
210 }
211
212 // Mimic joint also have an effect on the centroidal map momentum, so it's important to
213 // compute it and add it to its mimicked. It is done here Ag[prim] += oYCRB[sec] * J[sec]
214
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 motionSet::inertiaAction<ADDTO>(
215
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 data.oYcrb[mimicking_id], J_cols, jmodel.jointCols(data.Ag));
216 17 }
217 };
218
219 template<
220 typename Scalar,
221 int Options,
222 template<typename, int> class JointCollectionTpl,
223 typename ConfigVectorType>
224 struct CrbaLocalConventionForwardStep
225 : public fusion::JointUnaryVisitorBase<
226 CrbaLocalConventionForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType>>
227 {
228 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
229 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
230
231 typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &> ArgsType;
232
233 template<typename JointModel>
234 2232 static void algo(
235 const JointModelBase<JointModel> & jmodel,
236 JointDataBase<typename JointModel::JointDataDerived> & jdata,
237 const Model & model,
238 Data & data,
239 const Eigen::MatrixBase<ConfigVectorType> & q)
240 {
241 typedef typename Model::JointIndex JointIndex;
242
243
1/2
✓ Branch 1 taken 1116 times.
✗ Branch 2 not taken.
 2232 const JointIndex & i = jmodel.id();
244
1/2
✓ Branch 3 taken 1116 times.
✗ Branch 4 not taken.
 2232 jmodel.calc(jdata.derived(), q.derived());
245
246
6/10
✓ Branch 1 taken 1116 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1052 times.
✓ Branch 5 taken 64 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1052 times.
✓ Branch 9 taken 64 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1052 times.
✗ Branch 13 not taken.
 2232 data.liMi[i] = model.jointPlacements[i] * jdata.M();
247
1/2
✓ Branch 3 taken 1116 times.
✗ Branch 4 not taken.
 2232 data.Ycrb[i] = model.inertias[i];
248 }
249 };
250
251 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
252 struct CrbaLocalConventionBackwardStep
253 : public fusion::JointUnaryVisitorBase<
254 CrbaLocalConventionBackwardStep<Scalar, Options, JointCollectionTpl>>
255 {
256 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
257 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
258 typedef JointModelMimicTpl<Scalar, Options, JointCollectionTpl> JointModelMimic;
259
260 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
261
262 template<typename JointModel>
263 2232 static void algo(
264 const JointModelBase<JointModel> & jmodel,
265 JointDataBase<typename JointModel::JointDataDerived> & jdata,
266 const Model & model,
267 Data & data)
268 {
269 2232 algo_impl(jmodel, jdata, model, data);
270 }
271
272 template<typename JointModel>
273 2198 static void algo_impl(
274 const JointModelBase<JointModel> & jmodel,
275 JointDataBase<typename JointModel::JointDataDerived> & jdata,
276 const Model & model,
277 Data & data)
278 {
279 /*
280 * F[1:6,i] = Y*S
281 * M[i,SUBTREE] = S'*F[1:6,SUBTREE]
282 * if li>0
283 * Yli += liXi Yi
284 * F[1:6,SUBTREE] = liXi F[1:6,SUBTREE]
285 */
286
287 typedef typename Model::JointIndex JointIndex;
288 typedef typename Data::Matrix6x::ColsBlockXpr Block;
289
1/2
✓ Branch 1 taken 1099 times.
✗ Branch 2 not taken.
 2198 const JointIndex & i = jmodel.id();
290 /* F[1:6,i] = Y*S */
291 // data.Fcrb[i].block<6,JointModel::NV>(0,jmodel.idx_v()) = data.Ycrb[i] * jdata.S();
292
4/8
✓ Branch 1 taken 1099 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1099 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1099 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1099 times.
✗ Branch 13 not taken.
 2198 jmodel.jointCols(data.Fcrb[i]) = data.Ycrb[i] * jdata.S();
293
294 /* M[i,SUBTREE] = S'*F[1:6,SUBTREE] */
295
8/15
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1098 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1099 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1099 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1099 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1099 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1099 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
 2198 data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() =
296
6/11
✓ Branch 3 taken 1099 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1099 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1099 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1053 times.
✓ Branch 13 taken 45 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 1052 times.
✗ Branch 16 not taken.
 2198 jdata.S().transpose() * data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]);
297
298 2198 const JointIndex & parent = model.parents[i];
299
2/2
✓ Branch 0 taken 1056 times.
✓ Branch 1 taken 43 times.
 2198 if (parent > 0)
300 {
301 /* Yli += liXi Yi */
302
2/4
✓ Branch 3 taken 1056 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1056 times.
✗ Branch 8 not taken.
 2112 data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
303
304 /* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */
305
2/4
✓ Branch 3 taken 1056 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1056 times.
✗ Branch 7 not taken.
 2112 Block jF = data.Fcrb[parent].middleCols(jmodel.idx_v(), data.nvSubtree[i]);
306
2/4
✓ Branch 3 taken 1056 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1056 times.
✗ Branch 7 not taken.
 2112 Block iF = data.Fcrb[i].middleCols(jmodel.idx_v(), data.nvSubtree[i]);
307
1/2
✓ Branch 2 taken 1056 times.
✗ Branch 3 not taken.
 2112 forceSet::se3Action(data.liMi[i], iF, jF);
308 }
309 }
310
311 17 static void algo_impl(
312 const JointModelBase<JointModelMimic> & jmodel,
313 JointDataBase<typename JointModelMimic::JointDataDerived> &
314 /* jdata */,
315 const Model & model,
316 Data & data)
317 {
318 typedef typename Model::JointIndex JointIndex;
319 typedef typename Data::Matrix6x & Matrix;
320
321
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 const JointIndex & i = jmodel.id();
322 17 const JointIndex & parent = model.parents[i];
323
1/2
✓ Branch 0 taken 17 times.
✗ Branch 1 not taken.
 17 if (parent > 0)
324 {
325 /* Yli += liXi Yi */
326
2/4
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
 17 data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
327
328 /* F[1:6,SUBTREE] = liXi F[1:6,SUBTREE] */
329 17 Matrix jF = data.Fcrb[parent];
330 17 Matrix iF = data.Fcrb[i];
331 // Since we don't just copy columns, we need to use ADDTO method, to avoid ecrasing
332 // already filled columns, in case of parallel arms, it is important to not ecrase already
333 // filled data from the other part of the tree
334 // By using data.mimic_subtree_joint, we could do the same as for a non-mimic joint.
335 // But to do so, we need to change the either this visitor or data.mimic_subtree_joint
336
1/2
✓ Branch 2 taken 17 times.
✗ Branch 3 not taken.
 17 forceSet::se3Action<ADDTO>(data.liMi[i], iF, jF);
337 }
338 17 }
339 };
340
341 // In case of a tree with a mimic joint, as seen above, the backward pass, is truncated, in
342 // order not to replace the mimicked joint(s) info in the matrix. This step allows for the
343 // mimicking joint(s) contribution to be added. It is done in 3 steps:
344 // - First we compute the mimicking joint(s) Force matrix
345 // - Then we compute the "old" row of the mimicking joint in the matrix, by getting the subtree
346 // of mimicking joint
347 // - Since here only the upper part of the matrix is computed, we need to go over the support of
348 // the mimicking joint, and compute how the force matrix of the mimicking joint is affecting
349 // each joint ATTENTION : the last loop is spilt in 2, to only fill the upper part of the matrix
350 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
351 struct CrbaLocalConventionMimicStep
352 : public fusion::JointUnaryVisitorBase<
353 CrbaLocalConventionMimicStep<Scalar, Options, JointCollectionTpl>>
354 {
355 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
356 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
357 typedef JointModelMimicTpl<Scalar, Options, JointCollectionTpl> JointModelMimic;
358
359 typedef boost::fusion::vector<const Model &, Data &, const size_t &> ArgsType;
360
361 template<typename JointModel>
362 34 static void algo(
363 const JointModelBase<JointModel> & jmodel,
364 JointDataBase<typename JointModel::JointDataDerived> & jdata,
365 const Model & model,
366 Data & data,
367 const size_t & mims_id)
368 {
369 34 algo_impl(jmodel, jdata, model, data, mims_id);
370 }
371
372 template<typename JointModel>
373 ✗ static void algo_impl(
374 const JointModelBase<JointModel> &,
375 JointDataBase<typename JointModel::JointDataDerived> &,
376 const Model &,
377 Data &,
378 const size_t &)
379 {
380 }
381
382 17 static void algo_impl(
383 const JointModelBase<JointModelMimic> & jmodel,
384 JointDataBase<typename JointModelMimic::JointDataDerived> & jdata,
385 const Model & model,
386 Data & data,
387 const size_t & mims_id)
388 {
389 typedef JointModelMimic JointModel;
390
391
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 const JointIndex & mimicking_id = jmodel.id();
392
1/2
✓ Branch 3 taken 17 times.
✗ Branch 4 not taken.
 17 const JointIndex & mimicked_id = jmodel.derived().jmodel().id();
393 17 auto & F = data.Fcrb[mimicking_id];
394
395 /* F[1:6,i] = Y*S */
396
4/8
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 17 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 17 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 17 times.
✗ Branch 12 not taken.
 17 jmodel.jointCols(F) = data.Ycrb[mimicking_id] * jdata.S();
397
398 // Add mimicking joint contribution to the Matrix
399
7/14
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 17 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 17 times.
✗ Branch 20 not taken.
 17 jmodel.jointBlock(data.M).noalias() += jdata.S().transpose() * jmodel.jointCols(F);
400
401 // Add mimicking subtree contribution to the matrix.
402 17 JointIndex sub_mimic_id = data.mimic_subtree_joint[mims_id];
403
2/2
✓ Branch 0 taken 15 times.
✓ Branch 1 taken 2 times.
 17 if (sub_mimic_id != 0)
404 {
405
1/2
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
 15 jmodel.jointRows(data.M)
406
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 15 .middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id])
407
5/10
✓ Branch 1 taken 15 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 15 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 15 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 15 times.
✗ Branch 14 not taken.
 30 .noalias() += jdata.S().transpose()
408
1/2
✓ Branch 3 taken 15 times.
✗ Branch 4 not taken.
 30 * F.middleCols(model.idx_vs[sub_mimic_id], data.nvSubtree[sub_mimic_id]);
409 }
410
411 17 const auto & supports = model.supports[mimicking_id];
412 17 size_t j = supports.size() - 2;
413 Eigen::Matrix<
414 Scalar, Eigen::Dynamic, Eigen::Dynamic, Options, JointModel::MaxNVMimicked,
415 JointModel::MaxNVMimicked>
416
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 temp_JAG;
417
3/4
✓ Branch 3 taken 30 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 13 times.
✓ Branch 6 taken 17 times.
 30 for (; model.idx_vs[supports[j]] >= jmodel.idx_v(); j--)
418 {
419 13 const JointIndex & li = supports[j];
420 13 const JointIndex & i = supports[j + 1]; // Child link to compute placement
421
422
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
 13 forceSet::se3Action(data.liMi[i], jmodel.jointCols(F), jmodel.jointCols(F));
423
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 int row_idx = jmodel.idx_v();
424 13 int col_idx = model.idx_vs[li];
425
426
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 13 times.
✗ Branch 8 not taken.
 26 applyConstraintOnForceVisitor<SETTO>(
427 13 data.joints[li], jmodel.jointCols(F), temp_JAG.noalias());
428
4/8
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 13 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 13 times.
✗ Branch 12 not taken.
 13 data.M.block(row_idx, col_idx, jmodel.nvExtended(), model.nvs[li]).noalias() += temp_JAG;
429
2/2
✓ Branch 0 taken 9 times.
✓ Branch 1 taken 4 times.
 13 if (li == mimicked_id)
430
4/8
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 9 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 9 times.
✗ Branch 12 not taken.
 9 data.M.block(row_idx, col_idx, jmodel.nvExtended(), model.nvs[li]).noalias() +=
431 temp_JAG;
432 }
433
2/2
✓ Branch 0 taken 39 times.
✓ Branch 1 taken 17 times.
 56 for (; j > 0; j--)
434 {
435 39 const JointIndex & li = supports[j];
436 39 const JointIndex & i = supports[j + 1]; // Child link to compute placement
437
438
3/6
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 39 times.
✗ Branch 9 not taken.
 39 forceSet::se3Action(data.liMi[i], jmodel.jointCols(F), jmodel.jointCols(F));
439
1/2
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
 39 int col_idx = jmodel.idx_v();
440 39 int row_idx = model.idx_vs[li];
441
442
3/6
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 39 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 39 times.
✗ Branch 8 not taken.
 78 applyConstraintOnForceVisitor<ADDTO>(
443 39 data.joints[li], jmodel.jointCols(F),
444
2/4
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 39 times.
✗ Branch 6 not taken.
 78 data.M.block(row_idx, col_idx, model.nvs[li], jmodel.nvExtended()).noalias());
445 }
446 17 }
447 };
448
449 template<
450 typename Scalar,
451 int Options,
452 template<typename, int> class JointCollectionTpl,
453 typename ConfigVectorType>
454 44 const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & crbaLocalConvention(
455 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
456 DataTpl<Scalar, Options, JointCollectionTpl> & data,
457 const Eigen::MatrixBase<ConfigVectorType> & q)
458 {
459
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 44 assert(model.check(data) && "data is not consistent with model.");
460
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 43 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 44 PINOCCHIO_CHECK_ARGUMENT_SIZE(
461 q.size(), model.nq, "The configuration vector is not of right size");
462
463 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
464
465 typedef CrbaLocalConventionForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType>
466 Pass1;
467
2/2
✓ Branch 0 taken 1116 times.
✓ Branch 1 taken 43 times.
 1187 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
468 {
469
1/2
✓ Branch 1 taken 1116 times.
✗ Branch 2 not taken.
 1143 Pass1::run(
470 2286 model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived()));
471 }
472
473 typedef CrbaLocalConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
474
2/2
✓ Branch 0 taken 1116 times.
✓ Branch 1 taken 43 times.
 1187 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
475 {
476
1/2
✓ Branch 4 taken 1116 times.
✗ Branch 5 not taken.
 1143 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data));
477 }
478
479 typedef CrbaLocalConventionMimicStep<Scalar, Options, JointCollectionTpl> Pass3;
480
2/2
✓ Branch 1 taken 17 times.
✓ Branch 2 taken 43 times.
 61 for (size_t i = 0; i < model.mimicking_joints.size(); i++)
481 {
482
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 Pass3::run(
483 17 model.joints[model.mimicking_joints[i]], data.joints[model.mimicking_joints[i]],
484
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 34 typename Pass3::ArgsType(model, data, i));
485 }
486
487 // Add the armature contribution
488
1/2
✓ Branch 2 taken 43 times.
✗ Branch 3 not taken.
 44 data.M.diagonal() += model.armature;
489
490 44 return data.M;
491 }
492
493 template<
494 typename Scalar,
495 int Options,
496 template<typename, int> class JointCollectionTpl,
497 typename ConfigVectorType>
498 130 const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & crbaWorldConvention(
499 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
500 DataTpl<Scalar, Options, JointCollectionTpl> & data,
501 const Eigen::MatrixBase<ConfigVectorType> & q)
502 {
503
2/4
✓ Branch 1 taken 118 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 118 times.
✗ Branch 4 not taken.
 130 assert(model.check(data) && "data is not consistent with model.");
504
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 118 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 130 PINOCCHIO_CHECK_ARGUMENT_SIZE(
505 q.size(), model.nq, "The configuration vector is not of right size");
506
507 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
508
509
1/2
✓ Branch 2 taken 118 times.
✗ Branch 3 not taken.
 130 data.oYcrb[0].setZero();
510 typedef CrbaWorldConventionForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType>
511 Pass1;
512
2/2
✓ Branch 0 taken 2441 times.
✓ Branch 1 taken 118 times.
 2661 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
513 {
514
1/2
✓ Branch 1 taken 2441 times.
✗ Branch 2 not taken.
 2531 Pass1::run(
515
1/2
✓ Branch 2 taken 2441 times.
✗ Branch 3 not taken.
 5062 model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived()));
516 }
517
518 typedef CrbaWorldConventionBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
519
2/2
✓ Branch 0 taken 2441 times.
✓ Branch 1 taken 118 times.
 2661 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
520 {
521
2/4
✓ Branch 1 taken 2441 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2441 times.
✗ Branch 6 not taken.
 2531 Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data));
522 }
523
524 typedef CrbaWorldConventionMimicStep<Scalar, Options, JointCollectionTpl> Pass3;
525
2/2
✓ Branch 1 taken 17 times.
✓ Branch 2 taken 118 times.
 147 for (size_t i = 0; i < model.mimicking_joints.size(); i++)
526 {
527
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 17 Pass3::run(
528
1/2
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
 34 model.joints[model.mimicking_joints[i]], typename Pass3::ArgsType(model, data, i));
529 }
530
531 // Add the armature contribution
532
2/4
✓ Branch 1 taken 118 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 118 times.
✗ Branch 5 not taken.
 130 data.M.diagonal() += model.armature;
533
534 // Retrieve the Centroidal Momemtum map
535 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
536 typedef typename Data::Force Force;
537 typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x;
538
539
1/2
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 130 data.mass[0] = data.oYcrb[0].mass();
540
1/2
✓ Branch 4 taken 118 times.
✗ Branch 5 not taken.
 130 data.com[0] = data.oYcrb[0].lever();
541
542
1/2
✓ Branch 1 taken 118 times.
✗ Branch 2 not taken.
 130 const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
543
1/2
✓ Branch 1 taken 118 times.
✗ Branch 2 not taken.
 130 Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
544
2/2
✓ Branch 0 taken 2900 times.
✓ Branch 1 taken 118 times.
 3162 for (long i = 0; i < model.nv; ++i)
545
4/8
✓ Branch 1 taken 2900 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2900 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2900 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2900 times.
✗ Branch 12 not taken.
 3032 Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
546
547 130 return data.M;
548 }
549 } // namespace impl
550 // --- CHECKER ---------------------------------------------------------------
551 // --- CHECKER ---------------------------------------------------------------
552 // --- CHECKER ---------------------------------------------------------------
553
554 namespace internal
555 {
556 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
557 6495 inline bool isDescendant(
558 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
559 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex j,
560 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex root)
561 {
562 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
563 typedef typename Model::JointIndex JointIndex;
564
565
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 6471 times.
 6495 if (j >= (JointIndex)model.njoints)
566 24 return false;
567
2/2
✓ Branch 0 taken 774 times.
✓ Branch 1 taken 5697 times.
 6471 if (j == 0)
568 774 return root == 0;
569
4/4
✓ Branch 0 taken 5364 times.
✓ Branch 1 taken 333 times.
✓ Branch 4 taken 1044 times.
✓ Branch 5 taken 4320 times.
 5697 return (j == root) || isDescendant(model, model.parents[j], root);
570 }
571 } // namespace internal
572
573 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
574 inline bool
575 3 CRBAChecker::checkModel_impl(const ModelTpl<Scalar, Options, JointCollectionTpl> & model) const
576 {
577 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
578 typedef typename Model::JointIndex JointIndex;
579
580 // For CRBA, the tree must be "compact", i.e. all descendants of a node i are stored
581 // immediately after i in the "parents" map, i.e. forall joint i, the interval i+1..n-1
582 // can be separated in two intervals [i+1..k] and [k+1..n-1], where any [i+1..k] is a descendant
583 // of i and none of [k+1..n-1] is a descendant of i.
584
2/2
✓ Branch 0 taken 78 times.
✓ Branch 1 taken 3 times.
 81 for (JointIndex i = 1; i < (JointIndex)(model.njoints - 1);
585 ++i) // no need to check joints 0 and N-1
586 {
587 78 JointIndex k = i + 1;
588
2/2
✓ Branch 1 taken 333 times.
✓ Branch 2 taken 78 times.
 411 while (internal::isDescendant(model, k, i))
589 333 ++k;
590
2/2
✓ Branch 0 taken 720 times.
✓ Branch 1 taken 78 times.
 798 for (; int(k) < model.njoints; ++k)
591
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 720 times.
 720 if (internal::isDescendant(model, k, i))
592 ✗ return false;
593 }
594 3 return true;
595 }
596
597 template<
598 typename Scalar,
599 int Options,
600 template<typename, int> class JointCollectionTpl,
601 typename ConfigVectorType>
602 161 const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & crba(
603 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
604 DataTpl<Scalar, Options, JointCollectionTpl> & data,
605 const Eigen::MatrixBase<ConfigVectorType> & q,
606 const Convention convention)
607 {
608
2/3
✓ Branch 0 taken 42 times.
✓ Branch 1 taken 106 times.
✗ Branch 2 not taken.
 161 switch (convention)
609 {
610 43 case Convention::LOCAL:
611
1/2
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
 43 return ::pinocchio::impl::crbaLocalConvention(model, data, make_const_ref(q));
612 118 case Convention::WORLD:
613
1/2
✓ Branch 2 taken 106 times.
✗ Branch 3 not taken.
 118 return ::pinocchio::impl::crbaWorldConvention(model, data, make_const_ref(q));
614 ✗ default:
615 ✗ throw std::invalid_argument("Bad convention.");
616 }
617 }
618
619 } // namespace pinocchio
620
621 /// @endcond
622
623 #endif // ifndef __pinocchio_crba_hxx__
624