5 #ifndef __pinocchio_multibody_joint_universal_hpp__
6 #define __pinocchio_multibody_joint_universal_hpp__
8 #include "pinocchio/fwd.hpp"
9 #include "pinocchio/multibody/joint/joint-base.hpp"
10 #include "pinocchio/multibody/joint/joint-spherical.hpp"
11 #include "pinocchio/multibody/joint-motion-subspace.hpp"
12 #include "pinocchio/spatial/inertia.hpp"
13 #include "pinocchio/utils/check.hpp"
15 #include "pinocchio/math/matrix.hpp"
16 #include "pinocchio/math/rotation.hpp"
20 template<
typename Scalar,
int Options>
21 struct JointMotionSubspaceUniversalTpl;
23 template<
typename _Scalar,
int _Options>
26 typedef _Scalar Scalar;
38 typedef Eigen::Matrix<Scalar, 2, 1, Options> JointForce;
39 typedef Eigen::Matrix<Scalar, 6, 2, Options> DenseBase;
40 typedef Eigen::Matrix<Scalar, 3, 3, Options> ReducedSquaredMatrix;
42 typedef DenseBase MatrixReturnType;
43 typedef const DenseBase ConstMatrixReturnType;
45 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
46 typedef Eigen::Matrix<Scalar, 3, 2, Options> Matrix32;
47 typedef Eigen::Matrix<Scalar, 2, 2, Options> Matrix2;
49 typedef Matrix2 StDiagonalMatrixSOperationReturnType;
52 template<
typename Scalar,
int Options>
55 typedef Eigen::Matrix<Scalar, 6, 2, Options> ReturnType;
58 template<
typename Scalar,
int Options,
typename MotionDerived>
61 typedef Eigen::Matrix<Scalar, 6, 2, Options> ReturnType;
64 template<
typename Scalar,
int Options,
typename ForceDerived>
68 typedef Eigen::Matrix<
69 typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(
77 template<
typename Scalar,
int Options,
typename ForceSet>
82 Eigen::Transpose<const Matrix32>,
83 typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type>::type ReturnType;
86 template<
typename _Scalar,
int _Options>
90 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
105 template<
typename Matrix32Like>
111 template<
typename Vector3Like>
112 JointMotion __mult__(
const Eigen::MatrixBase<Vector3Like> & v)
const
115 return JointMotion(m_S * v);
118 template<
typename S1,
int O1>
119 typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType
122 typedef typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType ReturnType;
125 res.template middleRows<3>(ANGULAR).noalias() = m.rotation() * m_S;
127 m.translation(), res.template middleRows<3>(Motion::ANGULAR),
128 res.template middleRows<3>(LINEAR));
132 template<
typename S1,
int O1>
133 typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType
136 typedef typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType ReturnType;
139 cross(m.translation(), m_S, res.template middleRows<3>(ANGULAR));
140 res.template middleRows<3>(LINEAR).noalias() =
141 -m.rotation().transpose() * res.template middleRows<3>(ANGULAR);
144 res.template middleRows<3>(ANGULAR).noalias() = m.rotation().transpose() * m_S;
161 template<
typename ForceDerived>
162 typename ConstraintForceOp<JointMotionSubspaceUniversalTpl, ForceDerived>::ReturnType
165 return ref.m_S.transpose() * f.
angular();
169 template<
typename ForceSet>
170 typename ConstraintForceSetOp<JointMotionSubspaceUniversalTpl, ForceSet>::ReturnType
171 operator*(
const Eigen::MatrixBase<ForceSet> & F)
174 ForceSet::RowsAtCompileTime == 6, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
176 return ref.m_S.transpose() * F.template middleRows<3>(ANGULAR);
191 DenseBase matrix_impl()
const
194 S.template middleRows<3>(LINEAR).setZero();
195 S.template middleRows<3>(ANGULAR) = m_S;
199 template<
typename MotionDerived>
200 typename MotionAlgebraAction<JointMotionSubspaceUniversalTpl, MotionDerived>::ReturnType
201 motionAction(
const MotionDense<MotionDerived> & m)
const
203 const typename MotionDerived::ConstLinearType v = m.linear();
204 const typename MotionDerived::ConstAngularType w = m.angular();
207 cross(v, m_S, res.template middleRows<3>(LINEAR));
208 cross(w, m_S, res.template middleRows<3>(ANGULAR));
212 const Matrix32 & angularSubspace()
const
216 Matrix32 & angularSubspace()
221 bool isEqual(
const JointMotionSubspaceUniversalTpl & other)
const
223 return internal::comparison_eq(m_S, other.m_S);
233 template<
typename Scalar,
int Options>
241 return constraint.matrix().transpose() * constraint.matrix();
246 template<
typename S1,
int O1,
typename S2,
int O2>
249 typedef Eigen::Matrix<S2, 6, 2, O2> ReturnType;
255 template<
typename S1,
int O1,
typename S2,
int O2>
264 Eigen::Matrix<S1, 6, 3, O1> M;
265 alphaSkew(-Y.mass(), Y.lever(), M.template middleRows<3>(Constraint::LINEAR));
266 M.template middleRows<3>(Constraint::ANGULAR) =
267 (Y.inertia() -
typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix();
269 return (M * cru.angularSubspace()).eval();
274 template<
typename M6Like,
typename Scalar,
int Options>
276 Eigen::MatrixBase<M6Like>,
280 typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
283 typedef typename Constraint::Matrix32 Matrix32;
284 typedef const typename MatrixMatrixProduct<M6LikeColsNonConst, Matrix32>::type ReturnType;
290 template<
typename M6Like,
typename Scalar,
int Options>
292 Eigen::MatrixBase<M6Like>,
299 static inline ReturnType run(
const Eigen::MatrixBase<M6Like> & Y,
const Constraint & cru)
301 EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6);
302 return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.angularSubspace();
307 template<
typename Scalar,
int Options>
310 template<
typename _Scalar,
int _Options>
319 typedef _Scalar Scalar;
332 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
333 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
334 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
336 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
337 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
339 typedef boost::mpl::false_ is_mimicable_t;
341 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
344 template<
typename _Scalar,
int _Options>
348 typedef _Scalar Scalar;
351 template<
typename _Scalar,
int _Options>
355 typedef _Scalar Scalar;
358 template<
typename _Scalar,
int _Options>
361 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
364 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
366 ConfigVector_t joint_q;
367 TangentVector_t joint_v;
381 :
joint_q(ConfigVector_t::Zero())
382 ,
joint_v(TangentVector_t::Zero())
383 , M(Transformation_t::Identity())
384 , S(Constraint_t::Matrix32::Zero())
385 , v(Motion_t::Vector3::Zero())
386 , c(Bias_t::Vector3::Zero())
389 , UDinv(UD_t::Zero())
394 static std::string classname()
396 return std::string(
"JointDataUniversal");
398 std::string shortname()
const
406 template<
typename _Scalar,
int _Options>
409 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
412 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
413 typedef Eigen::Matrix<Scalar, 3, 3, _Options> Matrix3;
419 using Base::idx_vExtended;
420 using Base::setIndexes;
437 assert(
isUnitary(axis2) &&
"Second Rotation axis is not unitary");
438 assert(check_expression_if_real<Scalar>(
axis1.dot(axis2) == 0) &&
"Axii are not orthogonal");
441 template<
typename Vector3Like>
443 const Eigen::MatrixBase<Vector3Like> & axis1_,
const Eigen::MatrixBase<Vector3Like> & axis2_)
447 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
449 assert(
isUnitary(axis2) &&
"Second Rotation axis is not unitary");
451 check_expression_if_real<Scalar>(
axis1.dot(axis2) == Scalar(0))
452 &&
"Axii are not orthogonal");
455 JointDataDerived createData()
const
457 return JointDataDerived();
460 const std::vector<bool> hasConfigurationLimit()
const
465 const std::vector<bool> hasConfigurationLimitInTangent()
const
473 return Base::isEqual(other) && internal::comparison_eq(
axis1, other.
axis1)
474 && internal::comparison_eq(axis2, other.axis2);
477 template<
typename ConfigVector>
478 void calc(JointDataDerived & data,
const typename Eigen::MatrixBase<ConfigVector> & qs)
const
480 data.joint_q = qs.template segment<NQ>(idx_q());
482 SINCOS(data.joint_q(0), &s0, &c0);
484 SINCOS(data.joint_q(1), &s1, &c1);
489 data.M.rotation() = rot1 * rot2;
491 data.S.angularSubspace() << rot2.coeffRef(0, 0) *
axis1.x() + rot2.coeffRef(1, 0) *
axis1.y()
492 + rot2.coeffRef(2, 0) *
axis1.z(),
494 rot2.coeffRef(0, 1) *
axis1.x() + rot2.coeffRef(1, 1) *
axis1.y()
495 + rot2.coeffRef(2, 1) *
axis1.z(),
497 rot2.coeffRef(0, 2) *
axis1.x() + rot2.coeffRef(1, 2) *
axis1.y()
498 + rot2.coeffRef(2, 2) *
axis1.z(),
502 template<
typename TangentVector>
504 calc(JointDataDerived & data,
const Blank,
const typename Eigen::MatrixBase<TangentVector> & vs)
507 data.joint_v = vs.template segment<NV>(idx_v());
508 data.v().noalias() = data.S.angularSubspace() * data.joint_v;
525 template<
typename ConfigVector,
typename TangentVector>
527 JointDataDerived & data,
528 const typename Eigen::MatrixBase<ConfigVector> & qs,
529 const typename Eigen::MatrixBase<TangentVector> & vs)
const
531 data.joint_q = qs.template segment<NQ>(idx_q());
534 SINCOS(data.joint_q(0), &s0, &c0);
536 SINCOS(data.joint_q(1), &s1, &c1);
541 data.M.rotation() = rot1 * rot2;
543 data.S.angularSubspace() << rot2.coeffRef(0, 0) *
axis1.x() + rot2.coeffRef(1, 0) *
axis1.y()
544 + rot2.coeffRef(2, 0) *
axis1.z(),
546 rot2.coeffRef(0, 1) *
axis1.x() + rot2.coeffRef(1, 1) *
axis1.y()
547 + rot2.coeffRef(2, 1) *
axis1.z(),
549 rot2.coeffRef(0, 2) *
axis1.x() + rot2.coeffRef(1, 2) *
axis1.y()
550 + rot2.coeffRef(2, 2) *
axis1.z(),
553 data.joint_v = vs.template segment<NV>(idx_v());
554 data.v().noalias() = data.S.angularSubspace() * data.joint_v;
556 #define q_dot data.joint_v
558 tmp = (-s1 + axis2.x() * axis2.x() * s1) *
axis1.x()
559 + (axis2.x() * axis2.y() * s1 + axis2.z() * c1) *
axis1.y()
560 + (axis2.x() * axis2.z() * s1 - axis2.y() * c1) *
axis1.z();
561 data.c()(0) = tmp * q_dot(1) * q_dot(0);
562 tmp = (axis2.x() * axis2.y() * s1 - axis2.z() * c1) *
axis1.x()
563 + (-s1 + axis2.y() * axis2.y() * s1) *
axis1.y()
564 + (axis2.y() * axis2.z() * s1 + axis2.x() * c1) *
axis1.z();
565 data.c()(1) = tmp * q_dot(1) * q_dot(0);
566 tmp = (axis2.z() * axis2.x() * s1 + axis2.y() * c1) *
axis1.x()
567 + (axis2.y() * axis2.z() * s1 - axis2.x() * c1) *
axis1.y()
568 + (-s1 + axis2.z() * axis2.z() * s1) *
axis1.z();
569 data.c()(2) = tmp * q_dot(1) * q_dot(0);
573 template<
typename VectorLike,
typename Matrix6Like>
575 JointDataDerived & data,
576 const Eigen::MatrixBase<VectorLike> & armature,
577 const Eigen::MatrixBase<Matrix6Like> & I,
578 const bool update_I)
const
580 data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.angularSubspace();
582 data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Motion::ANGULAR);
583 data.StU.diagonal() += armature;
584 internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
586 data.UDinv.noalias() = data.U * data.Dinv;
589 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
592 static std::string classname()
594 return std::string(
"JointModelUniversal");
596 std::string shortname()
const
602 template<
typename NewScalar>
606 ReturnType res(
axis1.template cast<NewScalar>(), axis2.template cast<NewScalar>());
607 res.setIndexes(
id(), idx_q(), idx_v(), idx_vExtended());
619 #include <boost/type_traits.hpp>
623 template<
typename Scalar,
int Options>
625 :
public integral_constant<bool, true>
629 template<
typename Scalar,
int Options>
631 :
public integral_constant<bool, true>
635 template<
typename Scalar,
int Options>
637 :
public integral_constant<bool, true>
641 template<
typename Scalar,
int Options>
643 :
public integral_constant<bool, true>
ConstAngularType angular() const
Return the angular 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 SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
void alphaSkew(const Scalar alpha, const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector multiplied by a given scalar....
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.
void cross(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3xIn > &Min, const Eigen::MatrixBase< Matrix3xOut > &Mout)
Applies the cross product onto the columns of M.
Return type of the Constraint::Transpose * Force operation.
Return type of the Constraint::Transpose * ForceSet operation.
JointModelUniversalTpl< NewScalar, Options > cast() const
Vector3 axis1
3d main axii of the joint.
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,...
Common traits structure to fully define base classes for CRTP.