| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| 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(const SE3Tpl<Scalar,Options> & m, |
||
22 |
const Eigen::MatrixBase<Mat> & iF, |
||
23 |
Eigen::MatrixBase<MatRet> const & jF); |
||
24 |
|||
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 |
12842 |
static void run(const SE3Tpl<Scalar,Options> & m, |
|
33 |
const Eigen::MatrixBase<Mat> & iF, |
||
34 |
Eigen::MatrixBase<MatRet> const & jF) |
||
35 |
{ |
||
36 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); |
||
37 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); |
||
38 |
|||
39 |
typedef ForceRef<const Mat> ForceRefOnMat; |
||
40 |
typedef ForceRef<MatRet> ForceRefOnMatRet; |
||
41 |
|||
42 |
✓✓✗✓ ✗ |
12842 |
ForceRefOnMat fin(iF.derived()); |
43 |
✓✓✗✓ ✗ |
12842 |
ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF)); |
44 |
|||
45 |
switch(Op) |
||
46 |
{ |
||
47 |
case SETTO: |
||
48 |
✓✗✓✗ |
12762 |
fout = m.act(fin); |
49 |
12762 |
break; |
|
50 |
case ADDTO: |
||
51 |
✓✗✓✗ |
40 |
fout += m.act(fin); |
52 |
40 |
break; |
|
53 |
case RMTO: |
||
54 |
✓✗✓✗ |
40 |
fout -= m.act(fin); |
55 |
40 |
break; |
|
56 |
default: |
||
57 |
assert(false && "Wrong Op requesed value"); |
||
58 |
break; |
||
59 |
} |
||
60 |
12842 |
} |
|
61 |
}; |
||
62 |
|||
63 |
/* Specialized implementation of block action, using colwise operation. It |
||
64 |
* is empirically much faster than the true block operation, although I do |
||
65 |
* not understand why. */ |
||
66 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> |
||
67 |
1428 |
void ForceSetSe3Action<Op,Scalar,Options,Mat,MatRet,NCOLS>:: |
|
68 |
run(const SE3Tpl<Scalar,Options> & m, |
||
69 |
const Eigen::MatrixBase<Mat> & iF, |
||
70 |
Eigen::MatrixBase<MatRet> const & jF) |
||
71 |
{ |
||
72 |
✓✓ | 8734 |
for(int col=0;col<jF.cols();++col) |
73 |
{ |
||
74 |
✓✗ | 7306 |
typename MatRet::ColXpr jFc |
75 |
7306 |
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF).col(col); |
|
76 |
✓✗✓✗ |
7306 |
forceSet::se3Action<Op>(m,iF.col(col),jFc); |
77 |
} |
||
78 |
1428 |
} |
|
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(const MotionDense<MotionDerived> & v, |
||
86 |
const Eigen::MatrixBase<Mat> & iF, |
||
87 |
Eigen::MatrixBase<MatRet> const & jF); |
||
88 |
|||
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 |
13530 |
static void run(const MotionDense<MotionDerived> & v, |
|
96 |
const ForceDense<Fin> & fin, |
||
97 |
ForceDense<Fout> & fout) |
||
98 |
{ |
||
99 |
switch(Op) |
||
100 |
{ |
||
101 |
case SETTO: |
||
102 |
278 |
fin.motionAction(v,fout); |
|
103 |
278 |
break; |
|
104 |
case ADDTO: |
||
105 |
✓✗ | 13172 |
fout += v.cross(fin); |
106 |
13172 |
break; |
|
107 |
case RMTO: |
||
108 |
✓✗ | 80 |
fout -= v.cross(fin); |
109 |
80 |
break; |
|
110 |
default: |
||
111 |
assert(false && "Wrong Op requesed value"); |
||
112 |
break; |
||
113 |
} |
||
114 |
13530 |
} |
|
115 |
|||
116 |
120 |
static void run(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 |
✓✗ | 120 |
ForceRefOnMat fin(iF.derived()); |
127 |
✓✗✓✗ |
120 |
ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF)); |
128 |
|||
129 |
✓✗ | 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>:: |
|
135 |
run(const MotionDense<MotionDerived> & v, |
||
136 |
const Eigen::MatrixBase<Mat> & iF, |
||
137 |
Eigen::MatrixBase<MatRet> const & jF) |
||
138 |
{ |
||
139 |
✓✓ | 126 |
for(int col=0;col<jF.cols();++col) |
140 |
{ |
||
141 |
✓✗ | 120 |
typename MatRet::ColXpr jFc |
142 |
120 |
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF).col(col); |
|
143 |
✓✗✓✗ |
120 |
forceSet::motionAction<Op>(v,iF.col(col),jFc); |
144 |
} |
||
145 |
6 |
} |
|
146 |
|||
147 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> |
||
148 |
struct ForceSetSe3ActionInverse |
||
149 |
{ |
||
150 |
/* Compute jF = jXi * iF, where jXi is the dual action matrix associated |
||
151 |
* with m, and iF, jF are matrices whose columns are forces. The resolution |
||
152 |
* is done by block operation. It is less efficient than the colwise |
||
153 |
* operation and should not be used. */ |
||
154 |
static void run(const SE3Tpl<Scalar,Options> & m, |
||
155 |
const Eigen::MatrixBase<Mat> & iF, |
||
156 |
Eigen::MatrixBase<MatRet> const & jF); |
||
157 |
|||
158 |
}; |
||
159 |
|||
160 |
template<int Op, typename Scalar, int Options, typename Mat , typename MatRet> |
||
161 |
struct ForceSetSe3ActionInverse<Op,Scalar,Options,Mat,MatRet,1> |
||
162 |
{ |
||
163 |
/* Compute jF = jXi * iF, where jXi is the dual action matrix associated with m, |
||
164 |
* and iF, jF are vectors. */ |
||
165 |
160 |
static void run(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 |
✓✗ | 160 |
ForceRefOnMat fin(iF.derived()); |
176 |
✓✗✓✗ |
160 |
ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF)); |
177 |
|||
178 |
switch(Op) |
||
179 |
{ |
||
180 |
case SETTO: |
||
181 |
✓✗✓✗ |
80 |
fout = m.actInv(fin); |
182 |
80 |
break; |
|
183 |
case ADDTO: |
||
184 |
✓✗✓✗ |
40 |
fout += m.actInv(fin); |
185 |
40 |
break; |
|
186 |
case RMTO: |
||
187 |
✓✗✓✗ |
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 |
|||
197 |
/* Specialized implementation of block action, using colwise operation. It |
||
198 |
* is empirically much faster than the true block operation, although I do |
||
199 |
* not understand why. */ |
||
200 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> |
||
201 |
8 |
void ForceSetSe3ActionInverse<Op,Scalar,Options,Mat,MatRet,NCOLS>:: |
|
202 |
run(const SE3Tpl<Scalar,Options> & m, |
||
203 |
const Eigen::MatrixBase<Mat> & iF, |
||
204 |
Eigen::MatrixBase<MatRet> const & jF) |
||
205 |
{ |
||
206 |
✓✓ | 168 |
for(int col=0;col<jF.cols();++col) |
207 |
{ |
||
208 |
✓✗ | 160 |
typename MatRet::ColXpr jFc |
209 |
160 |
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF).col(col); |
|
210 |
✓✗✓✗ |
160 |
forceSet::se3ActionInverse<Op>(m,iF.col(col),jFc); |
211 |
} |
||
212 |
8 |
} |
|
213 |
|||
214 |
} // namespace internal |
||
215 |
|||
216 |
namespace forceSet |
||
217 |
{ |
||
218 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> |
||
219 |
7310 |
static void se3Action(const SE3Tpl<Scalar,Options> & m, |
|
220 |
const Eigen::MatrixBase<Mat> & iF, |
||
221 |
Eigen::MatrixBase<MatRet> const & jF) |
||
222 |
{ |
||
223 |
7310 |
internal::ForceSetSe3Action<Op,Scalar,Options,Mat,MatRet,Mat::ColsAtCompileTime>::run(m,iF,jF); |
|
224 |
7310 |
} |
|
225 |
|||
226 |
template<typename Scalar, int Options, typename Mat, typename MatRet> |
||
227 |
6959 |
static void se3Action(const SE3Tpl<Scalar,Options> & m, |
|
228 |
const Eigen::MatrixBase<Mat> & iF, |
||
229 |
Eigen::MatrixBase<MatRet> const & jF) |
||
230 |
{ |
||
231 |
6959 |
internal::ForceSetSe3Action<SETTO,Scalar,Options,Mat,MatRet,Mat::ColsAtCompileTime>::run(m,iF,jF); |
|
232 |
6959 |
} |
|
233 |
|||
234 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> |
||
235 |
164 |
static void se3ActionInverse(const SE3Tpl<Scalar,Options> & m, |
|
236 |
const Eigen::MatrixBase<Mat> & iF, |
||
237 |
Eigen::MatrixBase<MatRet> const & jF) |
||
238 |
{ |
||
239 |
164 |
internal::ForceSetSe3ActionInverse<Op,Scalar,Options,Mat,MatRet,Mat::ColsAtCompileTime>::run(m,iF,jF); |
|
240 |
164 |
} |
|
241 |
|||
242 |
template<typename Scalar, int Options, typename Mat, typename MatRet> |
||
243 |
2 |
static void se3ActionInverse(const SE3Tpl<Scalar,Options> & m, |
|
244 |
const Eigen::MatrixBase<Mat> & iF, |
||
245 |
Eigen::MatrixBase<MatRet> const & jF) |
||
246 |
{ |
||
247 |
2 |
internal::ForceSetSe3ActionInverse<SETTO,Scalar,Options,Mat,MatRet,Mat::ColsAtCompileTime>::run(m,iF,jF); |
|
248 |
2 |
} |
|
249 |
|||
250 |
template<int Op, typename MotionDerived, typename Mat, typename MatRet> |
||
251 |
124 |
static void motionAction(const MotionDense<MotionDerived> & v, |
|
252 |
const Eigen::MatrixBase<Mat> & iF, |
||
253 |
Eigen::MatrixBase<MatRet> const & jF) |
||
254 |
{ |
||
255 |
124 |
internal::ForceSetMotionAction<Op,MotionDerived,Mat,MatRet,Mat::ColsAtCompileTime>::run(v,iF,jF); |
|
256 |
124 |
} |
|
257 |
|||
258 |
template<typename MotionDerived, typename Mat, typename MatRet> |
||
259 |
1 |
static void motionAction(const MotionDense<MotionDerived> & v, |
|
260 |
const Eigen::MatrixBase<Mat> & iF, |
||
261 |
Eigen::MatrixBase<MatRet> const & jF) |
||
262 |
{ |
||
263 |
1 |
internal::ForceSetMotionAction<SETTO,MotionDerived,Mat,MatRet,Mat::ColsAtCompileTime>::run(v,iF,jF); |
|
264 |
1 |
} |
|
265 |
|||
266 |
} // namespace forceSet |
||
267 |
|||
268 |
namespace internal |
||
269 |
{ |
||
270 |
|||
271 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> |
||
272 |
struct MotionSetSe3Action |
||
273 |
{ |
||
274 |
/* Compute jF = jXi * iF, where jXi is the action matrix associated |
||
275 |
* with m, and iF, jF are matrices whose columns are motions. */ |
||
276 |
static void run(const SE3Tpl<Scalar,Options> & m, |
||
277 |
const Eigen::MatrixBase<Mat> & iF, |
||
278 |
Eigen::MatrixBase<MatRet> const & jF); |
||
279 |
|||
280 |
}; |
||
281 |
|||
282 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> |
||
283 |
struct MotionSetSe3Action<Op,Scalar,Options,Mat,MatRet,1> |
||
284 |
{ |
||
285 |
/* Compute jV = jXi * iV, where jXi is the action matrix associated with m, |
||
286 |
* and iV, jV are 6D vectors representing spatial velocities. */ |
||
287 |
225 |
static void run(const SE3Tpl<Scalar,Options> & m, |
|
288 |
const Eigen::MatrixBase<Mat> & iV, |
||
289 |
Eigen::MatrixBase<MatRet> const & jV) |
||
290 |
{ |
||
291 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); |
||
292 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); |
||
293 |
|||
294 |
typedef MotionRef<const Mat> MotionRefOnMat; |
||
295 |
typedef MotionRef<MatRet> MotionRefOnMatRet; |
||
296 |
|||
297 |
✓✓✗✓ ✗ |
225 |
MotionRefOnMat min(iV.derived()); |
298 |
✓✗✗✓ ✗ |
225 |
MotionRefOnMatRet mout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV)); |
299 |
|||
300 |
switch(Op) |
||
301 |
{ |
||
302 |
case SETTO: |
||
303 |
✓✗✓✗ |
145 |
mout = m.act(min); |
304 |
145 |
break; |
|
305 |
case ADDTO: |
||
306 |
✓✗✓✗ |
40 |
mout += m.act(min); |
307 |
40 |
break; |
|
308 |
case RMTO: |
||
309 |
✓✗✓✗ |
40 |
mout -= m.act(min); |
310 |
40 |
break; |
|
311 |
default: |
||
312 |
assert(false && "Wrong Op requesed value"); |
||
313 |
break; |
||
314 |
} |
||
315 |
225 |
} |
|
316 |
}; |
||
317 |
|||
318 |
/* Specialized implementation of block action, using colwise operation. It |
||
319 |
* is empirically much faster than the true block operation, although I do |
||
320 |
* not understand why. */ |
||
321 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> |
||
322 |
12 |
void MotionSetSe3Action<Op,Scalar,Options,Mat,MatRet,NCOLS>:: |
|
323 |
run(const SE3Tpl<Scalar,Options> & m, |
||
324 |
const Eigen::MatrixBase<Mat> & iV, |
||
325 |
Eigen::MatrixBase<MatRet> const & jV) |
||
326 |
{ |
||
327 |
✓✓ | 237 |
for(int col=0;col<jV.cols();++col) |
328 |
{ |
||
329 |
✓✗ | 225 |
typename MatRet::ColXpr jVc |
330 |
225 |
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV).col(col); |
|
331 |
✓✗✓✗ |
225 |
motionSet::se3Action<Op>(m,iV.col(col),jVc); |
332 |
} |
||
333 |
12 |
} |
|
334 |
|||
335 |
template<int Op, typename MotionDerived, typename Mat, typename MatRet, int NCOLS> |
||
336 |
struct MotionSetMotionAction |
||
337 |
{ |
||
338 |
/* Compute dV = v ^ V, where is the action operation associated |
||
339 |
* with v, and V, dV are matrices whose columns are motions. */ |
||
340 |
static void run(const MotionDense<MotionDerived> & v, |
||
341 |
const Eigen::MatrixBase<Mat> & iV, |
||
342 |
Eigen::MatrixBase<MatRet> const & jV); |
||
343 |
|||
344 |
}; |
||
345 |
|||
346 |
template<int Op, typename MotionDerived, typename Mat, typename MatRet, int NCOLS> |
||
347 |
1505 |
void MotionSetMotionAction<Op,MotionDerived,Mat,MatRet,NCOLS>:: |
|
348 |
run(const MotionDense<MotionDerived> & v, |
||
349 |
const Eigen::MatrixBase<Mat> & iV, |
||
350 |
Eigen::MatrixBase<MatRet> const & jV) |
||
351 |
{ |
||
352 |
✓✓ | 10508 |
for(int col=0;col<jV.cols();++col) |
353 |
{ |
||
354 |
✓✗ | 9003 |
typename MatRet::ColXpr jVc |
355 |
9003 |
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV).col(col); |
|
356 |
✓✗✓✗ |
9003 |
motionSet::motionAction<Op>(v,iV.col(col),jVc); |
357 |
} |
||
358 |
1505 |
} |
|
359 |
|||
360 |
template<int Op, typename MotionDerived, typename Mat, typename MatRet> |
||
361 |
struct MotionSetMotionAction<Op,MotionDerived,Mat,MatRet,1> |
||
362 |
{ |
||
363 |
65941 |
static void run(const MotionDense<MotionDerived> & v, |
|
364 |
const Eigen::MatrixBase<Mat> & iV, |
||
365 |
Eigen::MatrixBase<MatRet> const & jV) |
||
366 |
{ |
||
367 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); |
||
368 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); |
||
369 |
|||
370 |
typedef MotionRef<const Mat> MotionRefOnMat; |
||
371 |
typedef MotionRef<MatRet> MotionRefOnMatRet; |
||
372 |
|||
373 |
✓✓✗✓ ✗ |
65941 |
MotionRefOnMat min(iV.derived()); |
374 |
✓✓✗✓ ✗ |
65941 |
MotionRefOnMatRet mout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV)); |
375 |
|||
376 |
switch(Op) |
||
377 |
{ |
||
378 |
case SETTO: |
||
379 |
✓✗ | 54775 |
min.motionAction(v,mout); |
380 |
54775 |
break; |
|
381 |
case ADDTO: |
||
382 |
✓✗✓✗ |
11126 |
mout += v.cross(min); |
383 |
11126 |
break; |
|
384 |
case RMTO: |
||
385 |
✓✗✓✗ |
40 |
mout -= v.cross(min); |
386 |
40 |
break; |
|
387 |
default: |
||
388 |
assert(false && "Wrong Op requesed value"); |
||
389 |
break; |
||
390 |
} |
||
391 |
65941 |
} |
|
392 |
}; |
||
393 |
|||
394 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> |
||
395 |
struct MotionSetSe3ActionInverse |
||
396 |
{ |
||
397 |
/* Compute jF = jXi * iF, where jXi is the action matrix associated |
||
398 |
* with m, and iF, jF are matrices whose columns are motions. The resolution |
||
399 |
* is done by block operation. It is less efficient than the colwise |
||
400 |
* operation and should not be used. */ |
||
401 |
static void run(const SE3Tpl<Scalar,Options> & m, |
||
402 |
const Eigen::MatrixBase<Mat> & iF, |
||
403 |
Eigen::MatrixBase<MatRet> const & jF); |
||
404 |
|||
405 |
}; |
||
406 |
|||
407 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> |
||
408 |
struct MotionSetSe3ActionInverse<Op,Scalar,Options,Mat,MatRet,1> |
||
409 |
{ |
||
410 |
/* Compute jV = jXi * iV, where jXi is the action matrix associated with m, |
||
411 |
* and iV, jV are 6D vectors representing spatial velocities. */ |
||
412 |
773 |
static void run(const SE3Tpl<Scalar,Options> & m, |
|
413 |
const Eigen::MatrixBase<Mat> & iV, |
||
414 |
Eigen::MatrixBase<MatRet> const & jV) |
||
415 |
{ |
||
416 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); |
||
417 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); |
||
418 |
|||
419 |
typedef MotionRef<Mat> MotionRefOnMat; |
||
420 |
typedef MotionRef<MatRet> MotionRefOnMatRet; |
||
421 |
|||
422 |
✓✓✗✓ ✗ |
773 |
MotionRefOnMat min(iV.derived()); |
423 |
✓✓✗✓ ✗ |
773 |
MotionRefOnMatRet mout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV)); |
424 |
|||
425 |
switch(Op) |
||
426 |
{ |
||
427 |
case SETTO: |
||
428 |
✓✗✓✗ |
461 |
mout = m.actInv(min); |
429 |
461 |
break; |
|
430 |
case ADDTO: |
||
431 |
✓✗✓✗ |
272 |
mout += m.actInv(min); |
432 |
272 |
break; |
|
433 |
case RMTO: |
||
434 |
✓✗✓✗ |
40 |
mout -= m.actInv(min); |
435 |
40 |
break; |
|
436 |
default: |
||
437 |
assert(false && "Wrong Op requesed value"); |
||
438 |
break; |
||
439 |
} |
||
440 |
773 |
} |
|
441 |
}; |
||
442 |
|||
443 |
/* Specialized implementation of block action, using colwise operation. It |
||
444 |
* is empirically much faster than the true block operation, although I do |
||
445 |
* not understand why. */ |
||
446 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> |
||
447 |
65 |
void MotionSetSe3ActionInverse<Op,Scalar,Options,Mat,MatRet,NCOLS>:: |
|
448 |
run(const SE3Tpl<Scalar,Options> & m, |
||
449 |
const Eigen::MatrixBase<Mat> & iV, |
||
450 |
Eigen::MatrixBase<MatRet> const & jV) |
||
451 |
{ |
||
452 |
✓✓ | 558 |
for(int col=0;col<jV.cols();++col) |
453 |
{ |
||
454 |
✓✗ | 493 |
typename MatRet::ColXpr jVc |
455 |
493 |
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV).col(col); |
|
456 |
✓✗✓✗ |
493 |
motionSet::se3ActionInverse<Op>(m,iV.col(col),jVc); |
457 |
} |
||
458 |
65 |
} |
|
459 |
|||
460 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> |
||
461 |
struct MotionSetInertiaAction |
||
462 |
{ |
||
463 |
/* Compute dV = v ^ V, where is the action operation associated |
||
464 |
* with v, and V, dV are matrices whose columns are motions. */ |
||
465 |
static void run(const InertiaTpl<Scalar,Options> & I, |
||
466 |
const Eigen::MatrixBase<Mat> & iV, |
||
467 |
Eigen::MatrixBase<MatRet> const & jV); |
||
468 |
|||
469 |
}; |
||
470 |
|||
471 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet, int NCOLS> |
||
472 |
13033 |
void MotionSetInertiaAction<Op,Scalar,Options,Mat,MatRet,NCOLS>:: |
|
473 |
run(const InertiaTpl<Scalar,Options> & I, |
||
474 |
const Eigen::MatrixBase<Mat> & iV, |
||
475 |
Eigen::MatrixBase<MatRet> const & jV) |
||
476 |
{ |
||
477 |
✓✓ | 37238 |
for(int col=0;col<jV.cols();++col) |
478 |
{ |
||
479 |
✓✗ | 24205 |
typename MatRet::ColXpr jVc |
480 |
24205 |
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV).col(col); |
|
481 |
✓✗✓✗ |
24205 |
motionSet::inertiaAction<Op>(I,iV.col(col),jVc); |
482 |
} |
||
483 |
13033 |
} |
|
484 |
|||
485 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> |
||
486 |
struct MotionSetInertiaAction<Op,Scalar,Options,Mat,MatRet,1> |
||
487 |
{ |
||
488 |
79781 |
static void run(const InertiaTpl<Scalar,Options> & I, |
|
489 |
const Eigen::MatrixBase<Mat> & iV, |
||
490 |
Eigen::MatrixBase<MatRet> const & jV) |
||
491 |
{ |
||
492 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); |
||
493 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); |
||
494 |
|||
495 |
typedef MotionRef<const Mat> MotionRefOnMat; |
||
496 |
typedef ForceRef<MatRet> ForceRefOnMatRet; |
||
497 |
✓✓✗✓ ✗ |
79781 |
MotionRefOnMat min(iV.derived()); |
498 |
✓✓✗✓ ✗ |
79781 |
ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jV)); |
499 |
|||
500 |
switch(Op) |
||
501 |
{ |
||
502 |
case SETTO: |
||
503 |
✓✗ | 45261 |
I.__mult__(min,fout); |
504 |
45261 |
break; |
|
505 |
256 |
case ADDTO: |
|
506 |
✓✗✓✗ |
34480 |
fout += I*min; |
507 |
34480 |
break; |
|
508 |
case RMTO: |
||
509 |
✓✗✓✗ |
40 |
fout -= I*min; |
510 |
40 |
break; |
|
511 |
default: |
||
512 |
assert(false && "Wrong Op requesed value"); |
||
513 |
break; |
||
514 |
} |
||
515 |
79781 |
} |
|
516 |
}; |
||
517 |
|||
518 |
template<int Op, typename ForceDerived, typename Mat, typename MatRet, int NCOLS> |
||
519 |
struct MotionSetActOnForce |
||
520 |
{ |
||
521 |
426 |
static void run(const Eigen::MatrixBase<Mat> & iV, |
|
522 |
const ForceDense<ForceDerived> & f, |
||
523 |
Eigen::MatrixBase<MatRet> const & jF) |
||
524 |
{ |
||
525 |
✓✓ | 3048 |
for(int col=0;col<jF.cols();++col) |
526 |
{ |
||
527 |
✓✗ | 2622 |
typename MatRet::ColXpr jFc |
528 |
2622 |
= PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF).col(col); |
|
529 |
✓✗✓✗ |
2622 |
motionSet::act<Op>(iV.col(col),f,jFc); |
530 |
} |
||
531 |
426 |
} |
|
532 |
|||
533 |
}; |
||
534 |
|||
535 |
template<int Op, typename ForceDerived, typename Mat, typename MatRet> |
||
536 |
struct MotionSetActOnForce<Op,ForceDerived,Mat,MatRet,1> |
||
537 |
{ |
||
538 |
/* Compute jF = jXi * iF, where jXi is the dual action matrix associated with m, |
||
539 |
* and iF, jF are vectors. */ |
||
540 |
13410 |
static void run(const Eigen::MatrixBase<Mat> & iV, |
|
541 |
const ForceDense<ForceDerived> & f, |
||
542 |
Eigen::MatrixBase<MatRet> const & jF) |
||
543 |
{ |
||
544 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); |
||
545 |
EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); |
||
546 |
|||
547 |
typedef MotionRef<const Mat> MotionRefOnMat; |
||
548 |
typedef ForceRef<MatRet> ForceRefOnMatRet; |
||
549 |
|||
550 |
✓✓✗✓ ✗ |
13410 |
MotionRefOnMat vin(iV.derived()); |
551 |
✓✓✗✓ ✗ |
13410 |
ForceRefOnMatRet fout(PINOCCHIO_EIGEN_CONST_CAST(MatRet,jF)); |
552 |
✓✗ | 13410 |
ForceSetMotionAction<Op,MotionRefOnMat,Mat,MatRet,1>::run(vin,f,fout); |
553 |
13410 |
} |
|
554 |
|||
555 |
}; |
||
556 |
|||
557 |
|||
558 |
} // namespace internal |
||
559 |
|||
560 |
namespace motionSet |
||
561 |
{ |
||
562 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> |
||
563 |
229 |
static void se3Action(const SE3Tpl<Scalar,Options> & m, |
|
564 |
const Eigen::MatrixBase<Mat> & iV, |
||
565 |
Eigen::MatrixBase<MatRet> const & jV) |
||
566 |
{ |
||
567 |
229 |
internal::MotionSetSe3Action<Op,Scalar,Options,Mat,MatRet,Mat::ColsAtCompileTime>::run(m,iV,jV); |
|
568 |
229 |
} |
|
569 |
|||
570 |
template<typename Scalar, int Options, typename Mat, typename MatRet> |
||
571 |
7 |
static void se3Action(const SE3Tpl<Scalar,Options> & m, |
|
572 |
const Eigen::MatrixBase<Mat> & iV, |
||
573 |
Eigen::MatrixBase<MatRet> const & jV) |
||
574 |
{ |
||
575 |
7 |
internal::MotionSetSe3Action<SETTO,Scalar,Options,Mat,MatRet,Mat::ColsAtCompileTime>::run(m,iV,jV); |
|
576 |
7 |
} |
|
577 |
|||
578 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> |
||
579 |
629 |
static void se3ActionInverse(const SE3Tpl<Scalar,Options> & m, |
|
580 |
const Eigen::MatrixBase<Mat> & iV, |
||
581 |
Eigen::MatrixBase<MatRet> const & jV) |
||
582 |
{ |
||
583 |
629 |
internal::MotionSetSe3ActionInverse<Op,Scalar,Options,Mat,MatRet,Mat::ColsAtCompileTime>::run(m,iV,jV); |
|
584 |
629 |
} |
|
585 |
|||
586 |
template<typename Scalar, int Options, typename Mat, typename MatRet> |
||
587 |
219 |
static void se3ActionInverse(const SE3Tpl<Scalar,Options> & m, |
|
588 |
const Eigen::MatrixBase<Mat> & iV, |
||
589 |
Eigen::MatrixBase<MatRet> const & jV) |
||
590 |
{ |
||
591 |
219 |
internal::MotionSetSe3ActionInverse<SETTO,Scalar,Options,Mat,MatRet,Mat::ColsAtCompileTime>::run(m,iV,jV); |
|
592 |
219 |
} |
|
593 |
|||
594 |
template<int Op, typename MotionDerived, typename Mat, typename MatRet> |
||
595 |
19945 |
static void motionAction(const MotionDense<MotionDerived> & v, |
|
596 |
const Eigen::MatrixBase<Mat> & iV, |
||
597 |
Eigen::MatrixBase<MatRet> const & jV) |
||
598 |
{ |
||
599 |
19945 |
internal::MotionSetMotionAction<Op,MotionDerived,Mat,MatRet,Mat::ColsAtCompileTime>::run(v,iV,jV); |
|
600 |
19945 |
} |
|
601 |
|||
602 |
template<typename MotionDerived, typename Mat, typename MatRet> |
||
603 |
47500 |
static void motionAction(const MotionDense<MotionDerived> & v, |
|
604 |
const Eigen::MatrixBase<Mat> & iV, |
||
605 |
Eigen::MatrixBase<MatRet> const & jV) |
||
606 |
{ |
||
607 |
47500 |
internal::MotionSetMotionAction<SETTO,MotionDerived,Mat,MatRet,Mat::ColsAtCompileTime>::run(v,iV,jV); |
|
608 |
47500 |
} |
|
609 |
|||
610 |
template<int Op, typename Scalar, int Options, typename Mat, typename MatRet> |
||
611 |
54629 |
static void inertiaAction(const InertiaTpl<Scalar,Options> & I, |
|
612 |
const Eigen::MatrixBase<Mat> & iV, |
||
613 |
Eigen::MatrixBase<MatRet> const & jV) |
||
614 |
{ |
||
615 |
54629 |
internal::MotionSetInertiaAction<Op,Scalar,Options,Mat,MatRet,MatRet::ColsAtCompileTime>::run(I,iV,jV); |
|
616 |
54629 |
} |
|
617 |
|||
618 |
template<typename Scalar, int Options, typename Mat, typename MatRet> |
||
619 |
38184 |
static void inertiaAction(const InertiaTpl<Scalar,Options> & I, |
|
620 |
const Eigen::MatrixBase<Mat> & iV, |
||
621 |
Eigen::MatrixBase<MatRet> const & jV) |
||
622 |
{ |
||
623 |
38184 |
internal::MotionSetInertiaAction<SETTO,Scalar,Options,Mat,MatRet,MatRet::ColsAtCompileTime>::run(I,iV,jV); |
|
624 |
38184 |
} |
|
625 |
|||
626 |
template<int Op, typename ForceDerived, typename Mat, typename MatRet> |
||
627 |
13670 |
static void act(const Eigen::MatrixBase<Mat> & iV, |
|
628 |
const ForceDense<ForceDerived> & f, |
||
629 |
Eigen::MatrixBase<MatRet> const & jF) |
||
630 |
{ |
||
631 |
13670 |
internal::MotionSetActOnForce<Op,ForceDerived,Mat,MatRet,MatRet::ColsAtCompileTime>::run(iV,f,jF); |
|
632 |
13670 |
} |
|
633 |
|||
634 |
template<typename ForceDerived, typename Mat, typename MatRet> |
||
635 |
165 |
static void act(const Eigen::MatrixBase<Mat> & iV, |
|
636 |
const ForceDense<ForceDerived> & f, |
||
637 |
Eigen::MatrixBase<MatRet> const & jF) |
||
638 |
{ |
||
639 |
165 |
internal::MotionSetActOnForce<SETTO,ForceDerived,Mat,MatRet,MatRet::ColsAtCompileTime>::run(iV,f,jF); |
|
640 |
165 |
} |
|
641 |
|||
642 |
} // namespace motionSet |
||
643 |
|||
644 |
} // namespace pinocchio |
||
645 |
|||
646 |
#endif // ifndef __pinocchio_act_on_set_hxx__ |
| Generated by: GCOVR (Version 4.2) |