GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/centroidal.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 176 176 100.0%
Branches: 218 564 38.7%
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 2896 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 1448 times.
✗ Branch 2 not taken.
 2896 const JointIndex & i = jmodel.id();
137 2896 const JointIndex & parent = model.parents[i];
138
139
1/2
✓ Branch 1 taken 1448 times.
✗ Branch 2 not taken.
 2896 ColsBlock J_cols = jmodel.jointCols(data.J);
140
3/6
✓ Branch 2 taken 1448 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1448 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1448 times.
✗ Branch 9 not taken.
 2896 J_cols = data.oMi[i].act(jdata.S());
141
142
1/2
✓ Branch 1 taken 1448 times.
✗ Branch 2 not taken.
 2896 ColsBlock Ag_cols = jmodel.jointCols(data.Ag);
143
1/2
✓ Branch 2 taken 1448 times.
✗ Branch 3 not taken.
 2896 motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols);
144
1/2
✓ Branch 3 taken 1448 times.
✗ Branch 4 not taken.
 2896 data.oYcrb[parent] += data.oYcrb[i];
145 }
146
147 }; // struct CcrbaBackwardStep
148
149 template<
150 typename Scalar,
151 int Options,
152 template<typename, int> class JointCollectionTpl,
153 typename ConfigVectorType,
154 typename TangentVectorType>
155 50 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & ccrba(
156 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
157 DataTpl<Scalar, Options, JointCollectionTpl> & data,
158 const Eigen::MatrixBase<ConfigVectorType> & q,
159 const Eigen::MatrixBase<TangentVectorType> & v)
160 {
161
2/4
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
 50 assert(model.check(data) && "data is not consistent with model.");
162
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 50 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.
 50 PINOCCHIO_CHECK_ARGUMENT_SIZE(
163 q.size(), model.nq, "The configuration vector is not of right size");
164
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 50 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.
 50 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
165
166 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
167 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
168 typedef typename Model::JointIndex JointIndex;
169
170
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 ::pinocchio::impl::forwardKinematics(model, data, q);
171
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 data.oYcrb[0].setZero();
172
2/2
✓ Branch 0 taken 1393 times.
✓ Branch 1 taken 50 times.
 1443 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
173
2/4
✓ Branch 3 taken 1393 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1393 times.
✗ Branch 8 not taken.
 1393 data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
174
175 typedef CcrbaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
176
2/2
✓ Branch 0 taken 1393 times.
✓ Branch 1 taken 50 times.
 1443 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
177 {
178
2/4
✓ Branch 1 taken 1393 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 1393 times.
✗ Branch 7 not taken.
 1393 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data));
179 }
180
181 // Express the centroidal map around the center of mass
182
1/2
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 50 data.com[0] = data.oYcrb[0].lever();
183
184 typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x;
185
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
186
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
187
2/2
✓ Branch 0 taken 1729 times.
✓ Branch 1 taken 50 times.
 1779 for (Eigen::DenseIndex i = 0; i < model.nv; ++i)
188
4/8
✓ Branch 1 taken 1729 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1729 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1729 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1729 times.
✗ Branch 12 not taken.
 1729 Ag_ang.col(i) += Ag_lin.col(i).cross(data.com[0]);
189
190
4/8
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
 50 data.hg.toVector().noalias() = data.Ag * v;
191
192
0/2
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 50 data.Ig.mass() = data.oYcrb[0].mass();
193
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 50 data.Ig.lever().setZero();
194
1/2
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 50 data.Ig.inertia() = data.oYcrb[0].inertia();
195
196 50 return data.Ag;
197 }
198
199 template<
200 typename Scalar,
201 int Options,
202 template<typename, int> class JointCollectionTpl,
203 typename ConfigVectorType>
204 2 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeCentroidalMap(
205 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
206 DataTpl<Scalar, Options, JointCollectionTpl> & data,
207 const Eigen::MatrixBase<ConfigVectorType> & q)
208 {
209
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.");
210
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(
211 q.size(), model.nq, "The configuration vector is not of right size");
212
213 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
214 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
215 typedef typename Model::JointIndex JointIndex;
216
217
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 ::pinocchio::impl::forwardKinematics(model, data, q);
218
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 data.oYcrb[0].setZero();
219
2/2
✓ Branch 0 taken 55 times.
✓ Branch 1 taken 2 times.
 57 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
220
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]);
221
222 typedef CcrbaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
223
2/2
✓ Branch 0 taken 55 times.
✓ Branch 1 taken 2 times.
 57 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
