GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/centroidal.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 180 180 100.0%
Branches: 224 576 38.9%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2023 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_centroidal_hxx__
6 #define __pinocchio_algorithm_centroidal_hxx__
7
8 #include "pinocchio/multibody/visitor.hpp"
9 #include "pinocchio/spatial/act-on-set.hpp"
10 #include "pinocchio/algorithm/kinematics.hpp"
11 #include "pinocchio/algorithm/check.hpp"
12
13 /// @cond DEV
14
15 namespace pinocchio
16 {
17
18 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
19 4 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force & computeCentroidalMomentum(
20 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
21 DataTpl<Scalar, Options, JointCollectionTpl> & data)
22 {
23
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 assert(model.check(data) && "data is not consistent with model.");
24
25 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
26 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
27 typedef typename Model::JointIndex JointIndex;
28
29
2/2
✓ Branch 0 taken 112 times.
✓ Branch 1 taken 4 times.
 116 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
30 {
31 112 const typename Data::Inertia & Y = model.inertias[i];
32
33
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 112 data.mass[i] = Y.mass();
34
2/4
✓ Branch 3 taken 112 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 112 times.
✗ Branch 8 not taken.
 112 data.com[i] = Y.mass() * Y.lever();
35
36
1/2
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
 112 data.h[i] = Y * data.v[i];
37 }
38
39
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 4 data.mass[0] = 0.;
40 4 data.com[0].setZero();
41 4 data.h[0].setZero();
42
2/2
✓ Branch 0 taken 112 times.
✓ Branch 1 taken 4 times.
 116 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
43 {
44 112 const JointIndex & parent = model.parents[i];
45 112 const typename Data::SE3 & liMi = data.liMi[i];
46
47 112 data.mass[parent] += data.mass[i];
48
4/8
✓ Branch 5 taken 112 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 112 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 112 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 112 times.
✗ Branch 16 not taken.
 112 data.com[parent] += liMi.rotation() * data.com[i] + data.mass[i] * liMi.translation();
49
1/2
✓ Branch 5 taken 112 times.
✗ Branch 6 not taken.
 112 data.h[parent] += data.liMi[i].act(data.h[i]);
50 }
51
52 4 data.com[0] /= data.mass[0];
53
54 4 data.hg = data.h[0];
55
3/6
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 4 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
 4 data.hg.angular() += data.hg.linear().cross(data.com[0]);
56
57
3/6
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 4 times.
✗ Branch 11 not taken.
 4 data.vcom[0].noalias() = data.hg.linear() / data.mass[0];
58
59 4 return data.hg;
60 }
61
62 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
63 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force &
64 110 computeCentroidalMomentumTimeVariation(
65 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
66 DataTpl<Scalar, Options, JointCollectionTpl> & data)
67 {
68
1/2
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
 110 assert(model.check(data) && "data is not consistent with model.");
69
70 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
71 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
72 typedef typename Model::JointIndex JointIndex;
73
74
2/2
✓ Branch 0 taken 3079 times.
✓ Branch 1 taken 110 times.
 3189 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
75 {
76 3079 const typename Data::Inertia & Y = model.inertias[i];
77
78
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 3079 data.mass[i] = Y.mass();
79
2/4
✓ Branch 3 taken 3079 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 3079 times.
✗ Branch 8 not taken.
 3079 data.com[i] = Y.mass() * Y.lever();
80
81
1/2
✓ Branch 4 taken 3079 times.
✗ Branch 5 not taken.
 3079 data.h[i] = Y * data.v[i];
82
3/6
✓ Branch 5 taken 3079 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3079 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 3079 times.
✗ Branch 13 not taken.
 3079 data.f[i] = Y * data.a[i] + data.v[i].cross(data.h[i]);
83 }
84
85
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 110 data.mass[0] = 0.;
86 110 data.com[0].setZero();
87 110 data.h[0].setZero();
88 110 data.f[0].setZero();
89
90
2/2
✓ Branch 0 taken 3079 times.
✓ Branch 1 taken 110 times.
 3189 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
91 {
92 3079 const JointIndex & parent = model.parents[i];
93 3079 const typename Data::SE3 & liMi = data.liMi[i];
94
95 3079 data.mass[parent] += data.mass[i];
96
4/8
✓ Branch 5 taken 3079 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3079 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3079 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 3079 times.
✗ Branch 16 not taken.
 3079 data.com[parent] += liMi.rotation() * data.com[i] + data.mass[i] * liMi.translation();
97
1/2
✓ Branch 5 taken 3079 times.
✗ Branch 6 not taken.
 3079 data.h[parent] += data.liMi[i].act(data.h[i]);
98
1/2
✓ Branch 5 taken 3079 times.
✗ Branch 6 not taken.
 3079 data.f[parent] += data.liMi[i].act(data.f[i]);
99 }
100
101 110 data.com[0] /= data.mass[0];
102
103 110 data.hg = data.h[0];
104
3/6
✓ Branch 3 taken 110 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 110 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 110 times.
✗ Branch 10 not taken.
 110 data.hg.angular() += data.hg.linear().cross(data.com[0]);
105
106 110 data.dhg = data.f[0];
107
3/6
✓ Branch 3 taken 110 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 110 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 110 times.
✗ Branch 10 not taken.
 110 data.dhg.angular() += data.dhg.linear().cross(data.com[0]);
108
109
3/6
✓ Branch 3 taken 110 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 110 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 110 times.
✗ Branch 11 not taken.
 110 data.vcom[0].noalias() = data.hg.linear() / data.mass[0];
110
111 110 return data.dhg;
112 }
113 namespace impl
114 {
115 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
116 struct CcrbaBackwardStep
117 : public fusion::JointUnaryVisitorBase<CcrbaBackwardStep<Scalar, Options, JointCollectionTpl>>
118 {
119 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
120 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
121
122 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
123
124 template<typename JointModel>
125 3544 static void algo(
126 const JointModelBase<JointModel> & jmodel,
127 JointDataBase<typename JointModel::JointDataDerived> & jdata,
128 const Model & model,
129 Data & data)
130 {
131 typedef typename Model::JointIndex JointIndex;
132 typedef
133 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
134 ColsBlock;
135
136
1/2
✓ Branch 1 taken 1772 times.
✗ Branch 2 not taken.
 3544 const JointIndex & i = jmodel.id();
137 3544 const JointIndex & parent = model.parents[i];
138
139
1/2
✓ Branch 1 taken 1772 times.
✗ Branch 2 not taken.
 3544 ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J);
140
3/6
✓ Branch 2 taken 1772 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1772 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1772 times.
✗ Branch 9 not taken.
 3544 J_cols = data.oMi[i].act(jdata.S());
141
142
1/2
✓ Branch 1 taken 1772 times.
✗ Branch 2 not taken.
 3544 ColsBlock Ag_cols = jmodel.jointCols(data.Ag);
143 // Mimic joint contributes to the inertia of their mimicked and don't have their own
144
1/2
✓ Branch 2 taken 1772 times.
✗ Branch 3 not taken.
 3544 motionSet::inertiaAction<ADDTO>(data.oYcrb[i], J_cols, Ag_cols);
145
146
1/2
✓ Branch 3 taken 1772 times.
✗ Branch 4 not taken.
 3544 data.oYcrb[parent] += data.oYcrb[i];
147 }
148
149 }; // struct CcrbaBackwardStep
150
151 template<
152 typename Scalar,
153 int Options,
154 template<typename, int> class JointCollectionTpl,
155 typename ConfigVectorType,
156 typename TangentVectorType>
157 62 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & ccrba(
158 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
159 DataTpl<Scalar, Options, JointCollectionTpl> & data,
160 const Eigen::MatrixBase<ConfigVectorType> & q,
161 const Eigen::MatrixBase<TangentVectorType> & v)
162 {
163
2/4
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 62 times.
✗ Branch 4 not taken.
 62 assert(model.check(data) && "data is not consistent with model.");
164
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 62 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.
 62 PINOCCHIO_CHECK_ARGUMENT_SIZE(
165 q.size(), model.nq, "The configuration vector is not of right size");
166
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 62 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.
 62 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
167
168 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
169 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
170 typedef typename Model::JointIndex JointIndex;
171
172
1/2
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
 62 ::pinocchio::impl::forwardKinematics(model, data, q);
173
1/2
✓ Branch 2 taken 62 times.
✗ Branch 3 not taken.
 62 data.oYcrb[0].setZero();
174
2/2
✓ Branch 0 taken 1717 times.
✓ Branch 1 taken 62 times.
 1779 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
175
2/4
✓ Branch 3 taken 1717 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1717 times.
✗ Branch 8 not taken.
 1717 data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
176
177 typedef CcrbaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
178 // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to
179 // take mimic joint into account
180
1/2
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
 62 data.Ag.setZero();
181
2/2
✓ Branch 0 taken 1717 times.
✓ Branch 1 taken 62 times.
 1779 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
182 {
183
2/4
✓ Branch 1 taken 1717 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 1717 times.
✗ Branch 7 not taken.
 1717 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data));
184 }
185
186 // Express the centroidal map around the center of mass
187
1/2
✓ Branch 4 taken 62 times.
✗ Branch 5 not taken.
 62 data.com[0] = data.oYcrb[0].lever();
