GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2019 CNRS INRIA |
||
3 |
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. |
||
4 |
// |
||
5 |
|||
6 |
#ifndef __pinocchio_joint_translation_hpp__ |
||
7 |
#define __pinocchio_joint_translation_hpp__ |
||
8 |
|||
9 |
#include "pinocchio/macros.hpp" |
||
10 |
#include "pinocchio/multibody/joint/joint-base.hpp" |
||
11 |
#include "pinocchio/multibody/constraint.hpp" |
||
12 |
#include "pinocchio/spatial/inertia.hpp" |
||
13 |
#include "pinocchio/spatial/skew.hpp" |
||
14 |
|||
15 |
namespace pinocchio |
||
16 |
{ |
||
17 |
|||
18 |
template<typename Scalar, int Options=0> struct MotionTranslationTpl; |
||
19 |
typedef MotionTranslationTpl<double> MotionTranslation; |
||
20 |
|||
21 |
template<typename Scalar, int Options> |
||
22 |
struct SE3GroupAction< MotionTranslationTpl<Scalar,Options> > |
||
23 |
{ |
||
24 |
typedef MotionTpl<Scalar,Options> ReturnType; |
||
25 |
}; |
||
26 |
|||
27 |
template<typename Scalar, int Options, typename MotionDerived> |
||
28 |
struct MotionAlgebraAction< MotionTranslationTpl<Scalar,Options>, MotionDerived> |
||
29 |
{ |
||
30 |
typedef MotionTpl<Scalar,Options> ReturnType; |
||
31 |
}; |
||
32 |
|||
33 |
template<typename _Scalar, int _Options> |
||
34 |
struct traits< MotionTranslationTpl<_Scalar,_Options> > |
||
35 |
{ |
||
36 |
typedef _Scalar Scalar; |
||
37 |
enum { Options = _Options }; |
||
38 |
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3; |
||
39 |
typedef Eigen::Matrix<Scalar,6,1,Options> Vector6; |
||
40 |
typedef Eigen::Matrix<Scalar,4,4,Options> Matrix4; |
||
41 |
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6; |
||
42 |
typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType; |
||
43 |
typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType; |
||
44 |
typedef Vector3 AngularType; |
||
45 |
typedef Vector3 LinearType; |
||
46 |
typedef const Vector3 ConstAngularType; |
||
47 |
typedef const Vector3 ConstLinearType; |
||
48 |
typedef Matrix6 ActionMatrixType; |
||
49 |
typedef Matrix4 HomogeneousMatrixType; |
||
50 |
typedef MotionTpl<Scalar,Options> MotionPlain; |
||
51 |
typedef MotionPlain PlainReturnType; |
||
52 |
enum { |
||
53 |
LINEAR = 0, |
||
54 |
ANGULAR = 3 |
||
55 |
}; |
||
56 |
}; // traits MotionTranslationTpl |
||
57 |
|||
58 |
template<typename _Scalar, int _Options> |
||
59 |
struct MotionTranslationTpl |
||
60 |
: MotionBase< MotionTranslationTpl<_Scalar,_Options> > |
||
61 |
{ |
||
62 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
63 |
|||
64 |
MOTION_TYPEDEF_TPL(MotionTranslationTpl); |
||
65 |
|||
66 |
7 |
MotionTranslationTpl() {} |
|
67 |
|||
68 |
template<typename Vector3Like> |
||
69 |
3137 |
MotionTranslationTpl(const Eigen::MatrixBase<Vector3Like> & v) |
|
70 |
3137 |
: m_v(v) |
|
71 |
3137 |
{} |
|
72 |
|||
73 |
36 |
MotionTranslationTpl(const MotionTranslationTpl & other) |
|
74 |
36 |
: m_v(other.m_v) |
|
75 |
36 |
{} |
|
76 |
|||
77 |
Vector3 & operator()() { return m_v; } |
||
78 |
const Vector3 & operator()() const { return m_v; } |
||
79 |
|||
80 |
1028 |
inline PlainReturnType plain() const |
|
81 |
{ |
||
82 |
✓✗ | 1028 |
return PlainReturnType(m_v,PlainReturnType::Vector3::Zero()); |
83 |
} |
||
84 |
|||
85 |
17 |
bool isEqual_impl(const MotionTranslationTpl & other) const |
|
86 |
{ |
||
87 |
17 |
return m_v == other.m_v; |
|
88 |
} |
||
89 |
|||
90 |
MotionTranslationTpl & operator=(const MotionTranslationTpl & other) |
||
91 |
{ |
||
92 |
m_v = other.m_v; |
||
93 |
return *this; |
||
94 |
} |
||
95 |
|||
96 |
template<typename Derived> |
||
97 |
2 |
void addTo(MotionDense<Derived> & other) const |
|
98 |
{ |
||
99 |
✓✗ | 2 |
other.linear() += m_v; |
100 |
2 |
} |
|
101 |
|||
102 |
template<typename Derived> |
||
103 |
6 |
void setTo(MotionDense<Derived> & other) const |
|
104 |
{ |
||
105 |
✓✗ | 6 |
other.linear() = m_v; |
106 |
✓✗ | 6 |
other.angular().setZero(); |
107 |
6 |
} |
|
108 |
|||
109 |
template<typename S2, int O2, typename D2> |
||
110 |
1 |
void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const |
|
111 |
{ |
||
112 |
✓✗ | 1 |
v.angular().setZero(); |
113 |
✓✗✓✗ ✓✗ |
1 |
v.linear().noalias() = m.rotation() * m_v; // TODO: check efficiency |
114 |
1 |
} |
|
115 |
|||
116 |
template<typename S2, int O2> |
||
117 |
1 |
MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const |
|
118 |
{ |
||
119 |
1 |
MotionPlain res; |
|
120 |
1 |
se3Action_impl(m,res); |
|
121 |
1 |
return res; |
|
122 |
} |
||
123 |
|||
124 |
template<typename S2, int O2, typename D2> |
||
125 |
1 |
void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const |
|
126 |
{ |
||
127 |
// Linear |
||
128 |
✓✗✓✗ ✓✗✓✗ |
1 |
v.linear().noalias() = m.rotation().transpose() * m_v; |
129 |
|||
130 |
// Angular |
||
131 |
✓✗ | 1 |
v.angular().setZero(); |
132 |
1 |
} |
|
133 |
|||
134 |
template<typename S2, int O2> |
||
135 |
1 |
MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const |
|
136 |
{ |
||
137 |
1 |
MotionPlain res; |
|
138 |
1 |
se3ActionInverse_impl(m,res); |
|
139 |
1 |
return res; |
|
140 |
} |
||
141 |
|||
142 |
template<typename M1, typename M2> |
||
143 |
4 |
void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const |
|
144 |
{ |
||
145 |
// Linear |
||
146 |
✓✗✓✗ ✓✗✓✗ |
4 |
mout.linear().noalias() = v.angular().cross(m_v); |
147 |
|||
148 |
// Angular |
||
149 |
✓✗ | 4 |
mout.angular().setZero(); |
150 |
4 |
} |
|
151 |
|||
152 |
template<typename M1> |
||
153 |
4 |
MotionPlain motionAction(const MotionDense<M1> & v) const |
|
154 |
{ |
||
155 |
4 |
MotionPlain res; |
|
156 |
4 |
motionAction(v,res); |
|
157 |
4 |
return res; |
|
158 |
} |
||
159 |
|||
160 |
const Vector3 & linear() const { return m_v; } |
||
161 |
1062 |
Vector3 & linear() { return m_v; } |
|
162 |
|||
163 |
protected: |
||
164 |
|||
165 |
Vector3 m_v; |
||
166 |
|||
167 |
}; // struct MotionTranslationTpl |
||
168 |
|||
169 |
template<typename S1, int O1, typename MotionDerived> |
||
170 |
inline typename MotionDerived::MotionPlain |
||
171 |
operator+(const MotionTranslationTpl<S1,O1> & m1, |
||
172 |
const MotionDense<MotionDerived> & m2) |
||
173 |
{ |
||
174 |
return typename MotionDerived::MotionPlain(m2.linear() + m1.linear(), m2.angular()); |
||
175 |
} |
||
176 |
|||
177 |
template<typename Scalar, int Options> struct TransformTranslationTpl; |
||
178 |
|||
179 |
template<typename _Scalar, int _Options> |
||
180 |
struct traits< TransformTranslationTpl<_Scalar,_Options> > |
||
181 |
{ |
||
182 |
enum { |
||
183 |
Options = _Options, |
||
184 |
LINEAR = 0, |
||
185 |
ANGULAR = 3 |
||
186 |
}; |
||
187 |
typedef _Scalar Scalar; |
||
188 |
typedef SE3Tpl<Scalar,Options> PlainType; |
||
189 |
typedef Eigen::Matrix<Scalar,3,1,Options> Vector3; |
||
190 |
typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3; |
||
191 |
typedef typename Matrix3::IdentityReturnType AngularType; |
||
192 |
typedef AngularType AngularRef; |
||
193 |
typedef AngularType ConstAngularRef; |
||
194 |
typedef Vector3 LinearType; |
||
195 |
typedef LinearType & LinearRef; |
||
196 |
typedef const LinearType & ConstLinearRef; |
||
197 |
typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType; |
||
198 |
typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType; |
||
199 |
}; // traits TransformTranslationTpl |
||
200 |
|||
201 |
template<typename Scalar, int Options> |
||
202 |
struct SE3GroupAction< TransformTranslationTpl<Scalar,Options> > |
||
203 |
{ typedef typename traits <TransformTranslationTpl<Scalar,Options> >::PlainType ReturnType; }; |
||
204 |
|||
205 |
template<typename _Scalar, int _Options> |
||
206 |
struct TransformTranslationTpl |
||
207 |
: SE3Base< TransformTranslationTpl<_Scalar,_Options> > |
||
208 |
{ |
||
209 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
210 |
PINOCCHIO_SE3_TYPEDEF_TPL(TransformTranslationTpl); |
||
211 |
typedef typename traits<TransformTranslationTpl>::Vector3 Vector3; |
||
212 |
|||
213 |
113 |
TransformTranslationTpl() {} |
|
214 |
|||
215 |
template<typename Vector3Like> |
||
216 |
3151 |
TransformTranslationTpl(const Eigen::MatrixBase<Vector3Like> & translation) |
|
217 |
3151 |
: m_translation(translation) |
|
218 |
3151 |
{} |
|
219 |
|||
220 |
6300 |
PlainType plain() const |
|
221 |
{ |
||
222 |
6300 |
PlainType res(PlainType::Identity()); |
|
223 |
6300 |
res.rotation().setIdentity(); |
|
224 |
6300 |
res.translation() = translation(); |
|
225 |
|||
226 |
6300 |
return res; |
|
227 |
} |
||
228 |
|||
229 |
6300 |
operator PlainType() const { return plain(); } |
|
230 |
|||
231 |
template<typename S2, int O2> |
||
232 |
typename SE3GroupAction<TransformTranslationTpl>::ReturnType |
||
233 |
se3action(const SE3Tpl<S2,O2> & m) const |
||
234 |
{ |
||
235 |
typedef typename SE3GroupAction<TransformTranslationTpl>::ReturnType ReturnType; |
||
236 |
ReturnType res(m); |
||
237 |
res.translation() += translation(); |
||
238 |
|||
239 |
return res; |
||
240 |
} |
||
241 |
|||
242 |
6300 |
ConstLinearRef translation() const { return m_translation; } |
|
243 |
6348 |
LinearRef translation() { return m_translation; } |
|
244 |
|||
245 |
AngularType rotation() const { return AngularType(3,3); } |
||
246 |
|||
247 |
34 |
bool isEqual(const TransformTranslationTpl & other) const |
|
248 |
{ |
||
249 |
34 |
return m_translation == other.m_translation; |
|
250 |
} |
||
251 |
|||
252 |
protected: |
||
253 |
|||
254 |
LinearType m_translation; |
||
255 |
}; |
||
256 |
|||
257 |
template<typename Scalar, int Options> struct ConstraintTranslationTpl; |
||
258 |
|||
259 |
template<typename _Scalar, int _Options> |
||
260 |
struct traits< ConstraintTranslationTpl<_Scalar,_Options> > |
||
261 |
{ |
||
262 |
typedef _Scalar Scalar; |
||
263 |
|||
264 |
enum { Options = _Options }; |
||
265 |
enum { LINEAR = 0, ANGULAR = 3 }; |
||
266 |
|||
267 |
typedef MotionTranslationTpl<Scalar,Options> JointMotion; |
||
268 |
typedef Eigen::Matrix<Scalar,3,1,Options> JointForce; |
||
269 |
typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase; |
||
270 |
|||
271 |
typedef DenseBase MatrixReturnType; |
||
272 |
typedef const DenseBase ConstMatrixReturnType; |
||
273 |
}; // traits ConstraintTranslationTpl |
||
274 |
|||
275 |
template<typename _Scalar, int _Options> |
||
276 |
struct ConstraintTranslationTpl |
||
277 |
: ConstraintBase< ConstraintTranslationTpl<_Scalar,_Options> > |
||
278 |
{ |
||
279 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
||
280 |
|||
281 |
PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintTranslationTpl) |
||
282 |
|||
283 |
enum { NV = 3 }; |
||
284 |
|||
285 |
3129 |
ConstraintTranslationTpl() {} |
|
286 |
|||
287 |
// template<typename S1, int O1> |
||
288 |
// Motion operator*(const MotionTranslationTpl<S1,O1> & vj) const |
||
289 |
// { return Motion(vj(), Motion::Vector3::Zero()); } |
||
290 |
|||
291 |
template<typename Vector3Like> |
||
292 |
5 |
JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & v) const |
|
293 |
{ |
||
294 |
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); |
||
295 |
5 |
return JointMotion(v); |
|
296 |
} |
||
297 |
|||
298 |
22 |
int nv_impl() const { return NV; } |
|
299 |
|||
300 |
struct ConstraintTranspose |
||
301 |
{ |
||
302 |
const ConstraintTranslationTpl & ref; |
||
303 |
6 |
ConstraintTranspose(const ConstraintTranslationTpl & ref) : ref(ref) {} |
|
304 |
|||
305 |
template<typename Derived> |
||
306 |
typename ForceDense<Derived>::ConstLinearType |
||
307 |
4 |
operator* (const ForceDense<Derived> & phi) |
|
308 |
{ |
||
309 |
4 |
return phi.linear(); |
|
310 |
} |
||
311 |
|||
312 |
/* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */ |
||
313 |
template<typename MatrixDerived> |
||
314 |
const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType |
||
315 |
2 |
operator*(const Eigen::MatrixBase<MatrixDerived> & F) const |
|
316 |
{ |
||
317 |
✗✓ | 2 |
assert(F.rows()==6); |
318 |
2 |
return F.derived().template middleRows<3>(LINEAR); |
|
319 |
} |
||
320 |
|||
321 |
}; // struct ConstraintTranspose |
||
322 |
|||
323 |
6 |
ConstraintTranspose transpose () const { return ConstraintTranspose(*this); } |
|
324 |
|||
325 |
23 |
DenseBase matrix_impl() const |
|
326 |
{ |
||
327 |
23 |
DenseBase S; |
|
328 |
✓✗ | 23 |
S.template middleRows<3>(LINEAR).setIdentity(); |
329 |
✓✗ | 23 |
S.template middleRows<3>(ANGULAR).setZero(); |
330 |
23 |
return S; |
|
331 |
} |
||
332 |
|||
333 |
template<typename S1, int O1> |
||
334 |
5 |
Eigen::Matrix<S1,6,3,O1> se3Action(const SE3Tpl<S1,O1> & m) const |
|
335 |
{ |
||
336 |
5 |
Eigen::Matrix<S1,6,3,O1> M; |
|
337 |
✓✗ | 5 |
M.template middleRows<3>(LINEAR) = m.rotation(); |
338 |
✓✗ | 5 |
M.template middleRows<3>(ANGULAR).setZero(); |
339 |
|||
340 |
5 |
return M; |
|
341 |
} |
||
342 |
|||
343 |
template<typename S1, int O1> |
||
344 |
3 |
Eigen::Matrix<S1,6,3,O1> se3ActionInverse(const SE3Tpl<S1,O1> & m) const |
|
345 |
{ |
||
346 |
3 |
Eigen::Matrix<S1,6,3,O1> M; |
|
347 |
✓✗✓✗ |
3 |
M.template middleRows<3>(LINEAR) = m.rotation().transpose(); |
348 |
✓✗ | 3 |
M.template middleRows<3>(ANGULAR).setZero(); |
349 |
|||
350 |
3 |
return M; |
|
351 |
} |
||
352 |
|||
353 |
template<typename MotionDerived> |
||
354 |
2 |
DenseBase motionAction(const MotionDense<MotionDerived> & m) const |
|
355 |
{ |
||
356 |
✓✗ | 2 |
const typename MotionDerived::ConstAngularType w = m.angular(); |
357 |
|||
358 |
✓✗ | 2 |
DenseBase res; |
359 |
✓✗✓✗ |
2 |
skew(w,res.template middleRows<3>(LINEAR)); |
360 |
✓✗✓✗ |
2 |
res.template middleRows<3>(ANGULAR).setZero(); |
361 |
|||
362 |
4 |
return res; |
|
363 |
} |
||
364 |
|||
365 |
17 |
bool isEqual(const ConstraintTranslationTpl &) const { return true; } |
|
366 |
|||
367 |
}; // struct ConstraintTranslationTpl |
||
368 |
|||
369 |
template<typename MotionDerived, typename S2, int O2> |
||
370 |
inline typename MotionDerived::MotionPlain |
||
371 |
3 |
operator^(const MotionDense<MotionDerived> & m1, |
|
372 |
const MotionTranslationTpl<S2,O2> & m2) |
||
373 |
{ |
||
374 |
3 |
return m2.motionAction(m1); |
|
375 |
} |
||
376 |
|||
377 |
/* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */ |
||
378 |
template<typename S1, int O1, typename S2, int O2> |
||
379 |
inline Eigen::Matrix<S2,6,3,O2> |
||
380 |
3 |
operator*(const InertiaTpl<S1,O1> & Y, |
|
381 |
const ConstraintTranslationTpl<S2,O2> &) |
||
382 |
{ |
||
383 |
typedef ConstraintTranslationTpl<S2,O2> Constraint; |
||
384 |
3 |
Eigen::Matrix<S2,6,3,O2> M; |
|
385 |
✓✗ | 3 |
alphaSkew(Y.mass(),Y.lever(),M.template middleRows<3>(Constraint::ANGULAR)); |
386 |
✓✗ | 3 |
M.template middleRows<3>(Constraint::LINEAR).setZero(); |
387 |
✓✗✓✗ |
3 |
M.template middleRows<3>(Constraint::LINEAR).diagonal().fill(Y.mass ()); |
388 |
|||
389 |
3 |
return M; |
|
390 |
} |
||
391 |
|||
392 |
/* [ABA] Y*S operator*/ |
||
393 |
template<typename M6Like, typename S2, int O2> |
||
394 |
inline const typename SizeDepType<3>::ColsReturn<M6Like>::ConstType |
||
395 |
2 |
operator*(const Eigen::MatrixBase<M6Like> & Y, |
|
396 |
const ConstraintTranslationTpl<S2,O2> &) |
||
397 |
{ |
||
398 |
typedef ConstraintTranslationTpl<S2,O2> Constraint; |
||
399 |
2 |
return Y.derived().template middleCols<3>(Constraint::LINEAR); |
|
400 |
} |
||
401 |
|||
402 |
template<typename S1, int O1> |
||
403 |
struct SE3GroupAction< ConstraintTranslationTpl<S1,O1> > |
||
404 |
{ typedef Eigen::Matrix<S1,6,3,O1> ReturnType; }; |
||
405 |
|||
406 |
template<typename S1, int O1, typename MotionDerived> |
||
407 |
struct MotionAlgebraAction< ConstraintTranslationTpl<S1,O1>,MotionDerived > |
||
408 |
{ typedef Eigen::Matrix<S1,6,3,O1> ReturnType; }; |
||
409 |
|||
410 |
template<typename Scalar, int Options> struct JointTranslationTpl; |
||
411 |
|||
412 |
template<typename _Scalar, int _Options> |
||
413 |
struct traits< JointTranslationTpl<_Scalar,_Options> > |
||
414 |
{ |
||
415 |
enum { |
||
416 |
NQ = 3, |
||
417 |
NV = 3 |
||
418 |
}; |
||
419 |
typedef _Scalar Scalar; |
||
420 |
enum { Options = _Options }; |
||
421 |
typedef JointDataTranslationTpl<Scalar,Options> JointDataDerived; |
||
422 |
typedef JointModelTranslationTpl<Scalar,Options> JointModelDerived; |
||
423 |
typedef ConstraintTranslationTpl<Scalar,Options> Constraint_t; |
||
424 |
typedef TransformTranslationTpl<Scalar,Options> Transformation_t; |
||
425 |
typedef MotionTranslationTpl<Scalar,Options> Motion_t; |
||
426 |
typedef MotionZeroTpl<Scalar,Options> Bias_t; |
||
427 |
|||
428 |
// [ABA] |
||
429 |
typedef Eigen::Matrix<Scalar,6,NV,Options> U_t; |
||
430 |
typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t; |
||
431 |
typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t; |
||
432 |
|||
433 |
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE |
||
434 |
|||
435 |
typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t; |
||
436 |
typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t; |
||
437 |
}; // traits JointTranslationTpl |
||
438 |
|||
439 |
template<typename Scalar, int Options> |
||
440 |
struct traits< JointDataTranslationTpl<Scalar,Options> > |
||
441 |
{ typedef JointTranslationTpl<Scalar,Options> JointDerived; }; |
||
442 |
|||
443 |
template<typename Scalar, int Options> |
||
444 |
struct traits< JointModelTranslationTpl<Scalar,Options> > |
||
445 |
{ typedef JointTranslationTpl<Scalar,Options> JointDerived; }; |
||
446 |
|||
447 |
template<typename _Scalar, int _Options> |
||
448 |
struct JointDataTranslationTpl |
||
449 |
: public JointDataBase< JointDataTranslationTpl<_Scalar,_Options> > |
||
450 |
{ |
||
451 |
34 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
452 |
|||
453 |
typedef JointTranslationTpl<_Scalar,_Options> JointDerived; |
||
454 |
PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); |
||
455 |
670 |
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR |
|
456 |
|||
457 |
Constraint_t S; |
||
458 |
Transformation_t M; |
||
459 |
Motion_t v; |
||
460 |
Bias_t c; |
||
461 |
|||
462 |
// [ABA] specific data |
||
463 |
U_t U; |
||
464 |
D_t Dinv; |
||
465 |
UD_t UDinv; |
||
466 |
|||
467 |
3122 |
JointDataTranslationTpl() |
|
468 |
: M(Transformation_t::Vector3::Zero()) |
||
469 |
, v(Motion_t::Vector3::Zero()) |
||
470 |
, U(U_t::Zero()) |
||
471 |
, Dinv(D_t::Zero()) |
||
472 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
3122 |
, UDinv(UD_t::Zero()) |
473 |
3122 |
{} |
|
474 |
|||
475 |
✓✗ | 44 |
static std::string classname() { return std::string("JointDataTranslation"); } |
476 |
3 |
std::string shortname() const { return classname(); } |
|
477 |
}; // struct JointDataTranslationTpl |
||
478 |
|||
479 |
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTranslationTpl); |
||
480 |
template<typename _Scalar, int _Options> |
||
481 |
struct JointModelTranslationTpl |
||
482 |
: public JointModelBase< JointModelTranslationTpl<_Scalar,_Options> > |
||
483 |
{ |
||
484 |
136 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
485 |
|||
486 |
typedef JointTranslationTpl<_Scalar,_Options> JointDerived; |
||
487 |
PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); |
||
488 |
|||
489 |
typedef JointModelBase<JointModelTranslationTpl> Base; |
||
490 |
using Base::id; |
||
491 |
using Base::idx_q; |
||
492 |
using Base::idx_v; |
||
493 |
using Base::setIndexes; |
||
494 |
|||
495 |
3096 |
JointDataDerived createData() const { return JointDataDerived(); } |
|
496 |
|||
497 |
const std::vector<bool> hasConfigurationLimit() const |
||
498 |
{ |
||
499 |
return {true, true, true}; |
||
500 |
} |
||
501 |
|||
502 |
const std::vector<bool> hasConfigurationLimitInTangent() const |
||
503 |
{ |
||
504 |
return {true, true, true}; |
||
505 |
} |
||
506 |
|||
507 |
template<typename ConfigVector> |
||
508 |
6158 |
void calc(JointDataDerived & data, |
|
509 |
const typename Eigen::MatrixBase<ConfigVector> & qs) const |
||
510 |
{ |
||
511 |
✓✗ | 6158 |
data.M.translation() = this->jointConfigSelector(qs); |
512 |
6158 |
} |
|
513 |
|||
514 |
template<typename ConfigVector, typename TangentVector> |
||
515 |
1038 |
void calc(JointDataDerived & data, |
|
516 |
const typename Eigen::MatrixBase<ConfigVector> & qs, |
||
517 |
const typename Eigen::MatrixBase<TangentVector> & vs) const |
||
518 |
{ |
||
519 |
1038 |
calc(data,qs.derived()); |
|
520 |
|||
521 |
✓✗ | 1038 |
data.v.linear() = this->jointVelocitySelector(vs); |
522 |
1038 |
} |
|
523 |
|||
524 |
template<typename Matrix6Like> |
||
525 |
9 |
void calc_aba(JointDataDerived & data, |
|
526 |
const Eigen::MatrixBase<Matrix6Like> & I, |
||
527 |
const bool update_I) const |
||
528 |
{ |
||
529 |
✓✗ | 9 |
data.U = I.template middleCols<3>(Inertia::LINEAR); |
530 |
|||
531 |
// compute inverse |
||
532 |
// data.Dinv.setIdentity(); |
||
533 |
// data.U.template middleRows<3>(Inertia::LINEAR).llt().solveInPlace(data.Dinv); |
||
534 |
✓✗ | 9 |
internal::PerformStYSInversion<Scalar>::run(data.U.template middleRows<3>(Inertia::LINEAR),data.Dinv); |
535 |
|||
536 |
✓✗ | 9 |
data.UDinv.template middleRows<3>(Inertia::LINEAR).setIdentity(); // can be put in data constructor |
537 |
✓✗✓✗ ✓✗✓✗ |
9 |
data.UDinv.template middleRows<3>(Inertia::ANGULAR).noalias() = data.U.template middleRows<3>(Inertia::ANGULAR) * data.Dinv; |
538 |
|||
539 |
✓✓ | 9 |
if (update_I) |
540 |
{ |
||
541 |
3 |
Matrix6Like & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I); |
|
542 |
I_.template block<3,3>(Inertia::ANGULAR,Inertia::ANGULAR) |
||
543 |
✓✗✓✗ ✓✗✓✗ |
3 |
-= data.UDinv.template middleRows<3>(Inertia::ANGULAR) * I_.template block<3,3>(Inertia::LINEAR, Inertia::ANGULAR); |
544 |
✓✗ | 3 |
I_.template middleCols<3>(Inertia::LINEAR).setZero(); |
545 |
✓✗ | 3 |
I_.template block<3,3>(Inertia::LINEAR,Inertia::ANGULAR).setZero(); |
546 |
} |
||
547 |
9 |
} |
|
548 |
|||
549 |
✓✗ | 18430 |
static std::string classname() { return std::string("JointModelTranslation"); } |
550 |
18388 |
std::string shortname() const { return classname(); } |
|
551 |
|||
552 |
/// \returns An expression of *this with the Scalar type casted to NewScalar. |
||
553 |
template<typename NewScalar> |
||
554 |
4 |
JointModelTranslationTpl<NewScalar,Options> cast() const |
|
555 |
{ |
||
556 |
typedef JointModelTranslationTpl<NewScalar,Options> ReturnType; |
||
557 |
4 |
ReturnType res; |
|
558 |
4 |
res.setIndexes(id(),idx_q(),idx_v()); |
|
559 |
4 |
return res; |
|
560 |
} |
||
561 |
|||
562 |
}; // struct JointModelTranslationTpl |
||
563 |
|||
564 |
} // namespace pinocchio |
||
565 |
|||
566 |
#include <boost/type_traits.hpp> |
||
567 |
|||
568 |
namespace boost |
||
569 |
{ |
||
570 |
template<typename Scalar, int Options> |
||
571 |
struct has_nothrow_constructor< ::pinocchio::JointModelTranslationTpl<Scalar,Options> > |
||
572 |
: public integral_constant<bool,true> {}; |
||
573 |
|||
574 |
template<typename Scalar, int Options> |
||
575 |
struct has_nothrow_copy< ::pinocchio::JointModelTranslationTpl<Scalar,Options> > |
||
576 |
: public integral_constant<bool,true> {}; |
||
577 |
|||
578 |
template<typename Scalar, int Options> |
||
579 |
struct has_nothrow_constructor< ::pinocchio::JointDataTranslationTpl<Scalar,Options> > |
||
580 |
: public integral_constant<bool,true> {}; |
||
581 |
|||
582 |
template<typename Scalar, int Options> |
||
583 |
struct has_nothrow_copy< ::pinocchio::JointDataTranslationTpl<Scalar,Options> > |
||
584 |
: public integral_constant<bool,true> {}; |
||
585 |
} |
||
586 |
|||
587 |
#endif // ifndef __pinocchio_joint_translation_hpp__ |
Generated by: GCOVR (Version 4.2) |