GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/rnea.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 228 228 100.0%
Branches: 336 925 36.3%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2022 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_rnea_hxx__
6 #define __pinocchio_algorithm_rnea_hxx__
7
8 /// @cond DEV
9
10 #include "pinocchio/multibody/visitor.hpp"
11 #include "pinocchio/algorithm/check.hpp"
12
13 namespace pinocchio
14 {
15 namespace impl
16 {
17 template<
18 typename Scalar,
19 int Options,
20 template<typename, int> class JointCollectionTpl,
21 typename ConfigVectorType,
22 typename TangentVectorType1,
23 typename TangentVectorType2>
24 struct RneaForwardStep
25 : public fusion::JointUnaryVisitorBase<RneaForwardStep<
26 Scalar,
27 Options,
28 JointCollectionTpl,
29 ConfigVectorType,
30 TangentVectorType1,
31 TangentVectorType2>>
32 {
33 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
34 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
35
36 typedef boost::fusion::vector<
37 const Model &,
38 Data &,
39 const ConfigVectorType &,
40 const TangentVectorType1 &,
41 const TangentVectorType2 &>
42 ArgsType;
43
44 template<typename JointModel>
45 21322 static void algo(
46 const JointModelBase<JointModel> & jmodel,
47 JointDataBase<typename JointModel::JointDataDerived> & jdata,
48 const Model & model,
49 Data & data,
50 const Eigen::MatrixBase<ConfigVectorType> & q,
51 const Eigen::MatrixBase<TangentVectorType1> & v,
52 const Eigen::MatrixBase<TangentVectorType2> & a)
53 {
54 typedef typename Model::JointIndex JointIndex;
55
56 21322 const JointIndex i = jmodel.id();
57 21322 const JointIndex parent = model.parents[i];
58
59 21322 jmodel.calc(jdata.derived(), q.derived(), v.derived());
60
61
3/5
✓ Branch 4 taken 10250 times.
✓ Branch 5 taken 411 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 10250 times.
✗ Branch 9 not taken.
 21322 data.liMi[i] = model.jointPlacements[i] * jdata.M();
62
63 21322 data.v[i] = jdata.v();
64
2/2
✓ Branch 0 taken 10234 times.
✓ Branch 1 taken 427 times.
 21322 if (parent > 0)
65
1/2
✓ Branch 5 taken 10234 times.
✗ Branch 6 not taken.
 20468 data.v[i] += data.liMi[i].actInv(data.v[parent]);
66
67
4/8
✓ Branch 4 taken 10661 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 10654 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
 21322 data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v());
68
3/6
✓ Branch 2 taken 10661 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10661 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 10661 times.
✗ Branch 10 not taken.
 21322 data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a);
69
1/2
✓ Branch 5 taken 10661 times.
✗ Branch 6 not taken.
 21322 data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
70 //
71 // data.f[i] = model.inertias[i]*data.a_gf[i];// + model.inertias[i].vxiv(data.v[i]);
72 // // -f_ext data.h[i] = model.inertias[i]*data.v[i];
73 21322 model.inertias[i].__mult__(data.v[i], data.h[i]);
74 21322 model.inertias[i].__mult__(data.a_gf[i], data.f[i]);
75
1/2
✓ Branch 5 taken 10661 times.
✗ Branch 6 not taken.
 21322 data.f[i] += data.v[i].cross(data.h[i]);
76 // data.h[i].motionAction(data.v[i],data.f[i]);
77 // data.f[i] = model.inertias[i].vxiv(data.v[i]);
78 // data.f[i].setZero();
79 }
80 };
81
82 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
83 struct RneaBackwardStep
84 : public fusion::JointUnaryVisitorBase<RneaBackwardStep<Scalar, Options, JointCollectionTpl>>
85 {
86 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
87 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
88
89 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
90
91 template<typename JointModel>
92 21322 static void algo(
93 const JointModelBase<JointModel> & jmodel,
94 JointDataBase<typename JointModel::JointDataDerived> & jdata,
95 const Model & model,
96 Data & data)
97 {
98 typedef typename Model::JointIndex JointIndex;
99
100 21322 const JointIndex i = jmodel.id();
101 21322 const JointIndex parent = model.parents[i];
102
103
3/6
✓ Branch 4 taken 10661 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10661 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10661 times.
✗ Branch 11 not taken.
 21322 jmodel.jointVelocitySelector(data.tau) = jdata.S().transpose() * data.f[i];
104
105
2/2
✓ Branch 0 taken 10234 times.
✓ Branch 1 taken 427 times.
 21322 if (parent > 0)
106
1/2
✓ Branch 5 taken 10234 times.
✗ Branch 6 not taken.
 20468 data.f[parent] += data.liMi[i].act(data.f[i]);
107 }
108 };
109
110 template<
111 typename Scalar,
112 int Options,
113 template<typename, int> class JointCollectionTpl,
114 typename ConfigVectorType,
115 typename TangentVectorType1,
116 typename TangentVectorType2>
117 316 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & rnea(
118 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
119 DataTpl<Scalar, Options, JointCollectionTpl> & data,
120 const Eigen::MatrixBase<ConfigVectorType> & q,
121 const Eigen::MatrixBase<TangentVectorType1> & v,
122 const Eigen::MatrixBase<TangentVectorType2> & a)
123 {
124
2/4
✓ Branch 1 taken 314 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 314 times.
✗ Branch 4 not taken.
 316 assert(model.check(data) && "data is not consistent with model.");
125
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 314 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.
 316 PINOCCHIO_CHECK_ARGUMENT_SIZE(
126 q.size(), model.nq, "The configuration vector is not of right size");
127
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 314 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.
 316 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
128
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 314 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.
 316 PINOCCHIO_CHECK_ARGUMENT_SIZE(
129 a.size(), model.nv, "The acceleration vector is not of right size");
130
131 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
132 typedef typename Model::JointIndex JointIndex;
133
134
1/2
✓ Branch 2 taken 314 times.
✗ Branch 3 not taken.
 316 data.v[0].setZero();
135
2/4
✓ Branch 1 taken 314 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 314 times.
✗ Branch 6 not taken.
 316 data.a_gf[0] = -model.gravity;
136
137 typedef RneaForwardStep<
138 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1,
139 TangentVectorType2>
140 Pass1;
141
1/2
✓ Branch 4 taken 314 times.
✗ Branch 5 not taken.
 316 typename Pass1::ArgsType arg1(model, data, q.derived(), v.derived(), a.derived());
142
2/2
✓ Branch 0 taken 7606 times.
✓ Branch 1 taken 314 times.
 7924 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
143 {
144
2/4
✓ Branch 1 taken 7606 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 7606 times.
✗ Branch 7 not taken.
 7608 Pass1::run(model.joints[i], data.joints[i], arg1);
145 }
146
147 typedef RneaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
148
1/2
✓ Branch 1 taken 314 times.
✗ Branch 2 not taken.
 316 typename Pass2::ArgsType arg2(model, data);
149
2/2
✓ Branch 0 taken 7606 times.
✓ Branch 1 taken 314 times.
 7924 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
150 {
151
2/4
✓ Branch 1 taken 7606 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 7606 times.
✗ Branch 7 not taken.
 7608 Pass2::run(model.joints[i], data.joints[i], arg2);
152 }
153
154 // Add rotorinertia contribution
155
5/10
✓ Branch 1 taken 314 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 314 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 314 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 314 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 314 times.
✗ Branch 14 not taken.
 316 data.tau.array() += model.armature.array() * a.array(); // Check if there is memory allocation
156
157 316 return data.tau;
158 }
159
160 template<
161 typename Scalar,
162 int Options,
163 template<typename, int> class JointCollectionTpl,
164 typename ConfigVectorType,
165 typename TangentVectorType1,
166 typename TangentVectorType2,
167 typename ForceDerived>
168 113 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & rnea(
169 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
170 DataTpl<Scalar, Options, JointCollectionTpl> & data,
171 const Eigen::MatrixBase<ConfigVectorType> & q,
172 const Eigen::MatrixBase<TangentVectorType1> & v,
173 const Eigen::MatrixBase<TangentVectorType2> & a,
174 const container::aligned_vector<ForceDerived> & fext)
175 {
176
1/24
✗ Branch 2 not taken.
✓ Branch 3 taken 113 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 113 PINOCCHIO_CHECK_ARGUMENT_SIZE(fext.size(), model.joints.size());
177
1/2
✓ Branch 1 taken 113 times.
✗ Branch 2 not taken.
 113 assert(model.check(data) && "data is not consistent with model.");
178
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 113 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.
 113 PINOCCHIO_CHECK_ARGUMENT_SIZE(
179 q.size(), model.nq, "The configuration vector is not of right size");
180
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 113 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.
 113 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
181
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 113 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.
 113 PINOCCHIO_CHECK_ARGUMENT_SIZE(
182 a.size(), model.nv, "The acceleration vector is not of right size");
183
184 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
185 typedef typename Model::JointIndex JointIndex;
186
187 113 data.v[0].setZero();
188
1/2
✓ Branch 3 taken 113 times.
✗ Branch 4 not taken.
 113 data.a_gf[0] = -model.gravity;
189
190 typedef RneaForwardStep<
191 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1,
192 TangentVectorType2>
193 Pass1;
194
2/2
✓ Branch 0 taken 3055 times.
✓ Branch 1 taken 113 times.
 3168 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
195 {
196
1/2
✓ Branch 1 taken 3055 times.
✗ Branch 2 not taken.
 3055 Pass1::run(
197 3055 model.joints[i], data.joints[i],
198 3055 typename Pass1::ArgsType(model, data, q.derived(), v.derived(), a.derived()));
199 3055 data.f[i] -= fext[i];
200 }
201
202 typedef RneaBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
203
2/2
✓ Branch 0 taken 3055 times.
✓ Branch 1 taken 113 times.
 3168 for (JointIndex i = (JointIndex)model.njoints - 1; i > 0; --i)
204 {
205
1/2
✓ Branch 4 taken 3055 times.
✗ Branch 5 not taken.
 3055 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data));
