GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/center-of-mass.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 211 211 100.0%
Branches: 182 453 40.2%
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.jointCols(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.nv(); ++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 typedef JacobianCenterOfMassBackwardStep<Scalar, Options, JointCollectionTpl, Matrix3x> Pass2;
322
2/2
✓ Branch 0 taken 324 times.
✓ Branch 1 taken 12 times.
 336 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
323 {
324
1/2
✓ Branch 1 taken 324 times.
✗ Branch 2 not taken.
 324 Pass2::run(
325 324 model.joints[i], data.joints[i],
326 648 typename Pass2::ArgsType(model, data, data.Jcom, computeSubtreeComs));
327 }
328
329 12 data.com[0] /= data.mass[0];
330 12 data.Jcom /= data.mass[0];
331
332 12 return data.Jcom;
333 }
334
335 template<
336 typename Scalar,
337 int Options,
338 template<typename, int> class JointCollectionTpl,
339 typename Matrix3x>
340 struct JacobianSubtreeCenterOfMassBackwardStep
341 : public fusion::JointUnaryVisitorBase<
342 JacobianSubtreeCenterOfMassBackwardStep<Scalar, Options, JointCollectionTpl, Matrix3x>>
343 {
344 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
345 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
346
347 typedef boost::fusion::
348 vector<const Model &, Data &, const JointIndex &, const Eigen::MatrixBase<Matrix3x> &>
349 ArgsType;
350
351 template<typename JointModel>
352 222 static void algo(
353 const JointModelBase<JointModel> & jmodel,
354 JointDataBase<typename JointModel::JointDataDerived> & jdata,
355 const Model & model,
356 Data & data,
357 const JointIndex & subtree_root_id,
358 const Eigen::MatrixBase<Matrix3x> & Jcom)
359 {
360 PINOCCHIO_UNUSED_VARIABLE(model);
361
362
1/2
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
 222 const JointIndex & i = (JointIndex)jmodel.id();
363
364 typedef typename Data::Matrix6x Matrix6x;
365 typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6x>::Type ColBlock;
366
367 222 Matrix3x & Jcom_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix3x, Jcom);
368
369
1/2
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
 222 ColBlock Jcols = jmodel.jointCols(data.J);
370
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());
371
372
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)
373 {
374
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) =
375
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)
376
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));
377 }
378 }
379 };
380 namespace impl
381 {
382 template<
383 typename Scalar,
384 int Options,
385 template<typename, int> class JointCollectionTpl,
386 typename ConfigVectorType,
387 typename Matrix3xLike>
388 3 void jacobianSubtreeCenterOfMass(
389 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
390 DataTpl<Scalar, Options, JointCollectionTpl> & data,
391 const Eigen::MatrixBase<ConfigVectorType> & q,
392 const JointIndex & rootSubtreeId,
393 const Eigen::MatrixBase<Matrix3xLike> & res)
394 {
395 3 ::pinocchio::impl::forwardKinematics(model, data, q);
396 3 ::pinocchio::jacobianSubtreeCenterOfMass(model, data, rootSubtreeId, res);
397 3 }
398
399 template<
400 typename Scalar,
401 int Options,
402 template<typename, int> class JointCollectionTpl,
403 typename Matrix3xLike>
404 32 void jacobianSubtreeCenterOfMass(
405 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
406 DataTpl<Scalar, Options, JointCollectionTpl> & data,
407 const JointIndex & rootSubtreeId,
408 const Eigen::MatrixBase<Matrix3xLike> & res)
409 {
410 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
411 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
412
413
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.");
414
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.");
415
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(
416 res.rows(), 3, "the resulting matrix does not have the right size.");
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.cols(), model.nv, "the resulting matrix does not have the right size.");
419
420 32 Matrix3xLike & Jcom_subtree = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xLike, res);
421
422 typedef typename Data::SE3 SE3;
423 typedef typename Data::Inertia Inertia;
424
425 typedef typename Model::IndexVector IndexVector;
426 32 const IndexVector & subtree = model.subtrees[rootSubtreeId];
427
428 32 const bool computeSubtreeComs = true;
429
430
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 27 times.
 32 if (rootSubtreeId == 0)
431 {
432
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 5 data.mass[0] = 0;
433
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
 5 data.com[0].setZero();
434 }
435
436
2/2
✓ Branch 1 taken 273 times.
✓ Branch 2 taken 32 times.
 305 for (size_t k = 0; k < subtree.size(); ++k)
