GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-helical-unaligned.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 39 224 17.4%
Branches: 15 434 3.5%
Line Branch Exec Source
1 //
2 // Copyright (c) 2022-2023 INRIA
3 //
4
5 #ifndef __pinocchio_multibody_joint_helical_unaligned_hpp__
6 #define __pinocchio_multibody_joint_helical_unaligned_hpp__
7
8 #include "pinocchio/fwd.hpp"
9 #include "pinocchio/multibody/joint/joint-base.hpp"
10 #include "pinocchio/multibody/joint-motion-subspace.hpp"
11 #include "pinocchio/spatial/inertia.hpp"
12
13 #include "pinocchio/math/matrix.hpp"
14 #include "pinocchio/math/rotation.hpp"
15
16 #include <iostream>
17
18 namespace pinocchio
19 {
20
21 template<typename Scalar, int Options>
22 struct MotionHelicalUnalignedTpl;
23
24 template<typename Scalar, int Options>
25 struct SE3GroupAction<MotionHelicalUnalignedTpl<Scalar, Options>>
26 {
27 typedef MotionTpl<Scalar, Options> ReturnType;
28 };
29
30 template<typename Scalar, int Options, typename MotionDerived>
31 struct MotionAlgebraAction<MotionHelicalUnalignedTpl<Scalar, Options>, MotionDerived>
32 {
33 typedef MotionTpl<Scalar, Options> ReturnType;
34 };
35
36 template<typename _Scalar, int _Options>
37 struct traits<MotionHelicalUnalignedTpl<_Scalar, _Options>>
38 {
39 typedef _Scalar Scalar;
40 enum
41 {
42 Options = _Options
43 };
44 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
45 typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
46 typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
47 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
48 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
49 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
50 typedef Vector3 AngularType;
51 typedef Vector3 LinearType;
52 typedef const Vector3 ConstAngularType;
53 typedef const Vector3 ConstLinearType;
54 typedef Matrix6 ActionMatrixType;
55 typedef MotionTpl<Scalar, Options> MotionPlain;
56 typedef MotionPlain PlainReturnType;
57 typedef Matrix4 HomogeneousMatrixType;
58 enum
59 {
60 LINEAR = 0,
61 ANGULAR = 3
62 };
63 }; // traits MotionHelicalUnalignedTpl
64
65 template<typename _Scalar, int _Options>
66 struct MotionHelicalUnalignedTpl : MotionBase<MotionHelicalUnalignedTpl<_Scalar, _Options>>
67 {
68 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
69
70 MOTION_TYPEDEF_TPL(MotionHelicalUnalignedTpl);
71
72 ✗ MotionHelicalUnalignedTpl()
73 {
74 }
75
76 template<typename Vector3Like, typename OtherScalar>
77 23 MotionHelicalUnalignedTpl(
78 const Eigen::MatrixBase<Vector3Like> & axis, const OtherScalar & w, const OtherScalar & v)
79 23 : m_axis(axis)
80 23 , m_w(w)
81 23 , m_v(v)
82 {
83 23 }
84
85 ✗ inline PlainReturnType plain() const
86 {
87 ✗ return PlainReturnType(m_axis * m_v, m_axis * m_w);
88 }
89
90 template<typename OtherScalar>
91 MotionHelicalUnalignedTpl __mult__(const OtherScalar & alpha) const
92 {
93 return MotionHelicalUnalignedTpl(m_axis, alpha * m_w, alpha * m_v);
94 }
95
96 template<typename MotionDerived>
97 ✗ void setTo(MotionDense<MotionDerived> & m) const
98 {
99 ✗ for (Eigen::DenseIndex k = 0; k < 3; ++k)
100 {
101 ✗ m.angular().noalias() = m_axis * m_w;
102 ✗ m.linear().noalias() = m_axis * m_v;
103 }
104 }
105
106 template<typename MotionDerived>
107 ✗ inline void addTo(MotionDense<MotionDerived> & v) const
108 {
109 ✗ v.angular() += m_axis * m_w;
110 ✗ v.linear() += m_axis * m_v;
111 }
112
113 template<typename S2, int O2, typename D2>
114 ✗ inline void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
115 {
116 ✗ v.angular().noalias() = m_w * m.rotation() * m_axis;
117 ✗ v.linear().noalias() = m.translation().cross(v.angular()) + m_v * m.rotation() * m_axis;
118 }
119
120 template<typename S2, int O2>
121 ✗ MotionPlain se3Action_impl(const SE3Tpl<S2, O2> & m) const
122 {
123 ✗ MotionPlain res;
124 ✗ se3Action_impl(m, res);
125 ✗ return res;
126 }
127
128 template<typename S2, int O2, typename D2>
129 ✗ void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
130 {
131 // Linear
132 ✗ v.angular().noalias() = m_axis.cross(m.translation());
133 ✗ v.angular() *= m_w;
134 ✗ v.linear().noalias() =
135 ✗ m.rotation().transpose() * v.angular() + m_v * (m.rotation().transpose() * m_axis);
136
137 // Angular
138 ✗ v.angular().noalias() = m.rotation().transpose() * m_axis * m_w;
139 }
140
141 template<typename S2, int O2>
142 ✗ MotionPlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
143 {
144 ✗ MotionPlain res;
145 ✗ se3ActionInverse_impl(m, res);
146 ✗ return res;
147 }
148
149 template<typename M1, typename M2>
150 ✗ void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
151 {
152 // Linear
153 ✗ mout.linear().noalias() = v.linear().cross(m_axis);
154 ✗ mout.linear() *= m_w;
155 ✗ mout.angular().noalias() = v.angular().cross(m_axis);
156 ✗ mout.angular() *= m_v;
157 ✗ mout.linear() += mout.angular();
158
159 // Angular
160 ✗ mout.angular().noalias() = v.angular().cross(m_axis);
161 ✗ mout.angular() *= m_w;
162 }
163
164 template<typename M1>
165 ✗ MotionPlain motionAction(const MotionDense<M1> & v) const
166 {
167 ✗ MotionPlain res;
168 ✗ motionAction(v, res);
169 ✗ return res;
170 }
171
172 ✗ Scalar & angularRate()
173 {
174 ✗ return m_w;
175 }
176 const Scalar & angularRate() const
177 {
178 return m_w;
179 }
180
181 ✗ Scalar & linearRate()
182 {
183 ✗ return m_v;
184 }
185 const Scalar & linearRate() const
186 {
187 return m_v;
188 }
189
190 ✗ Vector3 & axis()
191 {
192 ✗ return m_axis;
193 }
194 const Vector3 & axis() const
195 {
196 return m_axis;
197 }
198
199 ✗ bool isEqual_impl(const MotionHelicalUnalignedTpl & other) const
200 {
201 ✗ return internal::comparison_eq(m_axis, other.m_axis)
202 ✗ && internal::comparison_eq(m_w, other.m_w) && internal::comparison_eq(m_v, other.m_v);
203 }
204
205 protected:
206 Vector3 m_axis;
207 Scalar m_w, m_v;
208
209 }; // struct MotionHelicalUnalignedTpl
210
211 template<typename S1, int O1, typename MotionDerived>
212 typename MotionDerived::MotionPlain
213 ✗ operator+(const MotionHelicalUnalignedTpl<S1, O1> & m1, const MotionDense<MotionDerived> & m2)
214 {
215 ✗ typename MotionDerived::MotionPlain res(m2);
216 ✗ res += m1;
217 ✗ return res;
218 }
219
220 template<typename MotionDerived, typename S2, int O2>
221 EIGEN_STRONG_INLINE typename MotionDerived::MotionPlain
222 ✗ operator^(const MotionDense<MotionDerived> & m1, const MotionHelicalUnalignedTpl<S2, O2> & m2)
223 {
224 ✗ return m2.motionAction(m1);
225 }
226
227 template<typename Scalar, int Options>
228 struct JointMotionSubspaceHelicalUnalignedTpl;
229
230 template<typename Scalar, int Options>
231 struct SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>>
232 {
233 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
234 };
235
236 template<typename Scalar, int Options, typename MotionDerived>
237 struct MotionAlgebraAction<JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>, MotionDerived>
238 {
239 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
240 };
241
242 template<typename Scalar, int Options, typename ForceDerived>
243 struct ConstraintForceOp<JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>, ForceDerived>
244 {
245 typedef typename Eigen::Matrix<Scalar, 1, 1> ReturnType;
246 };
247
248 template<typename Scalar, int Options, typename ForceSet>
249 struct ConstraintForceSetOp<JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>, ForceSet>
250 {
251 typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> ReturnType;
252 };
253
254 template<typename _Scalar, int _Options>
255 struct traits<JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options>>
256 {
257 typedef _Scalar Scalar;
258 enum
259 {
260 Options = _Options
261 };
262 enum
263 {
264 LINEAR = 0,
265 ANGULAR = 3
266 };
267
268 typedef MotionHelicalUnalignedTpl<Scalar, Options> JointMotion;
269 typedef Eigen::Matrix<Scalar, 1, 1, Options> JointForce;
270 typedef Eigen::Matrix<Scalar, 6, 1, Options> DenseBase;
271 typedef Eigen::Matrix<Scalar, 1, 1, Options> ReducedSquaredMatrix;
272
273 typedef DenseBase MatrixReturnType;
274 typedef const DenseBase ConstMatrixReturnType;
275
276 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
277
278 typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType;
279 }; // traits JointMotionSubspaceHelicalUnalignedTpl
280
281 template<typename _Scalar, int _Options>
282 struct JointMotionSubspaceHelicalUnalignedTpl
283 : JointMotionSubspaceBase<JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options>>
284 {
285 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
286
287 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceHelicalUnalignedTpl)
288 enum
289 {
290 NV = 1
291 };
292
293 ✗ JointMotionSubspaceHelicalUnalignedTpl()
294 {
295 }
296
297 typedef typename traits<JointMotionSubspaceHelicalUnalignedTpl>::Vector3 Vector3;
298
299 template<typename Vector3Like>
300 23 JointMotionSubspaceHelicalUnalignedTpl(
301 const Eigen::MatrixBase<Vector3Like> & axis, const Scalar & h)
302 23 : m_axis(axis)
303 23 , m_pitch(h)
304 {
305 23 }
306
307 template<typename Vector1Like>
308 1 JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
309 {
310 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like, 1);
311
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(v.size() == 1);
312
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 return JointMotion(m_axis, v[0], Scalar(v[0] * m_pitch));
313 }
314
315 template<typename S1, int O1>
316 typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType
317 ✗ se3Action(const SE3Tpl<S1, O1> & m) const
318 {
319 typedef
320 typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType ReturnType;
321 ✗ ReturnType res;
322 ✗ res.template segment<3>(ANGULAR).noalias() = m.rotation() * m_axis;
323 ✗ res.template segment<3>(LINEAR).noalias() =
324 ✗ m.translation().cross(res.template segment<3>(ANGULAR)) + m_pitch * (m.rotation() * m_axis);
325 ✗ return res;
326 }
327
328 template<typename S1, int O1>
329 typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType
330 ✗ se3ActionInverse(const SE3Tpl<S1, O1> & m) const
331 {
332 typedef
333 typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType ReturnType;
334
335 ✗ ReturnType res;
336 ✗ res.template segment<3>(ANGULAR).noalias() = m.rotation().transpose() * m_axis;
337 ✗ res.template segment<3>(LINEAR).noalias() =
338 ✗ -m.rotation().transpose() * m.translation().cross(m_axis)
339 ✗ + m.rotation().transpose() * m_axis * m_pitch;
340
341 ✗ return res;
342 }
343
344 ✗ int nv_impl() const
345 {
346 ✗ return NV;
347 }
348
349 struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceHelicalUnalignedTpl>
350 {
351 const JointMotionSubspaceHelicalUnalignedTpl & ref;
352 ✗ TransposeConst(const JointMotionSubspaceHelicalUnalignedTpl & ref)
353 ✗ : ref(ref)
354 {
355 }
356
357 template<typename ForceDerived>
358 typename ConstraintForceOp<JointMotionSubspaceHelicalUnalignedTpl, ForceDerived>::ReturnType
359 ✗ operator*(const ForceDense<ForceDerived> & f) const
360 {
361 typedef typename ConstraintForceOp<
362 JointMotionSubspaceHelicalUnalignedTpl, ForceDerived>::ReturnType ReturnType;
363 ✗ ReturnType res;
364 ✗ res[0] = ref.axis().dot(f.angular()) + ref.axis().dot(f.linear()) * ref.m_pitch;
365 ✗ return res;
366 }
367
368 /// [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
369 template<typename Derived>
370 typename ConstraintForceSetOp<JointMotionSubspaceHelicalUnalignedTpl, Derived>::ReturnType
371 ✗ operator*(const Eigen::MatrixBase<Derived> & F) const
372 {
373 ✗ assert(F.rows() == 6);
374 return (
375 ✗ ref.axis().transpose() * F.template middleRows<3>(ANGULAR)
376 ✗ + (ref.axis().transpose() * F.template middleRows<3>(LINEAR) * ref.m_pitch));
377 }
378 }; // struct TransposeConst
379
380 ✗ TransposeConst transpose() const
381 {
382 ✗ return TransposeConst(*this);
383 }
384
385 /* CRBA joint operators
386 * - ForceSet::Block = ForceSet
387 * - ForceSet operator* (Inertia Y,Constraint S)
388 * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
389 * - SE3::act(ForceSet::Block)
390 */
391 ✗ DenseBase matrix_impl() const
392 {
393 ✗ DenseBase S;
394 ✗ S.template segment<3>(LINEAR) = m_axis * m_pitch;
395 ✗ S.template segment<3>(ANGULAR) = m_axis;
396 ✗ return S;
397 }
398
399 template<typename MotionDerived>
400 typename MotionAlgebraAction<JointMotionSubspaceHelicalUnalignedTpl, MotionDerived>::ReturnType
401 ✗ motionAction(const MotionDense<MotionDerived> & m) const
402 {
403 ✗ const typename MotionDerived::ConstLinearType v = m.linear();
404 ✗ const typename MotionDerived::ConstAngularType w = m.angular();
405
406 ✗ DenseBase res;
407 ✗ res.template segment<3>(LINEAR).noalias() = v.cross(m_axis);
408 ✗ res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis * m_pitch);
409 ✗ res.template segment<3>(LINEAR).noalias() += res.template segment<3>(ANGULAR);
410 ✗ res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis);
411
412 ✗ return res;
413 }
414
415 ✗ bool isEqual(const JointMotionSubspaceHelicalUnalignedTpl & other) const
416 {
417 ✗ return internal::comparison_eq(m_axis, other.m_axis)
418 ✗ && internal::comparison_eq(m_pitch, other.m_pitch);
419 }
420
421 ✗ Vector3 & axis()
422 {
423 ✗ return m_axis;
424 }
425 ✗ const Vector3 & axis() const
426 {
427 ✗ return m_axis;
428 }
429
430 ✗ Scalar & h()
431 {
432 ✗ return m_pitch;
433 }
434 ✗ const Scalar & h() const
435 {
436 ✗ return m_pitch;
437 }
438
439 Vector3 m_axis;
440 Scalar m_pitch;
441
442 }; // struct JointMotionSubspaceHelicalUnalignedTpl
443
444 template<typename _Scalar, int _Options>
445 ✗ Eigen::Matrix<_Scalar, 1, 1, _Options> operator*(
446 const typename JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options>::TransposeConst &
447 S_transpose,
448 const JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options> & S)
449 {
450 ✗ Eigen::Matrix<_Scalar, 1, 1, _Options> res;
451 ✗ res(0) = (S_transpose.ref.axis() * S_transpose.ref.h()).dot(S.axis() * S.h())
452 ✗ + (S_transpose.ref.axis().dot(S.axis()));
453 ✗ return res;
454 }
455
456 template<typename S1, int O1, typename S2, int O2>
457 struct MultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalUnalignedTpl<S2, O2>>
458 {
459 typedef Eigen::Matrix<S2, 6, 1, O2> ReturnType;
460 };
461
462 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
463 namespace impl
464 {
465 template<typename S1, int O1, typename S2, int O2>
466 struct LhsMultiplicationOp<InertiaTpl<S1, O1>, JointMotionSubspaceHelicalUnalignedTpl<S2, O2>>
467 {
468 typedef InertiaTpl<S1, O1> Inertia;
469 typedef JointMotionSubspaceHelicalUnalignedTpl<S2, O2> Constraint;
470 typedef typename JointMotionSubspaceHelicalUnalignedTpl<S2, O2>::Vector3 Vector3;
471 typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType;
472 ✗ static inline ReturnType run(const Inertia & Y, const Constraint & cru)
473 {
474 ✗ ReturnType res;
475 /* YS = [ m -mcx ; mcx I-mcxcx ] [ h ; w ] = [mh-mcxw ; mcxh+Iw-mcxcxw ] */
476
477 ✗ const S2 & m_pitch = cru.h();
478 ✗ const typename Inertia::Scalar & m = Y.mass();
479 ✗ const typename Inertia::Vector3 & c = Y.lever();
480 ✗ const typename Inertia::Symmetric3 & I = Y.inertia();
481
482 ✗ res.template segment<3>(Inertia::LINEAR) = -m * c.cross(cru.axis());
483 ✗ res.template segment<3>(Inertia::ANGULAR).noalias() = I * cru.axis();
484 ✗ res.template segment<3>(Inertia::ANGULAR) += c.cross(cru.axis()) * m * m_pitch;
485 ✗ res.template segment<3>(Inertia::ANGULAR) +=
486 ✗ c.cross(res.template segment<3>(Inertia::LINEAR));
487 ✗ res.template segment<3>(Inertia::LINEAR) += m * m_pitch * cru.axis();
488 ✗ return res;
489 }
490 };
491 } // namespace impl
492
493 template<typename M6Like, typename Scalar, int Options>
494 struct MultiplicationOp<
495 Eigen::MatrixBase<M6Like>,
496 JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>>
497 {
498 typedef const Eigen::Matrix<Scalar, 6, 1> ReturnType;
499 };
500
501 /* [ABA] operator* (Inertia Y,Constraint S) */
502 namespace impl
503 {
504 template<typename M6Like, typename Scalar, int Options>
505 struct LhsMultiplicationOp<
506 Eigen::MatrixBase<M6Like>,
507 JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>>
508 {
509 typedef JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options> Constraint;
510 typedef Eigen::Matrix<Scalar, 6, 1> ReturnType;
511 ✗ static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & cru)
512 {
513 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6);
514 ✗ return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis()
515 ✗ + Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis() * cru.h();
516 }
517 };
518 } // namespace impl
519
520 template<typename Scalar, int Options>
521 struct JointHelicalUnalignedTpl;
522
523 template<typename _Scalar, int _Options>
524 struct traits<JointHelicalUnalignedTpl<_Scalar, _Options>>
525 {
526 enum
527 {
528 NQ = 1,
529 NV = 1
530 };
531 typedef _Scalar Scalar;
532 enum
533 {
534 Options = _Options
535 };
536 typedef JointDataHelicalUnalignedTpl<Scalar, Options> JointDataDerived;
537 typedef JointModelHelicalUnalignedTpl<Scalar, Options> JointModelDerived;
538 typedef JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options> Constraint_t;
539 typedef SE3Tpl<Scalar, Options> Transformation_t;
540 typedef MotionHelicalUnalignedTpl<Scalar, Options> Motion_t;
541 typedef MotionZeroTpl<Scalar, Options> Bias_t;
542
543 // [ABA]
544 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
545 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
546 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
547
548 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
549 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
550
551 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
552 };
553
554 template<typename _Scalar, int _Options>
555 struct traits<JointDataHelicalUnalignedTpl<_Scalar, _Options>>
556 {
557 typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived;
558 typedef _Scalar Scalar;
559 };
560
561 template<typename _Scalar, int _Options>
562 struct traits<JointModelHelicalUnalignedTpl<_Scalar, _Options>>
563 {
564 typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived;
565 typedef _Scalar Scalar;
566 };
567
568 template<typename _Scalar, int _Options>
569 struct JointDataHelicalUnalignedTpl
570 : public JointDataBase<JointDataHelicalUnalignedTpl<_Scalar, _Options>>
571 {
572 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
573 typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived;
574 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
575 1 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
576
577 ConfigVector_t joint_q;
578 TangentVector_t joint_v;
579
580 Constraint_t S;
581 Transformation_t M;
582 Motion_t v;
583 Bias_t c;
584
585 // [ABA] specific data
586 U_t U;
587 D_t Dinv;
588 UD_t UDinv;
589 D_t StU;
590
591 23 JointDataHelicalUnalignedTpl()
592
1/2
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
 23 : joint_q(ConfigVector_t::Zero())
593
1/2
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
 23 , joint_v(TangentVector_t::Zero())
594
2/4
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 23 , S(Constraint_t::Vector3::Zero(), (Scalar)0)
595 23 , M(Transformation_t::Identity())
596
2/4
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 23 , v(Constraint_t::Vector3::Zero(), (Scalar)0, (Scalar)0)
597
1/2
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
 23 , U(U_t::Zero())
598
1/2
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
 23 , Dinv(D_t::Zero())
599
1/2
✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
 23 , UDinv(UD_t::Zero())
600
1/2
✓ Branch 3 taken 23 times.
✗ Branch 4 not taken.
 46 , StU(D_t::Zero())
601 {
602 23 }
603
604 template<typename Vector3Like>
605 ✗ JointDataHelicalUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
606 ✗ : joint_q(ConfigVector_t::Zero())
607 ✗ , joint_v(TangentVector_t::Zero())
608 ✗ , S(axis, (Scalar)0)
609 ✗ , M(Transformation_t::Identity())
610 ✗ , v(axis, (Scalar)0, (Scalar)0)
611 ✗ , U(U_t::Zero())
612 ✗ , Dinv(D_t::Zero())
613 ✗ , UDinv(UD_t::Zero())
614 ✗ , StU(D_t::Zero())
615 {
616 }
617
618 40 static std::string classname()
619 {
620
1/2
✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
 40 return std::string("JointDataHelicalUnaligned");
621 }
622 ✗ std::string shortname() const
623 {
624 ✗ return classname();
625 }
626
627 }; // struct JointDataHelicalUnalignedTpl
628
629 PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelHelicalUnalignedTpl);
630 template<typename _Scalar, int _Options>
631 struct JointModelHelicalUnalignedTpl
632 : public JointModelBase<JointModelHelicalUnalignedTpl<_Scalar, _Options>>
633 {
634 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
635 typedef JointHelicalUnalignedTpl<_Scalar, _Options> JointDerived;
636 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
637 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
638
639 typedef JointModelBase<JointModelHelicalUnalignedTpl> Base;
640 using Base::id;
641 using Base::idx_q;
642 using Base::idx_v;
643 using Base::setIndexes;
644
645 20 JointModelHelicalUnalignedTpl()
646 20 {
647 20 }
648
649 template<typename Vector3Like>
650 ✗ JointModelHelicalUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
651 ✗ : axis(axis)
652 ✗ , m_pitch((Scalar)0)
653 {
654 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
655 ✗ assert(isUnitary(axis) && "Rotation axis is not unitary");
656 }
657
658 ✗ JointModelHelicalUnalignedTpl(
659 const Scalar & x, const Scalar & y, const Scalar & z, const Scalar & h)
660 ✗ : axis(x, y, z)
661 ✗ , m_pitch(h)
662 {
663 ✗ normalize(axis);
664 ✗ assert(isUnitary(axis) && "Rotation axis is not unitary");
665 }
666
667 template<typename Vector3Like>
668 ✗ JointModelHelicalUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis, const Scalar & h)
669 ✗ : axis(axis)
670 ✗ , m_pitch(h)
671 {
672 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
673 ✗ assert(isUnitary(axis) && "Rotation axis is not unitary");
674 }
675
676 ✗ const std::vector<bool> hasConfigurationLimit() const
677 {
678 ✗ return {true, true};
679 }
680
681 ✗ const std::vector<bool> hasConfigurationLimitInTangent() const
682 {
683 ✗ return {true, true};
684 }
685
686 3 JointDataDerived createData() const
687 {
688 3 return JointDataDerived();
689 }
690
691 using Base::isEqual;
692 ✗ bool isEqual(const JointModelHelicalUnalignedTpl & other) const
693 {
694 ✗ return Base::isEqual(other) && internal::comparison_eq(axis, other.axis)
695 ✗ && internal::comparison_eq(m_pitch, other.m_pitch);
696 }
697
698 template<typename ConfigVector>
699 ✗ void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
700 {
701 ✗ data.joint_q[0] = qs[idx_q()];
702
703 ✗ toRotationMatrix(axis, data.joint_q[0], data.M.rotation());
704 ✗ data.M.translation() = axis * data.joint_q[0] * m_pitch;
705 ✗ data.S.h() = m_pitch;
706 ✗ data.S.axis() = axis;
707 }
708
709 template<typename TangentVector>
710 void
711 ✗ calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
712 const
713 {
714 ✗ data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
715 ✗ data.v.axis() = axis;
716 ✗ data.v.linearRate() = static_cast<Scalar>(vs[idx_v()] * m_pitch);
717 }
718
719 template<typename ConfigVector, typename TangentVector>
720 3 void calc(
721 JointDataDerived & data,
722 const typename Eigen::MatrixBase<ConfigVector> & qs,
723 const typename Eigen::MatrixBase<TangentVector> & vs) const
724 {
725 3 calc(data, qs.derived());
726 3 data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
727 3 data.v.axis() = axis;
728 3 data.v.linearRate() = static_cast<Scalar>(vs[idx_v()] * m_pitch);
729 3 }
730
731 template<typename VectorLike, typename Matrix6Like>
732 ✗ void calc_aba(
733 JointDataDerived & data,
734 const Eigen::MatrixBase<VectorLike> & armature,
735 const Eigen::MatrixBase<Matrix6Like> & I,
736 const bool update_I) const
737 {
738 ✗ data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis
739 ✗ + m_pitch * I.template middleCols<3>(Motion::LINEAR) * axis;
740 ✗ data.StU[0] = axis.dot(data.U.template segment<3>(Motion::ANGULAR))
741 ✗ + m_pitch * axis.dot(data.U.template segment<3>(Motion::LINEAR)) + armature[0];
742 ✗ data.Dinv[0] = Scalar(1) / data.StU[0];
743 ✗ data.UDinv.noalias() = data.U * data.Dinv;
744 ✗ if (update_I)
745 ✗ PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
746 }
747
748 40 static std::string classname()
749 {
750
1/2
✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
 40 return std::string("JointModelHelicalUnaligned");
751 }
752 ✗ std::string shortname() const
753 {
754 ✗ return classname();
755 }
756
757 /// \returns An expression of *this with the Scalar type casted to NewScalar.
758 template<typename NewScalar>
759 ✗ JointModelHelicalUnalignedTpl<NewScalar, Options> cast() const
760 {
761 typedef JointModelHelicalUnalignedTpl<NewScalar, Options> ReturnType;
762 ✗ ReturnType res(axis.template cast<NewScalar>(), ScalarCast<NewScalar, Scalar>::cast(m_pitch));
763 ✗ res.setIndexes(id(), idx_q(), idx_v());
764 ✗ return res;
765 }
766
767 Vector3 axis;
768 Scalar m_pitch;
769
770 }; // struct JointModelHelicalUnalignedTpl
771
772 } // namespace pinocchio
773
774 #include <boost/type_traits.hpp>
775
776 namespace boost
777 {
778 template<typename Scalar, int Options>
779 struct has_nothrow_constructor<::pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
780 : public integral_constant<bool, true>
781 {
782 };
783
784 template<typename Scalar, int Options>
785 struct has_nothrow_copy<::pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
786 : public integral_constant<bool, true>
787 {
788 };
789
790 template<typename Scalar, int Options>
791 struct has_nothrow_constructor<::pinocchio::JointDataHelicalUnalignedTpl<Scalar, Options>>
792 : public integral_constant<bool, true>
793 {
794 };
795
796 template<typename Scalar, int Options>
797 struct has_nothrow_copy<::pinocchio::JointDataHelicalUnalignedTpl<Scalar, Options>>
798 : public integral_constant<bool, true>
799 {
800 };
801 } // namespace boost
802
803 #endif // ifndef __pinocchio_multibody_joint_helical_unaligned_hpp__
804