206 }
207
208 // Add armature contribution
209
3/6
✓ Branch 1 taken 113 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 113 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 113 times.
✗ Branch 8 not taken.
 113 data.tau.array() +=
210
1/2
✓ Branch 2 taken 113 times.
✗ Branch 3 not taken.
 113 model.armature.array() * a.array(); // TODO: check if there is memory allocation
211
212 113 return data.tau;
213 }
214
215 template<
216 typename Scalar,
217 int Options,
218 template<typename, int> class JointCollectionTpl,
219 typename ConfigVectorType,
220 typename TangentVectorType>
221 struct NLEForwardStep
222 : public fusion::JointUnaryVisitorBase<
223 NLEForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType>>
224 {
225 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
226 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
227
228 typedef boost::fusion::
229 vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &>
230 ArgsType;
231
232 template<typename JointModel>
233 702 static void algo(
234 const pinocchio::JointModelBase<JointModel> & jmodel,
235 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
236 const Model & model,
237 Data & data,
238 const Eigen::MatrixBase<ConfigVectorType> & q,
239 const Eigen::MatrixBase<TangentVectorType> & v)
240 {
241 typedef typename Model::JointIndex JointIndex;
242
243
1/2
✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
 702 const JointIndex & i = jmodel.id();
244 702 const JointIndex & parent = model.parents[i];
245
246
1/2
✓ Branch 4 taken 351 times.
✗ Branch 5 not taken.
 702 jmodel.calc(jdata.derived(), q.derived(), v.derived());
247
248
6/10
✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 338 times.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 338 times.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 338 times.
✗ Branch 13 not taken.
 702 data.liMi[i] = model.jointPlacements[i] * jdata.M();
249
250
2/4
✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 351 times.
✗ Branch 6 not taken.
 702 data.v[i] = jdata.v();
251
2/2
✓ Branch 0 taken 338 times.
✓ Branch 1 taken 13 times.
 702 if (parent > 0)
252
2/4
✓ Branch 3 taken 338 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 338 times.
✗ Branch 8 not taken.
 676 data.v[i] += data.liMi[i].actInv(data.v[parent]);
253
254
4/12
✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 351 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 351 times.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 351 times.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
 702 data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v());
255
2/4
✓ Branch 3 taken 351 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 351 times.
✗ Branch 8 not taken.
 702 data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);
