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;
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),
132 template<
typename S1,
int O1>
133 typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType
136 typedef typename SE3GroupAction<JointMotionSubspaceUniversalTpl>::ReturnType ReturnType;
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>
165 return ref.m_S.transpose() * f.angular();
169 template<
typename ForceSet>
171 operator*(
const Eigen::MatrixBase<ForceSet> &
F)
191 DenseBase matrix_impl()
const
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);
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
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
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;
441 template<
typename Vector3Like>
443 const Eigen::MatrixBase<Vector3Like> &
axis1_,
const Eigen::MatrixBase<Vector3Like> &
axis2_)
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
491 data.S.angularSubspace() <<
rot2.coeffRef(0, 0) *
axis1.x() +
rot2.coeffRef(1, 0) *
axis1.y()
502 template<
typename TangentVector>
504 calc(JointDataDerived & data,
const Blank,
const typename Eigen::MatrixBase<TangentVector> &
vs)
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
543 data.S.angularSubspace() <<
rot2.coeffRef(0, 0) *
axis1.x() +
rot2.coeffRef(1, 0) *
axis1.y()
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,
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>
607 res.setIndexes(
id(), idx_q(), idx_v(), idx_vExtended());
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.
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.
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.
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....
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.
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.