188
189 typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x;
190
1/2
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
 62 const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
191
1/2
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
 62 Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
192
2/2
✓ Branch 0 taken 2105 times.
✓ Branch 1 taken 62 times.
 2167 for (Eigen::DenseIndex i = 0; i < model.nv; ++i)
193
4/8
✓ Branch 1 taken 2105 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2105 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2105 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2105 times.
✗ Branch 12 not taken.
 2105 Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
194
195
4/8
✓ Branch 1 taken 62 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 62 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 62 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 62 times.
✗ Branch 11 not taken.
 62 data.hg.toVector().noalias() = data.Ag * v;
196
197
0/2
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 62 data.Ig.mass() = data.oYcrb[0].mass();
198
1/2
✓ Branch 2 taken 62 times.
✗ Branch 3 not taken.
 62 data.Ig.lever().setZero();
199
1/2
✓ Branch 4 taken 62 times.
✗ Branch 5 not taken.
 62 data.Ig.inertia() = data.oYcrb[0].inertia();
200
201 62 return data.Ag;
202 }
203
204 template<
205 typename Scalar,
206 int Options,
207 template<typename, int> class JointCollectionTpl,
208 typename ConfigVectorType>
209 2 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeCentroidalMap(
210 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
211 DataTpl<Scalar, Options, JointCollectionTpl> & data,
212 const Eigen::MatrixBase<ConfigVectorType> & q)
213 {
214
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 assert(model.check(data) && "data is not consistent with model.");
215
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 2 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.
 2 PINOCCHIO_CHECK_ARGUMENT_SIZE(
216 q.size(), model.nq, "The configuration vector is not of right size");
217
218 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
219 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
220 typedef typename Model::JointIndex JointIndex;
221
222
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 ::pinocchio::impl::forwardKinematics(model, data, q);
223
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 data.oYcrb[0].setZero();
224
2/2
✓ Branch 0 taken 55 times.
✓ Branch 1 taken 2 times.
 57 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
225
2/4
✓ Branch 3 taken 55 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 55 times.
✗ Branch 8 not taken.
 55 data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
226
227 typedef CcrbaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
228 // Set to zero, because it's not an assignation, that is done in the algorithm but a sum to
229 // take mimic joint into account
230
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 data.Ag.setZero();
231
2/2
✓ Branch 0 taken 55 times.
✓ Branch 1 taken 2 times.
 57 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
232 {
233
2/4
✓ Branch 1 taken 55 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 55 times.
✗ Branch 7 not taken.
 55 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data));