256
257
4/8
✓ Branch 3 taken 351 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 351 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 351 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 351 times.
✗ Branch 16 not taken.
 702 data.f[i] = model.inertias[i] * data.a_gf[i] + model.inertias[i].vxiv(data.v[i]); // -f_ext
258 }
259 };
260
261 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
262 struct NLEBackwardStep
263 : public fusion::JointUnaryVisitorBase<NLEBackwardStep<Scalar, Options, JointCollectionTpl>>
264 {
265 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
266 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
267
268 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
269
270 template<typename JointModel>
271 702 static void algo(
272 const JointModelBase<JointModel> & jmodel,
273 JointDataBase<typename JointModel::JointDataDerived> & jdata,
274 const Model & model,
275 Data & data)
276 {
277 typedef typename Model::JointIndex JointIndex;
278
279
1/2
✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
 702 const JointIndex & i = jmodel.id();
280 702 const JointIndex & parent = model.parents[i];
281
282
8/13
✓ Branch 2 taken 351 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 338 times.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 338 times.
✓ Branch 9 taken 13 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 338 times.
✓ Branch 12 taken 13 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 338 times.
✗ Branch 15 not taken.
 702 jmodel.jointVelocitySelector(data.nle) = jdata.S().transpose() * data.f[i];
283
2/2
✓ Branch 0 taken 338 times.
✓ Branch 1 taken 13 times.
 702 if (parent > 0)
284
2/4
✓ Branch 3 taken 338 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 338 times.
✗ Branch 8 not taken.
 676 data.f[parent] += data.liMi[i].act(data.f[i]);
285 }
286 };
287
288 template<
289 typename Scalar,
290 int Options,
291 template<typename, int> class JointCollectionTpl,
292 typename ConfigVectorType,
293 typename TangentVectorType>
294 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &
295 13 nonLinearEffects(
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
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
 13 assert(model.check(data) && "data is not consistent with model.");
302
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 13 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.
 13 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 13 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.
 13 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 typename Model::JointIndex JointIndex;
308
309 13 data.v[0].setZero();
310
1/2
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
 13 data.a_gf[0] = -model.gravity;
311
312 typedef NLEForwardStep<
313 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType>
314 Pass1;
315
2/2
✓ Branch 0 taken 351 times.
✓ Branch 1 taken 13 times.
 364 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
316 {
317
1/2
✓ Branch 1 taken 351 times.
✗ Branch 2 not taken.
 351 Pass1::run(
318 351 model.joints[i], data.joints[i],
319 702 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
320 }
321
322 typedef NLEBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
323
2/2
✓ Branch 0 taken 351 times.
✓ Branch 1 taken 13 times.
 364 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
324 {
325
1/2
✓ Branch 4 taken 351 times.
✗ Branch 5 not taken.
 351 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data));
326 }
327
328 13 return data.nle;
329 }
330
331 template<
332 typename Scalar,
333 int Options,
334 template<typename, int> class JointCollectionTpl,
335 typename ConfigVectorType>
336 struct ComputeGeneralizedGravityForwardStep
337 : public fusion::JointUnaryVisitorBase<
338 ComputeGeneralizedGravityForwardStep<Scalar, Options, JointCollectionTpl, ConfigVectorType>>
339 {
340 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
341 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
342
343 typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &> ArgsType;
344
345 template<typename JointModel>
346 3996 static void algo(
347 const pinocchio::JointModelBase<JointModel> & jmodel,
348 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
349 const Model & model,
350 Data & data,
351 const Eigen::MatrixBase<ConfigVectorType> & q)
352 {
353 typedef typename Model::JointIndex JointIndex;
354
355
1/2
✓ Branch 1 taken 1998 times.
✗ Branch 2 not taken.
 3996 const JointIndex & i = jmodel.id();
356 3996 const JointIndex & parent = model.parents[i];
357
358
1/2
✓ Branch 3 taken 1998 times.
✗ Branch 4 not taken.
 3996 jmodel.calc(jdata.derived(), q.derived());
359
360
6/10
✓ Branch 1 taken 1998 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1924 times.
✓ Branch 5 taken 74 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1924 times.
✓ Branch 9 taken 74 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1924 times.
✗ Branch 13 not taken.
 3996 data.liMi[i] = model.jointPlacements[i] * jdata.M();
361
362
2/4
✓ Branch 3 taken 1998 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1998 times.
✗ Branch 8 not taken.
 3996 data.a_gf[i] = data.liMi[i].actInv(data.a_gf[(size_t)parent]);
363
2/4
✓ Branch 3 taken 1998 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1998 times.
✗ Branch 8 not taken.
 3996 data.f[i] = model.inertias[i] * data.a_gf[i];
364 }
365 };
366
367 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
368 struct ComputeGeneralizedGravityBackwardStep
369 : public fusion::JointUnaryVisitorBase<
370 ComputeGeneralizedGravityBackwardStep<Scalar, Options, JointCollectionTpl>>
371 {
372 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
373 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
374
375 typedef boost::fusion::vector<const Model &, Data &, typename Data::VectorXs &> ArgsType;
376
377 template<typename JointModel>
378 3996 static void algo(
379 const JointModelBase<JointModel> & jmodel,
380 JointDataBase<typename JointModel::JointDataDerived> & jdata,
381 const Model & model,
382 Data & data,
383 typename Data::VectorXs & g)
384 {
385 typedef typename Model::JointIndex JointIndex;
386
387
1/2
✓ Branch 1 taken 1998 times.
✗ Branch 2 not taken.
 3996 const JointIndex & i = jmodel.id();
388 3996 const JointIndex & parent = model.parents[i];
389
390
8/13
✓ Branch 2 taken 1998 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1924 times.
✓ Branch 6 taken 74 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 1924 times.
✓ Branch 9 taken 74 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 1924 times.
✓ Branch 12 taken 74 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 1924 times.
✗ Branch 15 not taken.
 3996 jmodel.jointVelocitySelector(g) = jdata.S().transpose() * data.f[i];
391
2/2
✓ Branch 0 taken 1924 times.
✓ Branch 1 taken 74 times.
 3996 if (parent > 0)
392
2/4
✓ Branch 3 taken 1924 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1924 times.
✗ Branch 8 not taken.
 3848 data.f[(size_t)parent] += data.liMi[i].act(data.f[i]);
393 }
394 };
395
396 template<
397 typename Scalar,
398 int Options,
399 template<typename, int> class JointCollectionTpl,
400 typename ConfigVectorType>
401 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &
402 39 computeGeneralizedGravity(
403 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
404 DataTpl<Scalar, Options, JointCollectionTpl> & data,
405 const Eigen::MatrixBase<ConfigVectorType> & q)
406 {
407
1/2
✓ Branch 1 taken 39 times.
✗ Branch 2 not taken.
 39 assert(model.check(data) && "data is not consistent with model.");
408
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 39 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.
 39 PINOCCHIO_CHECK_ARGUMENT_SIZE(
409 q.size(), model.nq, "The configuration vector is not of right size");
410
411 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
412 typedef typename Model::JointIndex JointIndex;
413
414
1/2
✓ Branch 3 taken 39 times.
✗ Branch 4 not taken.
 39 data.a_gf[0] = -model.gravity;
415
416 typedef ComputeGeneralizedGravityForwardStep<
417 Scalar, Options, JointCollectionTpl, ConfigVectorType>
418 Pass1;
419
2/2
✓ Branch 0 taken 1053 times.
✓ Branch 1 taken 39 times.
 1092 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
420 {
421
1/2
✓ Branch 1 taken 1053 times.
✗ Branch 2 not taken.
 1053 Pass1::run(
422 2106 model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived()));
423 }
424
425 typedef ComputeGeneralizedGravityBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
426
2/2
✓ Branch 0 taken 1053 times.
✓ Branch 1 taken 39 times.
 1092 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
