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 |