234 }
235
236 // Express the centroidal map around the center of mass
237
1/2
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 data.com[0] = data.oYcrb[0].lever();
238
239 typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x;
240
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
241
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
242
2/2
✓ Branch 0 taken 67 times.
✓ Branch 1 taken 2 times.
 69 for (long i = 0; i < model.nv; ++i)
243
4/8
✓ Branch 1 taken 67 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 67 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 67 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 67 times.
✗ Branch 12 not taken.
 67 Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
244
245 2 return data.Ag;
246 }
247
248 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
249 struct DCcrbaBackwardStep
250 : public fusion::JointUnaryVisitorBase<DCcrbaBackwardStep<Scalar, Options, JointCollectionTpl>>
251 {
252 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
253 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
254
255 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
256
257 template<typename JointModel>
258 714 static void algo(
259 const JointModelBase<JointModel> & jmodel,
260 JointDataBase<typename JointModel::JointDataDerived> & jdata,
261 const Model & model,
262 Data & data)
263 {
264 typedef typename Model::JointIndex JointIndex;
265 typedef typename Data::Inertia Inertia;
266 typedef
267 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
268 ColsBlock;
269
270
1/2
✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
 714 const JointIndex & i = jmodel.id();
271 714 const JointIndex & parent = model.parents[i];
272 714 const Inertia & Y = data.oYcrb[i];
273 714 const typename Inertia::Matrix6 & doYcrb = data.doYcrb[i];
274
275
1/2
✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
 714 ColsBlock J_cols = jmodel.jointExtendedModelCols(data.J);
276
3/6
✓ Branch 2 taken 357 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 357 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 357 times.
✗ Branch 9 not taken.
 714 J_cols = data.oMi[i].act(jdata.S());
277
278
1/2
✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
 714 ColsBlock dJ_cols = jmodel.jointExtendedModelCols(data.dJ);
279
1/2
✓ Branch 2 taken 357 times.
✗ Branch 3 not taken.
 714 motionSet::motionAction(data.ov[i], J_cols, dJ_cols);
280
281
1/2
✓ Branch 2 taken 357 times.
✗ Branch 3 not taken.
 714 data.oYcrb[parent] += Y;
282
2/2
✓ Branch 0 taken 344 times.
✓ Branch 1 taken 13 times.
 714 if (parent > 0)
283
1/2
✓ Branch 2 taken 344 times.
✗ Branch 3 not taken.
 688 data.doYcrb[parent] += doYcrb;
284
285 // Calc Ag
286
1/2
✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
 714 ColsBlock Ag_cols = jmodel.jointCols(data.Ag);
287
1/2
✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
 714 motionSet::inertiaAction(Y, J_cols, Ag_cols);
288
289 // Calc dAg = Ivx + vxI
290
1/2
✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
 714 ColsBlock dAg_cols = jmodel.jointCols(data.dAg);
291
3/6
✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 357 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 357 times.
✗ Branch 8 not taken.
 714 dAg_cols.noalias() = doYcrb * J_cols;
292
1/2
✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
 714 motionSet::inertiaAction<ADDTO>(Y, dJ_cols, dAg_cols);
293 }
294
295 }; // struct DCcrbaBackwardStep
296
297 template<
298 typename Scalar,
299 int Options,
300 template<typename, int> class JointCollectionTpl,
301 typename ConfigVectorType,
302 typename TangentVectorType>
303 12 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & dccrba(
304 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
305 DataTpl<Scalar, Options, JointCollectionTpl> & data,
306 const Eigen::MatrixBase<ConfigVectorType> & q,
307 const Eigen::MatrixBase<TangentVectorType> & v)
308 {
309
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 assert(model.check(data) && "data is not consistent with model.");
310
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
311
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 12 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.
 12 PINOCCHIO_CHECK_ARGUMENT_SIZE(
312 q.size(), model.nq, "The configuration vector is not of right size");
313
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 12 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.
 12 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
314
315 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
316 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
317 typedef typename Model::JointIndex JointIndex;
318
319
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 ::pinocchio::impl::forwardKinematics(model, data, q, v);
320
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 data.oYcrb[0].setZero();
321
2/2
✓ Branch 0 taken 330 times.
✓ Branch 1 taken 12 times.
 342 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
322 {
323
2/4
✓ Branch 3 taken 330 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 330 times.
✗ Branch 8 not taken.
 330 data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
324
2/4
✓ Branch 3 taken 330 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 330 times.
✗ Branch 8 not taken.
 330 data.ov[i] = data.oMi[i].act(data.v[i]); // v_i expressed in the world frame
325
1/2
✓ Branch 3 taken 330 times.
✗ Branch 4 not taken.
 330 data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]);