427 {
428
1/2
✓ Branch 4 taken 1053 times.
✗ Branch 5 not taken.
 1053 Pass2::run(model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data, data.g));
429 }
430
431 39 return data.g;
432 }
433
434 template<
435 typename Scalar,
436 int Options,
437 template<typename, int> class JointCollectionTpl,
438 typename ConfigVectorType>
439 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &
440 35 computeStaticTorque(
441 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
442 DataTpl<Scalar, Options, JointCollectionTpl> & data,
443 const Eigen::MatrixBase<ConfigVectorType> & q,
444 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext)
445 {
446
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 assert(model.check(data) && "data is not consistent with model.");
447
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 35 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.
 35 PINOCCHIO_CHECK_ARGUMENT_SIZE(
448 q.size(), model.nq, "The configuration vector is not of right size");
449
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 35 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.
 35 PINOCCHIO_CHECK_ARGUMENT_SIZE(
450 fext.size(), (size_t)model.njoints, "The size of the external forces is not of right size");
451
452 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
453 typedef typename Model::JointIndex JointIndex;
454
455
1/2
✓ Branch 3 taken 35 times.
✗ Branch 4 not taken.
 35 data.a_gf[0] = -model.gravity;
456
457 typedef ComputeGeneralizedGravityForwardStep<
458 Scalar, Options, JointCollectionTpl, ConfigVectorType>
459 Pass1;
460
2/2
✓ Branch 0 taken 945 times.
✓ Branch 1 taken 35 times.
 980 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
461 {
462
1/2
✓ Branch 1 taken 945 times.
✗ Branch 2 not taken.
 945 Pass1::run(
463 945 model.joints[i], data.joints[i], typename Pass1::ArgsType(model, data, q.derived()));
464 945 data.f[i] -= fext[i];
465 }
466
467 typedef ComputeGeneralizedGravityBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
468
2/2
✓ Branch 0 taken 945 times.
✓ Branch 1 taken 35 times.
 980 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
469 {
470
1/2
✓ Branch 1 taken 945 times.
✗ Branch 2 not taken.
 945 Pass2::run(
471 1890 model.joints[i], data.joints[i], typename Pass2::ArgsType(model, data, data.tau));
472 }
473
474 35 return data.tau;
475 }
476
477 template<
478 typename Scalar,
479 int Options,
480 template<typename, int> class JointCollectionTpl,
481 typename ConfigVectorType,
482 typename TangentVectorType>
483 struct CoriolisMatrixForwardStep
484 : public fusion::JointUnaryVisitorBase<CoriolisMatrixForwardStep<
485 Scalar,
486 Options,
487 JointCollectionTpl,
488 ConfigVectorType,
489 TangentVectorType>>
490 {
491 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
492 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
493
494 typedef boost::fusion::
495 vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &>
496 ArgsType;
497
498 template<typename JointModel>
499 216 static void algo(
500 const pinocchio::JointModelBase<JointModel> & jmodel,
501 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
502 const Model & model,
503 Data & data,
504 const Eigen::MatrixBase<ConfigVectorType> & q,
505 const Eigen::MatrixBase<TangentVectorType> & v)
506 {
507 typedef typename Model::JointIndex JointIndex;
508
509
1/2
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
 216 const JointIndex & i = jmodel.id();
510 216 const JointIndex & parent = model.parents[i];
511
512
1/2
✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
 216 jmodel.calc(jdata.derived(), q.derived(), v.derived());
513
514
6/10
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 104 times.
✓ Branch 9 taken 4 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 104 times.
✗ Branch 13 not taken.
 216 data.liMi[i] = model.jointPlacements[i] * jdata.M();
515
2/2
✓ Branch 0 taken 104 times.
✓ Branch 1 taken 4 times.
 216 if (parent > 0)
516
2/4
✓ Branch 3 taken 104 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 104 times.
✗ Branch 8 not taken.
 208 data.oMi[i] = data.oMi[parent] * data.liMi[i];
517 else
518
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 8 data.oMi[i] = data.liMi[i];
519
520 // express quantities in the world frame
521
2/4
✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 108 times.
✗ Branch 8 not taken.
 216 data.oYcrb[i] = data.oMi[i].act(model.inertias[i]);
522
523
2/4
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
 216 data.v[i] = jdata.v();
524
2/2
✓ Branch 0 taken 104 times.
✓ Branch 1 taken 4 times.
 216 if (parent > 0)
525
2/4
✓ Branch 3 taken 104 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 104 times.
✗ Branch 8 not taken.
 208 data.v[i] += data.liMi[i].actInv(data.v[parent]);
526
2/4
✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 108 times.
✗ Branch 8 not taken.
 216 data.ov[i] = data.oMi[i].act(data.v[i]);
527
2/4
✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 108 times.
✗ Branch 8 not taken.
 216 data.oh[i] = data.oYcrb[i] * data.ov[i];
528
529 // computes S expressed at the world frame
530 typedef
531 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
532 ColsBlock;
533
1/2
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
 216 ColsBlock J_cols = jmodel.jointCols(data.J);
534
3/6
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
 216 J_cols = data.oMi[i].act(jdata.S()); // collection of S expressed at the world frame
535
536 // computes vxS expressed at the world frame
537
1/2
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
 216 ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
538
1/2
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
 216 motionSet::motionAction(data.ov[i], J_cols, dJ_cols);
539
540
2/6
✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 108 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 216 data.B[i] = data.oYcrb[i].variation(Scalar(0.5) * data.ov[i]);
541
2/6
✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 108 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 216 addForceCrossMatrix(Scalar(0.5) * data.oh[i], data.B[i]);
542 }
543
544 template<typename ForceDerived, typename M6>
545 static void
546 270 addForceCrossMatrix(const ForceDense<ForceDerived> & f, const Eigen::MatrixBase<M6> & mout)
547 {
548 270 M6 & mout_ = PINOCCHIO_EIGEN_CONST_CAST(M6, mout);
549
2/4
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
 270 addSkew(
550
1/2
✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
 270 -f.linear(), mout_.template block<3, 3>(ForceDerived::LINEAR, ForceDerived::ANGULAR));
551
2/4
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
 270 addSkew(
552
1/2
✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
 270 -f.linear(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::LINEAR));
553
2/4
✓ Branch 1 taken 135 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 135 times.
✗ Branch 5 not taken.
 270 addSkew(
554
1/2
✓ Branch 2 taken 135 times.
✗ Branch 3 not taken.
 270 -f.angular(), mout_.template block<3, 3>(ForceDerived::ANGULAR, ForceDerived::ANGULAR));
555 270 }
556 };
557
558 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
559 struct CoriolisMatrixBackwardStep
560 : public fusion::JointUnaryVisitorBase<
561 CoriolisMatrixBackwardStep<Scalar, Options, JointCollectionTpl>>
562 {
563 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
564 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
565
566 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
567
568 template<typename JointModel>
569 216 static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data)
570 {
571 typedef typename Model::JointIndex JointIndex;
572 typedef Eigen::Matrix<
573 Scalar, JointModel::NV, 6, Options, JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV,
574 6>
575 MatrixNV6;
576
577
1/2
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
 216 const JointIndex i = jmodel.id();
578 216 const JointIndex parent = model.parents[i];
579
580
2/4
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 108 times.
✗ Branch 5 not taken.
 216 typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) Mat_tmp(jmodel.nv(), 6);
