GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/act-on-set.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 184 184 100.0%
Branches: 94 172 54.7%
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 12650 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
1/2
✓ Branch 2 taken 7435 times.
✗ Branch 3 not taken.
 12650 ForceRefOnMat fin(iF.derived());
44
1/2
✓ Branch 2 taken 7435 times.
✗ Branch 3 not taken.
 12650 ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF));
45
46 switch (Op)
47 {
48 11530 case SETTO:
49
2/4
✓ Branch 1 taken 6875 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6875 times.
✗ Branch 5 not taken.
 11530 fout = m.act(fin);
50 11530 break;
51 1080 case ADDTO:
52
2/4
✓ Branch 1 taken 540 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 540 times.
✗ Branch 5 not taken.
 1080 fout += m.act(fin);
53 1080 break;
54 40 case RMTO:
55
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 fout -= m.act(fin);
56 40 break;
57 default:
58 assert(false && "Wrong Op requesed value");
59 break;
60 }
61 12650 }
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 2478 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
2/2
✓ Branch 1 taken 7435 times.
✓ Branch 2 taken 1350 times.
 15128 for (int col = 0; col < jF.cols(); ++col)
74 {
75
1/2
✓ Branch 2 taken 7435 times.
✗ Branch 3 not taken.
 12650 typename MatRet::ColXpr jFc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF).col(col);
76
2/4
✓ Branch 1 taken 7435 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7435 times.
✗ Branch 5 not taken.
 12650 forceSet::se3Action<Op>(m, iF.col(col), jFc);
77 }
78 2478 }
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 19482 static void run(
96 const MotionDense<MotionDerived> & v, const ForceDense<Fin> & fin, ForceDense<Fout> & fout)
97 {
98 switch (Op)
99 {
100 case SETTO:
101 534 fin.motionAction(v, fout);
102 534 break;
103 case ADDTO:
104
1/2
✓ Branch 2 taken 9434 times.
✗ Branch 3 not taken.
 18868 fout += v.cross(fin);
105 18868 break;
106 case RMTO:
107
1/2
✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
 80 fout -= v.cross(fin);
108 80 break;
109 default:
110 assert(false && "Wrong Op requesed value");
111 break;
112 }
113 19482 }
114
115 120 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
1/2
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
 120 ForceRefOnMat fin(iF.derived());
127
1/2
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
 120 ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF));
128
129
1/2
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
 120 run(v, fin, fout);
130 120 }
131 };
132
133 template<int Op, typename MotionDerived, typename Mat, typename MatRet, int NCOLS>
134 6 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
2/2
✓ Branch 1 taken 60 times.
✓ Branch 2 taken 3 times.
 126 for (int col = 0; col < jF.cols(); ++col)
140 {
141
1/2
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
 120 typename MatRet::ColXpr jFc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF).col(col);
142
2/4
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
 120 forceSet::motionAction<Op>(v, iF.col(col), jFc);
143 }
144 6 }
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 160 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
1/2
✓ Branch 2 taken 80 times.
✗ Branch 3 not taken.
 160 ForceRefOnMat fin(iF.derived());
176
1/2
✓ Branch 2 taken 80 times.
✗ Branch 3 not taken.
 160 ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF));
177
178 switch (Op)
179 {
180 80 case SETTO:
181
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
 80 fout = m.actInv(fin);
182 80 break;
183 40 case ADDTO:
184
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 fout += m.actInv(fin);
185 40 break;
186 40 case RMTO:
187
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 fout -= m.actInv(fin);
188 40 break;
189 default:
190 assert(false && "Wrong Op requesed value");
191 break;
192 }
193 160 }
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 8 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
2/2
✓ Branch 1 taken 80 times.
✓ Branch 2 taken 4 times.
 168 for (int col = 0; col < jF.cols(); ++col)
206 {
207
1/2
✓ Branch 2 taken 80 times.
✗ Branch 3 not taken.
 160 typename MatRet::ColXpr jFc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF).col(col);
208
2/4
✓ Branch 1 taken 80 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80 times.
✗ Branch 5 not taken.
 160 forceSet::se3ActionInverse<Op>(m, iF.col(col), jFc);