326 }
327
328 typedef DCcrbaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
329
2/2
✓ Branch 0 taken 330 times.
✓ Branch 1 taken 12 times.
 342 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
330 {
331
2/4
✓ Branch 1 taken 330 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 330 times.
✗ Branch 7 not taken.
 330 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data));
332 }
333
334 // Express the centroidal map around the center of mass
335
1/2
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 data.com[0] = data.oYcrb[0].lever();
336
337 typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x;
338
339
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
340
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
341
2/2
✓ Branch 0 taken 402 times.
✓ Branch 1 taken 12 times.
 414 for (Eigen::DenseIndex i = 0; i < model.nv; ++i)
342
4/8
✓ Branch 1 taken 402 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 402 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 402 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 402 times.
✗ Branch 12 not taken.
 402 Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
343
344
4/8
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
 12 data.hg.toVector().noalias() = data.Ag * v;
345
4/8
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 12 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
 12 data.vcom[0].noalias() = data.hg.linear() / data.oYcrb[0].mass();
346
347
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 const Block3x dAg_lin = data.dAg.template middleRows<3>(Force::LINEAR);
348
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 Block3x dAg_ang = data.dAg.template middleRows<3>(Force::ANGULAR);
349
2/2
✓ Branch 0 taken 402 times.
✓ Branch 1 taken 12 times.
 414 for (Eigen::DenseIndex i = 0; i < model.nv; ++i)
