5 #ifndef __pinocchio_multibody_joint_helical_hpp__
6 #define __pinocchio_multibody_joint_helical_hpp__
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"
18 template<
typename Scalar,
int Options,
int axis>
19 struct MotionHelicalTpl;
21 template<
typename Scalar,
int Options,
int axis>
27 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
33 template<
typename _Scalar,
int _Options,
int axis>
36 typedef _Scalar Scalar;
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;
62 template<
typename Scalar,
int Options,
int axis>
65 template<
typename _Scalar,
int _Options,
int _axis>
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;
89 template<
typename Scalar,
int Options,
int axis>
95 template<
typename _Scalar,
int _Options,
int axis>
98 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
110 , m_displacement(displacement)
114 PlainType plain()
const
116 PlainType res(PlainType::Identity());
117 _setRotation(res.rotation());
118 res.translation()[axis] = m_displacement;
122 operator PlainType()
const
127 template<
typename S2,
int O2>
128 typename SE3GroupAction<TransformHelicalTpl>::ReturnType
131 typedef typename SE3GroupAction<TransformHelicalTpl>::ReturnType ReturnType;
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));
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));
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);
154 assert(
false &&
"must never happen");
158 res.translation() = m.translation();
159 res.translation()[axis] += m_displacement;
163 const Scalar & sin()
const
172 const Scalar & cos()
const
181 const Scalar & displacement()
const
183 return m_displacement;
185 Scalar & displacement()
187 return m_displacement;
190 template<
typename Scalar1,
typename Scalar2,
typename Scalar3>
191 void setValues(
const Scalar1 & sin,
const Scalar2 & cos,
const Scalar3 & displacement)
195 m_displacement = displacement;
198 LinearType translation()
const
202 AngularType rotation()
const
204 AngularType m(AngularType::Identity());
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);
217 Scalar m_sin, m_cos, m_displacement;
218 inline void _setRotation(
typename PlainType::AngularRef & rot)
const
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;
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;
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;
244 assert(
false &&
"must never happen");
251 template<
typename _Scalar,
int _Options,
int axis>
254 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
272 inline PlainReturnType plain()
const
277 template<
typename OtherScalar>
283 template<
typename MotionDerived>
286 for (Eigen::DenseIndex k = 0; k < 3; ++k)
288 m.angular()[k] = k == axis ? m_w : (Scalar)0;
289 m.linear()[k] = k == axis ? m_v : (Scalar)0;
293 template<
typename MotionDerived>
296 typedef typename MotionDense<MotionDerived>::Scalar OtherScalar;
297 v.angular()[axis] += (OtherScalar)m_w;
298 v.linear()[axis] += (OtherScalar)m_v;
301 template<
typename S2,
int O2,
typename D2>
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));
308 template<
typename S2,
int O2>
313 se3Action_impl(m, res);
317 template<
typename S2,
int O2,
typename D2>
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));
326 v.angular().noalias() = m.rotation().transpose().col(axis) * m_w;
329 template<
typename S2,
int O2>
333 se3ActionInverse_impl(m, res);
337 template<
typename M1,
typename M2>
341 CartesianAxis3Linear::alphaCross(-m_w, v.linear(), mout.linear());
342 CartesianAxis3Linear::alphaCross(-m_v, v.angular(), mout.angular());
343 mout.linear() += mout.angular();
345 CartesianAxis3Angular::alphaCross(-m_w, v.angular(), mout.angular());
348 template<
typename M1>
352 motionAction(v, res);
356 Scalar & angularRate()
360 const Scalar & angularRate()
const
365 Scalar & linearRate()
369 const Scalar & linearRate()
const
376 return internal::comparison_eq(m_w, other.m_w) && internal::comparison_eq(m_v, other.m_v);
382 template<
typename S1,
int O1,
int axis,
typename MotionDerived>
383 typename MotionDerived::MotionPlain
386 typename MotionDerived::MotionPlain res(m2);
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)
395 return m2.motionAction(m1);
398 template<
typename Scalar,
int Options,
int axis>
399 struct JointMotionSubspaceHelicalTpl;
401 template<
typename Scalar,
int Options,
int axis>
404 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
407 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
410 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
413 template<
typename Scalar,
int Options,
int axis,
typename ForceDerived>
416 typedef typename Eigen::Matrix<Scalar, 1, 1> ReturnType;
419 template<
typename Scalar,
int Options,
int axis,
typename ForceSet>
422 typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> ReturnType;
425 template<
typename _Scalar,
int _Options,
int axis>
428 typedef _Scalar Scalar;
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;
444 typedef DenseBase MatrixReturnType;
445 typedef const DenseBase ConstMatrixReturnType;
447 typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType;
450 template<
class Constra
intDerived>
454 typename Eigen::Matrix<typename ConstraintDerived::Scalar, 1, 1, ConstraintDerived::Options>
458 template<
typename _Scalar,
int _Options,
int axis>
462 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
485 template<
typename Vector1Like>
486 JointMotion __mult__(
const Eigen::MatrixBase<Vector1Like> & v)
const
488 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like, 1);
489 assert(v.size() == 1);
490 return JointMotion(v[0], v[0] * m_pitch);
493 template<
typename S1,
int O1>
494 typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType
497 typedef typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType ReturnType;
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);
505 template<
typename S1,
int O1>
506 typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType
509 typedef typename SE3GroupAction<JointMotionSubspaceHelicalTpl>::ReturnType ReturnType;
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);
533 template<
typename ForceDerived>
534 typename ConstraintForceOp<JointMotionSubspaceHelicalTpl, ForceDerived>::ReturnType
537 return Eigen::Matrix<Scalar, 1, 1>(f.
angular()(axis) + f.
linear()(axis) * ref.m_pitch);
541 template<
typename Derived>
542 typename ConstraintForceSetOp<JointMotionSubspaceHelicalTpl, Derived>::ReturnType
545 assert(F.rows() == 6);
546 return F.row(ANGULAR + axis) + F.row(LINEAR + axis) * ref.m_pitch;
550 TransposeConst transpose()
const
552 return TransposeConst(*
this);
561 DenseBase matrix_impl()
const
564 MotionRef<DenseBase> v(S);
566 S(LINEAR + axis) = m_pitch;
570 template<
typename MotionDerived>
571 typename MotionAlgebraAction<JointMotionSubspaceHelicalTpl, MotionDerived>::ReturnType
572 motionAction(
const MotionDense<MotionDerived> & m)
const
574 typedef typename MotionAlgebraAction<JointMotionSubspaceHelicalTpl, MotionDerived>::ReturnType
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);
583 CartesianAxis3Angular::cross(-m.angular(), res.template segment<3>(ANGULAR));
587 bool isEqual(
const JointMotionSubspaceHelicalTpl &)
const
596 const Scalar & h()
const
605 template<
typename _Scalar,
int _Options,
int _axis>
606 Eigen::Matrix<_Scalar, 1, 1, _Options> operator*(
607 const typename JointMotionSubspaceHelicalTpl<_Scalar, _Options, _axis>::TransposeConst &
609 const JointMotionSubspaceHelicalTpl<_Scalar, _Options, _axis> & S)
611 Eigen::Matrix<_Scalar, 1, 1, _Options> res;
612 res(0) = 1.0 + S_transpose.ref.h() * S.h();
616 template<
typename _Scalar,
int _Options,
int _axis>
619 typedef _Scalar Scalar;
628 template<
typename S1,
int O1,
typename S2,
int O2,
int axis>
631 typedef Eigen::Matrix<S2, 6, 1, O2> ReturnType;
637 template<
typename S1,
int O1,
typename S2,
int O2>
646 const S2 & m_pitch = constraint.h();
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();
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;
660 template<
typename S1,
int O1,
typename S2,
int O2>
669 const S2 & m_pitch = constraint.h();
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();
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;
683 template<
typename S1,
int O1,
typename S2,
int O2>
692 const S2 & m_pitch = constraint.h();
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();
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);
707 template<
typename M6Like,
typename S2,
int O2,
int axis>
710 typedef Eigen::Matrix<S2, 6, 1> ReturnType;
716 template<
typename M6Like,
typename Scalar,
int Options,
int axis>
718 Eigen::MatrixBase<M6Like>,
722 typedef Eigen::Matrix<Scalar, 6, 1> ReturnType;
723 static inline ReturnType
724 run(
const Eigen::MatrixBase<M6Like> & Y,
const Constraint & constraint)
726 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6);
727 return (Y.col(Inertia::ANGULAR + axis) + Y.col(Inertia::LINEAR + axis) * constraint.h());
732 template<
typename _Scalar,
int _Options,
int axis>
741 typedef _Scalar Scalar;
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;
758 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
759 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
761 typedef boost::mpl::true_ is_mimicable_t;
763 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
766 template<
typename _Scalar,
int _Options,
int axis>
770 typedef _Scalar Scalar;
773 template<
typename _Scalar,
int _Options,
int axis>
777 typedef _Scalar Scalar;
780 template<
typename _Scalar,
int _Options,
int axis>
783 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
786 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
788 ConfigVector_t joint_q;
789 TangentVector_t joint_v;
803 :
joint_q(ConfigVector_t::Zero())
804 ,
joint_v(TangentVector_t::Zero())
806 , M((Scalar)0, (Scalar)1, (Scalar)0)
807 , v((Scalar)0, (Scalar)0)
810 , UDinv(UD_t::Zero())
815 static std::string classname()
817 return std::string(
"JointDataH") + axisLabel<axis>();
819 std::string shortname()
const
826 template<
typename NewScalar,
typename Scalar,
int Options,
int axis>
832 template<
typename _Scalar,
int _Options,
int axis>
835 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
843 using Base::idx_vExtended;
844 using Base::setIndexes;
846 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
848 JointDataDerived createData()
const
850 return JointDataDerived();
862 const std::vector<bool> hasConfigurationLimit()
const
867 const std::vector<bool> hasConfigurationLimitInTangent()
const
872 template<
typename ConfigVector>
873 PINOCCHIO_DONT_INLINE
void
874 calc(JointDataDerived & data,
const typename Eigen::MatrixBase<ConfigVector> & qs)
const
876 data.joint_q[0] = qs[idx_q()];
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;
883 template<
typename TangentVector>
884 PINOCCHIO_DONT_INLINE
void
885 calc(JointDataDerived & data,
const Blank,
const typename Eigen::MatrixBase<TangentVector> & vs)
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;
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
899 calc(data, qs.derived());
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;
906 template<
typename VectorLike,
typename Matrix6Like>
908 JointDataDerived & data,
909 const Eigen::MatrixBase<VectorLike> & armature,
910 const Eigen::MatrixBase<Matrix6Like> & I,
911 const bool update_I)
const
913 data.U = I.col(Inertia::ANGULAR + axis) + m_pitch * I.col(Inertia::LINEAR + axis);
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;
920 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
923 static std::string classname()
925 return std::string(
"JointModelH") + axisLabel<axis>();
927 std::string shortname()
const
932 Vector3 getMotionAxis()
const
937 return Vector3::UnitX();
939 return Vector3::UnitY();
941 return Vector3::UnitZ();
943 assert(
false &&
"must never happen");
949 template<
typename NewScalar>
954 res.setIndexes(
id(), idx_q(), idx_v(), idx_vExtended());
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;
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;
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;
974 template<
typename Scalar,
int Options,
int axis>
982 #include <boost/type_traits.hpp>
986 template<
typename Scalar,
int Options,
int axis>
988 :
public integral_constant<bool, true>
992 template<
typename Scalar,
int Options,
int axis>
994 :
public integral_constant<bool, true>
998 template<
typename Scalar,
int Options,
int axis>
1000 :
public integral_constant<bool, true>
1004 template<
typename Scalar,
int Options,
int axis>
1006 :
public integral_constant<bool, true>
ConstAngularType angular() const
Return the angular part of the force vector.
ConstLinearType linear() const
Return the linear part of the force vector.
Main pinocchio namespace.
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.
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar 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)
Return type of the ation of a Motion onto an object of type D.
Forward declaration of the multiplication operation return type. Should be overloaded,...
Base class for rigid transformation.
Cast scalar type from type FROM to type TO.
Common traits structure to fully define base classes for CRTP.