5 #ifndef __pinocchio_multibody_joint_helical_unaligned_hpp__
6 #define __pinocchio_multibody_joint_helical_unaligned_hpp__
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"
13 #include "pinocchio/math/matrix.hpp"
14 #include "pinocchio/math/rotation.hpp"
19 template<
typename Scalar,
int Options>
20 struct MotionHelicalUnalignedTpl;
22 template<
typename Scalar,
int Options>
28 template<
typename Scalar,
int Options,
typename MotionDerived>
34 template<
typename _Scalar,
int _Options>
37 typedef _Scalar Scalar;
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;
55 typedef Matrix4 HomogeneousMatrixType;
63 template<
typename _Scalar,
int _Options>
66 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
74 template<
typename Vector3Like,
typename OtherScalar>
76 const Eigen::MatrixBase<Vector3Like> & axis,
const OtherScalar & w,
const OtherScalar & v)
83 inline PlainReturnType plain()
const
85 return PlainReturnType(m_axis * m_v, m_axis * m_w);
88 template<
typename OtherScalar>
94 template<
typename MotionDerived>
97 for (Eigen::DenseIndex k = 0; k < 3; ++k)
99 m.angular().noalias() = m_axis * m_w;
100 m.linear().noalias() = m_axis * m_v;
104 template<
typename MotionDerived>
107 v.angular() += m_axis * m_w;
108 v.linear() += m_axis * m_v;
111 template<
typename S2,
int O2,
typename D2>
114 v.angular().noalias() = m_w * m.rotation() * m_axis;
115 v.linear().noalias() = m.translation().cross(v.angular()) + m_v * m.rotation() * m_axis;
118 template<
typename S2,
int O2>
122 se3Action_impl(m, res);
126 template<
typename S2,
int O2,
typename D2>
130 v.angular().noalias() = m_axis.cross(m.translation());
132 v.linear().noalias() =
133 m.rotation().transpose() * v.angular() + m_v * (m.rotation().transpose() * m_axis);
136 v.angular().noalias() = m.rotation().transpose() * m_axis * m_w;
139 template<
typename S2,
int O2>
143 se3ActionInverse_impl(m, res);
147 template<
typename M1,
typename M2>
151 mout.linear().noalias() = v.linear().cross(m_axis);
152 mout.linear() *= m_w;
153 mout.angular().noalias() = v.angular().cross(m_axis);
154 mout.angular() *= m_v;
155 mout.linear() += mout.angular();
158 mout.angular().noalias() = v.angular().cross(m_axis);
159 mout.angular() *= m_w;
162 template<
typename M1>
166 motionAction(v, res);
170 Scalar & angularRate()
174 const Scalar & angularRate()
const
179 Scalar & linearRate()
183 const Scalar & linearRate()
const
192 const Vector3 & axis()
const
199 return internal::comparison_eq(m_axis, other.m_axis)
200 && internal::comparison_eq(m_w, other.m_w) && internal::comparison_eq(m_v, other.m_v);
209 template<
typename S1,
int O1,
typename MotionDerived>
210 typename MotionDerived::MotionPlain
213 typename MotionDerived::MotionPlain res(m2);
218 template<
typename MotionDerived,
typename S2,
int O2>
219 EIGEN_STRONG_INLINE
typename MotionDerived::MotionPlain
220 operator^(
const MotionDense<MotionDerived> & m1,
const MotionHelicalUnalignedTpl<S2, O2> & m2)
222 return m2.motionAction(m1);
225 template<
typename Scalar,
int Options>
226 struct JointMotionSubspaceHelicalUnalignedTpl;
228 template<
typename Scalar,
int Options>
231 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
234 template<
typename Scalar,
int Options,
typename MotionDerived>
237 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
240 template<
typename Scalar,
int Options,
typename ForceDerived>
243 typedef typename Eigen::Matrix<Scalar, 1, 1> ReturnType;
246 template<
typename Scalar,
int Options,
typename ForceSet>
249 typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> ReturnType;
252 template<
typename _Scalar,
int _Options>
255 typedef _Scalar Scalar;
267 typedef Eigen::Matrix<Scalar, 1, 1, Options> JointForce;
268 typedef Eigen::Matrix<Scalar, 6, 1, Options> DenseBase;
269 typedef Eigen::Matrix<Scalar, 1, 1, Options> ReducedSquaredMatrix;
271 typedef DenseBase MatrixReturnType;
272 typedef const DenseBase ConstMatrixReturnType;
274 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
276 typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType;
279 template<
typename _Scalar,
int _Options>
283 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
297 template<
typename Vector3Like>
299 const Eigen::MatrixBase<Vector3Like> & axis,
const Scalar & h)
305 template<
typename Vector1Like>
306 JointMotion __mult__(
const Eigen::MatrixBase<Vector1Like> & v)
const
308 EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like, 1);
309 assert(v.size() == 1);
310 return JointMotion(m_axis, v[0], Scalar(v[0] * m_pitch));
313 template<
typename S1,
int O1>
314 typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType
318 typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType ReturnType;
320 res.template segment<3>(ANGULAR).noalias() = m.rotation() * m_axis;
321 res.template segment<3>(LINEAR).noalias() =
322 m.translation().cross(res.template segment<3>(ANGULAR)) + m_pitch * (m.rotation() * m_axis);
326 template<
typename S1,
int O1>
327 typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType
331 typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType ReturnType;
334 res.template segment<3>(ANGULAR).noalias() = m.rotation().transpose() * m_axis;
335 res.template segment<3>(LINEAR).noalias() =
336 -m.rotation().transpose() * m.translation().cross(m_axis)
337 + m.rotation().transpose() * m_axis * m_pitch;
355 template<
typename ForceDerived>
356 typename ConstraintForceOp<JointMotionSubspaceHelicalUnalignedTpl, ForceDerived>::ReturnType
362 res[0] = ref.axis().dot(f.
angular()) + ref.axis().dot(f.
linear()) * ref.m_pitch;
367 template<
typename Derived>
368 typename ConstraintForceSetOp<JointMotionSubspaceHelicalUnalignedTpl, Derived>::ReturnType
371 assert(F.rows() == 6);
373 ref.axis().transpose() * F.template middleRows<3>(ANGULAR)
374 + (ref.axis().transpose() * F.template middleRows<3>(LINEAR) * ref.m_pitch));
378 TransposeConst transpose()
const
380 return TransposeConst(*
this);
389 DenseBase matrix_impl()
const
392 S.template segment<3>(LINEAR) = m_axis * m_pitch;
393 S.template segment<3>(ANGULAR) = m_axis;
397 template<
typename MotionDerived>
398 typename MotionAlgebraAction<JointMotionSubspaceHelicalUnalignedTpl, MotionDerived>::ReturnType
399 motionAction(
const MotionDense<MotionDerived> & m)
const
401 const typename MotionDerived::ConstLinearType v = m.linear();
402 const typename MotionDerived::ConstAngularType w = m.angular();
405 res.template segment<3>(LINEAR).noalias() = v.cross(m_axis);
406 res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis * m_pitch);
407 res.template segment<3>(LINEAR).noalias() += res.template segment<3>(ANGULAR);
408 res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis);
413 bool isEqual(
const JointMotionSubspaceHelicalUnalignedTpl & other)
const
415 return internal::comparison_eq(m_axis, other.m_axis)
416 && internal::comparison_eq(m_pitch, other.m_pitch);
423 const Vector3 & axis()
const
432 const Scalar & h()
const
442 template<
typename _Scalar,
int _Options>
443 Eigen::Matrix<_Scalar, 1, 1, _Options> operator*(
444 const typename JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options>::TransposeConst &
446 const JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options> & S)
448 Eigen::Matrix<_Scalar, 1, 1, _Options> res;
449 res(0) = (S_transpose.ref.axis() * S_transpose.ref.h()).dot(S.axis() * S.h())
450 + (S_transpose.ref.axis().dot(S.axis()));
454 template<
typename S1,
int O1,
typename S2,
int O2>
457 typedef Eigen::Matrix<S2, 6, 1, O2> ReturnType;
463 template<
typename S1,
int O1,
typename S2,
int O2>
468 typedef typename JointMotionSubspaceHelicalUnalignedTpl<S2, O2>::Vector3 Vector3;
475 const S2 & m_pitch = cru.h();
476 const typename Inertia::Scalar & m = Y.mass();
477 const typename Inertia::Vector3 & c = Y.lever();
478 const typename Inertia::Symmetric3 & I = Y.inertia();
480 res.template segment<3>(Inertia::LINEAR) = -m * c.cross(cru.axis());
481 res.template segment<3>(Inertia::ANGULAR).noalias() = I * cru.axis();
482 res.template segment<3>(Inertia::ANGULAR) += c.cross(cru.axis()) * m * m_pitch;
483 res.template segment<3>(Inertia::ANGULAR) +=
484 c.cross(res.template segment<3>(Inertia::LINEAR));
485 res.template segment<3>(Inertia::LINEAR) += m * m_pitch * cru.axis();
491 template<
typename M6Like,
typename Scalar,
int Options>
493 Eigen::MatrixBase<M6Like>,
496 typedef const Eigen::Matrix<Scalar, 6, 1> ReturnType;
502 template<
typename M6Like,
typename Scalar,
int Options>
504 Eigen::MatrixBase<M6Like>,
508 typedef Eigen::Matrix<Scalar, 6, 1> ReturnType;
509 static inline ReturnType run(
const Eigen::MatrixBase<M6Like> & Y,
const Constraint & cru)
511 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6);
512 return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis()
513 + Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis() * cru.h();
518 template<
typename Scalar,
int Options>
521 template<
typename _Scalar,
int _Options>
530 typedef _Scalar Scalar;
543 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
544 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
545 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
547 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
548 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
550 typedef boost::mpl::true_ is_mimicable_t;
552 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
555 template<
typename _Scalar,
int _Options>
559 typedef _Scalar Scalar;
562 template<
typename _Scalar,
int _Options>
566 typedef _Scalar Scalar;
569 template<
typename _Scalar,
int _Options>
571 :
public JointDataBase<JointDataHelicalUnalignedTpl<_Scalar, _Options>>
573 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
576 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
578 ConfigVector_t joint_q;
579 TangentVector_t joint_v;
593 :
joint_q(ConfigVector_t::Zero())
594 ,
joint_v(TangentVector_t::Zero())
595 , S(Constraint_t::Vector3::Zero(), (Scalar)0)
596 , M(Transformation_t::Identity())
597 , v(Constraint_t::Vector3::Zero(), (Scalar)0, (Scalar)0)
600 , UDinv(UD_t::Zero())
605 template<
typename Vector3Like>
607 :
joint_q(ConfigVector_t::Zero())
608 ,
joint_v(TangentVector_t::Zero())
610 , M(Transformation_t::Identity())
611 , v(axis, (Scalar)0, (Scalar)0)
614 , UDinv(UD_t::Zero())
619 static std::string classname()
621 return std::string(
"JointDataHelicalUnaligned");
623 std::string shortname()
const
631 template<
typename _Scalar,
int _Options>
633 :
public JointModelBase<JointModelHelicalUnalignedTpl<_Scalar, _Options>>
635 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
638 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
644 using Base::idx_vExtended;
645 using Base::setIndexes;
651 template<
typename Vector3Like>
656 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
657 assert(
isUnitary(axis) &&
"Rotation axis is not unitary");
661 const Scalar & x,
const Scalar & y,
const Scalar & z,
const Scalar & h)
666 assert(
isUnitary(axis) &&
"Rotation axis is not unitary");
669 template<
typename Vector3Like>
674 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
675 assert(
isUnitary(axis) &&
"Rotation axis is not unitary");
678 const std::vector<bool> hasConfigurationLimit()
const
683 const std::vector<bool> hasConfigurationLimitInTangent()
const
688 JointDataDerived createData()
const
690 return JointDataDerived();
696 return Base::isEqual(other) && internal::comparison_eq(axis, other.axis)
697 && internal::comparison_eq(m_pitch, other.m_pitch);
700 template<
typename ConfigVector>
701 void calc(JointDataDerived & data,
const typename Eigen::MatrixBase<ConfigVector> & qs)
const
703 data.joint_q[0] = qs[idx_q()];
706 data.M.translation() = axis * data.joint_q[0] * m_pitch;
707 data.S.h() = m_pitch;
708 data.S.axis() = axis;
711 template<
typename TangentVector>
713 calc(JointDataDerived & data,
const Blank,
const typename Eigen::MatrixBase<TangentVector> & vs)
716 data.v.angularRate() =
static_cast<Scalar
>(vs[idx_v()]);
717 data.v.axis() = axis;
718 data.v.linearRate() =
static_cast<Scalar
>(vs[idx_v()] * m_pitch);
721 template<
typename ConfigVector,
typename TangentVector>
723 JointDataDerived & data,
724 const typename Eigen::MatrixBase<ConfigVector> & qs,
725 const typename Eigen::MatrixBase<TangentVector> & vs)
const
727 calc(data, qs.derived());
728 data.v.angularRate() =
static_cast<Scalar
>(vs[idx_v()]);
729 data.v.axis() = axis;
730 data.v.linearRate() =
static_cast<Scalar
>(vs[idx_v()] * m_pitch);
733 template<
typename VectorLike,
typename Matrix6Like>
735 JointDataDerived & data,
736 const Eigen::MatrixBase<VectorLike> & armature,
737 const Eigen::MatrixBase<Matrix6Like> & I,
738 const bool update_I)
const
740 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis
741 + m_pitch * I.template middleCols<3>(Motion::LINEAR) * axis;
742 data.StU[0] = axis.dot(data.U.template segment<3>(Motion::ANGULAR))
743 + m_pitch * axis.dot(data.U.template segment<3>(Motion::LINEAR)) + armature[0];
744 data.Dinv[0] = Scalar(1) / data.StU[0];
745 data.UDinv.noalias() = data.U * data.Dinv;
747 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
750 static std::string classname()
752 return std::string(
"JointModelHelicalUnaligned");
754 std::string shortname()
const
760 template<
typename NewScalar>
765 res.setIndexes(
id(), idx_q(), idx_v(), idx_vExtended());
776 #include <boost/type_traits.hpp>
780 template<
typename Scalar,
int Options>
782 :
public integral_constant<bool, true>
786 template<
typename Scalar,
int Options>
788 :
public integral_constant<bool, true>
792 template<
typename Scalar,
int Options>
794 :
public integral_constant<bool, true>
798 template<
typename Scalar,
int Options>
800 :
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.
bool isUnitary(const Eigen::MatrixBase< MatrixLike > &mat, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Check whether the input matrix is Unitary within the given precision.
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 normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
void toRotationMatrix(const Eigen::MatrixBase< Vector3 > &axis, const Scalar &cos_value, const Scalar &sin_value, const Eigen::MatrixBase< Matrix3 > &res)
Computes a rotation matrix from a vector and values of sin and cos orientations values.
Return type of the Constraint::Transpose * Force operation.
Return type of the Constraint::Transpose * ForceSet operation.
JointModelHelicalUnalignedTpl< NewScalar, Options > cast() const
ConstraintForceSetOp< JointMotionSubspaceHelicalUnalignedTpl, 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,...
Cast scalar type from type FROM to type TO.
Common traits structure to fully define base classes for CRTP.