224 {
225
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));
226 }
227
228 // Express the centroidal map around the center of mass
229
1/2
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 data.com[0] = data.oYcrb[0].lever();
230
231 typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x;
232
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
233
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
234
2/2
✓ Branch 0 taken 67 times.
✓ Branch 1 taken 2 times.
 69 for (long i = 0; i < model.nv; ++i)
235
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]);
236
237 2 return data.Ag;
238 }
239
240 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
241 struct DCcrbaBackwardStep
242 : public fusion::JointUnaryVisitorBase<DCcrbaBackwardStep<Scalar, Options, JointCollectionTpl>>
243 {
244 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
245 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
246
247 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
248
249 template<typename JointModel>
250 714 static void algo(
251 const JointModelBase<JointModel> & jmodel,
252 JointDataBase<typename JointModel::JointDataDerived> & jdata,
253 const Model & model,
254 Data & data)
255 {
256 typedef typename Model::JointIndex JointIndex;
257 typedef typename Data::Inertia Inertia;
258 typedef
259 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
260 ColsBlock;
261
262
1/2
✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
 714 const JointIndex & i = jmodel.id();
263 714 const JointIndex & parent = model.parents[i];
264 714 const Inertia & Y = data.oYcrb[i];
265 714 const typename Inertia::Matrix6 & doYcrb = data.doYcrb[i];
266
267
1/2
✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
 714 ColsBlock J_cols = jmodel.jointCols(data.J);
268
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());
269
270
1/2
✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
 714 ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
271
1/2
✓ Branch 2 taken 357 times.
✗ Branch 3 not taken.
 714 motionSet::motionAction(data.ov[i], J_cols, dJ_cols);
272
273
1/2
✓ Branch 2 taken 357 times.
✗ Branch 3 not taken.
 714 data.oYcrb[parent] += Y;
274
2/2
✓ Branch 0 taken 344 times.
✓ Branch 1 taken 13 times.
 714 if (parent > 0)
275
1/2
✓ Branch 2 taken 344 times.
✗ Branch 3 not taken.
 688 data.doYcrb[parent] += doYcrb;
276
277 // Calc Ag
278
1/2
✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
 714 ColsBlock Ag_cols = jmodel.jointCols(data.Ag);
279
1/2
✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
 714 motionSet::inertiaAction(Y, J_cols, Ag_cols);
280
281 // Calc dAg = Ivx + vxI
282
1/2
✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
 714 ColsBlock dAg_cols = jmodel.jointCols(data.dAg);
283
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;
284
1/2
✓ Branch 1 taken 357 times.
✗ Branch 2 not taken.
 714 motionSet::inertiaAction<ADDTO>(Y, dJ_cols, dAg_cols);
285 }
286
287 }; // struct DCcrbaBackwardStep
288
289 template<
290 typename Scalar,
291 int Options,
292 template<typename, int> class JointCollectionTpl,
293 typename ConfigVectorType,
294 typename TangentVectorType>
295 12 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & dccrba(
296 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
297 DataTpl<Scalar, Options, JointCollectionTpl> & data,
298 const Eigen::MatrixBase<ConfigVectorType> & q,
299 const Eigen::MatrixBase<TangentVectorType> & v)
300 {
301
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.");
302
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(
303 q.size(), model.nq, "The configuration vector is not of right size");
304
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");
305
306 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
307 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
308 typedef typename Model::JointIndex JointIndex;
309
310
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 ::pinocchio::impl::forwardKinematics(model, data, q, v);
311
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 data.oYcrb[0].setZero();
312
2/2
✓ Branch 0 taken 330 times.
✓ Branch 1 taken 12 times.
 342 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
313 {
314
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]);
315
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
316
1/2
✓ Branch 3 taken 330 times.
✗ Branch 4 not taken.
 330 data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]);
