GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-translation.hpp
Date: 2025-04-30 16:14:33
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 3269 MotionTranslationTpl(const Eigen::MatrixBase<Vector3Like> & v)
76 3269 : m_v(v)
77 {
78 3269 }
79
80 751 MotionTranslationTpl(const MotionTranslationTpl & other)
81 751 : m_v(other.m_v)
82 {
83 751 }
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 55 void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
140 {
141 // Linear
142
4/8
✓ Branch 3 taken 55 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 55 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 55 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 55 times.
✗ Branch 13 not taken.
 55 v.linear().noalias() = m.rotation().transpose() * m_v;
143
144 // Angular
145
1/2
✓ Branch 2 taken 55 times.
✗ Branch 3 not taken.
 55 v.angular().setZero();
146 55 }
147
148 template<typename S2, int O2>
149 55 MotionPlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
150 {
151 55 MotionPlain res;
152
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 55 se3ActionInverse_impl(m, res);
153 55 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 1123 Vector3 & linear()
179 {
180 1123 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 6445 TransformTranslationTpl(const Eigen::MatrixBase<Vector3Like> & translation)
240 6445 : m_translation(translation)
241 {
242 6445 }
243
244 12427 PlainType plain() const
245 {
246 12427 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.
 12427 res.rotation().setIdentity();
248
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 12427 res.translation() = translation();
249
250 12427 return res;
251 }
252
253 12427 operator PlainType() const
254 {
255 12427 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 12427 ConstLinearRef translation() const
270 {
271 12427 return m_translation;
272 }
273 12497 LinearRef translation()
274 {
275 12505 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 3269 JointMotionSubspaceTranslationTpl()
335 {
336 3269 }
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 138 Eigen::Matrix<S1, 6, 3, O1> se3ActionInverse(const SE3Tpl<S1, O1> & m) const
404 {
405 138 Eigen::Matrix<S1, 6, 3, O1> M;
406
2/10
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 138 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 138 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 138 M.template middleRows<3>(LINEAR) = m.rotation().transpose();
407
1/5
✗ Branch 1 not taken.
✓ Branch 2 taken 138 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 138 M.template middleRows<3>(ANGULAR).setZero();
408
409 138 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 NVExtended = 3
484 };
485 typedef _Scalar Scalar;
486 enum
487 {
488 Options = _Options
489 };
490 typedef JointDataTranslationTpl<Scalar, Options> JointDataDerived;
491 typedef JointModelTranslationTpl<Scalar, Options> JointModelDerived;
492 typedef JointMotionSubspaceTranslationTpl<Scalar, Options> Constraint_t;
493 typedef TransformTranslationTpl<Scalar, Options> Transformation_t;
494 typedef MotionTranslationTpl<Scalar, Options> Motion_t;
495 typedef MotionZeroTpl<Scalar, Options> Bias_t;
496
497 // [ABA]
498 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
499 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
500 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
501
502 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
503 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
504
505 typedef boost::mpl::false_ is_mimicable_t;
506
507 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
508 }; // traits JointTranslationTpl
509
510 template<typename _Scalar, int _Options>
511 struct traits<JointDataTranslationTpl<_Scalar, _Options>>
512 {
513 typedef JointTranslationTpl<_Scalar, _Options> JointDerived;
514 typedef _Scalar Scalar;
515 };
516
517 template<typename _Scalar, int _Options>
518 struct traits<JointModelTranslationTpl<_Scalar, _Options>>
519 {
520 typedef JointTranslationTpl<_Scalar, _Options> JointDerived;
521 typedef _Scalar Scalar;
522 };
523
524 template<typename _Scalar, int _Options>
525 struct JointDataTranslationTpl : public JointDataBase<JointDataTranslationTpl<_Scalar, _Options>>
526 {
527 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
528
529 typedef JointTranslationTpl<_Scalar, _Options> JointDerived;
530 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
531 1617 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
532
533 ConfigVector_t joint_q;
534 TangentVector_t joint_v;
535
536 Constraint_t S;
537 Transformation_t M;
538 Motion_t v;
539 Bias_t c;
540
541 // [ABA] specific data
542 U_t U;
543 D_t Dinv;
544 UD_t UDinv;
545 D_t StU;
546
547 3262 JointDataTranslationTpl()
548
1/2
✓ Branch 2 taken 3260 times.
✗ Branch 3 not taken.
 3262 : joint_q(ConfigVector_t::Zero())
549
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3255 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3262 , joint_v(TangentVector_t::Zero())
550
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3255 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3262 , M(Transformation_t::Vector3::Zero())
551
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3255 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3262 , v(Motion_t::Vector3::Zero())
552
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3255 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3262 , U(U_t::Zero())
553
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3255 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3262 , Dinv(D_t::Zero())
554
3/5
✓ Branch 1 taken 5 times.
✓ Branch 2 taken 3255 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 3262 , UDinv(UD_t::Zero())
555
3/5
✓ Branch 3 taken 5 times.
✓ Branch 4 taken 3255 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
 9786 , StU(D_t::Zero())
556 {
557 3262 }
558
559 150 static std::string classname()
560 {
561
1/2
✓ Branch 2 taken 150 times.
✗ Branch 3 not taken.
 150 return std::string("JointDataTranslation");
562 }
563 3 std::string shortname() const
564 {
565 3 return classname();
566 }
567 }; // struct JointDataTranslationTpl
568
569 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTranslationTpl);
570 template<typename _Scalar, int _Options>
571 struct JointModelTranslationTpl
572 : public JointModelBase<JointModelTranslationTpl<_Scalar, _Options>>
573 {
574 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
575
576 typedef JointTranslationTpl<_Scalar, _Options> JointDerived;
577 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
578
579 typedef JointModelBase<JointModelTranslationTpl> Base;
580 using Base::id;
581 using Base::idx_q;
582 using Base::idx_v;
583 using Base::idx_vExtended;
584 using Base::setIndexes;
585
586 3183 JointDataDerived createData() const
587 {
588 3183 return JointDataDerived();
589 }
590
591 ✗ const std::vector<bool> hasConfigurationLimit() const
592 {
593 ✗ return {true, true, true};
594 }
595
596 ✗ const std::vector<bool> hasConfigurationLimitInTangent() const
597 {
598 ✗ return {true, true, true};
599 }
600
601 template<typename ConfigVector>
602 6434 void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
603 {
604
1/2
✓ Branch 2 taken 6287 times.
✗ Branch 3 not taken.
 6434 data.joint_q = this->jointConfigSelector(qs);
605 6434 data.M.translation() = data.joint_q;
606 6434 }
607
608 template<typename TangentVector>
609 void
610 1 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
611 const
612 {
613
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 data.joint_v = this->jointVelocitySelector(vs);
614 1 data.v.linear() = data.joint_v;
615 1 }
616
617 template<typename ConfigVector, typename TangentVector>
618 1152 void calc(
619 JointDataDerived & data,
620 const typename Eigen::MatrixBase<ConfigVector> & qs,
621 const typename Eigen::MatrixBase<TangentVector> & vs) const
622 {
623 1152 calc(data, qs.derived());
624
625
1/2
✓ Branch 2 taken 1092 times.
✗ Branch 3 not taken.
 1152 data.joint_v = this->jointVelocitySelector(vs);
626 1152 data.v.linear() = data.joint_v;
627 1152 }
628
629 template<typename VectorLike, typename Matrix6Like>
630 9 void calc_aba(
631 JointDataDerived & data,
632 const Eigen::MatrixBase<VectorLike> & armature,
633 const Eigen::MatrixBase<Matrix6Like> & I,
634 const bool update_I) const
635 {
636
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 9 data.U = I.template middleCols<3>(Inertia::LINEAR);
637
638
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 9 data.StU = data.U.template middleRows<3>(Inertia::LINEAR);
639
1/2
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
 9 data.StU.diagonal() += armature;
640
641 9 internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
642
643
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;
644
645
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 5 times.
 9 if (update_I)
646
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();
647 9 }
648
649 19559 static std::string classname()
650 {
651
1/2
✓ Branch 2 taken 19559 times.
✗ Branch 3 not taken.
 19559 return std::string("JointModelTranslation");
652 }
653 19411 std::string shortname() const
654 {
655 19411 return classname();
656 }
657
658 /// \returns An expression of *this with the Scalar type casted to NewScalar.
659 template<typename NewScalar>
660 5 JointModelTranslationTpl<NewScalar, Options> cast() const
661 {
662 typedef JointModelTranslationTpl<NewScalar, Options> ReturnType;
663 5 ReturnType res;
664 5 res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended());
665 5 return res;
666 }
667
668 }; // struct JointModelTranslationTpl
669
670 template<typename Scalar, int Options>
671 struct ConfigVectorAffineTransform<JointTranslationTpl<Scalar, Options>>
672 {
673 typedef LinearAffineTransform Type;
674 };
675 } // namespace pinocchio
676
677 #include <boost/type_traits.hpp>
678
679 namespace boost
680 {
681 template<typename Scalar, int Options>
682 struct has_nothrow_constructor<::pinocchio::JointModelTranslationTpl<Scalar, Options>>
683 : public integral_constant<bool, true>
684 {
685 };
686
687 template<typename Scalar, int Options>
688 struct has_nothrow_copy<::pinocchio::JointModelTranslationTpl<Scalar, Options>>
689 : public integral_constant<bool, true>
690 {
691 };
692
693 template<typename Scalar, int Options>
694 struct has_nothrow_constructor<::pinocchio::JointDataTranslationTpl<Scalar, Options>>
695 : public integral_constant<bool, true>
696 {
697 };
698
699 template<typename Scalar, int Options>
700 struct has_nothrow_copy<::pinocchio::JointDataTranslationTpl<Scalar, Options>>
701 : public integral_constant<bool, true>
702 {
703 };
704 } // namespace boost
705
706 #endif // ifndef __pinocchio_multibody_joint_translation_hpp__
707