209 }
210 8 }
211
212 } // namespace internal
213
214 namespace forceSet
215 {
216 template<int Op, typename Scalar, int Options, typename Mat, typename MatRet>
217 12688 static void se3Action(
218 const SE3Tpl<Scalar, Options> & m,
219 const Eigen::MatrixBase<Mat> & iF,
220 Eigen::MatrixBase<MatRet> const & jF)
221 {
222 12688 internal::ForceSetSe3Action<Op, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(
223 m, iF, jF);
224 12688 }
225
226 template<typename Scalar, int Options, typename Mat, typename MatRet>
227 1357 static void se3Action(
228 const SE3Tpl<Scalar, Options> & m,
229 const Eigen::MatrixBase<Mat> & iF,
230 Eigen::MatrixBase<MatRet> const & jF)
231 {
232 1357 internal::ForceSetSe3Action<SETTO, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(
233 m, iF, jF);
234 1357 }
235
236 template<int Op, typename Scalar, int Options, typename Mat, typename MatRet>
237 164 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 164 Op, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(m, iF, jF);
244 164 }
245
246 template<typename Scalar, int Options, typename Mat, typename MatRet>
247 2 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 2 SETTO, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(m, iF, jF);
254 2 }
255
256 template<int Op, typename MotionDerived, typename Mat, typename MatRet>
257 124 static void motionAction(
258 const MotionDense<MotionDerived> & v,
259 const Eigen::MatrixBase<Mat> & iF,
260 Eigen::MatrixBase<MatRet> const & jF)
261 {
262 124 internal::ForceSetMotionAction<Op, MotionDerived, Mat, MatRet, Mat::ColsAtCompileTime>::run(
263 v, iF, jF);
264 124 }
265
266 template<typename MotionDerived, typename Mat, typename MatRet>
267 1 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 1 SETTO, MotionDerived, Mat, MatRet, Mat::ColsAtCompileTime>::run(v, iF, jF);
274 1 }
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 8255 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
1/2
✓ Branch 2 taken 7080 times.
✗ Branch 3 not taken.
 8255 MotionRefOnMat min(iV.derived());
309
1/2
✓ Branch 2 taken 7080 times.
✗ Branch 3 not taken.
 8255 MotionRefOnMatRet mout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV));
310
311 switch (Op)
312 {
313 8175 case SETTO:
314
2/4
✓ Branch 1 taken 7040 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7040 times.
✗ Branch 5 not taken.
 8175 mout = m.act(min);
315 8175 break;
316 40 case ADDTO:
317
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 mout += m.act(min);
318 40 break;
319 40 case RMTO:
320
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 mout -= m.act(min);
321 40 break;
322 default:
323 assert(false && "Wrong Op requesed value");
324 break;
325 }
326 8255 }
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 1874 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
2/2
✓ Branch 1 taken 7080 times.
✓ Branch 2 taken 1034 times.
 10129 for (int col = 0; col < jV.cols(); ++col)
339 {
340
1/2
✓ Branch 2 taken 7080 times.
✗ Branch 3 not taken.
 8255 typename MatRet::ColXpr jVc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV).col(col);
341
2/4
✓ Branch 1 taken 7080 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7080 times.
✗ Branch 5 not taken.
 8255 motionSet::se3Action<Op>(m, iV.col(col), jVc);
342 }
343 1874 }
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 2049 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 6019 times.
✓ Branch 2 taken 1025 times.
 14084 for (int col = 0; col < jV.cols(); ++col)
363 {
364
1/2
✓ Branch 2 taken 6019 times.
✗ Branch 3 not taken.
 12035 typename MatRet::ColXpr jVc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV).col(col);
365
2/4
✓ Branch 1 taken 6019 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6019 times.
✗ Branch 5 not taken.
 12035 motionSet::motionAction<Op>(v, iV.col(col), jVc);