581
582 typedef
583 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
584 ColsBlock;
585
1/2
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
 216 ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
586
1/2
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
 216 ColsBlock J_cols = jmodel.jointCols(data.J);
587
1/2
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
 216 ColsBlock Ag_cols = jmodel.jointCols(data.Ag);
588
589
2/4
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
 216 motionSet::inertiaAction(data.oYcrb[i], dJ_cols, jmodel.jointCols(data.dFdv));
590
4/8
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 108 times.
✗ Branch 12 not taken.
 216 jmodel.jointCols(data.dFdv).noalias() += data.B[i] * J_cols;
591
592
6/12
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 108 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 108 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 108 times.
✗ Branch 18 not taken.
 216 data.C.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() =
593
4/8
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 108 times.
✗ Branch 12 not taken.
 216 J_cols.transpose() * data.dFdv.middleCols(jmodel.idx_v(), data.nvSubtree[i]);
594
595
1/2
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
 216 motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols);
596
3/4
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 964 times.
✓ Branch 5 taken 108 times.
 2144 for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0;
597 1928 j = data.parents_fromRow[(JointIndex)j])
598
6/12
✓ Branch 1 taken 964 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 964 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 964 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 964 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 964 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 964 times.
✗ Branch 17 not taken.
 1928 data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() =