350
7/14
✓ Branch 1 taken 402 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 402 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 402 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 402 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 402 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 402 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 402 times.
✗ Branch 22 not taken.
 402 dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]) + Ag_lin.col(i).cross(data.vcom[0]);
351
352
0/2
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 12 data.Ig.mass() = data.oYcrb[0].mass();
353
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 data.Ig.lever().setZero();
354
1/2
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 data.Ig.inertia() = data.oYcrb[0].inertia();
355
356 12 return data.dAg;
357 }
358
359 template<
360 typename Scalar,
361 int Options,
362 template<typename, int> class JointCollectionTpl,
363 typename ConfigVectorType,
364 typename TangentVectorType>
365 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x &
366 1 computeCentroidalMapTimeVariation(
367 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
368 DataTpl<Scalar, Options, JointCollectionTpl> & data,
369 const Eigen::MatrixBase<ConfigVectorType> & q,
370 const Eigen::MatrixBase<TangentVectorType> & v)
371 {
372
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 assert(model.check(data) && "data is not consistent with model.");
373
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 1 assert(model.check(MimicChecker()) && "Function does not support mimic joints");
374
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
 1 PINOCCHIO_CHECK_ARGUMENT_SIZE(
375 q.size(), model.nq, "The configuration vector is not of right size");
376
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 1 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.
 1 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
377
378 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
379 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
380 typedef typename Model::JointIndex JointIndex;
381
382
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ::pinocchio::impl::forwardKinematics(model, data, q, v);
383
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 data.oYcrb[0].setZero();
384
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
385 {
386
2/4
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
 27 data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
387
2/4
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 27 times.
✗ Branch 8 not taken.
 27 data.ov[i] = data.oMi[i].act(data.v[i]); // v_i expressed in the world frame
388
1/2
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
 27 data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]);
389 }
390
391 typedef DCcrbaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
392
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
393 {
394
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 27 times.
✗ Branch 7 not taken.
 27 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data));
395 }
396
397 // Express the centroidal map around the center of mass
398
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 data.com[0] = data.oYcrb[0].lever();
399
400 typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x;
401
402
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
403
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
404
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 33 for (Eigen::DenseIndex i = 0; i < model.nv; ++i)
405
4/8
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 32 times.
✗ Branch 12 not taken.
 32 Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
