GCC Code Coverage Report

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