317 }
318
319 typedef DCcrbaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
320
2/2
✓ Branch 0 taken 330 times.
✓ Branch 1 taken 12 times.
 342 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
321 {
322
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));
323 }
324
325 // Express the centroidal map around the center of mass
326
1/2
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 data.com[0] = data.oYcrb[0].lever();
327
328 typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x;
329
330
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
331
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
332
2/2
✓ Branch 0 taken 402 times.
✓ Branch 1 taken 12 times.
 414 for (Eigen::DenseIndex i = 0; i < model.nv; ++i)
333
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]);
334
335
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;
336
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();
337
338
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 const Block3x dAg_lin = data.dAg.template middleRows<3>(Force::LINEAR);
339
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 Block3x dAg_ang = data.dAg.template middleRows<3>(Force::ANGULAR);
340
2/2
✓ Branch 0 taken 402 times.
✓ Branch 1 taken 12 times.
 414 for (Eigen::DenseIndex i = 0; i < model.nv; ++i)
341
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]);
342
343
0/2
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 12 data.Ig.mass() = data.oYcrb[0].mass();
344
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 data.Ig.lever().setZero();
345
1/2
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 data.Ig.inertia() = data.oYcrb[0].inertia();
346
347 12 return data.dAg;
348 }
349
350 template<
351 typename Scalar,
352 int Options,
353 template<typename, int> class JointCollectionTpl,
354 typename ConfigVectorType,
355 typename TangentVectorType>
356 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x &
357 1 computeCentroidalMapTimeVariation(
358 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
359 DataTpl<Scalar, Options, JointCollectionTpl> & data,
360 const Eigen::MatrixBase<ConfigVectorType> & q,
361 const Eigen::MatrixBase<TangentVectorType> & v)
362 {
363
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.");
364
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(
365 q.size(), model.nq, "The configuration vector is not of right size");
366
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");
367
368 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
369 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
370 typedef typename Model::JointIndex JointIndex;
371
372
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ::pinocchio::impl::forwardKinematics(model, data, q, v);
373
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 data.oYcrb[0].setZero();
374
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
375 {
376
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]);
377
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
378
1/2
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
 27 data.doYcrb[i] = data.oYcrb[i].variation(data.ov[i]);
379 }
380
381 typedef DCcrbaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
382
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
383 {
384
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));
385 }
386
387 // Express the centroidal map around the center of mass
388
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 data.com[0] = data.oYcrb[0].lever();
389
390 typedef Eigen::Block<typename Data::Matrix6x, 3, -1> Block3x;
391
392
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const Block3x Ag_lin = data.Ag.template middleRows<3>(Force::LINEAR);
393
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Block3x Ag_ang = data.Ag.template middleRows<3>(Force::ANGULAR);
394
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 33 for (Eigen::DenseIndex i = 0; i < model.nv; ++i)
395
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]);
396
397 // Express the centroidal time derivative map around the center of mass
398
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const Block3x dAg_lin = data.dAg.template middleRows<3>(Force::LINEAR);
399
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Block3x dAg_ang = data.dAg.template middleRows<3>(Force::ANGULAR);
400
2/2
✓ Branch 0 taken 32 times.
✓ Branch 1 taken 1 times.
 33 for (Eigen::DenseIndex i = 0; i < model.nv; ++i)