599
3/6
✓ Branch 1 taken 964 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 964 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 964 times.
✗ Branch 8 not taken.
 3856 Ag_cols.transpose() * data.dJ.col(j);
600
601
6/12
✓ Branch 2 taken 108 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 108 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 108 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 108 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 108 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 108 times.
✗ Branch 18 not taken.
 216 Mat_tmp.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.B[i];
602
3/4
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 964 times.
✓ Branch 5 taken 108 times.
 2144 for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0;
603 1928 j = data.parents_fromRow[(JointIndex)j])
604
6/12
✓ Branch 1 taken 964 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 964 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 964 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 964 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 964 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 964 times.
✗ Branch 17 not taken.
 1928 data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() +=
605
2/4
✓ Branch 1 taken 964 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 964 times.
✗ Branch 5 not taken.
 3856 Mat_tmp * data.J.col(j);
606
607
2/2
✓ Branch 0 taken 104 times.
✓ Branch 1 taken 4 times.
 216 if (parent > 0)
608 {
609
1/2
✓ Branch 3 taken 104 times.
✗ Branch 4 not taken.
 208 data.oYcrb[parent] += data.oYcrb[i];
610
1/2
✓ Branch 3 taken 104 times.
✗ Branch 4 not taken.
 208 data.B[parent] += data.B[i];
611 }
612 }
613 };
614
615 template<
616 typename Scalar,
617 int Options,
618 template<typename, int> class JointCollectionTpl,
619 typename ConfigVectorType,
620 typename TangentVectorType>
621 4 const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & computeCoriolisMatrix(
622 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
623 DataTpl<Scalar, Options, JointCollectionTpl> & data,
624 const Eigen::MatrixBase<ConfigVectorType> & q,
625 const Eigen::MatrixBase<TangentVectorType> & v)
626 {
627
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 assert(model.check(data) && "data is not consistent with model.");
628
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
 4 PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq);
