GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/kinematics.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 121 127 95.3%
Branches: 141 386 36.5%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016-2019 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_kinematics_hxx__
6 #define __pinocchio_kinematics_hxx__
7
8 #include "model.hpp"
9 #include "pinocchio/multibody/visitor.hpp"
10 #include "pinocchio/algorithm/check.hpp"
11
12 namespace pinocchio
13 {
14
15 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
16 2 void updateGlobalPlacements(
17 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
18 DataTpl<Scalar, Options, JointCollectionTpl> & data)
19 {
20
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 assert(model.check(data) && "data is not consistent with model.");
21
22 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
23
24
2/2
✓ Branch 0 taken 54 times.
✓ Branch 1 taken 2 times.
 56 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
25 {
26 54 const JointIndex & parent = model.parents[i];
27
28
2/2
✓ Branch 0 taken 52 times.
✓ Branch 1 taken 2 times.
 54 if (parent > 0)
29
1/2
✓ Branch 5 taken 52 times.
✗ Branch 6 not taken.
 52 data.oMi[i] = data.oMi[parent] * data.liMi[i];
30 else
31 2 data.oMi[i] = data.liMi[i];
32 }
33 2 }
34
35 namespace impl
36 {
37 template<
38 typename Scalar,
39 int Options,
40 template<typename, int> class JointCollectionTpl,
41 typename ConfigVectorType>
42 struct ForwardKinematicZeroStep
43 : fusion::JointUnaryVisitorBase<
44 ForwardKinematicZeroStep<Scalar, Options, JointCollectionTpl, ConfigVectorType>>
45 {
46 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
47 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
48
49 typedef boost::fusion::vector<const Model &, Data &, const ConfigVectorType &> ArgsType;
50
51 template<typename JointModel>
52 1066844 static void algo(
53 const JointModelBase<JointModel> & jmodel,
54 JointDataBase<typename JointModel::JointDataDerived> & jdata,
55 const Model & model,
56 Data & data,
57 const Eigen::MatrixBase<ConfigVectorType> & q)
58 {
59 typedef typename Model::JointIndex JointIndex;
60
61
1/2
✓ Branch 1 taken 533422 times.
✗ Branch 2 not taken.
 1066844 const JointIndex & i = jmodel.id();
62 1066844 const JointIndex & parent = model.parents[i];
63
64
1/2
✓ Branch 3 taken 533422 times.
✗ Branch 4 not taken.
 1066844 jmodel.calc(jdata.derived(), q.derived());
65
66
6/10
✓ Branch 1 taken 533422 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 514021 times.
✓ Branch 5 taken 19401 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 514021 times.
✓ Branch 9 taken 19401 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 514021 times.
✗ Branch 13 not taken.
 1066844 data.liMi[i] = model.jointPlacements[i] * jdata.M();
67
68
2/2
✓ Branch 0 taken 513947 times.
✓ Branch 1 taken 19475 times.
 1066844 if (parent > 0)
69
2/4
✓ Branch 3 taken 513947 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 513947 times.
✗ Branch 8 not taken.
 1027894 data.oMi[i] = data.oMi[parent] * data.liMi[i];
70 else
71
1/2
✓ Branch 3 taken 19475 times.
✗ Branch 4 not taken.
 38950 data.oMi[i] = data.liMi[i];
72 }
73 };
74
75 template<
76 typename Scalar,
77 int Options,
78 template<typename, int> class JointCollectionTpl,
79 typename ConfigVectorType>
80 19276 void forwardKinematics(
81 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
82 DataTpl<Scalar, Options, JointCollectionTpl> & data,
83 const Eigen::MatrixBase<ConfigVectorType> & q)
84 {
85
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 19276 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.
 19276 PINOCCHIO_CHECK_ARGUMENT_SIZE(
86 q.size(), model.nq, "The configuration vector is not of right size");
87
1/2
✓ Branch 1 taken 19276 times.
✗ Branch 2 not taken.
 19276 assert(model.check(data) && "data is not consistent with model.");
88
89 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
90
91 typedef ForwardKinematicZeroStep<Scalar, Options, JointCollectionTpl, ConfigVectorType> Algo;
92
2/2
✓ Branch 0 taken 532456 times.
✓ Branch 1 taken 19276 times.
 551732 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
93 {
94
1/2
✓ Branch 1 taken 532456 times.
✗ Branch 2 not taken.
 532456 Algo::run(
95 1064912 model.joints[i], data.joints[i], typename Algo::ArgsType(model, data, q.derived()));
96 }
97 19276 }
98
99 template<
100 typename Scalar,
101 int Options,
102 template<typename, int> class JointCollectionTpl,
103 typename ConfigVectorType,
104 typename TangentVectorType>
105 struct ForwardKinematicFirstStep
106 : fusion::JointUnaryVisitorBase<ForwardKinematicFirstStep<
107 Scalar,
108 Options,
109 JointCollectionTpl,
110 ConfigVectorType,
111 TangentVectorType>>
112 {
113 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
114 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
115
116 typedef boost::fusion::
117 vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &>
118 ArgsType;
119
120 template<typename JointModel>
121 46282 static void algo(
122 const JointModelBase<JointModel> & jmodel,
123 JointDataBase<typename JointModel::JointDataDerived> & jdata,
124 const Model & model,
125 Data & data,
126 const Eigen::MatrixBase<ConfigVectorType> & q,
127 const Eigen::MatrixBase<TangentVectorType> & v)
128 {
129 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
130
131
1/2
✓ Branch 1 taken 23141 times.
✗ Branch 2 not taken.
 46282 const JointIndex & i = jmodel.id();
132 46282 const JointIndex & parent = model.parents[i];
133
134
1/2
✓ Branch 4 taken 23141 times.
✗ Branch 5 not taken.
 46282 jmodel.calc(jdata.derived(), q.derived(), v.derived());
135
136
2/4
✓ Branch 1 taken 23141 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23141 times.
✗ Branch 6 not taken.
 46282 data.v[i] = jdata.v();
137
6/10
✓ Branch 1 taken 23141 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21615 times.
✓ Branch 5 taken 1526 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 21615 times.
✓ Branch 9 taken 1526 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 21615 times.
✗ Branch 13 not taken.
 46282 data.liMi[i] = model.jointPlacements[i] * jdata.M();
138
139
2/2
✓ Branch 0 taken 22264 times.
✓ Branch 1 taken 877 times.
 46282 if (parent > 0)
140 {
141
2/4
✓ Branch 3 taken 22264 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 22264 times.
✗ Branch 8 not taken.
 44528 data.oMi[i] = data.oMi[parent] * data.liMi[i];
142
2/4
✓ Branch 3 taken 22264 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 22264 times.
✗ Branch 8 not taken.
 44528 data.v[i] += data.liMi[i].actInv(data.v[parent]);
143 }
144 else
145
1/2
✓ Branch 3 taken 877 times.
✗ Branch 4 not taken.
 1754 data.oMi[i] = data.liMi[i];
146 }
147 };
148
149 template<
150 typename Scalar,
151 int Options,
152 template<typename, int> class JointCollectionTpl,
153 typename ConfigVectorType,
154 typename TangentVectorType>
155 879 void forwardKinematics(
156 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
157 DataTpl<Scalar, Options, JointCollectionTpl> & data,
158 const Eigen::MatrixBase<ConfigVectorType> & q,
159 const Eigen::MatrixBase<TangentVectorType> & v)
160 {
161
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 877 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.
 879 PINOCCHIO_CHECK_ARGUMENT_SIZE(
162 q.size(), model.nq, "The configuration vector is not of right size");
163
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 877 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.
 879 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
164
1/2
✓ Branch 1 taken 877 times.
✗ Branch 2 not taken.
 879 assert(model.check(data) && "data is not consistent with model.");
165
166 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
167
168 879 data.v[0].setZero();
169
170 typedef ForwardKinematicFirstStep<
171 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType>
172 Algo;
173
2/2
✓ Branch 0 taken 23141 times.
✓ Branch 1 taken 877 times.
 24022 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
174 {
175
1/2
✓ Branch 1 taken 23141 times.
✗ Branch 2 not taken.
 23143 Algo::run(
176 23143 model.joints[i], data.joints[i],
177 46286 typename Algo::ArgsType(model, data, q.derived(), v.derived()));
178 }
179 879 }
180
181 template<
182 typename Scalar,
183 int Options,
184 template<typename, int> class JointCollectionTpl,
185 typename ConfigVectorType,
186 typename TangentVectorType1,
187 typename TangentVectorType2>
188 struct ForwardKinematicSecondStep
189 : fusion::JointUnaryVisitorBase<ForwardKinematicSecondStep<
190 Scalar,
191 Options,
192 JointCollectionTpl,
193 ConfigVectorType,
194 TangentVectorType1,
195 TangentVectorType2>>
196 {
197 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
198 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
199
200 typedef boost::fusion::vector<
201 const Model &,
202 Data &,
203 const ConfigVectorType &,
204 const TangentVectorType1 &,
205 const TangentVectorType2 &>
206 ArgsType;
207
208 template<typename JointModel>
209 52306 static void algo(
210 const JointModelBase<JointModel> & jmodel,
211 JointDataBase<typename JointModel::JointDataDerived> & jdata,
212 const Model & model,
213 Data & data,
214 const Eigen::MatrixBase<ConfigVectorType> & q,
215 const Eigen::MatrixBase<TangentVectorType1> & v,
216 const Eigen::MatrixBase<TangentVectorType2> & a)
217 {
218 typedef typename Model::JointIndex JointIndex;
219
220
1/2
✓ Branch 1 taken 26153 times.
✗ Branch 2 not taken.
 52306 const JointIndex & i = jmodel.id();
221 52306 const JointIndex & parent = model.parents[i];
222
223
1/2
✓ Branch 4 taken 26153 times.
✗ Branch 5 not taken.
 52306 jmodel.calc(jdata.derived(), q.derived(), v.derived());
224
225
2/4
✓ Branch 1 taken 26153 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 26153 times.
✗ Branch 6 not taken.
 52306 data.v[i] = jdata.v();
226
6/10
✓ Branch 1 taken 26153 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25068 times.
✓ Branch 5 taken 1085 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 25068 times.
✓ Branch 9 taken 1085 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 25068 times.
✗ Branch 13 not taken.
 52306 data.liMi[i] = model.jointPlacements[i] * jdata.M();
227
228
2/2
✓ Branch 0 taken 25185 times.
✓ Branch 1 taken 968 times.
 52306 if (parent > 0)
229 {
230
2/4
✓ Branch 3 taken 25185 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 25185 times.
✗ Branch 8 not taken.
 50370 data.oMi[i] = data.oMi[parent] * data.liMi[i];
231
2/4
✓ Branch 3 taken 25185 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 25185 times.
✗ Branch 8 not taken.
 50370 data.v[i] += data.liMi[i].actInv(data.v[parent]);
232 }
233 else
234
1/2
✓ Branch 3 taken 968 times.
✗ Branch 4 not taken.
 1936 data.oMi[i] = data.liMi[i];
235
236
8/14
✓ Branch 1 taken 26153 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25189 times.
✓ Branch 5 taken 964 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 25189 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10 times.
✓ Branch 11 taken 25179 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 10 times.
✗ Branch 14 not taken.
✓ Branch 17 taken 10 times.
✗ Branch 18 not taken.
 104632 data.a[i] =
237
8/15
✓ Branch 1 taken 26143 times.
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26153 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 26153 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 26143 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 26143 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 964 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 964 times.
✗ Branch 21 not taken.
 104666 jdata.S() * jmodel.JointMappedVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v());
238
2/4
✓ Branch 3 taken 26153 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 26153 times.
✗ Branch 8 not taken.
 52306 data.a[i] += data.liMi[i].actInv(data.a[parent]);
239 }
240 };
241
242 template<
243 typename Scalar,
244 int Options,
245 template<typename, int> class JointCollectionTpl,
246 typename ConfigVectorType,
247 typename TangentVectorType1,
248 typename TangentVectorType2>
249 968 void forwardKinematics(
250 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
251 DataTpl<Scalar, Options, JointCollectionTpl> & data,
252 const Eigen::MatrixBase<ConfigVectorType> & q,
253 const Eigen::MatrixBase<TangentVectorType1> & v,
254 const Eigen::MatrixBase<TangentVectorType2> & a)
255 {
256
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 968 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.
 968 PINOCCHIO_CHECK_ARGUMENT_SIZE(
257 q.size(), model.nq, "The configuration vector is not of right size");
258
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 968 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.
 968 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
259
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 968 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.
 968 PINOCCHIO_CHECK_ARGUMENT_SIZE(
260 a.size(), model.nv, "The acceleration vector is not of right size");
261
1/2
✓ Branch 1 taken 968 times.
✗ Branch 2 not taken.
 968 assert(model.check(data) && "data is not consistent with model.");
262
263 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
264
265 968 data.v[0].setZero();
266 968 data.a[0].setZero();
267
268 typedef ForwardKinematicSecondStep<
269 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1,
270 TangentVectorType2>
271 Algo;
272
2/2
✓ Branch 0 taken 26153 times.
✓ Branch 1 taken 968 times.
 27121 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
273 {
274
1/2
✓ Branch 1 taken 26153 times.
✗ Branch 2 not taken.
 26153 Algo::run(
275 26153 model.joints[i], data.joints[i],
276 52306 typename Algo::ArgsType(model, data, q.derived(), v.derived(), a.derived()));
277 }
278 968 }
279 } // namespace impl
280
281 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
282 50 SE3Tpl<Scalar, Options> getRelativePlacement(
283 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
284 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
285 const JointIndex jointIdRef,
286 const JointIndex jointIdTarget,
287 const Convention convention)
288 {
289
1/2
✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
 50 assert(model.check(data) && "data is not consistent with model.");
290
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 50 times.
 50 assert(jointIdRef >= 0 && jointIdRef < (JointIndex)model.njoints && "invalid joint id");
291
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 50 times.
 50 assert(jointIdTarget >= 0 && jointIdTarget < (JointIndex)model.njoints && "invalid joint id");
292
2/3
✓ Branch 0 taken 25 times.
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 50 switch (convention)
293 {
294 25 case Convention::LOCAL: {
295
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 SE3Tpl<Scalar, Options> ancestorMref(1); // Initialize to Identity
296
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 SE3Tpl<Scalar, Options> ancestorMtarget(1); // Initialize to Identity
297
298 size_t ancestor_ref, ancestor_target;
299
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 findCommonAncestor(model, jointIdRef, jointIdTarget, ancestor_ref, ancestor_target);
300
301 // Traverse the kinematic chain backward from Ref to the common ancestor
302
2/2
✓ Branch 2 taken 68 times.
✓ Branch 3 taken 25 times.
 93 for (size_t i = model.supports[jointIdRef].size() - 1; i > ancestor_ref; i--)
303 {
304 68 const JointIndex j = model.supports[jointIdRef][(size_t)i];
305
2/4
✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 68 times.
✗ Branch 6 not taken.
 68 ancestorMref = data.liMi[j].act(ancestorMref);
306 }
307
308 // Traverse the kinematic chain backward from Target to the common ancestor
309
2/2
✓ Branch 2 taken 68 times.
✓ Branch 3 taken 25 times.
 93 for (size_t i = model.supports[jointIdTarget].size() - 1; i > ancestor_target; i--)
310 {
311 68 const JointIndex j = model.supports[jointIdTarget][(size_t)i];
312
2/4
✓ Branch 2 taken 68 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 68 times.
✗ Branch 6 not taken.
 68 ancestorMtarget = data.liMi[j].act(ancestorMtarget);
313 }
314
315
1/2
✓ Branch 1 taken 25 times.
✗ Branch 2 not taken.
 25 return ancestorMref.actInv(ancestorMtarget);
316 }
317 25 case Convention::WORLD:
318 25 return data.oMi[jointIdRef].actInv(data.oMi[jointIdTarget]);
319 ✗ default:
320 ✗ throw std::invalid_argument("Bad convention.");
321 }
322 }
323
324 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
325 421 MotionTpl<Scalar, Options> getVelocity(
326 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
327 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
328 const JointIndex jointId,
329 const ReferenceFrame rf)
330 {
331
1/2
✓ Branch 1 taken 421 times.
✗ Branch 2 not taken.
 421 assert(model.check(data) && "data is not consistent with model.");
332 PINOCCHIO_UNUSED_VARIABLE(model);
333
3/4
✓ Branch 0 taken 116 times.
✓ Branch 1 taken 58 times.
✓ Branch 2 taken 247 times.
✗ Branch 3 not taken.
 421 switch (rf)
334 {
335 116 case LOCAL:
336 116 return data.v[jointId];
337 58 case WORLD:
338 58 return data.oMi[jointId].act(data.v[jointId]);
339 247 case LOCAL_WORLD_ALIGNED:
340 return MotionTpl<Scalar, Options>(
341
3/6
✓ Branch 2 taken 247 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 247 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 247 times.
✗ Branch 10 not taken.
 247 data.oMi[jointId].rotation() * data.v[jointId].linear(),
342
3/6
✓ Branch 4 taken 247 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 247 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 247 times.
✗ Branch 11 not taken.
 741 data.oMi[jointId].rotation() * data.v[jointId].angular());
343 ✗ default:
344 ✗ throw std::invalid_argument("Bad reference frame.");
345 }
346 }
347
348 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
349 232 MotionTpl<Scalar, Options> getAcceleration(
350 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
351 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
352 const JointIndex jointId,
353 const ReferenceFrame rf)
354 {
355
1/2
✓ Branch 1 taken 232 times.
✗ Branch 2 not taken.
 232 assert(model.check(data) && "data is not consistent with model.");
356 PINOCCHIO_UNUSED_VARIABLE(model);
357
3/4
✓ Branch 0 taken 116 times.
✓ Branch 1 taken 58 times.
✓ Branch 2 taken 58 times.
✗ Branch 3 not taken.
 232 switch (rf)
358 {
359 116 case LOCAL:
360 116 return data.a[jointId];
361 58 case WORLD:
362 58 return data.oMi[jointId].act(data.a[jointId]);
363 58 case LOCAL_WORLD_ALIGNED:
364 return MotionTpl<Scalar, Options>(
365
3/6
✓ Branch 2 taken 58 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 58 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 58 times.
✗ Branch 10 not taken.
 58 data.oMi[jointId].rotation() * data.a[jointId].linear(),
366
3/6
✓ Branch 4 taken 58 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 58 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 58 times.
✗ Branch 11 not taken.
 174 data.oMi[jointId].rotation() * data.a[jointId].angular());
367 ✗ default:
368 ✗ throw std::invalid_argument("Bad reference frame.");
369 }
370 }
371
372 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
373 116 MotionTpl<Scalar, Options> getClassicalAcceleration(
374 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
375 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
376 const JointIndex jointId,
377 const ReferenceFrame rf)
378 {
379
2/4
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 116 times.
✗ Branch 4 not taken.
 116 assert(model.check(data) && "data is not consistent with model.");
380
381 typedef MotionTpl<Scalar, Options> Motion;
382
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 Motion vel = getVelocity(model, data, jointId, rf);
383
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 116 Motion acc = getAcceleration(model, data, jointId, rf);
384
385
5/10
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 116 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 116 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 116 times.
✗ Branch 14 not taken.
 116 acc.linear() += vel.angular().cross(vel.linear());
386
387 232 return acc;
388 }
389
390 template<
391 typename Scalar,
392 int Options,
393 template<typename, int> class JointCollectionTpl,
394 typename ConfigVectorType>
395 18289 void forwardKinematics(
396 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
397 DataTpl<Scalar, Options, JointCollectionTpl> & data,
398 const Eigen::MatrixBase<ConfigVectorType> & q)
399 {
400
1/2
✓ Branch 2 taken 18286 times.
✗ Branch 3 not taken.
 18289 impl::forwardKinematics(model, data, make_const_ref(q));
401 18289 }
402
403 template<
404 typename Scalar,
405 int Options,
406 template<typename, int> class JointCollectionTpl,
407 typename ConfigVectorType,
408 typename TangentVectorType>
409 751 void forwardKinematics(
410 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
411 DataTpl<Scalar, Options, JointCollectionTpl> & data,
412 const Eigen::MatrixBase<ConfigVectorType> & q,
413 const Eigen::MatrixBase<TangentVectorType> & v)
414 {
415
2/4
✓ Branch 2 taken 737 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 737 times.
✗ Branch 6 not taken.
 751 impl::forwardKinematics(model, data, make_const_ref(q), make_const_ref(v));
416 751 }
417
418 template<
419 typename Scalar,
420 int Options,
421 template<typename, int> class JointCollectionTpl,
422 typename ConfigVectorType,
423 typename TangentVectorType1,
424 typename TangentVectorType2>
425 972 void forwardKinematics(
426 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
427 DataTpl<Scalar, Options, JointCollectionTpl> & data,
428 const Eigen::MatrixBase<ConfigVectorType> & q,
429 const Eigen::MatrixBase<TangentVectorType1> & v,
430 const Eigen::MatrixBase<TangentVectorType2> & a)
431 {
432
3/6
✓ Branch 2 taken 853 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 853 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 853 times.
✗ Branch 9 not taken.
 972 impl::forwardKinematics(model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a));
433 972 }
434 } // namespace pinocchio
435
436 #endif // ifndef __pinocchio_kinematics_hxx__
437