401 {
402
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]);
403 }
404
405 1 return data.dAg;
406 }
407
408 template<
409 typename Scalar,
410 int Options,
411 template<typename, int> class JointCollectionTpl,
412 typename ConfigVectorType,
413 typename TangentVectorType>
414 3 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force & computeCentroidalMomentum(
415 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
416 DataTpl<Scalar, Options, JointCollectionTpl> & data,
417 const Eigen::MatrixBase<ConfigVectorType> & q,
418 const Eigen::MatrixBase<TangentVectorType> & v)
419 {
420 3 ::pinocchio::impl::forwardKinematics(model, data, q.derived(), v.derived());
421 3 return ::pinocchio::computeCentroidalMomentum(model, data);
422 }
423
424 template<
425 typename Scalar,
426 int Options,
427 template<typename, int> class JointCollectionTpl,
428 typename ConfigVectorType,
429 typename TangentVectorType1,
430 typename TangentVectorType2>
431 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force &
432 109 computeCentroidalMomentumTimeVariation(
433 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
434 DataTpl<Scalar, Options, JointCollectionTpl> & data,
435 const Eigen::MatrixBase<ConfigVectorType> & q,
436 const Eigen::MatrixBase<TangentVectorType1> & v,
437 const Eigen::MatrixBase<TangentVectorType2> & a)
438 {
439 109 ::pinocchio::impl::forwardKinematics(model, data, q, v, a);
440 109 return computeCentroidalMomentumTimeVariation(model, data);
441 }
442 } // namespace impl
443
444 template<
445 typename Scalar,
446 int Options,
447 template<typename, int> class JointCollectionTpl,
448 typename ConfigVectorType,
449 typename TangentVectorType>
450 3 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force & computeCentroidalMomentum(
451 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
452 DataTpl<Scalar, Options, JointCollectionTpl> & data,
453 const Eigen::MatrixBase<ConfigVectorType> & q,
454 const Eigen::MatrixBase<TangentVectorType> & v)
455 {
456
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));
457 }
458
459 template<
460 typename Scalar,
461 int Options,
462 template<typename, int> class JointCollectionTpl,
463 typename ConfigVectorType,
464 typename TangentVectorType1,
465 typename TangentVectorType2>
466 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Force &
467 109 computeCentroidalMomentumTimeVariation(
468 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
469 DataTpl<Scalar, Options, JointCollectionTpl> & data,
470 const Eigen::MatrixBase<ConfigVectorType> & q,
471 const Eigen::MatrixBase<TangentVectorType1> & v,
472 const Eigen::MatrixBase<TangentVectorType2> & a)
473 {
474
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(
475 218 model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a));
476 }
477
478 template<
479 typename Scalar,
480 int Options,
481 template<typename, int> class JointCollectionTpl,
482 typename ConfigVectorType,
483 typename TangentVectorType>
484 50 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & ccrba(
485 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
486 DataTpl<Scalar, Options, JointCollectionTpl> & data,
487 const Eigen::MatrixBase<ConfigVectorType> & q,
488 const Eigen::MatrixBase<TangentVectorType> & v)
489 {
490
2/4
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 50 times.
✗ Branch 6 not taken.
 50 return impl::ccrba(model, data, make_const_ref(q), make_const_ref(v));
491 }
492
493 template<
494 typename Scalar,
495 int Options,
496 template<typename, int> class JointCollectionTpl,
497 typename ConfigVectorType>
498 2 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeCentroidalMap(
499 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
500 DataTpl<Scalar, Options, JointCollectionTpl> & data,
501 const Eigen::MatrixBase<ConfigVectorType> & q)
502 {
503
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 return impl::computeCentroidalMap(model, data, make_const_ref(q));
504 }
505
506 template<
507 typename Scalar,
508 int Options,
509 template<typename, int> class JointCollectionTpl,
510 typename ConfigVectorType,
511 typename TangentVectorType>
512 19 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & dccrba(
513 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
514 DataTpl<Scalar, Options, JointCollectionTpl> & data,
515 const Eigen::MatrixBase<ConfigVectorType> & q,
516 const Eigen::MatrixBase<TangentVectorType> & v)
517 {
518
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));
519 }
520
521 template<
522 typename Scalar,
523 int Options,
524 template<typename, int> class JointCollectionTpl,
525 typename ConfigVectorType,
526 typename TangentVectorType>
527 const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x &
528 1 computeCentroidalMapTimeVariation(
529 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
530 DataTpl<Scalar, Options, JointCollectionTpl> & data,
531 const Eigen::MatrixBase<ConfigVectorType> & q,
532 const Eigen::MatrixBase<TangentVectorType> & v)
533 {
534
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 return impl::computeCentroidalMapTimeVariation(
535 2 model, data, make_const_ref(q), make_const_ref(v));
536 }
537
538 } // namespace pinocchio
539
540 /// @endcond
541
542 #endif // ifndef __pinocchio_algorithm_centroidal_hxx__
543