GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/center-of-mass.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 197 209 94.3%
Branches: 173 406 42.6%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_center_of_mass_hxx__
6 #define __pinocchio_algorithm_center_of_mass_hxx__
7
8 #include "pinocchio/algorithm/check.hpp"
9 #include "pinocchio/multibody/visitor.hpp"
10 #include "pinocchio/algorithm/kinematics.hpp"
11 #include "pinocchio/algorithm/jacobian.hpp"
12
13 /// @cond DEV
14
15 namespace pinocchio
16 {
17 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
18 5 Scalar computeTotalMass(const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
19 {
20 5 Scalar m = Scalar(0);
21
2/2
✓ Branch 0 taken 124 times.
✓ Branch 1 taken 5 times.
 129 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
22 {
23 124 m += model.inertias[i].mass();
24 }
25 5 return m;
26 }
27
28 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
29 1 Scalar computeTotalMass(
30 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
31 DataTpl<Scalar, Options, JointCollectionTpl> & data)
32 {
33 1 data.mass[0] = computeTotalMass(model);
34 1 return data.mass[0];
35 }
36
37 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
38 1 void computeSubtreeMasses(
39 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
40 DataTpl<Scalar, Options, JointCollectionTpl> & data)
41 {
42 1 data.mass[0] = Scalar(0);
43
44 // Forward Step
45
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
46 {
47 27 data.mass[i] = model.inertias[i].mass();
48 }
49
50 // Backward Step
51
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
52 {
53 27 const JointIndex & parent = model.parents[i];
54 27 data.mass[parent] += data.mass[i];
55 }
56 1 }
57 namespace impl
58 {
59 template<
60 typename Scalar,
61 int Options,
62 template<typename, int>
63 class JointCollectionTpl,
64 typename ConfigVectorType>
65 899 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
66 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
67 DataTpl<Scalar, Options, JointCollectionTpl> & data,
68 const Eigen::MatrixBase<ConfigVectorType> & q,
69 const bool computeSubtreeComs)
70 {
71 899 ::pinocchio::impl::forwardKinematics(model, data, q.derived());
72
73 899 centerOfMass(model, data, POSITION, computeSubtreeComs);
74 899 return data.com[0];
75 }
76
77 template<
78 typename Scalar,
79 int Options,
80 template<typename, int>
81 class JointCollectionTpl,
82 typename ConfigVectorType,
83 typename TangentVectorType>
84 44 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
85 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
86 DataTpl<Scalar, Options, JointCollectionTpl> & data,
87 const Eigen::MatrixBase<ConfigVectorType> & q,
88 const Eigen::MatrixBase<TangentVectorType> & v,
89 const bool computeSubtreeComs)
90 {
91 44 ::pinocchio::impl::forwardKinematics(model, data, q.derived(), v.derived());
92
93 44 centerOfMass(model, data, VELOCITY, computeSubtreeComs);
94 44 return data.com[0];
95 }
96
97 template<
98 typename Scalar,
99 int Options,
100 template<typename, int>
101 class JointCollectionTpl,
102 typename ConfigVectorType,
103 typename TangentVectorType1,
104 typename TangentVectorType2>
105 3 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
106 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
107 DataTpl<Scalar, Options, JointCollectionTpl> & data,
108 const Eigen::MatrixBase<ConfigVectorType> & q,
109 const Eigen::MatrixBase<TangentVectorType1> & v,
110 const Eigen::MatrixBase<TangentVectorType2> & a,
111 const bool computeSubtreeComs)
112 {
113 3 ::pinocchio::impl::forwardKinematics(model, data, q, v, a);
114
115 3 centerOfMass(model, data, ACCELERATION, computeSubtreeComs);
116 3 return data.com[0];
117 }
118 } // namespace impl
119 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
120 946 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
121 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
122 DataTpl<Scalar, Options, JointCollectionTpl> & data,
123 KinematicLevel kinematic_level,
124 const bool computeSubtreeComs)
125 {
126
1/2
✓ Branch 1 taken 946 times.
✗ Branch 2 not taken.
 946 assert(model.check(data) && "data is not consistent with model.");
127
2/6
✓ Branch 0 taken 946 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 946 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 946 PINOCCHIO_CHECK_INPUT_ARGUMENT(kinematic_level >= 0 && kinematic_level <= 2);
128
129 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
130 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
131
132 typedef typename Model::JointIndex JointIndex;
133
134 typedef typename Data::SE3 SE3;
135 typedef typename Data::Motion Motion;
136 typedef typename Data::Inertia Inertia;
137
138 946 const bool do_position = (kinematic_level >= POSITION);
139 946 const bool do_velocity = (kinematic_level >= VELOCITY);
140 946 const bool do_acceleration = (kinematic_level >= ACCELERATION);
141
142 946 data.mass[0] = 0;
143
1/2
✓ Branch 0 taken 946 times.
✗ Branch 1 not taken.
 946 if (do_position)
144 946 data.com[0].setZero();
145
2/2
✓ Branch 0 taken 47 times.
✓ Branch 1 taken 899 times.
 946 if (do_velocity)
146 47 data.vcom[0].setZero();
147
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 943 times.
 946 if (do_acceleration)
148 3 data.acom[0].setZero();
149
150 // Forward Step
151
2/2
✓ Branch 0 taken 25628 times.
✓ Branch 1 taken 946 times.
 26574 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
152 {
153 25628 const typename Inertia::Scalar & mass = model.inertias[i].mass();
154 25628 const typename SE3::Vector3 & lever = model.inertias[i].lever();
155
156 25628 const Motion & v = data.v[i];
157 25628 const Motion & a = data.a[i];
158
159 25628 data.mass[i] = mass;
160
161
1/2
✓ Branch 0 taken 25628 times.
✗ Branch 1 not taken.
 25628 if (do_position)
162
3/6
✓ Branch 1 taken 25628 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 25628 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 25628 times.
✗ Branch 9 not taken.
 25628 data.com[i].noalias() = mass * lever;
163
164
2/2
✓ Branch 0 taken 1343 times.
✓ Branch 1 taken 24285 times.
 25628 if (do_velocity)
165
7/14
✓ Branch 1 taken 1343 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1343 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1343 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1343 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1343 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 1343 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1343 times.
✗ Branch 21 not taken.
 1343 data.vcom[i].noalias() = mass * (v.angular().cross(lever) + v.linear());
166
167
2/2
✓ Branch 0 taken 82 times.
✓ Branch 1 taken 25546 times.
 25628 if (do_acceleration)
168
7/14
✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 82 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 82 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 82 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 82 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 82 times.
✗ Branch 21 not taken.
 164 data.acom[i].noalias() =
169
2/4
✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 82 times.
✗ Branch 5 not taken.
 82 mass * (a.angular().cross(lever) + a.linear())
170
1/2
✓ Branch 1 taken 82 times.
✗ Branch 2 not taken.
 164 + v.angular().cross(
171 82 data.vcom[i]); // take into accound the coriolis part of the acceleration
172 }
173
174 // Backward Step
175
2/2
✓ Branch 0 taken 25628 times.
✓ Branch 1 taken 946 times.
 26574 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
176 {
177 25628 const JointIndex & parent = model.parents[i];
178 25628 const SE3 & liMi = data.liMi[i];
179
180 25628 data.mass[parent] += data.mass[i];
181
182
1/2
✓ Branch 0 taken 25628 times.
✗ Branch 1 not taken.
 25628 if (do_position)
183
4/8
✓ Branch 5 taken 25628 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 25628 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 25628 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 25628 times.
✗ Branch 16 not taken.
 25628 data.com[parent] += (liMi.rotation() * data.com[i] + data.mass[i] * liMi.translation());
184
185
2/2
✓ Branch 0 taken 1343 times.
✓ Branch 1 taken 24285 times.
 25628 if (do_velocity)
186
1/2
✓ Branch 5 taken 1343 times.
✗ Branch 6 not taken.
 1343 data.vcom[parent] += liMi.rotation() * data.vcom[i];
187
188
2/2
✓ Branch 0 taken 82 times.
✓ Branch 1 taken 25546 times.
 25628 if (do_acceleration)
189
1/2
✓ Branch 5 taken 82 times.
✗ Branch 6 not taken.
 82 data.acom[parent] += liMi.rotation() * data.acom[i];
190
191
2/2
✓ Branch 0 taken 25600 times.
✓ Branch 1 taken 28 times.
 25628 if (computeSubtreeComs)
192 {
193
1/2
✓ Branch 0 taken 25600 times.
✗ Branch 1 not taken.
 25600 if (do_position)
194 25600 data.com[i] /= data.mass[i];
195
2/2
✓ Branch 0 taken 1315 times.
✓ Branch 1 taken 24285 times.
 25600 if (do_velocity)
196 1315 data.vcom[i] /= data.mass[i];
197
2/2
✓ Branch 0 taken 82 times.
✓ Branch 1 taken 25518 times.
 25600 if (do_acceleration)
198 82 data.acom[i] /= data.mass[i];
199 }
200 }
201
202
1/2
✓ Branch 0 taken 946 times.
✗ Branch 1 not taken.
 946 if (do_position)
203 946 data.com[0] /= data.mass[0];
204
2/2
✓ Branch 0 taken 47 times.
✓ Branch 1 taken 899 times.
 946 if (do_velocity)
205 47 data.vcom[0] /= data.mass[0];
206
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 943 times.
 946 if (do_acceleration)
207 3 data.acom[0] /= data.mass[0];
208
209 946 return data.com[0];
210 }
211
212 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
213 1 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & getComFromCrba(
214 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
215 DataTpl<Scalar, Options, JointCollectionTpl> & data)
216 {
217 PINOCCHIO_UNUSED_VARIABLE(model);
218
219
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(model.check(data) && "data is not consistent with model.");
220 1 return data.com[0] = data.liMi[1].act(data.oYcrb[1].lever());
221 }
222
223 /* --- JACOBIAN ---------------------------------------------------------- */
224 /* --- JACOBIAN ---------------------------------------------------------- */
225 /* --- JACOBIAN ---------------------------------------------------------- */
226
227 template<
228 typename Scalar,
229 int Options,
230 template<typename, int>
231 class JointCollectionTpl,
232 typename Matrix3x>
233 struct JacobianCenterOfMassBackwardStep
234 : public fusion::JointUnaryVisitorBase<
235 JacobianCenterOfMassBackwardStep<Scalar, Options, JointCollectionTpl, Matrix3x>>
236 {
237 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
238 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
239
240 typedef boost::fusion::
241 vector<const Model &, Data &, const Eigen::MatrixBase<Matrix3x> &, const bool &>
242 ArgsType;
243
244 template<typename JointModel>
245 654 static void algo(
246 const JointModelBase<JointModel> & jmodel,
247 JointDataBase<typename JointModel::JointDataDerived> & jdata,
248 const Model & model,
249 Data & data,
250 const Eigen::MatrixBase<Matrix3x> & Jcom,
251 const bool & computeSubtreeComs)
252 {
253
1/2
✓ Branch 1 taken 327 times.
✗ Branch 2 not taken.
 654 const JointIndex & i = (JointIndex)jmodel.id();
254 654 const JointIndex & parent = model.parents[i];
255
256
1/2
✓ Branch 3 taken 327 times.
✗ Branch 4 not taken.
 654 data.com[parent] += data.com[i];
257 654 data.mass[parent] += data.mass[i];
258
259 typedef typename Data::Matrix6x Matrix6x;
260 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
261
262 654 Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x, Jcom);
263
264
1/2
✓ Branch 1 taken 327 times.
✗ Branch 2 not taken.
 654 ColBlock Jcols = jmodel.jointCols(data.J);
265
3/6
✓ Branch 2 taken 327 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 327 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 327 times.
✗ Branch 9 not taken.
 654 Jcols = data.oMi[i].act(jdata.S());
266
267
3/4
✓ Branch 1 taken 694 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 367 times.
✓ Branch 4 taken 327 times.
 1388 for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id)
