GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/kinematics.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 103 107 96.3%
Branches: 120 349 34.4%
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 "pinocchio/multibody/visitor.hpp"
9 #include "pinocchio/algorithm/check.hpp"
10
11 namespace pinocchio
12 {
13
14 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
15 1 void updateGlobalPlacements(
16 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
17 DataTpl<Scalar, Options, JointCollectionTpl> & data)
18 {
19
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 assert(model.check(data) && "data is not consistent with model.");
20
21 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
22
23
2/2
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 1 times.
 28 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
24 {
25 27 const JointIndex & parent = model.parents[i];
26
27
2/2
✓ Branch 0 taken 26 times.
✓ Branch 1 taken 1 times.
 27 if (parent > 0)
28
1/2
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
 26 data.oMi[i] = data.oMi[parent] * data.liMi[i];
29 else
30 1 data.oMi[i] = data.liMi[i];
31 }
32 1 }
33
34 namespace impl
35 {
36 template<
37 typename Scalar,
38 int Options,
39 template<typename, int>
40 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 1050434 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 525217 times.
✗ Branch 2 not taken.
 1050434 const JointIndex & i = jmodel.id();
62 1050434 const JointIndex & parent = model.parents[i];
63
64
1/2
✓ Branch 3 taken 525217 times.
✗ Branch 4 not taken.
 1050434 jmodel.calc(jdata.derived(), q.derived());
65
66
6/10
✓ Branch 1 taken 525217 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 506495 times.
✓ Branch 5 taken 18722 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 506495 times.
✓ Branch 9 taken 18722 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 506495 times.
✗ Branch 13 not taken.
 1050434 data.liMi[i] = model.jointPlacements[i] * jdata.M();
67
68
2/2
✓ Branch 0 taken 506346 times.
✓ Branch 1 taken 18871 times.
 1050434 if (parent > 0)
69
2/4
✓ Branch 3 taken 506346 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 506346 times.
✗ Branch 8 not taken.
 1012692 data.oMi[i] = data.oMi[parent] * data.liMi[i];
70 else
71
1/2
✓ Branch 3 taken 18871 times.
✗ Branch 4 not taken.
 37742 data.oMi[i] = data.liMi[i];
72 }
73 };
74
75 template<
76 typename Scalar,
77 int Options,
78 template<typename, int>
79 class JointCollectionTpl,
80 typename ConfigVectorType>
81 18861 void forwardKinematics(
82 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
83 DataTpl<Scalar, Options, JointCollectionTpl> & data,
84 const Eigen::MatrixBase<ConfigVectorType> & q)
85 {
86
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 18861 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.
 18861 PINOCCHIO_CHECK_ARGUMENT_SIZE(
87 q.size(), model.nq, "The configuration vector is not of right size");
88
1/2
✓ Branch 1 taken 18861 times.
✗ Branch 2 not taken.
 18861 assert(model.check(data) && "data is not consistent with model.");
89
90 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
91
92 typedef ForwardKinematicZeroStep<Scalar, Options, JointCollectionTpl, ConfigVectorType> Algo;
93
2/2
✓ Branch 0 taken 525217 times.
✓ Branch 1 taken 18861 times.
 544078 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
94 {
95
1/2
✓ Branch 1 taken 525217 times.
✗ Branch 2 not taken.
 525217 Algo::run(
96 1050434 model.joints[i], data.joints[i], typename Algo::ArgsType(model, data, q.derived()));
97 }
98 18861 }
99
100 template<
101 typename Scalar,
102 int Options,
103 template<typename, int>
104 class JointCollectionTpl,
105 typename ConfigVectorType,
106 typename TangentVectorType>
107 struct ForwardKinematicFirstStep
108 : fusion::JointUnaryVisitorBase<ForwardKinematicFirstStep<
109 Scalar,
110 Options,
111 JointCollectionTpl,
112 ConfigVectorType,
113 TangentVectorType>>
114 {
115 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
116 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
117
118 typedef boost::fusion::
119 vector<const Model &, Data &, const ConfigVectorType &, const TangentVectorType &>
120 ArgsType;
121
122 template<typename JointModel>
123 13294 static void algo(
124 const JointModelBase<JointModel> & jmodel,
125 JointDataBase<typename JointModel::JointDataDerived> & jdata,
126 const Model & model,
127 Data & data,
128 const Eigen::MatrixBase<ConfigVectorType> & q,
129 const Eigen::MatrixBase<TangentVectorType> & v)
130 {
131 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
132
133
1/2
✓ Branch 1 taken 6647 times.
✗ Branch 2 not taken.
 13294 const JointIndex & i = jmodel.id();
134 13294 const JointIndex & parent = model.parents[i];
135
136
1/2
✓ Branch 4 taken 6647 times.
✗ Branch 5 not taken.
 13294 jmodel.calc(jdata.derived(), q.derived(), v.derived());
137
138
2/4
✓ Branch 1 taken 6647 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6647 times.
✗ Branch 6 not taken.
 13294 data.v[i] = jdata.v();
139
6/10
✓ Branch 1 taken 6647 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6384 times.
✓ Branch 5 taken 263 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6384 times.
✓ Branch 9 taken 263 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 6384 times.
✗ Branch 13 not taken.
 13294 data.liMi[i] = model.jointPlacements[i] * jdata.M();
140
141
2/2
✓ Branch 0 taken 6386 times.
✓ Branch 1 taken 261 times.
 13294 if (parent > 0)
142 {
143
2/4
✓ Branch 3 taken 6386 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 6386 times.
✗ Branch 8 not taken.
 12772 data.oMi[i] = data.oMi[parent] * data.liMi[i];
144
2/4
✓ Branch 3 taken 6386 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 6386 times.
✗ Branch 8 not taken.
 12772 data.v[i] += data.liMi[i].actInv(data.v[parent]);
145 }
146 else
147
1/2
✓ Branch 3 taken 261 times.
✗ Branch 4 not taken.
 522 data.oMi[i] = data.liMi[i];
148 }
149 };
150
151 template<
152 typename Scalar,
153 int Options,
154 template<typename, int>
155 class JointCollectionTpl,
156 typename ConfigVectorType,
157 typename TangentVectorType>
158 261 void forwardKinematics(
159 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
160 DataTpl<Scalar, Options, JointCollectionTpl> & data,
161 const Eigen::MatrixBase<ConfigVectorType> & q,
162 const Eigen::MatrixBase<TangentVectorType> & v)
163 {
164
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 261 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.
 261 PINOCCHIO_CHECK_ARGUMENT_SIZE(
165 q.size(), model.nq, "The configuration vector is not of right size");
166
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 261 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.
 261 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
167
1/2
✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
 261 assert(model.check(data) && "data is not consistent with model.");
168
169 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
170
171 261 data.v[0].setZero();
172
173 typedef ForwardKinematicFirstStep<
174 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType>
175 Algo;
176
2/2
✓ Branch 0 taken 6647 times.
✓ Branch 1 taken 261 times.
 6908 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
177 {
178
1/2
✓ Branch 1 taken 6647 times.
✗ Branch 2 not taken.
 6647 Algo::run(
179 6647 model.joints[i], data.joints[i],
180 13294 typename Algo::ArgsType(model, data, q.derived(), v.derived()));
181 }
182 261 }
183
184 template<
185 typename Scalar,
186 int Options,
187 template<typename, int>
188 class JointCollectionTpl,
189 typename ConfigVectorType,
190 typename TangentVectorType1,
191 typename TangentVectorType2>
192 struct ForwardKinematicSecondStep
193 : fusion::JointUnaryVisitorBase<ForwardKinematicSecondStep<
194 Scalar,
195 Options,
196 JointCollectionTpl,
197 ConfigVectorType,
198 TangentVectorType1,
199 TangentVectorType2>>
200 {
201 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
202 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
203
204 typedef boost::fusion::vector<
205 const Model &,
206 Data &,
207 const ConfigVectorType &,
208 const TangentVectorType1 &,
209 const TangentVectorType2 &>
210 ArgsType;
211
212 template<typename JointModel>
213 51118 static void algo(
214 const JointModelBase<JointModel> & jmodel,
215 JointDataBase<typename JointModel::JointDataDerived> & jdata,
216 const Model & model,
217 Data & data,
218 const Eigen::MatrixBase<ConfigVectorType> & q,
219 const Eigen::MatrixBase<TangentVectorType1> & v,
220 const Eigen::MatrixBase<TangentVectorType2> & a)
221 {
222 typedef typename Model::JointIndex JointIndex;
223
224
1/2
✓ Branch 1 taken 25559 times.
✗ Branch 2 not taken.
 51118 const JointIndex & i = jmodel.id();
225 51118 const JointIndex & parent = model.parents[i];
226
227
1/2
✓ Branch 4 taken 25559 times.
✗ Branch 5 not taken.
 51118 jmodel.calc(jdata.derived(), q.derived(), v.derived());
228
229
2/4
✓ Branch 1 taken 25559 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 25559 times.
✗ Branch 6 not taken.
 51118 data.v[i] = jdata.v();
230
6/10
✓ Branch 1 taken 25559 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24506 times.
✓ Branch 5 taken 1053 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 24506 times.
✓ Branch 9 taken 1053 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 24506 times.
✗ Branch 13 not taken.
 51118 data.liMi[i] = model.jointPlacements[i] * jdata.M();
231
232
2/2
✓ Branch 0 taken 24613 times.
✓ Branch 1 taken 946 times.
 51118 if (parent > 0)
233 {
234
2/4
✓ Branch 3 taken 24613 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 24613 times.
✗ Branch 8 not taken.
 49226 data.oMi[i] = data.oMi[parent] * data.liMi[i];
235
2/4
✓ Branch 3 taken 24613 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 24613 times.
✗ Branch 8 not taken.
 49226 data.v[i] += data.liMi[i].actInv(data.v[parent]);
236 }
237 else
238
1/2
✓ Branch 3 taken 946 times.
✗ Branch 4 not taken.
 1892 data.oMi[i] = data.liMi[i];
239
240
5/9
✓ Branch 1 taken 25559 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24617 times.
✓ Branch 5 taken 942 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 24617 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 24617 times.
✗ Branch 12 not taken.
 102236 data.a[i] =
241
7/14
✓ Branch 1 taken 25559 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 25559 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 25559 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 25559 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 25559 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 942 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 942 times.
✗ Branch 21 not taken.
 102236 jdata.S() * jmodel.jointVelocitySelector(a) + jdata.c() + (data.v[i] ^ jdata.v());
242
2/4
✓ Branch 3 taken 25559 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 25559 times.
✗ Branch 8 not taken.
 51118 data.a[i] += data.liMi[i].actInv(data.a[parent]);
243 }
244 };
245
246 template<
247 typename Scalar,
248 int Options,
249 template<typename, int>
250 class JointCollectionTpl,
251 typename ConfigVectorType,
252 typename TangentVectorType1,
253 typename TangentVectorType2>
254 946 void forwardKinematics(
255 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
256 DataTpl<Scalar, Options, JointCollectionTpl> & data,
257 const Eigen::MatrixBase<ConfigVectorType> & q,
258 const Eigen::MatrixBase<TangentVectorType1> & v,
259 const Eigen::MatrixBase<TangentVectorType2> & a)
260 {
261
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 946 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.
 946 PINOCCHIO_CHECK_ARGUMENT_SIZE(
262 q.size(), model.nq, "The configuration vector is not of right size");
263
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 946 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.
 946 PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv, "The velocity vector is not of right size");
