GCC Code Coverage Report

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