437 {
438 273 const JointIndex joint_id = subtree[k];
439
0/2
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 273 const typename Inertia::Scalar & mass = model.inertias[joint_id].mass();
440 273 const typename SE3::Vector3 & lever = model.inertias[joint_id].lever();
441
442
0/2
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 273 data.mass[joint_id] = mass;
443
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);
444 }
445
446 // Backward step
447 typedef JacobianCenterOfMassBackwardStep<Scalar, Options, JointCollectionTpl, Matrix3xLike>
448 Pass2;
449
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)
450 {
451 273 const JointIndex joint_id = subtree[(size_t)k];
452
1/2
✓ Branch 1 taken 273 times.
✗ Branch 2 not taken.
 273 Pass2::run(
453 273 model.joints[joint_id], data.joints[joint_id],
454
1/2
✓ Branch 1 taken 273 times.
✗ Branch 2 not taken.
 546 typename Pass2::ArgsType(model, data, Jcom_subtree, computeSubtreeComs));
455 }
456
457
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(
458 check_expression_if_real<Scalar>(data.mass[rootSubtreeId] > 0.),
459 "The mass of the subtree is not positive.");
460
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];
461 32 typename Data::Vector3 & com_subtree = data.com[rootSubtreeId];
462 if (!computeSubtreeComs)
463 com_subtree *= mass_inv_subtree;
464
465
2/2
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 27 times.
 32 if (rootSubtreeId == 0)
466 {
467
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 Jcom_subtree *= mass_inv_subtree;
468 5 return; // skip the rest
469 }
470
471
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 const int idx_v = model.joints[rootSubtreeId].idx_v();
472 27 const int nv_subtree = data.nvSubtree[rootSubtreeId];
473
474
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;
475
476 // Second backward step
477 typedef JacobianSubtreeCenterOfMassBackwardStep<
478 Scalar, Options, JointCollectionTpl, Matrix3xLike>
479 Pass3;
480
2/2
✓ Branch 1 taken 111 times.
✓ Branch 2 taken 27 times.
 138 for (JointIndex parent = model.parents[rootSubtreeId]; parent > 0;
481 111 parent = model.parents[parent])
482 {
483
1/2
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
 111 Pass3::run(
484 111 model.joints[parent], data.joints[parent],
485
1/2
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
 222 typename Pass3::ArgsType(model, data, rootSubtreeId, Jcom_subtree));
486 }
487 }
488
489 template<
490 typename Scalar,
491 int Options,
492 template<typename, int> class JointCollectionTpl,
493 typename Matrix3xLike>
494 29 void getJacobianSubtreeCenterOfMass(
495 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
496 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
497 const JointIndex & rootSubtreeId,
498 const Eigen::MatrixBase<Matrix3xLike> & res)
499 {
500 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
501
502
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.");
503
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.");
504
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(
505 res.rows(), 3, "the resulting matrix does not have the right size.");
506
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(
507 res.cols(), model.nv, "the resulting matrix does not have the right size.");
508
509 29 Matrix3xLike & Jcom_subtree = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xLike, res);
510
511
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 27 times.
 29 if (rootSubtreeId == 0)