629
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 4 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.
 4 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv);
630
631 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
632 typedef typename Model::JointIndex JointIndex;
633
634 typedef CoriolisMatrixForwardStep<
635 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType>
636 Pass1;
637
2/2
✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
 112 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
638 {
639
1/2
✓ Branch 1 taken 108 times.
✗ Branch 2 not taken.
 108 Pass1::run(
640 108 model.joints[i], data.joints[i],
641 216 typename Pass1::ArgsType(model, data, q.derived(), v.derived()));
642 }
643
644 typedef CoriolisMatrixBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
645
2/2
✓ Branch 0 taken 108 times.
✓ Branch 1 taken 4 times.
 112 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
646 {
647
1/2
✓ Branch 3 taken 108 times.
✗ Branch 4 not taken.
 108 Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data));
648 }
649
650 4 return data.C;
651 }
652 } // namespace impl
653 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
654 struct GetCoriolisMatrixBackwardStep
655 : public fusion::JointUnaryVisitorBase<
656 GetCoriolisMatrixBackwardStep<Scalar, Options, JointCollectionTpl>>
657 {
658 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
659 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
660
661 typedef boost::fusion::vector<const Model &, Data &> ArgsType;
662
663 template<typename JointModel>
664 54 static void algo(const JointModelBase<JointModel> & jmodel, const Model & model, Data & data)
665 {
666 typedef typename Model::JointIndex JointIndex;
667 typedef Eigen::Matrix<
668 Scalar, JointModel::NV, 6, Options, JointModel::NV == Eigen::Dynamic ? 6 : JointModel::NV,
669 6>
670 MatrixNV6;
671
672
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 const JointIndex i = jmodel.id();
673 54 const JointIndex parent = model.parents[i];
674
675
2/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
 54 typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixNV6) Mat_tmp(jmodel.nv(), 6);
676
677 typedef
678 typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type
679 ColsBlock;
680
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 ColsBlock dJ_cols = jmodel.jointCols(data.dJ);
681
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 ColsBlock J_cols = jmodel.jointCols(data.J);
682
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 ColsBlock Ag_cols = jmodel.jointCols(data.Ag);
683 54 typename Data::Matrix6x & dFdv = data.Fcrb[0];
684
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 ColsBlock dFdv_cols = jmodel.jointCols(dFdv);
685
686
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 54 motionSet::inertiaAction(data.oYcrb[i], dJ_cols, dFdv_cols);
687
3/6
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
 54 dFdv_cols.noalias() += data.B[i] * J_cols;
688
689
6/12
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 27 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
 54 data.C.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), data.nvSubtree[i]).noalias() =
690
4/8
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
 54 J_cols.transpose() * dFdv.middleCols(jmodel.idx_v(), data.nvSubtree[i]);
691
692
1/2
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
 54 motionSet::inertiaAction(data.oYcrb[i], J_cols, Ag_cols);
693
3/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✓ Branch 5 taken 27 times.
 536 for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0;
694 482 j = data.parents_fromRow[(JointIndex)j])
695
6/12
✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 241 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 241 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 241 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 241 times.
✗ Branch 17 not taken.
 482 data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j).noalias() =
696
3/6
✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 241 times.
✗ Branch 8 not taken.
 964 Ag_cols.transpose() * data.dJ.col(j);
697
698
6/12
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 27 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 27 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 27 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 27 times.
✗ Branch 18 not taken.
 54 Mat_tmp.topRows(jmodel.nv()).noalias() = J_cols.transpose() * data.B[i];
699
3/4
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✓ Branch 5 taken 27 times.
 536 for (int j = data.parents_fromRow[(JointIndex)jmodel.idx_v()]; j >= 0;
700 482 j = data.parents_fromRow[(JointIndex)j])
701
7/14
✓ Branch 1 taken 241 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 241 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 241 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 241 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 241 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 241 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 241 times.
✗ Branch 20 not taken.
 482 data.C.middleRows(jmodel.idx_v(), jmodel.nv()).col(j) += Mat_tmp * data.J.col(j);
702
703
2/2
✓ Branch 0 taken 26 times.
✓ Branch 1 taken 1 times.
 54 if (parent > 0)
704
1/2
✓ Branch 3 taken 26 times.
✗ Branch 4 not taken.
 52 data.B[parent] += data.B[i];