264
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 946 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.
 946 PINOCCHIO_CHECK_ARGUMENT_SIZE(
265 a.size(), model.nv, "The acceleration vector is not of right size");
266
1/2
✓ Branch 1 taken 946 times.
✗ Branch 2 not taken.
 946 assert(model.check(data) && "data is not consistent with model.");
267
268 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex JointIndex;
269
270 946 data.v[0].setZero();
271 946 data.a[0].setZero();
272
273 typedef ForwardKinematicSecondStep<
274 Scalar, Options, JointCollectionTpl, ConfigVectorType, TangentVectorType1,
275 TangentVectorType2>
276 Algo;
277
2/2
✓ Branch 0 taken 25559 times.
✓ Branch 1 taken 946 times.
 26505 for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
278 {
279
1/2
✓ Branch 1 taken 25559 times.
✗ Branch 2 not taken.
 25559 Algo::run(
280 25559 model.joints[i], data.joints[i],
281 51118 typename Algo::ArgsType(model, data, q.derived(), v.derived(), a.derived()));
282 }
283 946 }
284 } // namespace impl
285 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
286 224 MotionTpl<Scalar, Options> getVelocity(
287 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
288 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
289 const JointIndex jointId,
290 const ReferenceFrame rf)
291 {
292
1/2
✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
 224 assert(model.check(data) && "data is not consistent with model.");
293 PINOCCHIO_UNUSED_VARIABLE(model);
294
3/4
✓ Branch 0 taken 112 times.
✓ Branch 1 taken 56 times.
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
 224 switch (rf)
295 {
296 112 case LOCAL:
297 112 return data.v[jointId];
298 56 case WORLD:
299 56 return data.oMi[jointId].act(data.v[jointId]);
300 56 case LOCAL_WORLD_ALIGNED:
301 return MotionTpl<Scalar, Options>(
302
3/6
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 56 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 56 times.
✗ Branch 10 not taken.
 56 data.oMi[jointId].rotation() * data.v[jointId].linear(),
303
3/6
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 56 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 56 times.
✗ Branch 11 not taken.
 168 data.oMi[jointId].rotation() * data.v[jointId].angular());
304 ✗ default:
305 ✗ throw std::invalid_argument("Bad reference frame.");
306 }
307 }
308
309 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
310 224 MotionTpl<Scalar, Options> getAcceleration(
311 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
312 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
313 const JointIndex jointId,
314 const ReferenceFrame rf)
315 {
316
1/2
✓ Branch 1 taken 224 times.
✗ Branch 2 not taken.
 224 assert(model.check(data) && "data is not consistent with model.");
317 PINOCCHIO_UNUSED_VARIABLE(model);
318
3/4
✓ Branch 0 taken 112 times.
✓ Branch 1 taken 56 times.
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
 224 switch (rf)
319 {
320 112 case LOCAL:
321 112 return data.a[jointId];
322 56 case WORLD:
323 56 return data.oMi[jointId].act(data.a[jointId]);
324 56 case LOCAL_WORLD_ALIGNED:
325 return MotionTpl<Scalar, Options>(
326
3/6
✓ Branch 2 taken 56 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 56 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 56 times.
✗ Branch 10 not taken.
 56 data.oMi[jointId].rotation() * data.a[jointId].linear(),
327
3/6
✓ Branch 4 taken 56 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 56 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 56 times.
✗ Branch 11 not taken.
 168 data.oMi[jointId].rotation() * data.a[jointId].angular());
328 ✗ default:
329 ✗ throw std::invalid_argument("Bad reference frame.");
330 }
331 }
332
333 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
334 112 MotionTpl<Scalar, Options> getClassicalAcceleration(
335 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
336 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
337 const JointIndex jointId,
338 const ReferenceFrame rf)
339 {
340
2/4
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 112 times.
✗ Branch 4 not taken.
 112 assert(model.check(data) && "data is not consistent with model.");
341
342 typedef MotionTpl<Scalar, Options> Motion;
343
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 112 Motion vel = getVelocity(model, data, jointId, rf);
344
1/2
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
 112 Motion acc = getAcceleration(model, data, jointId, rf);
345
346
5/10
✓ Branch 1 taken 112 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 112 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 112 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 112 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 112 times.
✗ Branch 14 not taken.
 112 acc.linear() += vel.angular().cross(vel.linear());
347
348 224 return acc;
349 }
350
351 template<
352 typename Scalar,
353 int Options,
354 template<typename, int>
355 class JointCollectionTpl,
356 typename ConfigVectorType>
357 4126 void forwardKinematics(
358 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
359 DataTpl<Scalar, Options, JointCollectionTpl> & data,
360 const Eigen::MatrixBase<ConfigVectorType> & q)
361 {
362
1/2
✓ Branch 2 taken 4126 times.
✗ Branch 3 not taken.
 4126 impl::forwardKinematics(model, data, make_const_ref(q));
363 4126 }
364
365 template<
366 typename Scalar,
367 int Options,
368 template<typename, int>
369 class JointCollectionTpl,
370 typename ConfigVectorType,
371 typename TangentVectorType>
372 2 void forwardKinematics(
373 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
374 DataTpl<Scalar, Options, JointCollectionTpl> & data,
375 const Eigen::MatrixBase<ConfigVectorType> & q,
376 const Eigen::MatrixBase<TangentVectorType> & v)
377 {
378
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 impl::forwardKinematics(model, data, make_const_ref(q), make_const_ref(v));
379 2 }
380
381 template<
382 typename Scalar,
383 int Options,
384 template<typename, int>
385 class JointCollectionTpl,
386 typename ConfigVectorType,
387 typename TangentVectorType1,
388 typename TangentVectorType2>
389 239 void forwardKinematics(
390 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
391 DataTpl<Scalar, Options, JointCollectionTpl> & data,
392 const Eigen::MatrixBase<ConfigVectorType> & q,
393 const Eigen::MatrixBase<TangentVectorType1> & v,
394 const Eigen::MatrixBase<TangentVectorType2> & a)
395 {
396
3/6
✓ Branch 2 taken 223 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 223 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 223 times.
✗ Branch 9 not taken.
 239 impl::forwardKinematics(model, data, make_const_ref(q), make_const_ref(v), make_const_ref(a));
397 239 }
398 } // namespace pinocchio
399
400 #endif // ifndef __pinocchio_kinematics_hxx__
401