Directory: | ./ |
---|---|
File: | include/pinocchio/spatial/act-on-set.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 72 | 153 | 47.1% |
Branches: | 40 | 172 | 23.3% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2015-2018 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_act_on_set_hxx__ | ||
6 | #define __pinocchio_act_on_set_hxx__ | ||
7 | |||
8 | namespace pinocchio | ||
9 | { | ||
10 | |||
11 | namespace internal | ||
12 | { | ||
13 | |||
14 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> | ||
15 | struct ForceSetSe3Action | ||
16 | { | ||
17 | /* Compute jF = jXi * iF, where jXi is the dual action matrix associated | ||
18 | * with m, and iF, jF are matrices whose columns are forces. The resolution | ||
19 | * is done by block operation. It is less efficient than the colwise | ||
20 | * operation and should not be used. */ | ||
21 | static void run( | ||
22 | const SE3Tpl<Scalar, Options> & m, | ||
23 | const Eigen::MatrixBase<Mat> & iF, | ||
24 | Eigen::MatrixBase<MatRet> const & jF); | ||
25 | }; | ||
26 | |||
27 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> | ||
28 | struct ForceSetSe3Action<Op, Scalar, Options, Mat, MatRet, 1> | ||
29 | { | ||
30 | /* Compute jF = jXi * iF, where jXi is the dual action matrix associated with m, | ||
31 | * and iF, jF are vectors. */ | ||
32 | ✗ | static void run( | |
33 | const SE3Tpl<Scalar, Options> & m, | ||
34 | const Eigen::MatrixBase<Mat> & iF, | ||
35 | Eigen::MatrixBase<MatRet> const & jF) | ||
36 | { | ||
37 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); | ||
38 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); | ||
39 | |||
40 | typedef ForceRef<const Mat> ForceRefOnMat; | ||
41 | typedef ForceRef<MatRet> ForceRefOnMatRet; | ||
42 | |||
43 | ✗ | ForceRefOnMat fin(iF.derived()); | |
44 | ✗ | ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF)); | |
45 | |||
46 | switch (Op) | ||
47 | { | ||
48 | ✗ | case SETTO: | |
49 | ✗ | fout = m.act(fin); | |
50 | ✗ | break; | |
51 | ✗ | case ADDTO: | |
52 | ✗ | fout += m.act(fin); | |
53 | ✗ | break; | |
54 | ✗ | case RMTO: | |
55 | ✗ | fout -= m.act(fin); | |
56 | ✗ | break; | |
57 | default: | ||
58 | assert(false && "Wrong Op requesed value"); | ||
59 | break; | ||
60 | } | ||
61 | } | ||
62 | }; | ||
63 | |||
64 | /* Specialized implementation of block action, using colwise operation. It | ||
65 | * is empirically much faster than the true block operation, although I do | ||
66 | * not understand why. */ | ||
67 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> | ||
68 | ✗ | void ForceSetSe3Action<Op, Scalar, Options, Mat, MatRet, NCOLS>::run( | |
69 | const SE3Tpl<Scalar, Options> & m, | ||
70 | const Eigen::MatrixBase<Mat> & iF, | ||
71 | Eigen::MatrixBase<MatRet> const & jF) | ||
72 | { | ||
73 | ✗ | for (int col = 0; col < jF.cols(); ++col) | |
74 | { | ||
75 | ✗ | typename MatRet::ColXpr jFc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF).col(col); | |
76 | ✗ | forceSet::se3Action<Op>(m, iF.col(col), jFc); | |
77 | } | ||
78 | } | ||
79 | |||
80 | template<int Op, typename MotionDerived, typename Mat, typename MatRet, int NCOLS> | ||
81 | struct ForceSetMotionAction | ||
82 | { | ||
83 | /* Compute dF = v ^ F, where is the dual action operation associated | ||
84 | * with v, and F, dF are matrices whose columns are forces. */ | ||
85 | static void run( | ||
86 | const MotionDense<MotionDerived> & v, | ||
87 | const Eigen::MatrixBase<Mat> & iF, | ||
88 | Eigen::MatrixBase<MatRet> const & jF); | ||
89 | }; | ||
90 | |||
91 | template<int Op, typename MotionDerived, typename Mat, typename MatRet> | ||
92 | struct ForceSetMotionAction<Op, MotionDerived, Mat, MatRet, 1> | ||
93 | { | ||
94 | template<typename Fin, typename Fout> | ||
95 | 18346 | static void run( | |
96 | const MotionDense<MotionDerived> & v, const ForceDense<Fin> & fin, ForceDense<Fout> & fout) | ||
97 | { | ||
98 | switch (Op) | ||
99 | { | ||
100 | case SETTO: | ||
101 | 326 | fin.motionAction(v, fout); | |
102 | 326 | break; | |
103 | case ADDTO: | ||
104 |
1/2✓ Branch 2 taken 9010 times.
✗ Branch 3 not taken.
|
18020 | fout += v.cross(fin); |
105 | 18020 | break; | |
106 | case RMTO: | ||
107 | ✗ | fout -= v.cross(fin); | |
108 | ✗ | break; | |
109 | default: | ||
110 | assert(false && "Wrong Op requesed value"); | ||
111 | break; | ||
112 | } | ||
113 | } | ||
114 | |||
115 | ✗ | static void run( | |
116 | const MotionDense<MotionDerived> & v, | ||
117 | const Eigen::MatrixBase<Mat> & iF, | ||
118 | Eigen::MatrixBase<MatRet> const & jF) | ||
119 | { | ||
120 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); | ||
121 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); | ||
122 | |||
123 | typedef ForceRef<const Mat> ForceRefOnMat; | ||
124 | typedef ForceRef<MatRet> ForceRefOnMatRet; | ||
125 | |||
126 | ✗ | ForceRefOnMat fin(iF.derived()); | |
127 | ✗ | ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF)); | |
128 | |||
129 | ✗ | run(v, fin, fout); | |
130 | } | ||
131 | }; | ||
132 | |||
133 | template<int Op, typename MotionDerived, typename Mat, typename MatRet, int NCOLS> | ||
134 | ✗ | void ForceSetMotionAction<Op, MotionDerived, Mat, MatRet, NCOLS>::run( | |
135 | const MotionDense<MotionDerived> & v, | ||
136 | const Eigen::MatrixBase<Mat> & iF, | ||
137 | Eigen::MatrixBase<MatRet> const & jF) | ||
138 | { | ||
139 | ✗ | for (int col = 0; col < jF.cols(); ++col) | |
140 | { | ||
141 | ✗ | typename MatRet::ColXpr jFc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF).col(col); | |
142 | ✗ | forceSet::motionAction<Op>(v, iF.col(col), jFc); | |
143 | } | ||
144 | } | ||
145 | |||
146 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> | ||
147 | struct ForceSetSe3ActionInverse | ||
148 | { | ||
149 | /* Compute jF = jXi * iF, where jXi is the dual action matrix associated | ||
150 | * with m, and iF, jF are matrices whose columns are forces. The resolution | ||
151 | * is done by block operation. It is less efficient than the colwise | ||
152 | * operation and should not be used. */ | ||
153 | static void run( | ||
154 | const SE3Tpl<Scalar, Options> & m, | ||
155 | const Eigen::MatrixBase<Mat> & iF, | ||
156 | Eigen::MatrixBase<MatRet> const & jF); | ||
157 | }; | ||
158 | |||
159 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> | ||
160 | struct ForceSetSe3ActionInverse<Op, Scalar, Options, Mat, MatRet, 1> | ||
161 | { | ||
162 | /* Compute jF = jXi * iF, where jXi is the dual action matrix associated with m, | ||
163 | * and iF, jF are vectors. */ | ||
164 | ✗ | static void run( | |
165 | const SE3Tpl<Scalar, Options> & m, | ||
166 | const Eigen::MatrixBase<Mat> & iF, | ||
167 | Eigen::MatrixBase<MatRet> const & jF) | ||
168 | { | ||
169 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); | ||
170 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); | ||
171 | |||
172 | typedef ForceRef<const Mat> ForceRefOnMat; | ||
173 | typedef ForceRef<MatRet> ForceRefOnMatRet; | ||
174 | |||
175 | ✗ | ForceRefOnMat fin(iF.derived()); | |
176 | ✗ | ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF)); | |
177 | |||
178 | switch (Op) | ||
179 | { | ||
180 | ✗ | case SETTO: | |
181 | ✗ | fout = m.actInv(fin); | |
182 | ✗ | break; | |
183 | ✗ | case ADDTO: | |
184 | ✗ | fout += m.actInv(fin); | |
185 | ✗ | break; | |
186 | ✗ | case RMTO: | |
187 | ✗ | fout -= m.actInv(fin); | |
188 | ✗ | break; | |
189 | default: | ||
190 | assert(false && "Wrong Op requesed value"); | ||
191 | break; | ||
192 | } | ||
193 | } | ||
194 | }; | ||
195 | |||
196 | /* Specialized implementation of block action, using colwise operation. It | ||
197 | * is empirically much faster than the true block operation, although I do | ||
198 | * not understand why. */ | ||
199 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> | ||
200 | ✗ | void ForceSetSe3ActionInverse<Op, Scalar, Options, Mat, MatRet, NCOLS>::run( | |
201 | const SE3Tpl<Scalar, Options> & m, | ||
202 | const Eigen::MatrixBase<Mat> & iF, | ||
203 | Eigen::MatrixBase<MatRet> const & jF) | ||
204 | { | ||
205 | ✗ | for (int col = 0; col < jF.cols(); ++col) | |
206 | { | ||
207 | ✗ | typename MatRet::ColXpr jFc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF).col(col); | |
208 | ✗ | forceSet::se3ActionInverse<Op>(m, iF.col(col), jFc); | |
209 | } | ||
210 | } | ||
211 | |||
212 | } // namespace internal | ||
213 | |||
214 | namespace forceSet | ||
215 | { | ||
216 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> | ||
217 | ✗ | static void se3Action( | |
218 | const SE3Tpl<Scalar, Options> & m, | ||
219 | const Eigen::MatrixBase<Mat> & iF, | ||
220 | Eigen::MatrixBase<MatRet> const & jF) | ||
221 | { | ||
222 | ✗ | internal::ForceSetSe3Action<Op, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run( | |
223 | m, iF, jF); | ||
224 | } | ||
225 | |||
226 | template<typename Scalar, int Options, typename Mat, typename MatRet> | ||
227 | 111 | static void se3Action( | |
228 | const SE3Tpl<Scalar, Options> & m, | ||
229 | const Eigen::MatrixBase<Mat> & iF, | ||
230 | Eigen::MatrixBase<MatRet> const & jF) | ||
231 | { | ||
232 | 111 | internal::ForceSetSe3Action<SETTO, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run( | |
233 | m, iF, jF); | ||
234 | 111 | } | |
235 | |||
236 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> | ||
237 | ✗ | static void se3ActionInverse( | |
238 | const SE3Tpl<Scalar, Options> & m, | ||
239 | const Eigen::MatrixBase<Mat> & iF, | ||
240 | Eigen::MatrixBase<MatRet> const & jF) | ||
241 | { | ||
242 | internal::ForceSetSe3ActionInverse< | ||
243 | ✗ | Op, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(m, iF, jF); | |
244 | } | ||
245 | |||
246 | template<typename Scalar, int Options, typename Mat, typename MatRet> | ||
247 | ✗ | static void se3ActionInverse( | |
248 | const SE3Tpl<Scalar, Options> & m, | ||
249 | const Eigen::MatrixBase<Mat> & iF, | ||
250 | Eigen::MatrixBase<MatRet> const & jF) | ||
251 | { | ||
252 | internal::ForceSetSe3ActionInverse< | ||
253 | ✗ | SETTO, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(m, iF, jF); | |
254 | } | ||
255 | |||
256 | template<int Op, typename MotionDerived, typename Mat, typename MatRet> | ||
257 | ✗ | static void motionAction( | |
258 | const MotionDense<MotionDerived> & v, | ||
259 | const Eigen::MatrixBase<Mat> & iF, | ||
260 | Eigen::MatrixBase<MatRet> const & jF) | ||
261 | { | ||
262 | ✗ | internal::ForceSetMotionAction<Op, MotionDerived, Mat, MatRet, Mat::ColsAtCompileTime>::run( | |
263 | v, iF, jF); | ||
264 | } | ||
265 | |||
266 | template<typename MotionDerived, typename Mat, typename MatRet> | ||
267 | ✗ | static void motionAction( | |
268 | const MotionDense<MotionDerived> & v, | ||
269 | const Eigen::MatrixBase<Mat> & iF, | ||
270 | Eigen::MatrixBase<MatRet> const & jF) | ||
271 | { | ||
272 | internal::ForceSetMotionAction< | ||
273 | ✗ | SETTO, MotionDerived, Mat, MatRet, Mat::ColsAtCompileTime>::run(v, iF, jF); | |
274 | } | ||
275 | |||
276 | } // namespace forceSet | ||
277 | |||
278 | namespace internal | ||
279 | { | ||
280 | |||
281 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> | ||
282 | struct MotionSetSe3Action | ||
283 | { | ||
284 | /* Compute jF = jXi * iF, where jXi is the action matrix associated | ||
285 | * with m, and iF, jF are matrices whose columns are motions. */ | ||
286 | static void run( | ||
287 | const SE3Tpl<Scalar, Options> & m, | ||
288 | const Eigen::MatrixBase<Mat> & iF, | ||
289 | Eigen::MatrixBase<MatRet> const & jF); | ||
290 | }; | ||
291 | |||
292 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> | ||
293 | struct MotionSetSe3Action<Op, Scalar, Options, Mat, MatRet, 1> | ||
294 | { | ||
295 | /* Compute jV = jXi * iV, where jXi is the action matrix associated with m, | ||
296 | * and iV, jV are 6D vectors representing spatial velocities. */ | ||
297 | ✗ | static void run( | |
298 | const SE3Tpl<Scalar, Options> & m, | ||
299 | const Eigen::MatrixBase<Mat> & iV, | ||
300 | Eigen::MatrixBase<MatRet> const & jV) | ||
301 | { | ||
302 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); | ||
303 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); | ||
304 | |||
305 | typedef MotionRef<const Mat> MotionRefOnMat; | ||
306 | typedef MotionRef<MatRet> MotionRefOnMatRet; | ||
307 | |||
308 | ✗ | MotionRefOnMat min(iV.derived()); | |
309 | ✗ | MotionRefOnMatRet mout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV)); | |
310 | |||
311 | switch (Op) | ||
312 | { | ||
313 | ✗ | case SETTO: | |
314 | ✗ | mout = m.act(min); | |
315 | ✗ | break; | |
316 | ✗ | case ADDTO: | |
317 | ✗ | mout += m.act(min); | |
318 | ✗ | break; | |
319 | ✗ | case RMTO: | |
320 | ✗ | mout -= m.act(min); | |
321 | ✗ | break; | |
322 | default: | ||
323 | assert(false && "Wrong Op requesed value"); | ||
324 | break; | ||
325 | } | ||
326 | } | ||
327 | }; | ||
328 | |||
329 | /* Specialized implementation of block action, using colwise operation. It | ||
330 | * is empirically much faster than the true block operation, although I do | ||
331 | * not understand why. */ | ||
332 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> | ||
333 | ✗ | void MotionSetSe3Action<Op, Scalar, Options, Mat, MatRet, NCOLS>::run( | |
334 | const SE3Tpl<Scalar, Options> & m, | ||
335 | const Eigen::MatrixBase<Mat> & iV, | ||
336 | Eigen::MatrixBase<MatRet> const & jV) | ||
337 | { | ||
338 | ✗ | for (int col = 0; col < jV.cols(); ++col) | |
339 | { | ||
340 | ✗ | typename MatRet::ColXpr jVc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV).col(col); | |
341 | ✗ | motionSet::se3Action<Op>(m, iV.col(col), jVc); | |
342 | } | ||
343 | } | ||
344 | |||
345 | template<int Op, typename MotionDerived, typename Mat, typename MatRet, int NCOLS> | ||
346 | struct MotionSetMotionAction | ||
347 | { | ||
348 | /* Compute dV = v ^ V, where is the action operation associated | ||
349 | * with v, and V, dV are matrices whose columns are motions. */ | ||
350 | static void run( | ||
351 | const MotionDense<MotionDerived> & v, | ||
352 | const Eigen::MatrixBase<Mat> & iV, | ||
353 | Eigen::MatrixBase<MatRet> const & jV); | ||
354 | }; | ||
355 | |||
356 | template<int Op, typename MotionDerived, typename Mat, typename MatRet, int NCOLS> | ||
357 | 1324 | void MotionSetMotionAction<Op, MotionDerived, Mat, MatRet, NCOLS>::run( | |
358 | const MotionDense<MotionDerived> & v, | ||
359 | const Eigen::MatrixBase<Mat> & iV, | ||
360 | Eigen::MatrixBase<MatRet> const & jV) | ||
361 | { | ||
362 |
2/2✓ Branch 1 taken 3914 times.
✓ Branch 2 taken 662 times.
|
9152 | for (int col = 0; col < jV.cols(); ++col) |
363 | { | ||
364 |
1/2✓ Branch 2 taken 3914 times.
✗ Branch 3 not taken.
|
7828 | typename MatRet::ColXpr jVc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV).col(col); |
365 |
2/4✓ Branch 1 taken 3914 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3914 times.
✗ Branch 5 not taken.
|
7828 | motionSet::motionAction<Op>(v, iV.col(col), jVc); |
366 | } | ||
367 | } | ||
368 | |||
369 | template<int Op, typename MotionDerived, typename Mat, typename MatRet> | ||
370 | struct MotionSetMotionAction<Op, MotionDerived, Mat, MatRet, 1> | ||
371 | { | ||
372 | 54976 | static void run( | |
373 | const MotionDense<MotionDerived> & v, | ||
374 | const Eigen::MatrixBase<Mat> & iV, | ||
375 | Eigen::MatrixBase<MatRet> const & jV) | ||
376 | { | ||
377 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); | ||
378 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); | ||
379 | |||
380 | typedef MotionRef<const Mat> MotionRefOnMat; | ||
381 | typedef MotionRef<MatRet> MotionRefOnMatRet; | ||
382 | |||
383 |
1/2✓ Branch 2 taken 27488 times.
✗ Branch 3 not taken.
|
54976 | MotionRefOnMat min(iV.derived()); |
384 |
1/2✓ Branch 2 taken 27488 times.
✗ Branch 3 not taken.
|
54976 | MotionRefOnMatRet mout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV)); |
385 | |||
386 | switch (Op) | ||
387 | { | ||
388 | 47026 | case SETTO: | |
389 |
1/2✓ Branch 1 taken 23513 times.
✗ Branch 2 not taken.
|
47026 | min.motionAction(v, mout); |
390 | 47026 | break; | |
391 | 7950 | case ADDTO: | |
392 |
2/4✓ Branch 1 taken 3975 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3975 times.
✗ Branch 5 not taken.
|
7950 | mout += v.cross(min); |
393 | 7950 | break; | |
394 | ✗ | case RMTO: | |
395 | ✗ | mout -= v.cross(min); | |
396 | ✗ | break; | |
397 | default: | ||
398 | assert(false && "Wrong Op requesed value"); | ||
399 | break; | ||
400 | } | ||
401 | } | ||
402 | }; | ||
403 | |||
404 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> | ||
405 | struct MotionSetSe3ActionInverse | ||
406 | { | ||
407 | /* Compute jF = jXi * iF, where jXi is the action matrix associated | ||
408 | * with m, and iF, jF are matrices whose columns are motions. The resolution | ||
409 | * is done by block operation. It is less efficient than the colwise | ||
410 | * operation and should not be used. */ | ||
411 | static void run( | ||
412 | const SE3Tpl<Scalar, Options> & m, | ||
413 | const Eigen::MatrixBase<Mat> & iF, | ||
414 | Eigen::MatrixBase<MatRet> const & jF); | ||
415 | }; | ||
416 | |||
417 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> | ||
418 | struct MotionSetSe3ActionInverse<Op, Scalar, Options, Mat, MatRet, 1> | ||
419 | { | ||
420 | /* Compute jV = jXi * iV, where jXi is the action matrix associated with m, | ||
421 | * and iV, jV are 6D vectors representing spatial velocities. */ | ||
422 | 2294 | static void run( | |
423 | const SE3Tpl<Scalar, Options> & m, | ||
424 | const Eigen::MatrixBase<Mat> & iV, | ||
425 | Eigen::MatrixBase<MatRet> const & jV) | ||
426 | { | ||
427 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); | ||
428 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); | ||
429 | |||
430 | typedef MotionRef<Mat> MotionRefOnMat; | ||
431 | typedef MotionRef<MatRet> MotionRefOnMatRet; | ||
432 | |||
433 |
1/2✓ Branch 2 taken 1147 times.
✗ Branch 3 not taken.
|
2294 | MotionRefOnMat min(iV.derived()); |
434 |
1/2✓ Branch 2 taken 1147 times.
✗ Branch 3 not taken.
|
2294 | MotionRefOnMatRet mout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV)); |
435 | |||
436 | switch (Op) | ||
437 | { | ||
438 | 1688 | case SETTO: | |
439 |
2/4✓ Branch 1 taken 844 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 844 times.
✗ Branch 5 not taken.
|
1688 | mout = m.actInv(min); |
440 | 1688 | break; | |
441 | 606 | case ADDTO: | |
442 |
2/4✓ Branch 1 taken 303 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 303 times.
✗ Branch 5 not taken.
|
606 | mout += m.actInv(min); |
443 | 606 | break; | |
444 | ✗ | case RMTO: | |
445 | ✗ | mout -= m.actInv(min); | |
446 | ✗ | break; | |
447 | default: | ||
448 | assert(false && "Wrong Op requesed value"); | ||
449 | break; | ||
450 | } | ||
451 | } | ||
452 | }; | ||
453 | |||
454 | /* Specialized implementation of block action, using colwise operation. It | ||
455 | * is empirically much faster than the true block operation, although I do | ||
456 | * not understand why. */ | ||
457 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> | ||
458 | 190 | void MotionSetSe3ActionInverse<Op, Scalar, Options, Mat, MatRet, NCOLS>::run( | |
459 | const SE3Tpl<Scalar, Options> & m, | ||
460 | const Eigen::MatrixBase<Mat> & iV, | ||
461 | Eigen::MatrixBase<MatRet> const & jV) | ||
462 | { | ||
463 |
2/2✓ Branch 1 taken 570 times.
✓ Branch 2 taken 95 times.
|
1330 | for (int col = 0; col < jV.cols(); ++col) |
464 | { | ||
465 |
1/2✓ Branch 2 taken 570 times.
✗ Branch 3 not taken.
|
1140 | typename MatRet::ColXpr jVc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV).col(col); |
466 |
2/4✓ Branch 1 taken 570 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 570 times.
✗ Branch 5 not taken.
|
1140 | motionSet::se3ActionInverse<Op>(m, iV.col(col), jVc); |
467 | } | ||
468 | } | ||
469 | |||
470 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> | ||
471 | struct MotionSetInertiaAction | ||
472 | { | ||
473 | /* Compute dV = v ^ V, where is the action operation associated | ||
474 | * with v, and V, dV are matrices whose columns are motions. */ | ||
475 | static void run( | ||
476 | const InertiaTpl<Scalar, Options> & I, | ||
477 | const Eigen::MatrixBase<Mat> & iV, | ||
478 | Eigen::MatrixBase<MatRet> const & jV); | ||
479 | }; | ||
480 | |||
481 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> | ||
482 | 6400 | void MotionSetInertiaAction<Op, Scalar, Options, Mat, MatRet, NCOLS>::run( | |
483 | const InertiaTpl<Scalar, Options> & I, | ||
484 | const Eigen::MatrixBase<Mat> & iV, | ||
485 | Eigen::MatrixBase<MatRet> const & jV) | ||
486 | { | ||
487 |
2/2✓ Branch 1 taken 19139 times.
✓ Branch 2 taken 3200 times.
|
44678 | for (int col = 0; col < jV.cols(); ++col) |
488 | { | ||
489 |
1/2✓ Branch 2 taken 19139 times.
✗ Branch 3 not taken.
|
38278 | typename MatRet::ColXpr jVc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV).col(col); |
490 |
2/4✓ Branch 1 taken 19139 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19139 times.
✗ Branch 5 not taken.
|
38278 | motionSet::inertiaAction<Op>(I, iV.col(col), jVc); |
491 | } | ||
492 | } | ||
493 | |||
494 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> | ||
495 | struct MotionSetInertiaAction<Op, Scalar, Options, Mat, MatRet, 1> | ||
496 | { | ||
497 | 184176 | static void run( | |
498 | const InertiaTpl<Scalar, Options> & I, | ||
499 | const Eigen::MatrixBase<Mat> & iV, | ||
500 | Eigen::MatrixBase<MatRet> const & jV) | ||
501 | { | ||
502 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); | ||
503 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); | ||
504 | |||
505 | typedef MotionRef<const Mat> MotionRefOnMat; | ||
506 | typedef ForceRef<MatRet> ForceRefOnMatRet; | ||
507 |
1/2✓ Branch 2 taken 92088 times.
✗ Branch 3 not taken.
|
184176 | MotionRefOnMat min(iV.derived()); |
508 |
1/2✓ Branch 2 taken 92088 times.
✗ Branch 3 not taken.
|
184176 | ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV)); |
509 | |||
510 | switch (Op) | ||
511 | { | ||
512 | 146706 | case SETTO: | |
513 |
1/2✓ Branch 1 taken 73353 times.
✗ Branch 2 not taken.
|
146706 | I.__mult__(min, fout); |
514 | 146706 | break; | |
515 | 37470 | case ADDTO: | |
516 |
2/4✓ Branch 1 taken 18735 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18735 times.
✗ Branch 5 not taken.
|
37470 | fout += I * min; |
517 | 37470 | break; | |
518 | ✗ | case RMTO: | |
519 | ✗ | fout -= I * min; | |
520 | ✗ | break; | |
521 | default: | ||
522 | assert(false && "Wrong Op requesed value"); | ||
523 | break; | ||
524 | } | ||
525 | } | ||
526 | }; | ||
527 | |||
528 | template<int Op, typename ForceDerived, typename Mat, typename MatRet, int NCOLS> | ||
529 | struct MotionSetActOnForce | ||
530 | { | ||
531 | 578 | static void run( | |
532 | const Eigen::MatrixBase<Mat> & iV, | ||
533 | const ForceDense<ForceDerived> & f, | ||
534 | Eigen::MatrixBase<MatRet> const & jF) | ||
535 | { | ||
536 |
2/2✓ Branch 1 taken 1725 times.
✓ Branch 2 taken 289 times.
|
4028 | for (int col = 0; col < jF.cols(); ++col) |
537 | { | ||
538 |
1/2✓ Branch 2 taken 1725 times.
✗ Branch 3 not taken.
|
3450 | typename MatRet::ColXpr jFc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF).col(col); |
539 |
2/4✓ Branch 1 taken 1725 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1725 times.
✗ Branch 5 not taken.
|
3450 | motionSet::act<Op>(iV.col(col), f, jFc); |
540 | } | ||
541 | } | ||
542 | }; | ||
543 | |||
544 | template<int Op, typename ForceDerived, typename Mat, typename MatRet> | ||
545 | struct MotionSetActOnForce<Op, ForceDerived, Mat, MatRet, 1> | ||
546 | { | ||
547 | /* Compute jF = jXi * iF, where jXi is the dual action matrix associated with m, | ||
548 | * and iF, jF are vectors. */ | ||
549 | 18346 | static void run( | |
550 | const Eigen::MatrixBase<Mat> & iV, | ||
551 | const ForceDense<ForceDerived> & f, | ||
552 | Eigen::MatrixBase<MatRet> const & jF) | ||
553 | { | ||
554 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); | ||
555 | EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); | ||
556 | |||
557 | typedef MotionRef<const Mat> MotionRefOnMat; | ||
558 | typedef ForceRef<MatRet> ForceRefOnMatRet; | ||
559 | |||
560 |
1/2✓ Branch 2 taken 9173 times.
✗ Branch 3 not taken.
|
18346 | MotionRefOnMat vin(iV.derived()); |
561 |
1/2✓ Branch 2 taken 9173 times.
✗ Branch 3 not taken.
|
18346 | ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF)); |
562 |
1/2✓ Branch 1 taken 9173 times.
✗ Branch 2 not taken.
|
18346 | ForceSetMotionAction<Op, MotionRefOnMat, Mat, MatRet, 1>::run(vin, f, fout); |
563 | } | ||
564 | }; | ||
565 | |||
566 | } // namespace internal | ||
567 | |||
568 | namespace motionSet | ||
569 | { | ||
570 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> | ||
571 | ✗ | static void se3Action( | |
572 | const SE3Tpl<Scalar, Options> & m, | ||
573 | const Eigen::MatrixBase<Mat> & iV, | ||
574 | Eigen::MatrixBase<MatRet> const & jV) | ||
575 | { | ||
576 | ✗ | internal::MotionSetSe3Action<Op, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run( | |
577 | m, iV, jV); | ||
578 | } | ||
579 | |||
580 | template<typename Scalar, int Options, typename Mat, typename MatRet> | ||
581 | ✗ | static void se3Action( | |
582 | const SE3Tpl<Scalar, Options> & m, | ||
583 | const Eigen::MatrixBase<Mat> & iV, | ||
584 | Eigen::MatrixBase<MatRet> const & jV) | ||
585 | { | ||
586 | internal::MotionSetSe3Action< | ||
587 | ✗ | SETTO, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(m, iV, jV); | |
588 | } | ||
589 | |||
590 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> | ||
591 | 1496 | static void se3ActionInverse( | |
592 | const SE3Tpl<Scalar, Options> & m, | ||
593 | const Eigen::MatrixBase<Mat> & iV, | ||
594 | Eigen::MatrixBase<MatRet> const & jV) | ||
595 | { | ||
596 | internal::MotionSetSe3ActionInverse< | ||
597 | 1496 | Op, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(m, iV, jV); | |
598 | } | ||
599 | |||
600 | template<typename Scalar, int Options, typename Mat, typename MatRet> | ||
601 | 988 | static void se3ActionInverse( | |
602 | const SE3Tpl<Scalar, Options> & m, | ||
603 | const Eigen::MatrixBase<Mat> & iV, | ||
604 | Eigen::MatrixBase<MatRet> const & jV) | ||
605 | { | ||
606 | internal::MotionSetSe3ActionInverse< | ||
607 | 988 | SETTO, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(m, iV, jV); | |
608 | } | ||
609 | |||
610 | template<int Op, typename MotionDerived, typename Mat, typename MatRet> | ||
611 | 22582 | static void motionAction( | |
612 | const MotionDense<MotionDerived> & v, | ||
613 | const Eigen::MatrixBase<Mat> & iV, | ||
614 | Eigen::MatrixBase<MatRet> const & jV) | ||
615 | { | ||
616 | 22582 | internal::MotionSetMotionAction<Op, MotionDerived, Mat, MatRet, Mat::ColsAtCompileTime>::run( | |
617 | v, iV, jV); | ||
618 | } | ||
619 | |||
620 | template<typename MotionDerived, typename Mat, typename MatRet> | ||
621 | 61504 | static void motionAction( | |
622 | const MotionDense<MotionDerived> & v, | ||
623 | const Eigen::MatrixBase<Mat> & iV, | ||
624 | Eigen::MatrixBase<MatRet> const & jV) | ||
625 | { | ||
626 | internal::MotionSetMotionAction< | ||
627 | 61504 | SETTO, MotionDerived, Mat, MatRet, Mat::ColsAtCompileTime>::run(v, iV, jV); | |
628 | 61504 | } | |
629 | |||
630 | template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> | ||
631 | 71286 | static void inertiaAction( | |
632 | const InertiaTpl<Scalar, Options> & I, | ||
633 | const Eigen::MatrixBase<Mat> & iV, | ||
634 | Eigen::MatrixBase<MatRet> const & jV) | ||
635 | { | ||
636 | internal::MotionSetInertiaAction< | ||
637 | 71286 | Op, Scalar, Options, Mat, MatRet, MatRet::ColsAtCompileTime>::run(I, iV, jV); | |
638 | } | ||
639 | |||
640 | template<typename Scalar, int Options, typename Mat, typename MatRet> | ||
641 | 125518 | static void inertiaAction( | |
642 | const InertiaTpl<Scalar, Options> & I, | ||
643 | const Eigen::MatrixBase<Mat> & iV, | ||
644 | Eigen::MatrixBase<MatRet> const & jV) | ||
645 | { | ||
646 | internal::MotionSetInertiaAction< | ||
647 | 125518 | SETTO, Scalar, Options, Mat, MatRet, MatRet::ColsAtCompileTime>::run(I, iV, jV); | |
648 | } | ||
649 | |||
650 | template<int Op, typename ForceDerived, typename Mat, typename MatRet> | ||
651 | 18652 | static void act( | |
652 | const Eigen::MatrixBase<Mat> & iV, | ||
653 | const ForceDense<ForceDerived> & f, | ||
654 | Eigen::MatrixBase<MatRet> const & jF) | ||
655 | { | ||
656 | 18652 | internal::MotionSetActOnForce<Op, ForceDerived, Mat, MatRet, MatRet::ColsAtCompileTime>::run( | |
657 | iV, f, jF); | ||
658 | } | ||
659 | |||
660 | template<typename ForceDerived, typename Mat, typename MatRet> | ||
661 | 272 | static void act( | |
662 | const Eigen::MatrixBase<Mat> & iV, | ||
663 | const ForceDense<ForceDerived> & f, | ||
664 | Eigen::MatrixBase<MatRet> const & jF) | ||
665 | { | ||
666 | internal::MotionSetActOnForce< | ||
667 | 272 | SETTO, ForceDerived, Mat, MatRet, MatRet::ColsAtCompileTime>::run(iV, f, jF); | |
668 | } | ||
669 | |||
670 | } // namespace motionSet | ||
671 | |||
672 | } // namespace pinocchio | ||
673 | |||
674 | #endif // ifndef __pinocchio_act_on_set_hxx__ | ||
675 |