GCC Code Coverage Report

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