366 }
367 2049 }
368
369 template<int Op, typename MotionDerived, typename Mat, typename MatRet>
370 struct MotionSetMotionAction<Op, MotionDerived, Mat, MatRet, 1>
371 {
372 90547 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 45275 times.
✗ Branch 3 not taken.
 90547 MotionRefOnMat min(iV.derived());
384
1/2
✓ Branch 2 taken 45275 times.
✗ Branch 3 not taken.
 90547 MotionRefOnMatRet mout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV));
385
386 switch (Op)
387 {
388 74881 case SETTO:
389
1/2
✓ Branch 1 taken 37442 times.
✗ Branch 2 not taken.
 74881 min.motionAction(v, mout);
390 74881 break;
391 15626 case ADDTO:
392
2/4
✓ Branch 1 taken 7813 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7813 times.
✗ Branch 5 not taken.
 15626 mout += v.cross(min);
393 15626 break;
394 40 case RMTO:
395
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 mout -= v.cross(min);
396 40 break;
397 default:
398 assert(false && "Wrong Op requesed value");
399 break;
400 }
401 90547 }
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 3068 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 1534 times.
✗ Branch 3 not taken.
 3068 MotionRefOnMat min(iV.derived());
434
1/2
✓ Branch 2 taken 1534 times.
✗ Branch 3 not taken.
 3068 MotionRefOnMatRet mout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV));
435
436 switch (Op)
437 {
438 2342 case SETTO:
439
2/4
✓ Branch 1 taken 1171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1171 times.
✗ Branch 5 not taken.
 2342 mout = m.actInv(min);
440 2342 break;
441 686 case ADDTO:
442
2/4
✓ Branch 1 taken 343 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 343 times.
✗ Branch 5 not taken.
 686 mout += m.actInv(min);
443 686 break;
444 40 case RMTO:
445
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 mout -= m.actInv(min);
446 40 break;
447 default:
448 assert(false && "Wrong Op requesed value");
449 break;
450 }
451 3068 }
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 422 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 933 times.
✓ Branch 2 taken 211 times.
 2288 for (int col = 0; col < jV.cols(); ++col)
464 {
465
1/2
✓ Branch 2 taken 933 times.
✗ Branch 3 not taken.
 1866 typename MatRet::ColXpr jVc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV).col(col);
466
2/4
✓ Branch 1 taken 933 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 933 times.
✗ Branch 5 not taken.
 1866 motionSet::se3ActionInverse<Op>(m, iV.col(col), jVc);
467 }
468 422 }
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 7319 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 21345 times.
✓ Branch 2 taken 3660 times.
 50006 for (int col = 0; col < jV.cols(); ++col)
488 {
489
1/2
✓ Branch 2 taken 21345 times.
✗ Branch 3 not taken.
 42687 typename MatRet::ColXpr jVc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV).col(col);
490
2/4
✓ Branch 1 taken 21345 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21345 times.
✗ Branch 5 not taken.
 42687 motionSet::inertiaAction<Op>(I, iV.col(col), jVc);
491 }
492 7319 }
493
494 template<int Op, typename Scalar, int Options, typename Mat, typename MatRet>
495 struct MotionSetInertiaAction<Op, Scalar, Options, Mat, MatRet, 1>
496 {
497 202747 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 101439 times.
✗ Branch 3 not taken.
 202747 MotionRefOnMat min(iV.derived());
508
1/2
✓ Branch 2 taken 101439 times.
✗ Branch 3 not taken.
 202747 ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV));
509
510 switch (Op)
511 {
512 158827 case SETTO:
513
1/2
✓ Branch 1 taken 79415 times.
✗ Branch 2 not taken.
 158827 I.__mult__(min, fout);
514 158827 break;
515 43880 case ADDTO:
516
2/4
✓ Branch 1 taken 22004 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22004 times.
✗ Branch 5 not taken.
 43880 fout += I * min;
517 43880 break;
518 40 case RMTO:
519
2/4
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
 40 fout -= I * min;
520 40 break;
521 default:
522 assert(false && "Wrong Op requesed value");
523 break;
524 }
525 202747 }
526 };
527
528 template<int Op, typename ForceDerived, typename Mat, typename MatRet, int NCOLS>
529 struct MotionSetActOnForce
530 {
531 612 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 1869 times.
✓ Branch 2 taken 306 times.
 4350 for (int col = 0; col < jF.cols(); ++col)
537 {
538
1/2
✓ Branch 2 taken 1869 times.
✗ Branch 3 not taken.
 3738 typename MatRet::ColXpr jFc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF).col(col);
539
2/4
✓ Branch 1 taken 1869 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1869 times.
✗ Branch 5 not taken.
 3738 motionSet::act<Op>(iV.col(col), f, jFc);
540 }
541 612 }
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 19362 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 9681 times.
✗ Branch 3 not taken.
 19362 MotionRefOnMat vin(iV.derived());