268 {
269
6/12
✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 367 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 367 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 367 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 367 times.
✗ Branch 17 not taken.
 1468 jmodel.jointCols(Jcom_).col(col_id) =
270
2/4
✓ Branch 1 taken 367 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 367 times.
✗ Branch 5 not taken.
 734 data.mass[i] * Jcols.col(col_id).template segment<3>(Motion::LINEAR)
271
2/4
✓ Branch 2 taken 367 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 367 times.
✗ Branch 6 not taken.
 1468 - data.com[i].cross(Jcols.col(col_id).template segment<3>(Motion::ANGULAR));
272 }
273
274
1/2
✓ Branch 0 taken 327 times.
✗ Branch 1 not taken.
 654 if (computeSubtreeComs)
275
1/2
✓ Branch 3 taken 327 times.
✗ Branch 4 not taken.
 654 data.com[i] /= data.mass[i];
276 }
277 };
278 namespace impl
279 {
280 template<
281 typename Scalar,
282 int Options,
283 template<typename, int>
284 class JointCollectionTpl,
285 typename ConfigVectorType>
286 5 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & jacobianCenterOfMass(
287 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
288 DataTpl<Scalar, Options, JointCollectionTpl> & data,
289 const Eigen::MatrixBase<ConfigVectorType> & q,
290 const bool computeSubtreeComs)
291 {
292 5 ::pinocchio::impl::forwardKinematics(model, data, q);
293 5 return jacobianCenterOfMass(model, data, computeSubtreeComs);
294 }
295 } // namespace impl
296 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
297 5 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & jacobianCenterOfMass(
298 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
299 DataTpl<Scalar, Options, JointCollectionTpl> & data,
300 const bool computeSubtreeComs)
301 {
302
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 assert(model.check(data) && "data is not consistent with model.");
303
304 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
305 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
306
307 typedef typename Model::JointIndex JointIndex;
308
309 typedef typename Data::Matrix3x Matrix3x;
310 typedef typename Data::SE3 SE3;
311 typedef typename Data::Inertia Inertia;
312
313 5 data.com[0].setZero();
314 5 data.mass[0] = Scalar(0);
315
316
2/2
✓ Branch 0 taken 135 times.
✓ Branch 1 taken 5 times.
 140 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
317 {
318 135 const typename Inertia::Scalar & mass = model.inertias[i].mass();
319 135 const typename SE3::Vector3 & lever = model.inertias[i].lever();
320
321 135 data.mass[i] = mass;
322
4/8
✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 135 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 135 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 135 times.
✗ Branch 13 not taken.
 135 data.com[i].noalias() = mass * data.oMi[i].act(lever);
323 }
324
325 // Backward step
326 typedef JacobianCenterOfMassBackwardStep<Scalar, Options, JointCollectionTpl, Matrix3x> Pass2;
327
2/2
✓ Branch 0 taken 135 times.
✓ Branch 1 taken 5 times.
 140 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
328 {
329
1/2
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
 135 Pass2::run(
330 135 model.joints[i], data.joints[i],
331 270 typename Pass2::ArgsType(model, data, data.Jcom, computeSubtreeComs));
332 }
333
334 5 data.com[0] /= data.mass[0];
335 5 data.Jcom /= data.mass[0];
336
337 5 return data.Jcom;
338 }
339
340 template<
341 typename Scalar,
342 int Options,
343 template<typename, int>
344 class JointCollectionTpl,
345 typename Matrix3x>
346 struct JacobianSubtreeCenterOfMassBackwardStep
347 : public fusion::JointUnaryVisitorBase<
348 JacobianSubtreeCenterOfMassBackwardStep<Scalar, Options, JointCollectionTpl, Matrix3x>>
349 {
350 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
351 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
352
353 typedef boost::fusion::
354 vector<const Model &, Data &, const JointIndex &, const Eigen::MatrixBase<Matrix3x> &>
355 ArgsType;
356
357 template<typename JointModel>
358 222 static void algo(
359 const JointModelBase<JointModel> & jmodel,
360 JointDataBase<typename JointModel::JointDataDerived> & jdata,
361 const Model & model,
362 Data & data,
363 const JointIndex & subtree_root_id,
364 const Eigen::MatrixBase<Matrix3x> & Jcom)
365 {
366 PINOCCHIO_UNUSED_VARIABLE(model);
367
368
1/2
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
 222 const JointIndex & i = (JointIndex)jmodel.id();
369
370 typedef typename Data::Matrix6x Matrix6x;
371 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
372
373 222 Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x, Jcom);
374
375
1/2
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
 222 ColBlock Jcols = jmodel.jointCols(data.J);
376
3/6
✓ Branch 2 taken 111 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 111 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 111 times.
✗ Branch 9 not taken.
 222 Jcols = data.oMi[i].act(jdata.S());
377
378
3/4
✓ Branch 1 taken 352 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 241 times.
✓ Branch 4 taken 111 times.
 704 for (Eigen::DenseIndex col_id = 0; col_id < jmodel.nv(); ++col_id)
379 {
380
5/10
✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 241 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 241 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 241 times.
✗ Branch 14 not taken.
 964 jmodel.jointCols(Jcom_).col(col_id) =
381
2/4
✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✗ Branch 5 not taken.
 482 Jcols.col(col_id).template segment<3>(Motion::LINEAR)
382
2/4
✓ Branch 2 taken 241 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 241 times.
✗ Branch 6 not taken.
 964 - data.com[subtree_root_id].cross(Jcols.col(col_id).template segment<3>(Motion::ANGULAR));
383 }
384 }
385 };
386 namespace impl
387 {
388 template<
389 typename Scalar,
390 int Options,
391 template<typename, int>
392 class JointCollectionTpl,
393 typename ConfigVectorType,
394 typename Matrix3xLike>
395 1 void jacobianSubtreeCenterOfMass(
396 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
397 DataTpl<Scalar, Options, JointCollectionTpl> & data,
398 const Eigen::MatrixBase<ConfigVectorType> & q,
399 const JointIndex & rootSubtreeId,
400 const Eigen::MatrixBase<Matrix3xLike> & res)
401 {
402 1 ::pinocchio::impl::forwardKinematics(model, data, q);
403 1 ::pinocchio::jacobianSubtreeCenterOfMass(model, data, rootSubtreeId, res);
404 1 }
405
406 template<
407 typename Scalar,
408 int Options,
409 template<typename, int>
410 class JointCollectionTpl,
411 typename Matrix3xLike>
412 29 void jacobianSubtreeCenterOfMass(
413 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
414 DataTpl<Scalar, Options, JointCollectionTpl> & data,
415 const JointIndex & rootSubtreeId,
416 const Eigen::MatrixBase<Matrix3xLike> & res)
417 {
418 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
419 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
420
421
2/4
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 29 times.
✗ Branch 4 not taken.
 29 assert(model.check(data) && "data is not consistent with model.");
422
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 29 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 29 PINOCCHIO_CHECK_INPUT_ARGUMENT((int)rootSubtreeId < model.njoints, "Invalid joint id.");
423
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 29 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.
 29 PINOCCHIO_CHECK_ARGUMENT_SIZE(
424 res.rows(), 3, "the resulting matrix does not have the right size.");
425
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 29 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.
 29 PINOCCHIO_CHECK_ARGUMENT_SIZE(
426 res.cols(), model.nv, "the resulting matrix does not have the right size.");
427
428 29 Matrix3xLike & Jcom_subtree = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xLike, res);
429
430 typedef typename Data::SE3 SE3;
431 typedef typename Data::Inertia Inertia;
432
433 typedef typename Model::IndexVector IndexVector;
434 29 const IndexVector & subtree = model.subtrees[rootSubtreeId];
435
436 29 const bool computeSubtreeComs = true;
437
438
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 27 times.
 29 if (rootSubtreeId == 0)
439 {
440 2 data.mass[0] = 0;
441
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 data.com[0].setZero();
442 }
443
444
2/2
✓ Branch 1 taken 192 times.
✓ Branch 2 taken 29 times.
 221 for (size_t k = 0; k < subtree.size(); ++k)
445 {
446 192 const JointIndex joint_id = subtree[k];
447 192 const typename Inertia::Scalar & mass = model.inertias[joint_id].mass();
448 192 const typename SE3::Vector3 & lever = model.inertias[joint_id].lever();
449
450 192 data.mass[joint_id] = mass;
451
3/6
✓ Branch 2 taken 192 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 192 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 192 times.
✗ Branch 10 not taken.
 192 data.com[joint_id] = mass * data.oMi[joint_id].act(lever);
452 }
453
454 // Backward step
455 typedef JacobianCenterOfMassBackwardStep<Scalar, Options, JointCollectionTpl, Matrix3xLike>
456 Pass2;
457
2/2
✓ Branch 1 taken 192 times.
✓ Branch 2 taken 29 times.
 221 for (Eigen::DenseIndex k = (Eigen::DenseIndex)subtree.size() - 1; k >= 0; --k)
458 {
459 192 const JointIndex joint_id = subtree[(size_t)k];
460
1/2
✓ Branch 1 taken 192 times.
✗ Branch 2 not taken.
 192 Pass2::run(
461 192 model.joints[joint_id], data.joints[joint_id],
462
1/2
✓ Branch 1 taken 192 times.
✗ Branch 2 not taken.
 384 typename Pass2::ArgsType(model, data, Jcom_subtree, computeSubtreeComs));
463 }
464
465
2/6
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 29 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 29 PINOCCHIO_CHECK_INPUT_ARGUMENT(
466 check_expression_if_real<Scalar>(data.mass[rootSubtreeId] > 0.),
467 "The mass of the subtree is not positive.");
468 29 const Scalar mass_inv_subtree = Scalar(1) / data.mass[rootSubtreeId];
469 29 typename Data::Vector3 & com_subtree = data.com[rootSubtreeId];
470 if (!computeSubtreeComs)
471 com_subtree *= mass_inv_subtree;
472
473
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 27 times.
 29 if (rootSubtreeId == 0)
474 {
475
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Jcom_subtree *= mass_inv_subtree;
476 2 return; // skip the rest
477 }
478
479
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 const int idx_v = model.joints[rootSubtreeId].idx_v();
480 27 const int nv_subtree = data.nvSubtree[rootSubtreeId];
481
482
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
 27 Jcom_subtree.middleCols(idx_v, nv_subtree) *= mass_inv_subtree;
483
484 // Second backward step
485 typedef JacobianSubtreeCenterOfMassBackwardStep<
486 Scalar, Options, JointCollectionTpl, Matrix3xLike>
487 Pass3;
488
2/2
✓ Branch 1 taken 111 times.
✓ Branch 2 taken 27 times.
 138 for (JointIndex parent = model.parents[rootSubtreeId]; parent > 0;
489 111 parent = model.parents[parent])
490 {
491
1/2
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
 111 Pass3::run(
492 111 model.joints[parent], data.joints[parent],
493
1/2
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
 222 typename Pass3::ArgsType(model, data, rootSubtreeId, Jcom_subtree));
494 }
495 }
496
497 template<
498 typename Scalar,
499 int Options,
500 template<typename, int>
501 class JointCollectionTpl,
502 typename Matrix3xLike>
503 28 void getJacobianSubtreeCenterOfMass(
504 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
505 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
506 const JointIndex & rootSubtreeId,
507 const Eigen::MatrixBase<Matrix3xLike> & res)
508 {
509 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
510
511
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 28 times.
✗ Branch 4 not taken.
 28 assert(model.check(data) && "data is not consistent with model.");
512
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 28 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 28 PINOCCHIO_CHECK_INPUT_ARGUMENT(((int)rootSubtreeId < model.njoints), "Invalid joint id.");
513
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 28 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.
 28 PINOCCHIO_CHECK_ARGUMENT_SIZE(
514 res.rows(), 3, "the resulting matrix does not have the right size.");
515
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 28 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.
 28 PINOCCHIO_CHECK_ARGUMENT_SIZE(
516 res.cols(), model.nv, "the resulting matrix does not have the right size.");
517
518 28 Matrix3xLike & Jcom_subtree = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xLike, res);
519
520
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 27 times.
 28 if (rootSubtreeId == 0)
