GCC Code Coverage Report

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