561
1/2
✓ Branch 2 taken 9681 times.
✗ Branch 3 not taken.
 19362 ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF));
562
1/2
✓ Branch 1 taken 9681 times.
✗ Branch 2 not taken.
 19362 ForceSetMotionAction<Op, MotionRefOnMat, Mat, MatRet, 1>::run(vin, f, fout);
563 19362 }
564 };
565
566 } // namespace internal
567
568 namespace motionSet
569 {
570 template<int Op, typename Scalar, int Options, typename Mat, typename MatRet>
571 8259 static void se3Action(
572 const SE3Tpl<Scalar, Options> & m,
573 const Eigen::MatrixBase<Mat> & iV,
574 Eigen::MatrixBase<MatRet> const & jV)
575 {
576 8259 internal::MotionSetSe3Action<Op, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(
577 m, iV, jV);
578 8259 }
579
580 template<typename Scalar, int Options, typename Mat, typename MatRet>
581 1869 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 1869 SETTO, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(m, iV, jV);
588 1869 }
589
590 template<int Op, typename Scalar, int Options, typename Mat, typename MatRet>
591 2246 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 2246 Op, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(m, iV, jV);
598 2246 }
599
600 template<typename Scalar, int Options, typename Mat, typename MatRet>
601 1242 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 1242 SETTO, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(m, iV, jV);
608 1242 }
609
610 template<int Op, typename MotionDerived, typename Mat, typename MatRet>
611 27577 static void motionAction(
612 const MotionDense<MotionDerived> & v,
613 const Eigen::MatrixBase<Mat> & iV,
614 Eigen::MatrixBase<MatRet> const & jV)
615 {
616 27577 internal::MotionSetMotionAction<Op, MotionDerived, Mat, MatRet, Mat::ColsAtCompileTime>::run(
617 v, iV, jV);
618 27577 }
619
620 template<typename MotionDerived, typename Mat, typename MatRet>
621 65018 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 65018 SETTO, MotionDerived, Mat, MatRet, Mat::ColsAtCompileTime>::run(v, iV, jV);
628 65018 }
629
630 template<int Op, typename Scalar, int Options, typename Mat, typename MatRet>
631 80987 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 80987 Op, Scalar, Options, Mat, MatRet, MatRet::ColsAtCompileTime>::run(I, iV, jV);
638 80987 }
639
640 template<typename Scalar, int Options, typename Mat, typename MatRet>
641 129078 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 129078 SETTO, Scalar, Options, Mat, MatRet, MatRet::ColsAtCompileTime>::run(I, iV, jV);
648 129078 }
649
650 template<int Op, typename ForceDerived, typename Mat, typename MatRet>
651 19592 static void act(
652 const Eigen::MatrixBase<Mat> & iV,
653 const ForceDense<ForceDerived> & f,
654 Eigen::MatrixBase<MatRet> const & jF)
655 {
656 19592 internal::MotionSetActOnForce<Op, ForceDerived, Mat, MatRet, MatRet::ColsAtCompileTime>::run(
657 iV, f, jF);
658 19592 }
659
660 template<typename ForceDerived, typename Mat, typename MatRet>
661 381 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 381 SETTO, ForceDerived, Mat, MatRet, MatRet::ColsAtCompileTime>::run(iV, f, jF);
668 381 }
669
670 } // namespace motionSet
671
672 } // namespace pinocchio
673
674 #endif // ifndef __pinocchio_act_on_set_hxx__
675