512 {
513
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Jcom_subtree = data.Jcom;
514 2 return;
515 }
516
517
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 27 const int idx_v = model.joints[rootSubtreeId].idx_v();
518 27 const int nv_subtree = data.nvSubtree[rootSubtreeId];
519
520
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 27 const Scalar mass_ratio = data.mass[0] / data.mass[rootSubtreeId];
521
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) =
522
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 27 mass_ratio * data.Jcom.middleCols(idx_v, nv_subtree);
523
524 27 const typename Data::Vector3 & com_subtree = data.com[rootSubtreeId];
525
526
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;
527 241 parent = data.parents_fromRow[(size_t)parent])
528 {
529
1/2
✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
 241 typename Data::Matrix6x::ConstColXpr Jcol = data.J.col(parent);
530
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() =
531
1/2
✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
 241 Jcol.template segment<3>(Motion::LINEAR)
532
1/2
✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
 482 - com_subtree.cross(Jcol.template segment<3>(Motion::ANGULAR));
533 }
534 }
535 } // namespace impl
536 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
537 7 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & getJacobianComFromCrba(
538 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
539 DataTpl<Scalar, Options, JointCollectionTpl> & data)
540 {
541 PINOCCHIO_UNUSED_VARIABLE(model);
542
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 assert(model.check(data) && "data is not consistent with model.");
543
544 // Extract the total mass of the system.
545 7 data.mass[0] = data.oYcrb[0].mass();
546 7 data.com[0] = data.oYcrb[0].lever();
547
548
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];
549
550 7 return data.Jcom;
551 }
552
553 template<
554 typename Scalar,
555 int Options,
556 template<typename, int> class JointCollectionTpl,
557 typename ConfigVectorType>
558 907 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
559 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
560 DataTpl<Scalar, Options, JointCollectionTpl> & data,
561 const Eigen::MatrixBase<ConfigVectorType> & q,
562 const bool computeSubtreeComs)
563 {
564
1/2
✓ Branch 2 taken 907 times.
✗ Branch 3 not taken.
 907 return impl::centerOfMass(model, data, make_const_ref(q), computeSubtreeComs);
565 }
566
567 template<
568 typename Scalar,
569 int Options,
570 template<typename, int> class JointCollectionTpl,
571 typename ConfigVectorType,
572 typename TangentVectorType>
573 82 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
574 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
575 DataTpl<Scalar, Options, JointCollectionTpl> & data,
576 const Eigen::MatrixBase<ConfigVectorType> & q,
577 const Eigen::MatrixBase<TangentVectorType> & v,
578 const bool computeSubtreeComs)
579 {
580
2/4
✓ Branch 2 taken 82 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 82 times.
✗ Branch 6 not taken.
 164 return impl::centerOfMass(
581 164 model, data, make_const_ref(q), make_const_ref(v), computeSubtreeComs);
582 }
583
584 template<
585 typename Scalar,
586 int Options,
587 template<typename, int> class JointCollectionTpl,
588 typename ConfigVectorType,
589 typename TangentVectorType1,
590 typename TangentVectorType2>
591 6 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Vector3 & centerOfMass(
592 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
593 DataTpl<Scalar, Options, JointCollectionTpl> & data,
594 const Eigen::MatrixBase<ConfigVectorType> & q,
595 const Eigen::MatrixBase<TangentVectorType1> & v,
596 const Eigen::MatrixBase<TangentVectorType2> & a,
597 const bool computeSubtreeComs)
598 {
599
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(
600 12 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), computeSubtreeComs);
601 }
602
603 template<
604 typename Scalar,
605 int Options,
606 template<typename, int> class JointCollectionTpl,
607 typename ConfigVectorType>
608 10 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix3x & jacobianCenterOfMass(
609 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
610 DataTpl<Scalar, Options, JointCollectionTpl> & data,
611 const Eigen::MatrixBase<ConfigVectorType> & q,
612 const bool computeSubtreeComs)
613 {
614
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 return impl::jacobianCenterOfMass(model, data, make_const_ref(q), computeSubtreeComs);
615 }
616
617 template<
618 typename Scalar,
619 int Options,
620 template<typename, int> class JointCollectionTpl,
621 typename ConfigVectorType,
622 typename Matrix3xLike>
623 3 void jacobianSubtreeCenterOfMass(
624 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
625 DataTpl<Scalar, Options, JointCollectionTpl> & data,
626 const Eigen::MatrixBase<ConfigVectorType> & q,
627 const JointIndex & rootSubtreeId,
628 const Eigen::MatrixBase<Matrix3xLike> & res)
629 {
630
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));
631 3 }
632
633 template<
634 typename Scalar,
635 int Options,
636 template<typename, int> class JointCollectionTpl,
637 typename Matrix3xLike>
638 32 void jacobianSubtreeCenterOfMass(
639 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
640 DataTpl<Scalar, Options, JointCollectionTpl> & data,
641 const JointIndex & rootSubtreeId,
642 const Eigen::MatrixBase<Matrix3xLike> & res)
643 {
644
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 32 impl::jacobianSubtreeCenterOfMass(model, data, rootSubtreeId, make_ref(res));
645 32 }
646
647 template<
648 typename Scalar,
649 int Options,
650 template<typename, int> class JointCollectionTpl,
651 typename Matrix3xLike>
652 29 void getJacobianSubtreeCenterOfMass(
653 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
654 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
655 const JointIndex & rootSubtreeId,
656 const Eigen::MatrixBase<Matrix3xLike> & res)
657 {
658
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
 29 impl::getJacobianSubtreeCenterOfMass(model, data, rootSubtreeId, make_ref(res));
659 29 }
660
661 } // namespace pinocchio
662
663 /// @endcond
664
665 #endif // ifndef __pinocchio_algorithm_center_of_mass_hxx__
666