GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/act-on-set.hxx
Date: 2025-02-12 21:03:38
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 4927 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 4867 times.
✗ Branch 3 not taken.
 4927 ForceRefOnMat fin(iF.derived());
44
1/2
✓ Branch 2 taken 4867 times.
✗ Branch 3 not taken.
 4927 ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF));
45
46 switch (Op)
47 {
48 4847 case SETTO:
49
2/4
✓ Branch 1 taken 4827 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4827 times.
✗ Branch 5 not taken.
 4847 fout = m.act(fin);
50 4847 break;
51 40 case ADDTO:
52
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);
53 40 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 4927 }
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 807 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 4867 times.
✓ Branch 2 taken 804 times.
 5734 for (int col = 0; col < jF.cols(); ++col)
74 {
75
1/2
✓ Branch 2 taken 4867 times.
✗ Branch 3 not taken.
 4927 typename MatRet::ColXpr jFc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jF).col(col);
76
2/4
✓ Branch 1 taken 4867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4867 times.
✗ Branch 5 not taken.
 4927 forceSet::se3Action<Op>(m, iF.col(col), jFc);
77 }
78 807 }
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 4931 static void se3Action(
218 const SE3Tpl<Scalar, Options> & m,
219 const Eigen::MatrixBase<Mat> & iF,
220 Eigen::MatrixBase<MatRet> const & jF)
221 {
222 4931 internal::ForceSetSe3Action<Op, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(
223 m, iF, jF);
224 4931 }
225
226 template<typename Scalar, int Options, typename Mat, typename MatRet>
227 802 static void se3Action(
228 const SE3Tpl<Scalar, Options> & m,
229 const Eigen::MatrixBase<Mat> & iF,
230 Eigen::MatrixBase<MatRet> const & jF)
231 {
232 802 internal::ForceSetSe3Action<SETTO, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(
233 m, iF, jF);
234 802 }
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 241 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 181 times.
✗ Branch 3 not taken.
 241 MotionRefOnMat min(iV.derived());
309
1/2
✓ Branch 2 taken 181 times.
✗ Branch 3 not taken.
 241 MotionRefOnMatRet mout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV));
310
311 switch (Op)
312 {
313 161 case SETTO:
314
2/4
✓ Branch 1 taken 141 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 141 times.
✗ Branch 5 not taken.
 161 mout = m.act(min);
315 161 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 241 }
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 20 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 181 times.
✓ Branch 2 taken 17 times.
 261 for (int col = 0; col < jV.cols(); ++col)
339 {
340
1/2
✓ Branch 2 taken 181 times.
✗ Branch 3 not taken.
 241 typename MatRet::ColXpr jVc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV).col(col);
341
2/4
✓ Branch 1 taken 181 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 181 times.
✗ Branch 5 not taken.
 241 motionSet::se3Action<Op>(m, iV.col(col), jVc);
342 }
343 20 }
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 2009 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 5999 times.
✓ Branch 2 taken 1005 times.
 14004 for (int col = 0; col < jV.cols(); ++col)
363 {
364
1/2
✓ Branch 2 taken 5999 times.
✗ Branch 3 not taken.
 11995 typename MatRet::ColXpr jVc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV).col(col);
365
2/4
✓ Branch 1 taken 5999 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5999 times.
✗ Branch 5 not taken.
 11995 motionSet::motionAction<Op>(v, iV.col(col), jVc);
366 }
367 2009 }
368
369 template<int Op, typename MotionDerived, typename Mat, typename MatRet>
370 struct MotionSetMotionAction<Op, MotionDerived, Mat, MatRet, 1>
371 {
372 90515 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 45259 times.
✗ Branch 3 not taken.
 90515 MotionRefOnMat min(iV.derived());
384
1/2
✓ Branch 2 taken 45259 times.
✗ Branch 3 not taken.
 90515 MotionRefOnMatRet mout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV));
385
386 switch (Op)
387 {
388 74849 case SETTO:
389
1/2
✓ Branch 1 taken 37426 times.
✗ Branch 2 not taken.
 74849 min.motionAction(v, mout);
390 74849 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 90515 }
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 2583 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 1296 times.
✗ Branch 3 not taken.
 2583 MotionRefOnMat min(iV.derived());
434
1/2
✓ Branch 2 taken 1296 times.
✗ Branch 3 not taken.
 2583 MotionRefOnMatRet mout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV));
