GCC Code Coverage Report

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