5 #ifndef __pinocchio_math_quaternion_hpp__ 6 #define __pinocchio_math_quaternion_hpp__ 8 #ifndef PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE 9 #define PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE 1e-8 12 #include "pinocchio/math/fwd.hpp" 13 #include "pinocchio/math/comparison-operators.hpp" 14 #include "pinocchio/math/matrix.hpp" 15 #include "pinocchio/math/sincos.hpp" 16 #include "pinocchio/utils/static-if.hpp" 18 #include <boost/type_traits.hpp> 19 #include <Eigen/Geometry> 33 template<
typename D1,
typename D2>
36 const Eigen::QuaternionBase<D2> & q2)
39 const Scalar innerprod = q1.dot(q2);
40 Scalar theta = math::acos(innerprod);
41 static const Scalar PI_value = PI<Scalar>();
58 template<
typename D1,
typename D2>
60 const Eigen::QuaternionBase<D2> & q2,
61 const typename D1::RealScalar & prec = Eigen::NumTraits<typename D1::Scalar>::dummy_precision())
63 return (q1.coeffs().isApprox(q2.coeffs(), prec) || q1.coeffs().isApprox(-q2.coeffs(), prec) );
91 const Scalar N2 = q.squaredNorm();
93 const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
95 assert(static_leq::op(math::fabs(static_cast<Scalar>(N2-
Scalar(1))), epsilon));
101 assert(static_leq::op(math::fabs(static_cast<Scalar>(q.norm() -
Scalar(1))),
107 template<
typename Derived>
117 const Scalar mult1 = sqrt(
Scalar(1)-u1);
118 const Scalar mult2 = sqrt(u1);
120 static const Scalar PI_value = PI<Scalar>();
133 template<typename Scalar, bool value = is_floating_point<Scalar>::value>
136 template<Eigen::DenseIndex i>
139 template<
typename Scalar,
typename Matrix3,
typename QuaternionDerived>
141 Eigen::QuaternionBase<QuaternionDerived> &
q,
144 using pinocchio::math::sqrt;
146 Eigen::DenseIndex j = (
i+1)%3;
147 Eigen::DenseIndex k = (j+1)%3;
149 t = sqrt(mat.coeff(
i,
i)-mat.coeff(j,j)-mat.coeff(k,k) +
Scalar(1.0));
150 q.coeffs().coeffRef(
i) =
Scalar(0.5) *
t;
152 q.w() = (mat.coeff(k,j)-mat.coeff(j,k))*t;
153 q.coeffs().coeffRef(j) = (mat.coeff(j,
i)+mat.coeff(
i,j))*t;
154 q.coeffs().coeffRef(k) = (mat.coeff(k,
i)+mat.coeff(
i,k))*t;
160 template<
typename Scalar,
typename Matrix3,
typename QuaternionDerived>
162 Eigen::QuaternionBase<QuaternionDerived> &
q,
165 using pinocchio::math::sqrt;
167 t = sqrt(t +
Scalar(1.0));
170 q.x() = (mat.coeff(2,1) - mat.coeff(1,2)) * t;
171 q.y() = (mat.coeff(0,2) - mat.coeff(2,0)) * t;
172 q.z() = (mat.coeff(1,0) - mat.coeff(0,1)) * t;
176 template<
typename Scalar>
179 template<
typename Matrix3,
typename QuaternionDerived>
180 static inline void run(Eigen::QuaternionBase<QuaternionDerived> &
q,
183 using pinocchio::math::sqrt;
190 Eigen::DenseIndex
i = 0;
191 if (mat.coeff(1,1) > mat.coeff(0,0))
193 if (mat.coeff(2,2) > mat.coeff(i,i))
208 template<
typename D,
typename Matrix3>
210 const Eigen::MatrixBase<Matrix3> & R)
224 template<
typename Quaternion>
225 inline bool isNormalized(
const Eigen::QuaternionBase<Quaternion> & quat,
226 const typename Quaternion::Coefficients::RealScalar & prec =
227 Eigen::NumTraits< typename Quaternion::Coefficients::RealScalar >::dummy_precision())
235 #endif //#ifndef __pinocchio_math_quaternion_hpp__ bool defineSameRotation(const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2, const typename D1::RealScalar &prec=Eigen::NumTraits< typename D1::Scalar >::dummy_precision())
Check if two quaternions define the same rotations.
D1::Scalar angleBetweenQuaternions(const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2)
Compute the minimal angle between q1 and q2.
AD< Scalar > max(const AD< Scalar > &x, const AD< Scalar > &y)
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
bool isNormalized(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
Check whether a configuration vector is normalized within the given precision provided by prec...
static void run(Scalar t, Eigen::QuaternionBase< QuaternionDerived > &q, const Matrix3 &mat)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
void uniformRandom(const Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Main pinocchio namespace.
void assignQuaternion(Eigen::QuaternionBase< D > &quat, const Eigen::MatrixBase< Matrix3 > &R)
static void run(Eigen::QuaternionBase< QuaternionDerived > &q, const Matrix3 &mat)
bool isNormalized(const Eigen::QuaternionBase< Quaternion > &quat, const typename Quaternion::Coefficients::RealScalar &prec=Eigen::NumTraits< typename Quaternion::Coefficients::RealScalar >::dummy_precision())
Check whether the input quaternion is Normalized within the given precision.
if_then_else_impl< LhsType, RhsType, ThenType, ElseType >::ReturnType if_then_else(const ComparisonOperators op, const LhsType &lhs_value, const RhsType &rhs_value, const ThenType &then_value, const ElseType &else_value)
static void run(Scalar t, Eigen::QuaternionBase< QuaternionDerived > &q, const Matrix3 &mat)