705 }
706 };
707
708 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
709 1 const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & getCoriolisMatrix(
710 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
711 DataTpl<Scalar, Options, JointCollectionTpl> & data)
712 {
713
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(model.check(data) && "data is not consistent with model.");
714
715 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
716 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
717 typedef typename Model::JointIndex JointIndex;
718 typedef typename Model::ConfigVectorType ConfigVectorType;
719 typedef typename Model::TangentVectorType TangentVectorType;
720
721 typedef impl::CoriolisMatrixForwardStep<
722 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType>
723 Pass1;
724
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
725 {
726 typedef typename Data::Inertia Inertia;
727
1/2
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
 27 const Inertia oY = data.oMi[i].act(model.inertias[i]);
728
2/6
✓ Branch 2 taken 27 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 27 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 27 data.B[i] = oY.variation(Scalar(0.5) * data.ov[i]);
729
2/6
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 27 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 27 Pass1::addForceCrossMatrix(Scalar(0.5) * data.oh[i], data.B[i]);
730 }
731
732 typedef GetCoriolisMatrixBackwardStep<Scalar, Options, JointCollectionTpl> Pass2;
733
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex i = (JointIndex)(model.njoints - 1); i > 0; --i)
734 {
735
1/2
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
 27 Pass2::run(model.joints[i], typename Pass2::ArgsType(model, data));
736 }
737
738 1 return data.C;
739 }
740
741 template<
742 typename Scalar,
743 int Options,
744 template<typename, int> class JointCollectionTpl,
745 typename ConfigVectorType,
746 typename TangentVectorType1,
747 typename TangentVectorType2>
748 520 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & rnea(
749 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
750 DataTpl<Scalar, Options, JointCollectionTpl> & data,
751 const Eigen::MatrixBase<ConfigVectorType> & q,
752 const Eigen::MatrixBase<TangentVectorType1> & v,
753 const Eigen::MatrixBase<TangentVectorType2> & a)
754 {
755
3/6
✓ Branch 2 taken 253 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 253 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 253 times.
✗ Branch 9 not taken.
 520 return impl::rnea(model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a));
756 }
757
758 template<
759 typename Scalar,
760 int Options,
761 template<typename, int> class JointCollectionTpl,
762 typename ConfigVectorType,
763 typename TangentVectorType1,
764 typename TangentVectorType2,
765 typename ForceDerived>
766 115 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & rnea(
767 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
768 DataTpl<Scalar, Options, JointCollectionTpl> & data,
769 const Eigen::MatrixBase<ConfigVectorType> & q,
770 const Eigen::MatrixBase<TangentVectorType1> & v,
771 const Eigen::MatrixBase<TangentVectorType2> & a,
772 const container::aligned_vector<ForceDerived> & fext)
773 {
774
3/6
✓ Branch 2 taken 113 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 113 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 113 times.
✗ Branch 9 not taken.
 115 return impl::rnea(model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a), fext);
775 }
776
777 template<
778 typename Scalar,
779 int Options,
780 template<typename, int> class JointCollectionTpl,
781 typename ConfigVectorType,
782 typename TangentVectorType>
783 13 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType & nonLinearEffects(
784 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
785 DataTpl<Scalar, Options, JointCollectionTpl> & data,
786 const Eigen::MatrixBase<ConfigVectorType> & q,
787 const Eigen::MatrixBase<TangentVectorType> & v)
788 {
789
2/4
✓ Branch 2 taken 13 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 13 times.
✗ Branch 6 not taken.
 13 return impl::nonLinearEffects(model, data, make_const_ref(q), make_const_ref(v));
790 }
791
792 template<
793 typename Scalar,
794 int Options,
795 template<typename, int> class JointCollectionTpl,
796 typename ConfigVectorType>
797 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &
798 39 computeGeneralizedGravity(
799 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
800 DataTpl<Scalar, Options, JointCollectionTpl> & data,
801 const Eigen::MatrixBase<ConfigVectorType> & q)
802 {
803
1/2
✓ Branch 2 taken 39 times.
✗ Branch 3 not taken.
 39 return impl::computeGeneralizedGravity(model, data, make_const_ref(q));
804 }
805
806 template<
807 typename Scalar,
808 int Options,
809 template<typename, int> class JointCollectionTpl,
810 typename ConfigVectorType>
811 const typename DataTpl<Scalar, Options, JointCollectionTpl>::TangentVectorType &
812 35 computeStaticTorque(
813 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
814 DataTpl<Scalar, Options, JointCollectionTpl> & data,
815 const Eigen::MatrixBase<ConfigVectorType> & q,
816 const container::aligned_vector<ForceTpl<Scalar, Options>> & fext)
817 {
818
1/2
✓ Branch 2 taken 35 times.
✗ Branch 3 not taken.
 35 return impl::computeStaticTorque(model, data, make_const_ref(q), fext);
819 }
820
821 template<
822 typename Scalar,
823 int Options,
824 template<typename, int> class JointCollectionTpl,
825 typename ConfigVectorType,
826 typename TangentVectorType>
827 4 const typename DataTpl<Scalar, Options, JointCollectionTpl>::MatrixXs & computeCoriolisMatrix(
828 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
829 DataTpl<Scalar, Options, JointCollectionTpl> & data,
830 const Eigen::MatrixBase<ConfigVectorType> & q,
831 const Eigen::MatrixBase<TangentVectorType> & v)
832 {
833
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 4 return impl::computeCoriolisMatrix(model, data, make_const_ref(q), make_const_ref(v));
834 }
835 } // namespace pinocchio
836
837 /// @endcond
838
839 #endif // ifndef __pinocchio_algorithm_rnea_hxx__
840