GCC Code Coverage Report

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