406
407 // Express the centroidal time derivative map around the center of mass
408
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const Block3x dAg_lin = data.dAg.template middleRows<3>(Force::LINEAR);
409
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Block3x dAg_ang = data.dAg.template middleRows<3>(Force::ANGULAR);
410
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 33 for (Eigen::DenseIndex i = 0; i < model.nv; ++i)
411 {
412
7/14
✓ Branch 1 taken 32 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 32 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 32 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 32 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 32 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 32 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 32 times.
✗ Branch 22 not taken.
 32 dAg_ang.col(i) += dAg_lin.col(i).cross(data.com[0]) + Ag_lin.col(i).cross(data.vcom[0]);
413 }
414
415 1 return data.dAg;
416 }
417
418 template<
419 typename Scalar,
420 int Options,
421 template<typename, int> class JointCollectionTpl,
422 typename ConfigVectorType,
423 typename TangentVectorType>
424 3 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force & computeCentroidalMomentum(
425 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
426 DataTpl<Scalar, Options, JointCollectionTpl> & data,
427 const Eigen::MatrixBase<ConfigVectorType> & q,
428 const Eigen::MatrixBase<TangentVectorType> & v)
429 {
430 3 ::pinocchio::impl::forwardKinematics(model, data, q.derived(), v.derived());
431 3 return ::pinocchio::computeCentroidalMomentum(model, data);
432 }
433
434 template<
435 typename Scalar,
436 int Options,
437 template<typename, int> class JointCollectionTpl,
438 typename ConfigVectorType,
439 typename TangentVectorType1,
440 typename TangentVectorType2>
441 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force &
442 109 computeCentroidalMomentumTimeVariation(
443 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
444 DataTpl<Scalar, Options, JointCollectionTpl> & data,
445 const Eigen::MatrixBase<ConfigVectorType> & q,
446 const Eigen::MatrixBase<TangentVectorType1> & v,
447 const Eigen::MatrixBase<TangentVectorType2> & a)
448 {
449 109 ::pinocchio::impl::forwardKinematics(model, data, q, v, a);
450 109 return computeCentroidalMomentumTimeVariation(model, data);
451 }
452 } // namespace impl
453
454 template<
455 typename Scalar,
456 int Options,
457 template<typename, int> class JointCollectionTpl,
458 typename ConfigVectorType,
459 typename TangentVectorType>
460 3 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force & computeCentroidalMomentum(
461 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
462 DataTpl<Scalar, Options, JointCollectionTpl> & data,
463 const Eigen::MatrixBase<ConfigVectorType> & q,
464 const Eigen::MatrixBase<TangentVectorType> & v)
465 {
466
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 return impl::computeCentroidalMomentum(model, data, make_const_ref(q), make_const_ref(v));
467 }
468
469 template<
470 typename Scalar,
471 int Options,
472 template<typename, int> class JointCollectionTpl,
473 typename ConfigVectorType,
474 typename TangentVectorType1,
475 typename TangentVectorType2>
476 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force &
477 109 computeCentroidalMomentumTimeVariation(
478 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
479 DataTpl<Scalar, Options, JointCollectionTpl> & data,
480 const Eigen::MatrixBase<ConfigVectorType> & q,
481 const Eigen::MatrixBase<TangentVectorType1> & v,
482 const Eigen::MatrixBase<TangentVectorType2> & a)
483 {
484
3/6
✓ Branch 2 taken 109 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 109 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 109 times.
✗ Branch 9 not taken.
 218 return impl::computeCentroidalMomentumTimeVariation(
485 218 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a));
486 }
487
488 template<
489 typename Scalar,
490 int Options,
491 template<typename, int> class JointCollectionTpl,
492 typename ConfigVectorType,
493 typename TangentVectorType>
494 62 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & ccrba(
495 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
496 DataTpl<Scalar, Options, JointCollectionTpl> & data,
497 const Eigen::MatrixBase<ConfigVectorType> & q,
498 const Eigen::MatrixBase<TangentVectorType> & v)
499 {
500
2/4
✓ Branch 2 taken 62 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 62 times.
✗ Branch 6 not taken.
 62 return impl::ccrba(model, data, make_const_ref(q), make_const_ref(v));
501 }
502
503 template<
504 typename Scalar,
505 int Options,
506 template<typename, int> class JointCollectionTpl,
507 typename ConfigVectorType>
508 2 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeCentroidalMap(
509 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
510 DataTpl<Scalar, Options, JointCollectionTpl> & data,
511 const Eigen::MatrixBase<ConfigVectorType> & q)
512 {
513
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 return impl::computeCentroidalMap(model, data, make_const_ref(q));
514 }
515
516 template<
517 typename Scalar,
518 int Options,
519 template<typename, int> class JointCollectionTpl,
520 typename ConfigVectorType,
521 typename TangentVectorType>
522 19 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & dccrba(
523 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
524 DataTpl<Scalar, Options, JointCollectionTpl> & data,
525 const Eigen::MatrixBase<ConfigVectorType> & q,
526 const Eigen::MatrixBase<TangentVectorType> & v)
527 {
528
2/4
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
 19 return impl::dccrba(model, data, make_const_ref(q), make_const_ref(v));
529 }
530
531 template<
532 typename Scalar,
533 int Options,
534 template<typename, int> class JointCollectionTpl,
535 typename ConfigVectorType,
536 typename TangentVectorType>
537 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x &
538 1 computeCentroidalMapTimeVariation(
539 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
540 DataTpl<Scalar, Options, JointCollectionTpl> & data,
541 const Eigen::MatrixBase<ConfigVectorType> & q,
542 const Eigen::MatrixBase<TangentVectorType> & v)
543 {
544
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 return impl::computeCentroidalMapTimeVariation(
545 2 model, data, make_const_ref(q), make_const_ref(v));
546 }
547
548 } // namespace pinocchio
549
550 /// @endcond
551
552 #endif // ifndef __pinocchio_algorithm_centroidal_hxx__
553