6 #ifndef __pinocchio_multibody_joint_revolute_hpp__
7 #define __pinocchio_multibody_joint_revolute_hpp__
9 #include "pinocchio/math/sincos.hpp"
10 #include "pinocchio/spatial/inertia.hpp"
11 #include "pinocchio/multibody/joint-motion-subspace.hpp"
12 #include "pinocchio/multibody/joint/joint-base.hpp"
13 #include "pinocchio/spatial/spatial-axis.hpp"
14 #include "pinocchio/utils/axis-label.hpp"
19 template<
typename Scalar,
int Options,
int axis>
20 struct MotionRevoluteTpl;
22 template<
typename Scalar,
int Options,
int axis>
28 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
34 template<
typename _Scalar,
int _Options,
int axis>
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;
53 typedef Matrix4 HomogeneousMatrixType;
63 template<
typename Scalar,
int Options,
int axis>
66 template<
typename _Scalar,
int _Options,
int _axis>
76 typedef _Scalar Scalar;
78 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
79 typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
80 typedef Matrix3 AngularType;
81 typedef Matrix3 AngularRef;
82 typedef Matrix3 ConstAngularRef;
83 typedef typename Vector3::ConstantReturnType LinearType;
84 typedef typename Vector3::ConstantReturnType LinearRef;
85 typedef const typename Vector3::ConstantReturnType ConstLinearRef;
90 template<
typename Scalar,
int Options,
int axis>
96 template<
typename _Scalar,
int _Options,
int axis>
99 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
111 PlainType plain()
const
113 PlainType res(PlainType::Identity());
114 _setRotation(res.rotation());
118 operator PlainType()
const
123 template<
typename S2,
int O2>
124 typename SE3GroupAction<TransformRevoluteTpl>::ReturnType
127 typedef typename SE3GroupAction<TransformRevoluteTpl>::ReturnType ReturnType;
132 res.rotation().col(0) = m.rotation().col(0);
133 res.rotation().col(1).noalias() = m_cos * m.rotation().col(1) + m_sin * m.rotation().col(2);
134 res.rotation().col(2).noalias() = res.rotation().col(0).cross(res.rotation().col(1));
138 res.rotation().col(2).noalias() = m_cos * m.rotation().col(2) + m_sin * m.rotation().col(0);
139 res.rotation().col(1) = m.rotation().col(1);
140 res.rotation().col(0).noalias() = res.rotation().col(1).cross(res.rotation().col(2));
144 res.rotation().col(0).noalias() = m_cos * m.rotation().col(0) + m_sin * m.rotation().col(1);
145 res.rotation().col(1).noalias() = res.rotation().col(2).cross(res.rotation().col(0));
146 res.rotation().col(2) = m.rotation().col(2);
150 assert(
false &&
"must never happened");
154 res.translation() = m.translation();
158 const Scalar & sin()
const
167 const Scalar & cos()
const
176 template<
typename OtherScalar>
177 void setValues(
const OtherScalar & sin,
const OtherScalar & cos)
183 LinearType translation()
const
185 return LinearType::PlainObject::Zero(3);
187 AngularType rotation()
const
189 AngularType m(AngularType::Identity());
196 return internal::comparison_eq(m_cos, other.m_cos)
197 && internal::comparison_eq(m_sin, other.m_sin);
202 inline void _setRotation(
typename PlainType::AngularRef & rot)
const
207 rot.coeffRef(1, 1) = m_cos;
208 rot.coeffRef(1, 2) = -m_sin;
209 rot.coeffRef(2, 1) = m_sin;
210 rot.coeffRef(2, 2) = m_cos;
214 rot.coeffRef(0, 0) = m_cos;
215 rot.coeffRef(0, 2) = m_sin;
216 rot.coeffRef(2, 0) = -m_sin;
217 rot.coeffRef(2, 2) = m_cos;
221 rot.coeffRef(0, 0) = m_cos;
222 rot.coeffRef(0, 1) = -m_sin;
223 rot.coeffRef(1, 0) = m_sin;
224 rot.coeffRef(1, 1) = m_cos;
228 assert(
false &&
"must never happened");
235 template<
typename _Scalar,
int _Options,
int axis>
238 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
253 template<
typename Vector1Like>
257 using namespace Eigen;
258 EIGEN_STATIC_ASSERT_SIZE_1x1(Vector1Like);
261 inline PlainReturnType plain()
const
266 template<
typename OtherScalar>
272 template<
typename MotionDerived>
275 m.linear().setZero();
276 for (Eigen::DenseIndex k = 0; k < 3; ++k)
278 m.angular()[k] = k == axis ? m_w : Scalar(0);
282 template<
typename MotionDerived>
285 typedef typename MotionDense<MotionDerived>::Scalar OtherScalar;
286 v.angular()[axis] += (OtherScalar)m_w;
289 template<
typename S2,
int O2,
typename D2>
292 v.angular().noalias() = m.rotation().col(axis) * m_w;
293 v.linear().noalias() = m.translation().cross(v.angular());
296 template<
typename S2,
int O2>
300 se3Action_impl(m, res);
304 template<
typename S2,
int O2,
typename D2>
308 CartesianAxis3::alphaCross(m_w, m.translation(), v.angular());
309 v.linear().noalias() = m.rotation().transpose() * v.angular();
312 v.angular().noalias() = m.rotation().transpose().col(axis) * m_w;
315 template<
typename S2,
int O2>
319 se3ActionInverse_impl(m, res);
323 template<
typename M1,
typename M2>
327 CartesianAxis3::alphaCross(-m_w, v.linear(), mout.linear());
330 CartesianAxis3::alphaCross(-m_w, v.angular(), mout.angular());
333 template<
typename M1>
337 motionAction(v, res);
341 Scalar & angularRate()
345 const Scalar & angularRate()
const
352 return internal::comparison_eq(m_w, other.m_w);
359 template<
typename S1,
int O1,
int axis,
typename MotionDerived>
360 typename MotionDerived::MotionPlain
363 typename MotionDerived::MotionPlain res(m2);
368 template<
typename MotionDerived,
typename S2,
int O2,
int axis>
369 EIGEN_STRONG_INLINE
typename MotionDerived::MotionPlain
370 operator^(
const MotionDense<MotionDerived> & m1,
const MotionRevoluteTpl<S2, O2, axis> & m2)
372 return m2.motionAction(m1);
375 template<
typename Scalar,
int Options,
int axis>
376 struct JointMotionSubspaceRevoluteTpl;
378 template<
typename Scalar,
int Options,
int axis>
381 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
384 template<
typename Scalar,
int Options,
int axis,
typename MotionDerived>
387 typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
390 template<
typename Scalar,
int Options,
int axis,
typename ForceDerived>
394 ForceDerived>::ConstAngularType::template ConstFixedSegmentReturnType<1>::Type ReturnType;
397 template<
typename Scalar,
int Options,
int axis,
typename ForceSet>
400 typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType;
403 template<
typename _Scalar,
int _Options,
int axis>
406 typedef _Scalar Scalar;
418 typedef Eigen::Matrix<Scalar, 1, 1, Options> JointForce;
419 typedef Eigen::Matrix<Scalar, 6, 1, Options> DenseBase;
420 typedef Eigen::Matrix<Scalar, 1, 1, Options> ReducedSquaredMatrix;
422 typedef DenseBase MatrixReturnType;
423 typedef const DenseBase ConstMatrixReturnType;
425 typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType;
428 template<
typename _Scalar,
int _Options,
int axis>
432 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
446 template<
typename Vector1Like>
447 JointMotion __mult__(
const Eigen::MatrixBase<Vector1Like> & v)
const
449 return JointMotion(v[0]);
452 template<
typename S1,
int O1>
453 typename SE3GroupAction<JointMotionSubspaceRevoluteTpl>::ReturnType
456 typedef typename SE3GroupAction<JointMotionSubspaceRevoluteTpl>::ReturnType ReturnType;
458 res.template segment<3>(LINEAR) = m.translation().cross(m.rotation().col(axis));
459 res.template segment<3>(ANGULAR) = m.rotation().col(axis);
463 template<
typename S1,
int O1>
464 typename SE3GroupAction<JointMotionSubspaceRevoluteTpl>::ReturnType
467 typedef typename SE3GroupAction<JointMotionSubspaceRevoluteTpl>::ReturnType ReturnType;
470 res.template segment<3>(LINEAR).noalias() =
471 m.rotation().transpose() * CartesianAxis3::cross(m.translation());
472 res.template segment<3>(ANGULAR) = m.rotation().transpose().col(axis);
489 template<
typename ForceDerived>
490 typename ConstraintForceOp<JointMotionSubspaceRevoluteTpl, ForceDerived>::ReturnType
493 return f.
angular().template segment<1>(axis);
497 template<
typename Derived>
498 typename ConstraintForceSetOp<JointMotionSubspaceRevoluteTpl, Derived>::ReturnType
501 assert(F.rows() == 6);
502 return F.row(ANGULAR + axis);
506 TransposeConst transpose()
const
508 return TransposeConst(*
this);
517 DenseBase matrix_impl()
const
520 MotionRef<DenseBase> v(S);
525 template<
typename MotionDerived>
526 typename MotionAlgebraAction<JointMotionSubspaceRevoluteTpl, MotionDerived>::ReturnType
527 motionAction(
const MotionDense<MotionDerived> & m)
const
530 typename MotionAlgebraAction<JointMotionSubspaceRevoluteTpl, MotionDerived>::ReturnType
533 MotionRef<ReturnType> v(res);
538 bool isEqual(
const JointMotionSubspaceRevoluteTpl &)
const
545 template<
typename _Scalar,
int _Options,
int _axis>
548 typedef _Scalar Scalar;
557 template<
typename S1,
int O1,
typename S2,
int O2,
int axis>
560 typedef Eigen::Matrix<S2, 6, 1, O2> ReturnType;
566 template<
typename S1,
int O1,
typename S2,
int O2>
577 const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2];
578 const typename Inertia::Symmetric3 & I = Y.inertia();
580 res << (S2)0, -m * z, m * y, I(0, 0) + m * (y * y + z * z), I(0, 1) - m * x * y,
587 template<
typename S1,
int O1,
typename S2,
int O2>
598 const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2];
599 const typename Inertia::Symmetric3 & I = Y.inertia();
601 res << m * z, (S2)0, -m * x, I(1, 0) - m * x * y, I(1, 1) + m * (x * x + z * z),
608 template<
typename S1,
int O1,
typename S2,
int O2>
619 const S1 &m = Y.mass(), &x = Y.lever()[0], &y = Y.lever()[1], &z = Y.lever()[2];
620 const typename Inertia::Symmetric3 & I = Y.inertia();
622 res << -m * y, m * x, (S2)0, I(2, 0) - m * x * z, I(2, 1) - m * y * z,
623 I(2, 2) + m * (x * x + y * y);
630 template<
typename M6Like,
typename S2,
int O2,
int axis>
633 typedef typename M6Like::ConstColXpr ReturnType;
639 template<
typename M6Like,
typename Scalar,
int Options,
int axis>
641 Eigen::MatrixBase<M6Like>,
647 static inline ReturnType
648 run(
const Eigen::MatrixBase<M6Like> & Y,
const Constraint & )
650 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6);
651 return Y.col(Inertia::ANGULAR + axis);
656 template<
typename _Scalar,
int _Options,
int axis>
665 typedef _Scalar Scalar;
678 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
679 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
680 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
682 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
683 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
685 typedef boost::mpl::true_ is_mimicable_t;
687 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
690 template<
typename _Scalar,
int _Options,
int axis>
694 typedef _Scalar Scalar;
697 template<
typename _Scalar,
int _Options,
int axis>
701 typedef _Scalar Scalar;
704 template<
typename _Scalar,
int _Options,
int axis>
707 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
710 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
727 :
joint_q(ConfigVector_t::Zero())
728 ,
joint_v(TangentVector_t::Zero())
729 , M((Scalar)0, (Scalar)1)
733 , UDinv(UD_t::Zero())
738 static std::string classname()
740 return std::string(
"JointDataR") + axisLabel<axis>();
749 template<
typename NewScalar,
typename Scalar,
int Options,
int axis>
755 template<
typename _Scalar,
int _Options,
int axis>
757 :
public JointModelBase<JointModelRevoluteTpl<_Scalar, _Options, axis>>
759 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
767 using Base::idx_vExtended;
768 using Base::setIndexes;
770 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
772 JointDataDerived createData()
const
774 return JointDataDerived();
791 template<
typename ConfigVector>
792 PINOCCHIO_DONT_INLINE
void
793 calc(JointDataDerived & data,
const typename Eigen::MatrixBase<ConfigVector> & qs)
const
795 data.joint_q[0] = qs[
idx_q()];
797 SINCOS(data.joint_q[0], &sa, &ca);
798 data.M.setValues(sa, ca);
801 template<
typename TangentVector>
802 PINOCCHIO_DONT_INLINE
void
803 calc(JointDataDerived & data,
const Blank,
const typename Eigen::MatrixBase<TangentVector> & vs)
806 data.joint_v[0] = vs[
idx_v()];
807 data.v.angularRate() = data.joint_v[0];
810 template<
typename ConfigVector,
typename TangentVector>
811 PINOCCHIO_DONT_INLINE
void calc(
812 JointDataDerived & data,
813 const typename Eigen::MatrixBase<ConfigVector> & qs,
814 const typename Eigen::MatrixBase<TangentVector> & vs)
const
816 calc(data, qs.derived());
818 data.joint_v[0] = vs[
idx_v()];
819 data.v.angularRate() = data.joint_v[0];
822 template<
typename VectorLike,
typename Matrix6Like>
824 JointDataDerived & data,
825 const Eigen::MatrixBase<VectorLike> & armature,
826 const Eigen::MatrixBase<Matrix6Like> & I,
827 const bool update_I)
const
829 data.U = I.col(Inertia::ANGULAR + axis);
831 Scalar(1) / (I(Inertia::ANGULAR + axis, Inertia::ANGULAR + axis) + armature[0]);
832 data.UDinv.noalias() = data.U * data.Dinv[0];
835 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
838 static std::string classname()
840 return std::string(
"JointModelR") + axisLabel<axis>();
847 Vector3 getMotionAxis()
const
852 return Vector3::UnitX();
854 return Vector3::UnitY();
856 return Vector3::UnitZ();
858 assert(
false &&
"must never happen");
864 template<
typename NewScalar>
875 typedef JointRevoluteTpl<context::Scalar, context::Options, 0> JointRX;
876 typedef JointDataRevoluteTpl<context::Scalar, context::Options, 0> JointDataRX;
877 typedef JointModelRevoluteTpl<context::Scalar, context::Options, 0> JointModelRX;
879 typedef JointRevoluteTpl<context::Scalar, context::Options, 1> JointRY;
880 typedef JointDataRevoluteTpl<context::Scalar, context::Options, 1> JointDataRY;
881 typedef JointModelRevoluteTpl<context::Scalar, context::Options, 1> JointModelRY;
883 typedef JointRevoluteTpl<context::Scalar, context::Options, 2> JointRZ;
884 typedef JointDataRevoluteTpl<context::Scalar, context::Options, 2> JointDataRZ;
885 typedef JointModelRevoluteTpl<context::Scalar, context::Options, 2> JointModelRZ;
887 template<
typename Scalar,
int Options,
int axis>
894 #include <boost/type_traits.hpp>
898 template<
typename Scalar,
int Options,
int axis>
900 :
public integral_constant<bool, true>
904 template<
typename Scalar,
int Options,
int axis>
906 :
public integral_constant<bool, true>
910 template<
typename Scalar,
int Options,
int axis>
912 :
public integral_constant<bool, true>
916 template<
typename Scalar,
int Options,
int axis>
918 :
public integral_constant<bool, true>
ConstAngularType angular() const
Return the angular part of the force vector.
Main pinocchio namespace.
void calc_aba(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< VectorLike > &armature, const Eigen::MatrixBase< Matrix6Type > &I, const bool update_I)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcAbaVisitor to.
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.
int idx_vExtended(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdvExtendedVisitor to get the index in the model extended tangent ...
int idx_v(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxVVisitor to get the index in the model tangent space correspond...
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.
const std::vector< bool > hasConfigurationLimit(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointConfigurationLimitVisitor to get the configurations limits.
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
const std::vector< bool > hasConfigurationLimitInTangent(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointConfigurationLimitInTangentVisitor to get the configurations limit...
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model.
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
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.
JointModelRevoluteTpl< NewScalar, Options, axis > cast() const
ConstraintForceSetOp< JointMotionSubspaceRevoluteTpl, 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.
Common traits structure to fully define base classes for CRTP.