521 {
522
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Jcom_subtree = data.Jcom;
523 1 return;
524 }
525
526
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 const int idx_v = model.joints[rootSubtreeId].idx_v();
527 27 const int nv_subtree = data.nvSubtree[rootSubtreeId];
528
529 27 const Scalar mass_ratio = data.mass[0] / data.mass[rootSubtreeId];
530
3/6
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
 27 Jcom_subtree.middleCols(idx_v, nv_subtree) =
531
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 mass_ratio * data.Jcom.middleCols(idx_v, nv_subtree);
532
533 27 const typename Data::Vector3 & com_subtree = data.com[rootSubtreeId];
534
535
2/2
✓ Branch 1 taken 241 times.
✓ Branch 2 taken 27 times.
 268 for (int parent = data.parents_fromRow[(size_t)idx_v]; parent >= 0;
536 241 parent = data.parents_fromRow[(size_t)parent])
537 {
538
1/2
✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
 241 typename Data::Matrix6x::ConstColXpr Jcol = data.J.col(parent);
539
5/10
✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 241 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 241 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 241 times.
✗ Branch 14 not taken.
 482 Jcom_subtree.col(parent).noalias() =
540
1/2
✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
 241 Jcol.template segment<3>(Motion::LINEAR)
541
1/2
✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
 482 - com_subtree.cross(Jcol.template segment<3>(Motion::ANGULAR));
542 }
543 }
544 } // namespace impl
545 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
546 7 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & getJacobianComFromCrba(
547 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
548 DataTpl<Scalar, Options, JointCollectionTpl> & data)
549 {
550 PINOCCHIO_UNUSED_VARIABLE(model);
551
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 assert(model.check(data) && "data is not consistent with model.");
552
553 // Extract the total mass of the system.
554 7 data.mass[0] = data.oYcrb[0].mass();
555 7 data.com[0] = data.oYcrb[0].lever();
556
557
3/6
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 7 times.
✗ Branch 10 not taken.
 7 data.Jcom.noalias() = data.Ag.template middleRows<3>(Force::LINEAR) / data.mass[0];
558
559 7 return data.Jcom;
560 }
561
562 template<
563 typename Scalar,
564 int Options,
565 template<typename, int>
566 class JointCollectionTpl,
567 typename ConfigVectorType>
568 2 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
569 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
570 DataTpl<Scalar, Options, JointCollectionTpl> & data,
571 const Eigen::MatrixBase<ConfigVectorType> & q,
572 const bool computeSubtreeComs)
573 {
574
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 return impl::centerOfMass(model, data, make_const_ref(q), computeSubtreeComs);
575 }
576
577 template<
578 typename Scalar,
579 int Options,
580 template<typename, int>
581 class JointCollectionTpl,
582 typename ConfigVectorType,
583 typename TangentVectorType>
584 ✗ const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
585 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
586 DataTpl<Scalar, Options, JointCollectionTpl> & data,
587 const Eigen::MatrixBase<ConfigVectorType> & q,
588 const Eigen::MatrixBase<TangentVectorType> & v,
589 const bool computeSubtreeComs)
590 {
591 ✗ return impl::centerOfMass(
592 ✗ model, data, make_const_ref(q), make_const_ref(v), computeSubtreeComs);
593 }
594
595 template<
596 typename Scalar,
597 int Options,
598 template<typename, int>
599 class JointCollectionTpl,
600 typename ConfigVectorType,
601 typename TangentVectorType1,
602 typename TangentVectorType2>
603 ✗ const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
604 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
605 DataTpl<Scalar, Options, JointCollectionTpl> & data,
606 const Eigen::MatrixBase<ConfigVectorType> & q,
607 const Eigen::MatrixBase<TangentVectorType1> & v,
608 const Eigen::MatrixBase<TangentVectorType2> & a,
609 const bool computeSubtreeComs)
610 {
611 ✗ return impl::centerOfMass(
612 ✗ model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), computeSubtreeComs);
613 }
614
615 template<
616 typename Scalar,
617 int Options,
618 template<typename, int>
619 class JointCollectionTpl,
620 typename ConfigVectorType>
621 ✗ const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & jacobianCenterOfMass(
622 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
623 DataTpl<Scalar, Options, JointCollectionTpl> & data,
624 const Eigen::MatrixBase<ConfigVectorType> & q,
625 const bool computeSubtreeComs)
626 {
627 ✗ return impl::jacobianCenterOfMass(model, data, make_const_ref(q), computeSubtreeComs);
628 }
629
630 template<
631 typename Scalar,
632 int Options,
633 template<typename, int>
634 class JointCollectionTpl,
635 typename ConfigVectorType,
636 typename Matrix3xLike>
637 ✗ void jacobianSubtreeCenterOfMass(
638 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
639 DataTpl<Scalar, Options, JointCollectionTpl> & data,
640 const Eigen::MatrixBase<ConfigVectorType> & q,
641 const JointIndex & rootSubtreeId,
642 const Eigen::MatrixBase<Matrix3xLike> & res)
643 {
644 ✗ impl::jacobianSubtreeCenterOfMass(model, data, make_const_ref(q), rootSubtreeId, make_ref(res));
645 }
646
647 template<
648 typename Scalar,
649 int Options,
650 template<typename, int>
651 class JointCollectionTpl,
652 typename Matrix3xLike>
653 1 void jacobianSubtreeCenterOfMass(
654 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
655 DataTpl<Scalar, Options, JointCollectionTpl> & data,
656 const JointIndex & rootSubtreeId,
657 const Eigen::MatrixBase<Matrix3xLike> & res)
658 {
659
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 impl::jacobianSubtreeCenterOfMass(model, data, rootSubtreeId, make_ref(res));
660 1 }
661
662 template<
663 typename Scalar,
664 int Options,
665 template<typename, int>
666 class JointCollectionTpl,
667 typename Matrix3xLike>
668 ✗ void getJacobianSubtreeCenterOfMass(
669 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
670 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
671 const JointIndex & rootSubtreeId,
672 const Eigen::MatrixBase<Matrix3xLike> & res)
673 {
674 ✗ impl::getJacobianSubtreeCenterOfMass(model, data, rootSubtreeId, make_ref(res));
675 }
676
677 } // namespace pinocchio
678
679 /// @endcond
680
681 #endif // ifndef __pinocchio_algorithm_center_of_mass_hxx__
682