GCC Code Coverage Report

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