435
436 switch (Op)
437 {
438 1857 case SETTO:
439
2/4
✓ Branch 1 taken 933 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 933 times.
✗ Branch 5 not taken.
 1857 mout = m.actInv(min);
440 1857 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 2583 }
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 213 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 695 times.
✓ Branch 2 taken 108 times.
 1594 for (int col = 0; col < jV.cols(); ++col)
464 {
465
1/2
✓ Branch 2 taken 695 times.
✗ Branch 3 not taken.
 1381 typename MatRet::ColXpr jVc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV).col(col);
466
2/4
✓ Branch 1 taken 695 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 695 times.
✗ Branch 5 not taken.
 1381 motionSet::se3ActionInverse<Op>(m, iV.col(col), jVc);
467 }
468 213 }
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 7105 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 21078 times.
✓ Branch 2 taken 3553 times.
 49258 for (int col = 0; col < jV.cols(); ++col)
488 {
489
1/2
✓ Branch 2 taken 21078 times.
✗ Branch 3 not taken.
 42153 typename MatRet::ColXpr jVc = PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV).col(col);
490
2/4
✓ Branch 1 taken 21078 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 21078 times.
✗ Branch 5 not taken.
 42153 motionSet::inertiaAction<Op>(I, iV.col(col), jVc);
491 }
492 7105 }
493
494 template<int Op, typename Scalar, int Options, typename Mat, typename MatRet>
495 struct MotionSetInertiaAction<Op, Scalar, Options, Mat, MatRet, 1>
496 {
497 200597 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 100364 times.
✗ Branch 3 not taken.
 200597 MotionRefOnMat min(iV.derived());
508
1/2
✓ Branch 2 taken 100364 times.
✗ Branch 3 not taken.
 200597 ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet, jV));
509
510 switch (Op)
511 {
512 161071 case SETTO:
513
1/2
✓ Branch 1 taken 80537 times.
✗ Branch 2 not taken.
 161071 I.__mult__(min, fout);
514 161071 break;
515 39486 case ADDTO:
516
2/4
✓ Branch 1 taken 19807 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 19807 times.
✗ Branch 5 not taken.
 39486 fout += I * min;
517 39486 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 200597 }
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 245 static void se3Action(
572 const SE3Tpl<Scalar, Options> & m,
573 const Eigen::MatrixBase<Mat> & iV,
574 Eigen::MatrixBase<MatRet> const & jV)
575 {
576 245 internal::MotionSetSe3Action<Op, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(
577 m, iV, jV);
578 245 }
579
580 template<typename Scalar, int Options, typename Mat, typename MatRet>
581 15 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 15 SETTO, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(m, iV, jV);
588 15 }
589
590 template<int Op, typename Scalar, int Options, typename Mat, typename MatRet>
591 1761 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 1761 Op, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(m, iV, jV);
598 1761 }
599
600 template<typename Scalar, int Options, typename Mat, typename MatRet>
601 1033 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 1033 SETTO, Scalar, Options, Mat, MatRet, Mat::ColsAtCompileTime>::run(m, iV, jV);
608 1033 }
609
610 template<int Op, typename MotionDerived, typename Mat, typename MatRet>
611 27537 static void motionAction(
612 const MotionDense<MotionDerived> & v,
613 const Eigen::MatrixBase<Mat> & iV,
614 Eigen::MatrixBase<MatRet> const & jV)
615 {
616 27537 internal::MotionSetMotionAction<Op, MotionDerived, Mat, MatRet, Mat::ColsAtCompileTime>::run(
617 v, iV, jV);
618 27537 }
619
620 template<typename MotionDerived, typename Mat, typename MatRet>
621 64986 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 64986 SETTO, MotionDerived, Mat, MatRet, Mat::ColsAtCompileTime>::run(v, iV, jV);
628 64986 }
629
630 template<int Op, typename Scalar, int Options, typename Mat, typename MatRet>
631 76875 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 76875 Op, Scalar, Options, Mat, MatRet, MatRet::ColsAtCompileTime>::run(I, iV, jV);
638 76875 }
639
640 template<typename Scalar, int Options, typename Mat, typename MatRet>
641 130826 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 130826 SETTO, Scalar, Options, Mat, MatRet, MatRet::ColsAtCompileTime>::run(I, iV, jV);
648 130826 }
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