pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
joint-helical.hpp
1//
2// Copyright (c) 2022-2023 INRIA
3//
4
5#ifndef __pinocchio_multibody_joint_helical_hpp__
6#define __pinocchio_multibody_joint_helical_hpp__
7
8#include "pinocchio/math/sincos.hpp"
9#include "pinocchio/spatial/inertia.hpp"
10#include "pinocchio/multibody/joint-motion-subspace.hpp"
11#include "pinocchio/multibody/joint/joint-base.hpp"
12#include "pinocchio/spatial/spatial-axis.hpp"
13#include "pinocchio/utils/axis-label.hpp"
14
15namespace pinocchio
16{
17
18 template<typename Scalar, int Options, int axis>
19 struct MotionHelicalTpl;
20
21 template<typename Scalar, int Options, int axis>
22 struct SE3GroupAction<MotionHelicalTpl<Scalar, Options, axis>>
23 {
25 };
26
27 template<typename Scalar, int Options, int axis, typename MotionDerived>
28 struct MotionAlgebraAction<MotionHelicalTpl<Scalar, Options, axis>, MotionDerived>
29 {
31 };
32
33 template<typename _Scalar, int _Options, int axis>
35 {
36 typedef _Scalar Scalar;
37 enum
38 {
39 Options = _Options
40 };
41 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
42 typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
43 typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
44 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
45 typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
46 typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
47 typedef Vector3 AngularType;
48 typedef Vector3 LinearType;
49 typedef const Vector3 ConstAngularType;
50 typedef const Vector3 ConstLinearType;
51 typedef Matrix6 ActionMatrixType;
54 typedef Matrix4 HomogeneousMatrixType;
55 enum
56 {
57 LINEAR = 0,
58 ANGULAR = 3
59 };
60 }; // traits MotionHelicalTpl
61
62 template<typename Scalar, int Options, int axis>
64
65 template<typename _Scalar, int _Options, int _axis>
67 {
68 enum
69 {
70 axis = _axis,
71 Options = _Options,
72 LINEAR = 0,
73 ANGULAR = 3
74 };
75 typedef _Scalar Scalar;
77 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
78 typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
79 typedef Matrix3 AngularType;
80 typedef Matrix3 AngularRef;
81 typedef Matrix3 ConstAngularRef;
82 typedef Vector3 LinearType;
83 typedef typename Vector3::ConstantReturnType LinearRef;
84 typedef const typename Vector3::ConstantReturnType ConstLinearRef;
85 typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
86 typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
87 }; // traits TransformHelicalTpl
88
89 template<typename Scalar, int Options, int axis>
90 struct SE3GroupAction<TransformHelicalTpl<Scalar, Options, axis>>
91 {
92 typedef typename traits<TransformHelicalTpl<Scalar, Options, axis>>::PlainType ReturnType;
93 };
94
95 template<typename _Scalar, int _Options, int axis>
96 struct TransformHelicalTpl : SE3Base<TransformHelicalTpl<_Scalar, _Options, axis>>
97 {
99 PINOCCHIO_SE3_TYPEDEF_TPL(TransformHelicalTpl);
100
102 typedef typename AxisLinear::CartesianAxis3 CartesianAxis3Linear;
103
105 {
106 }
107 TransformHelicalTpl(const Scalar & sin, const Scalar & cos, const Scalar & displacement)
108 : m_sin(sin)
109 , m_cos(cos)
110 , m_displacement(displacement)
111 {
112 }
113
114 PlainType plain() const
115 {
116 PlainType res(PlainType::Identity());
117 _setRotation(res.rotation());
118 res.translation()[axis] = m_displacement;
119 return res;
120 }
121
122 operator PlainType() const
123 {
124 return plain();
125 }
126
127 template<typename S2, int O2>
128 typename SE3GroupAction<TransformHelicalTpl>::ReturnType
129 se3action(const SE3Tpl<S2, O2> & m) const
130 {
131 typedef typename SE3GroupAction<TransformHelicalTpl>::ReturnType ReturnType;
132 ReturnType res;
133 switch (axis)
134 {
135 case 0: {
136 res.rotation().col(0) = m.rotation().col(0);
137 res.rotation().col(1).noalias() = m_cos * m.rotation().col(1) + m_sin * m.rotation().col(2);
138 res.rotation().col(2).noalias() = res.rotation().col(0).cross(res.rotation().col(1));
139 break;
140 }
141 case 1: {
142 res.rotation().col(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0);
143 res.rotation().col(1) = m.rotation().col(1);
144 res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2));
145 break;
146 }
147 case 2: {
148 res.rotation().col(0).noalias() = m_cos * m.rotation().col(0) + m_sin * m.rotation().col(1);
149 res.rotation().col(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0));
150 res.rotation().col(2) = m.rotation().col(2);
151 break;
152 }
153 default: {
154 assert(false && "must never happen");
155 break;
156 }
157 }
158 res.translation() = m.translation();
159 res.translation()[axis] += m_displacement;
160 return res;
161 }
162
163 const Scalar & sin() const
164 {
165 return m_sin;
166 }
167 Scalar & sin()
168 {
169 return m_sin;
170 }
171
172 const Scalar & cos() const
173 {
174 return m_cos;
175 }
176 Scalar & cos()
177 {
178 return m_cos;
179 }
180
181 const Scalar & displacement() const
182 {
183 return m_displacement;
184 }
185 Scalar & displacement()
186 {
187 return m_displacement;
188 }
189
190 template<typename Scalar1, typename Scalar2, typename Scalar3>
191 void setValues(const Scalar1 & sin, const Scalar2 & cos, const Scalar3 & displacement)
192 {
193 m_sin = sin;
194 m_cos = cos;
195 m_displacement = displacement;
196 }
197
198 LinearType translation() const
199 {
200 return CartesianAxis3Linear() * displacement();
201 }
202 AngularType rotation() const
203 {
204 AngularType m(AngularType::Identity());
205 _setRotation(m);
206 return m;
207 }
208
209 bool isEqual(const TransformHelicalTpl & other) const
210 {
211 return internal::comparison_eq(m_cos, other.m_cos)
212 && internal::comparison_eq(m_sin, other.m_sin)
213 && internal::comparison_eq(m_displacement, other.m_displacement);
214 }
215
216 protected:
217 Scalar m_sin, m_cos, m_displacement;
218 inline void _setRotation(typename PlainType::AngularRef & rot) const
219 {
220 switch (axis)
221 {
222 case 0: {
223 rot.coeffRef(1, 1) = m_cos;
224 rot.coeffRef(1, 2) = -m_sin;
225 rot.coeffRef(2, 1) = m_sin;
226 rot.coeffRef(2, 2) = m_cos;
227 break;
228 }
229 case 1: {
230 rot.coeffRef(0, 0) = m_cos;
231 rot.coeffRef(0, 2) = m_sin;
232 rot.coeffRef(2, 0) = -m_sin;
233 rot.coeffRef(2, 2) = m_cos;
234 break;
235 }
236 case 2: {
237 rot.coeffRef(0, 0) = m_cos;
238 rot.coeffRef(0, 1) = -m_sin;
239 rot.coeffRef(1, 0) = m_sin;
240 rot.coeffRef(1, 1) = m_cos;
241 break;
242 }
243 default: {
244 assert(false && "must never happen");
245 break;
246 }
247 }
248 }
249 }; // struct TransformHelicalTpl
250
251 template<typename _Scalar, int _Options, int axis>
252 struct MotionHelicalTpl : MotionBase<MotionHelicalTpl<_Scalar, _Options, axis>>
253 {
255
256 MOTION_TYPEDEF_TPL(MotionHelicalTpl);
258 typedef typename AxisAngular::CartesianAxis3 CartesianAxis3Angular;
260 typedef typename AxisLinear::CartesianAxis3 CartesianAxis3Linear;
261
263 {
264 }
265
266 MotionHelicalTpl(const Scalar & w, const Scalar & v)
267 : m_w(w)
268 , m_v(v)
269 {
270 }
271
272 inline PlainReturnType plain() const
273 {
274 return PlainReturnType(CartesianAxis3Linear() * m_v, CartesianAxis3Angular() * m_w);
275 }
276
277 template<typename OtherScalar>
278 MotionHelicalTpl __mult__(const OtherScalar & alpha) const
279 {
280 return MotionHelicalTpl(alpha * m_w, alpha * m_v);
281 }
282
283 template<typename MotionDerived>
284 void setTo(MotionDense<MotionDerived> & m) const
285 {
286 for (Eigen::DenseIndex k = 0; k < 3; ++k)
287 {
288 m.angular()[k] = k == axis ? m_w : (Scalar)0;
289 m.linear()[k] = k == axis ? m_v : (Scalar)0;
290 }
291 }
292
293 template<typename MotionDerived>
294 inline void addTo(MotionDense<MotionDerived> & v) const
295 {
296 typedef typename MotionDense<MotionDerived>::Scalar OtherScalar;
297 v.angular()[axis] += (OtherScalar)m_w;
298 v.linear()[axis] += (OtherScalar)m_v;
299 }
300
301 template<typename S2, int O2, typename D2>
302 inline void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
303 {
304 v.angular().noalias() = m.rotation().col(axis) * m_w;
305 v.linear().noalias() = m.translation().cross(v.angular()) + m_v * (m.rotation().col(axis));
306 }
307
308 template<typename S2, int O2>
309 MotionPlain se3Action_impl(const SE3Tpl<S2, O2> & m) const
310 {
311
312 MotionPlain res;
313 se3Action_impl(m, res);
314 return res;
315 }
316
317 template<typename S2, int O2, typename D2>
318 void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
319 {
320 // Linear
321 CartesianAxis3Linear::alphaCross(m_w, m.translation(), v.angular());
322 v.linear().noalias() =
323 m.rotation().transpose() * v.angular() + m_v * (m.rotation().transpose().col(axis));
324
325 // Angular
326 v.angular().noalias() = m.rotation().transpose().col(axis) * m_w;
327 }
328
329 template<typename S2, int O2>
330 MotionPlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
331 {
332 MotionPlain res;
333 se3ActionInverse_impl(m, res);
334 return res;
335 }
336
337 template<typename M1, typename M2>
338 EIGEN_STRONG_INLINE void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
339 {
340 // Linear
341 CartesianAxis3Linear::alphaCross(-m_w, v.linear(), mout.linear());
342 CartesianAxis3Linear::alphaCross(-m_v, v.angular(), mout.angular());
343 mout.linear() += mout.angular();
344 // Angular
345 CartesianAxis3Angular::alphaCross(-m_w, v.angular(), mout.angular());
346 }
347
348 template<typename M1>
349 MotionPlain motionAction(const MotionDense<M1> & v) const
350 {
351 MotionPlain res;
352 motionAction(v, res);
353 return res;
354 }
355
356 Scalar & angularRate()
357 {
358 return m_w;
359 }
360 const Scalar & angularRate() const
361 {
362 return m_w;
363 }
364
365 Scalar & linearRate()
366 {
367 return m_v;
368 }
369 const Scalar & linearRate() const
370 {
371 return m_v;
372 }
373
374 bool isEqual_impl(const MotionHelicalTpl & other) const
375 {
376 return internal::comparison_eq(m_w, other.m_w) && internal::comparison_eq(m_v, other.m_v);
377 }
378
379 protected:
380 Scalar m_w, m_v;
381 }; // struct MotionHelicalTpl
382 template<typename S1, int O1, int axis, typename MotionDerived>
383 typename MotionDerived::MotionPlain
385 {
386 typename MotionDerived::MotionPlain res(m2);
387 res += m1;
388 return res;
389 }
390
391 template<typename MotionDerived, typename S2, int O2, int axis>
392 EIGEN_STRONG_INLINE typename MotionDerived::MotionPlain
393 operator^(const MotionDense<MotionDerived> & m1, const MotionHelicalTpl<S2, O2, axis> & m2)
394 {
395 return m2.motionAction(m1);
396 }
397
398 template<typename Scalar, int Options, int axis>
399 struct JointMotionSubspaceHelicalTpl;
400
401 template<typename Scalar, int Options, int axis>
402 struct SE3GroupAction<JointMotionSubspaceHelicalTpl<Scalar, Options, axis>>
403 {
404 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
405 };
406
407 template<typename Scalar, int Options, int axis, typename MotionDerived>
409 {
410 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
411 };
412
413 template<typename Scalar, int Options, int axis, typename ForceDerived>
415 {
416 typedef typename Eigen::Matrix<Scalar, 1, 1> ReturnType;
417 };
418
419 template<typename Scalar, int Options, int axis, typename ForceSet>
421 {
422 typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> ReturnType;
423 };
424
425 template<typename _Scalar, int _Options, int axis>
427 {
428 typedef _Scalar Scalar;
429 enum
430 {
431 Options = _Options
432 };
433 enum
434 {
435 LINEAR = 0,
436 ANGULAR = 3
437 };
438
440 typedef Eigen::Matrix<Scalar, 1, 1, Options> JointForce;
441 typedef Eigen::Matrix<Scalar, 6, 1, Options> DenseBase;
442 typedef Eigen::Matrix<Scalar, 1, 1, Options> ReducedSquaredMatrix;
443
444 typedef DenseBase MatrixReturnType;
445 typedef const DenseBase ConstMatrixReturnType;
446
447 typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType;
448 }; // traits JointMotionSubspaceHelicalTpl
449
450 template<class ConstraintDerived>
452 {
453 typedef
454 typename Eigen::Matrix<typename ConstraintDerived::Scalar, 1, 1, ConstraintDerived::Options>
455 ReturnType;
456 };
457
458 template<typename _Scalar, int _Options, int axis>
460 : JointMotionSubspaceBase<JointMotionSubspaceHelicalTpl<_Scalar, _Options, axis>>
461 {
463
464 PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceHelicalTpl)
465 enum
466 {
467 NV = 1
468 };
469
472
473 typedef typename AxisAngular::CartesianAxis3 CartesianAxis3Angular;
474 typedef typename AxisLinear::CartesianAxis3 CartesianAxis3Linear;
475
477 {
478 }
479
480 JointMotionSubspaceHelicalTpl(const Scalar & h)
481 : m_pitch(h)
482 {
483 }
484
485 template<typename Vector1Like>
486 JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
487 {
489 assert(v.size() == 1);
490 return JointMotion(v[0], v[0] * m_pitch);
491 }
492
493 template<typename S1, int O1>
494 typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType
495 se3Action(const SE3Tpl<S1, O1> & m) const
496 {
497 typedef typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType ReturnType;
498 ReturnType res;
499 res.template segment<3>(LINEAR) =
500 m.translation().cross(m.rotation().col(axis)) + m_pitch * (m.rotation().col(axis));
501 res.template segment<3>(ANGULAR) = m.rotation().col(axis);
502 return res;
503 }
504
505 template<typename S1, int O1>
506 typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType
507 se3ActionInverse(const SE3Tpl<S1, O1> & m) const
508 {
509 typedef typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType ReturnType;
510 typedef typename AxisAngular::CartesianAxis3 CartesianAxis3;
511 ReturnType res;
512 res.template segment<3>(LINEAR).noalias() =
513 m.rotation().transpose() * CartesianAxis3::cross(m.translation())
514 + m.rotation().transpose().col(axis) * m_pitch;
515 res.template segment<3>(ANGULAR) = m.rotation().transpose().col(axis);
516 return res;
517 }
518
519 int nv_impl() const
520 {
521 return NV;
522 }
523
524 // For force T
525 struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceHelicalTpl>
526 {
529 : ref(ref)
530 {
531 }
532
533 template<typename ForceDerived>
535 operator*(const ForceDense<ForceDerived> & f) const
536 {
537 return Eigen::Matrix<Scalar, 1, 1>(f.angular()(axis) + f.linear()(axis) * ref.m_pitch);
538 }
539
541 template<typename Derived>
543 operator*(const Eigen::MatrixBase<Derived> & F) const
544 {
545 assert(F.rows() == 6);
546 return F.row(ANGULAR + axis) + F.row(LINEAR + axis) * ref.m_pitch;
547 }
548 }; // struct TransposeConst
549
550 TransposeConst transpose() const
551 {
552 return TransposeConst(*this);
553 }
554
555 /* CRBA joint operators
556 * - ForceSet::Block = ForceSet
557 * - ForceSet operator* (Inertia Y,Constraint S)
558 * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
559 * - SE3::act(ForceSet::Block)
560 */
561 DenseBase matrix_impl() const
562 {
563 DenseBase S;
564 MotionRef<DenseBase> v(S);
565 v << AxisAngular();
566 S(LINEAR + axis) = m_pitch;
567 return S;
568 }
569
570 template<typename MotionDerived>
571 typename MotionAlgebraAction<JointMotionSubspaceHelicalTpl, MotionDerived>::ReturnType
572 motionAction(const MotionDense<MotionDerived> & m) const
573 {
574 typedef typename MotionAlgebraAction<JointMotionSubspaceHelicalTpl, MotionDerived>::ReturnType
575 ReturnType;
576 ReturnType res;
577 // Linear
578 CartesianAxis3Linear::cross(-m.linear(), res.template segment<3>(LINEAR));
579 CartesianAxis3Linear::alphaCross(-m_pitch, m.angular(), res.template segment<3>(ANGULAR));
580 res.template segment<3>(LINEAR) += res.template segment<3>(ANGULAR);
581
582 // Angular
583 CartesianAxis3Angular::cross(-m.angular(), res.template segment<3>(ANGULAR));
584 return res;
585 }
586
587 bool isEqual(const JointMotionSubspaceHelicalTpl &) const
588 {
589 return true;
590 }
591
592 Scalar & h()
593 {
594 return m_pitch;
595 }
596 const Scalar & h() const
597 {
598 return m_pitch;
599 }
600
601 protected:
602 Scalar m_pitch;
603 }; // struct JointMotionSubspaceHelicalTpl
604
605 template<typename _Scalar, int _Options, int _axis>
606 Eigen::Matrix<_Scalar, 1, 1, _Options> operator*(
607 const typename JointMotionSubspaceHelicalTpl<_Scalar, _Options, _axis>::TransposeConst &
608 S_transpose,
609 const JointMotionSubspaceHelicalTpl<_Scalar, _Options, _axis> & S)
610 {
611 Eigen::Matrix<_Scalar, 1, 1, _Options> res;
612 res(0) = 1.0 + S_transpose.ref.h() * S.h();
613 return res;
614 }
615
616 template<typename _Scalar, int _Options, int _axis>
618 {
619 typedef _Scalar Scalar;
620
621 enum
622 {
623 Options = _Options,
624 axis = _axis
625 };
626 };
627
628 template<typename S1, int O1, typename S2, int O2, int axis>
630 {
631 typedef Eigen::Matrix<S2, 6, 1, O2> ReturnType;
632 };
633
634 /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
635 namespace impl
636 {
637 template<typename S1, int O1, typename S2, int O2>
639 {
642 typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType;
643 static inline ReturnType run(const Inertia & Y, const Constraint & constraint)
644 {
645 ReturnType res;
646 const S2 & m_pitch = constraint.h();
647
648 /* Y(:,3) = ( 0,-z, y, I00+yy+zz, I01-xy , I02-xz ) */
649 /* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */
650 const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2];
651 const typename Inertia::Symmetric3 & I = Y.inertia();
652
653 res << m * m_pitch, -m * z, m * y, I(0, 0) + m * (y * y + z * z),
654 I(0, 1) - m * x * y + m * z * m_pitch, I(0, 2) - m * x * z - m * y * m_pitch;
655
656 return res;
657 }
658 };
659
660 template<typename S1, int O1, typename S2, int O2>
662 {
665 typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType;
666 static inline ReturnType run(const Inertia & Y, const Constraint & constraint)
667 {
668 ReturnType res;
669 const S2 & m_pitch = constraint.h();
670
671 /* Y(:,4) = ( z, 0,-x, I10-xy , I11+xx+zz, I12-yz ) */
672 /* Y(:,1) = ( 0,1, 0, -z , 0 , x) */
673 const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2];
674 const typename Inertia::Symmetric3 & I = Y.inertia();
675
676 res << m * z, m * m_pitch, -m * x, I(1, 0) - m * x * y - m * z * m_pitch,
677 I(1, 1) + m * (x * x + z * z), I(1, 2) - m * y * z + m * x * m_pitch;
678
679 return res;
680 }
681 };
682
683 template<typename S1, int O1, typename S2, int O2>
685 {
688 typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType;
689 static inline ReturnType run(const Inertia & Y, const Constraint & constraint)
690 {
691 ReturnType res;
692 const S2 & m_pitch = constraint.h();
693
694 /* Y(:,5) = (-y, x, 0, I20-xz , I21-yz , I22+xx+yy) */
695 /* Y(:,2) = ( 0,0, 1, y , -x , 0) */
696 const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2];
697 const typename Inertia::Symmetric3 & I = Y.inertia();
698
699 res << -m * y, m * x, m * m_pitch, I(2, 0) - m * x * z + m * y * m_pitch,
700 I(2, 1) - m * y * z - m * x * m_pitch, I(2, 2) + m * (x * x + y * y);
701
702 return res;
703 }
704 };
705 } // namespace impl
706
707 template<typename M6Like, typename S2, int O2, int axis>
708 struct MultiplicationOp<Eigen::MatrixBase<M6Like>, JointMotionSubspaceHelicalTpl<S2, O2, axis>>
709 {
710 typedef Eigen::Matrix<S2, 6, 1> ReturnType;
711 };
712
713 /* [ABA] operator* (Inertia Y,Constraint S) */
714 namespace impl
715 {
716 template<typename M6Like, typename Scalar, int Options, int axis>
718 Eigen::MatrixBase<M6Like>,
719 JointMotionSubspaceHelicalTpl<Scalar, Options, axis>>
720 {
722 typedef Eigen::Matrix<Scalar, 6, 1> ReturnType;
723 static inline ReturnType
724 run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & constraint)
725 {
727 return (Y.col(Inertia::ANGULAR + axis) + Y.col(Inertia::LINEAR + axis) * constraint.h());
728 }
729 };
730 } // namespace impl
731
732 template<typename _Scalar, int _Options, int axis>
734 {
735 enum
736 {
737 NQ = 1,
738 NV = 1,
739 NVExtended = 1
740 };
741 typedef _Scalar Scalar;
742 enum
743 {
744 Options = _Options
745 };
752
753 // [ABA]
754 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
755 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
756 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
757
758 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
759 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
760
761 typedef boost::mpl::true_ is_mimicable_t;
762
763 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
764 };
765
766 template<typename _Scalar, int _Options, int axis>
772
773 template<typename _Scalar, int _Options, int axis>
779
780 template<typename _Scalar, int _Options, int axis>
781 struct JointDataHelicalTpl : public JointDataBase<JointDataHelicalTpl<_Scalar, _Options, axis>>
782 {
785 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
786 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
787
788 ConfigVector_t joint_q;
789 TangentVector_t joint_v;
790
791 Constraint_t S;
792 Transformation_t M;
793 Motion_t v;
794 Bias_t c;
795
796 // [ABA] specific data
797 U_t U;
798 D_t Dinv;
799 UD_t UDinv;
800 D_t StU;
801
803 : joint_q(ConfigVector_t::Zero())
804 , joint_v(TangentVector_t::Zero())
805 , S((Scalar)0)
806 , M((Scalar)0, (Scalar)1, (Scalar)0)
807 , v((Scalar)0, (Scalar)0)
808 , U(U_t::Zero())
809 , Dinv(D_t::Zero())
810 , UDinv(UD_t::Zero())
811 , StU(D_t::Zero())
812 {
813 }
814
815 static std::string classname()
816 {
817 return std::string("JointDataH") + axisLabel<axis>();
818 }
819 std::string shortname() const
820 {
821 return classname();
822 }
823
824 }; // struct JointDataHelicalTpl
825
826 template<typename NewScalar, typename Scalar, int Options, int axis>
827 struct CastType<NewScalar, JointModelHelicalTpl<Scalar, Options, axis>>
828 {
830 };
831
832 template<typename _Scalar, int _Options, int axis>
833 struct JointModelHelicalTpl : public JointModelBase<JointModelHelicalTpl<_Scalar, _Options, axis>>
834 {
837 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
838
840 using Base::id;
841 using Base::idx_q;
842 using Base::idx_v;
843 using Base::idx_vExtended;
844 using Base::setIndexes;
845
846 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
847
848 JointDataDerived createData() const
849 {
850 return JointDataDerived();
851 }
852
854 {
855 }
856
857 explicit JointModelHelicalTpl(const Scalar & h)
858 : m_pitch(h)
859 {
860 }
861
862 const std::vector<bool> hasConfigurationLimit() const
863 {
864 return {true, true};
865 }
866
867 const std::vector<bool> hasConfigurationLimitInTangent() const
868 {
869 return {true, true};
870 }
871
872 template<typename ConfigVector>
873 PINOCCHIO_DONT_INLINE void
874 calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
875 {
876 data.joint_q[0] = qs[idx_q()];
877 Scalar ca, sa;
878 SINCOS(data.joint_q[0], &sa, &ca);
879 data.M.setValues(sa, ca, data.joint_q[0] * m_pitch);
880 data.S.h() = m_pitch;
881 }
882
883 template<typename TangentVector>
884 PINOCCHIO_DONT_INLINE void
885 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
886 const
887 {
888 data.joint_v[0] = vs[idx_v()];
889 data.v.angularRate() = data.joint_v[0];
890 data.v.linearRate() = data.joint_v[0] * m_pitch;
891 }
892
893 template<typename ConfigVector, typename TangentVector>
894 PINOCCHIO_DONT_INLINE void calc(
895 JointDataDerived & data,
896 const typename Eigen::MatrixBase<ConfigVector> & qs,
897 const typename Eigen::MatrixBase<TangentVector> & vs) const
898 {
899 calc(data, qs.derived());
900
901 data.joint_v[0] = vs[idx_v()];
902 data.v.angularRate() = data.joint_v[0];
903 data.v.linearRate() = data.joint_v[0] * m_pitch;
904 }
905
906 template<typename VectorLike, typename Matrix6Like>
907 void calc_aba(
908 JointDataDerived & data,
909 const Eigen::MatrixBase<VectorLike> & armature,
910 const Eigen::MatrixBase<Matrix6Like> & I,
911 const bool update_I) const
912 {
913 data.U = I.col(Inertia::ANGULAR + axis) + m_pitch * I.col(Inertia::LINEAR + axis);
914 data.StU[0] =
915 data.U(Inertia::ANGULAR + axis) + m_pitch * data.U(Inertia::LINEAR + axis) + armature[0];
916 data.Dinv[0] = Scalar(1) / data.StU[0];
917 data.UDinv.noalias() = data.U * data.Dinv;
918
919 if (update_I)
920 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
921 }
922
923 static std::string classname()
924 {
925 return std::string("JointModelH") + axisLabel<axis>();
926 }
927 std::string shortname() const
928 {
929 return classname();
930 }
931
932 Vector3 getMotionAxis() const
933 {
934 switch (axis)
935 {
936 case 0:
937 return Vector3::UnitX();
938 case 1:
939 return Vector3::UnitY();
940 case 2:
941 return Vector3::UnitZ();
942 default:
943 assert(false && "must never happen");
944 break;
945 }
946 }
947
949 template<typename NewScalar>
951 {
953 ReturnType res(ScalarCast<NewScalar, Scalar>::cast(m_pitch));
954 res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended());
955 return res;
956 }
957
958 Scalar m_pitch;
959
960 }; // struct JointModelHelicalTpl
961
962 typedef JointHelicalTpl<context::Scalar, context::Options, 0> JointHX;
963 typedef JointDataHelicalTpl<context::Scalar, context::Options, 0> JointDataHX;
964 typedef JointModelHelicalTpl<context::Scalar, context::Options, 0> JointModelHX;
965
966 typedef JointHelicalTpl<context::Scalar, context::Options, 1> JointHY;
967 typedef JointDataHelicalTpl<context::Scalar, context::Options, 1> JointDataHY;
968 typedef JointModelHelicalTpl<context::Scalar, context::Options, 1> JointModelHY;
969
970 typedef JointHelicalTpl<context::Scalar, context::Options, 2> JointHZ;
971 typedef JointDataHelicalTpl<context::Scalar, context::Options, 2> JointDataHZ;
972 typedef JointModelHelicalTpl<context::Scalar, context::Options, 2> JointModelHZ;
973
974 template<typename Scalar, int Options, int axis>
975 struct ConfigVectorAffineTransform<JointHelicalTpl<Scalar, Options, axis>>
976 {
978 };
979
980} // namespace pinocchio
981
982#include <boost/type_traits.hpp>
983
984namespace boost
985{
986 template<typename Scalar, int Options, int axis>
987 struct has_nothrow_constructor<::pinocchio::JointModelHelicalTpl<Scalar, Options, axis>>
988 : public integral_constant<bool, true>
989 {
990 };
991
992 template<typename Scalar, int Options, int axis>
993 struct has_nothrow_copy<::pinocchio::JointModelHelicalTpl<Scalar, Options, axis>>
994 : public integral_constant<bool, true>
995 {
996 };
997
998 template<typename Scalar, int Options, int axis>
999 struct has_nothrow_constructor<::pinocchio::JointDataHelicalTpl<Scalar, Options, axis>>
1000 : public integral_constant<bool, true>
1001 {
1002 };
1003
1004 template<typename Scalar, int Options, int axis>
1005 struct has_nothrow_copy<::pinocchio::JointDataHelicalTpl<Scalar, Options, axis>>
1006 : public integral_constant<bool, true>
1007 {
1008 };
1009} // namespace boost
1010
1011#endif // ifndef __pinocchio_multibody_joint_helical_hpp__
Main pinocchio namespace.
Definition treeview.dox:11
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition sincos.hpp:27
JointDataTpl< Scalar, Options, JointCollectionTpl >::TangentVector_t joint_v(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint velocity vector.
JointDataTpl< Scalar, Options, JointCollectionTpl >::ConfigVector_t joint_q(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint configuration vector.
Blank type.
Definition fwd.hpp:77
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition fwd.hpp:99
Assign the correct configuration vector space affine transformation according to the joint type....
Return type of the Constraint::Transpose * Force operation.
Return type of the Constraint::Transpose * ForceSet operation.
JointModelHelicalTpl< NewScalar, Options, axis > cast() const
ConstraintForceSetOp< JointMotionSubspaceHelicalTpl, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
Linear affine transformation of the configuration vector. Valide for most common joints which are evo...
Return type of the ation of a Motion onto an object of type D.
Definition motion.hpp:46
Forward declaration of the multiplication operation return type. Should be overloaded,...
Definition binary-op.hpp:15
Base class for rigid transformation.
Definition se3-base.hpp:31
Cast scalar type from type FROM to type TO.
Definition fwd.hpp:106
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72