Go to the documentation of this file.
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
18 #include <boost/type_traits.hpp>
19 #include <Eigen/Geometry>
33 template<
typename D1,
typename D2>
35 const Eigen::QuaternionBase<D1> &
q1,
const Eigen::QuaternionBase<D2> & q2)
39 Scalar theta = math::acos(innerprod);
40 static const Scalar PI_value = PI<Scalar>();
56 template<
typename D1,
typename D2>
58 const Eigen::QuaternionBase<D1> &
q1,
59 const Eigen::QuaternionBase<D2> & q2,
60 const typename D1::RealScalar & prec =
61 Eigen::NumTraits<typename D1::Scalar>::dummy_precision())
63 return (
q1.coeffs().isApprox(
q2.coeffs(), prec) ||
q1.coeffs().isApprox(-
q2.coeffs(), prec));
92 const Scalar N2 =
q.squaredNorm();
94 const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
97 assert(static_leq::op(math::fabs(
static_cast<Scalar>(N2 -
Scalar(1))), epsilon));
104 assert(static_leq::op(
108 Eigen::NumTraits<Scalar>::dummy_precision())));
113 template<
typename Derived>
124 const Scalar mult2 = sqrt(u1);
126 static const Scalar PI_value = PI<Scalar>();
141 template<typename Scalar, bool value = is_floating_point<Scalar>::value>
144 template<Eigen::DenseIndex i>
147 template<
typename Scalar,
typename Matrix3,
typename QuaternionDerived>
149 run(
Scalar t, Eigen::QuaternionBase<QuaternionDerived> &
q,
const Matrix3 & mat)
151 using pinocchio::math::sqrt;
153 Eigen::DenseIndex j = (
i + 1) % 3;
154 Eigen::DenseIndex k = (j + 1) % 3;
157 q.coeffs().coeffRef(
i) =
Scalar(0.5) *
t;
159 q.w() = (
mat.coeff(k, j) -
mat.coeff(j, k)) *
t;
160 q.coeffs().coeffRef(j) = (
mat.coeff(j,
i) +
mat.coeff(
i, j)) *
t;
161 q.coeffs().coeffRef(k) = (
mat.coeff(k,
i) +
mat.coeff(
i, k)) *
t;
167 template<
typename Scalar,
typename Matrix3,
typename QuaternionDerived>
169 run(
Scalar t, Eigen::QuaternionBase<QuaternionDerived> &
q,
const Matrix3 & mat)
171 using pinocchio::math::sqrt;
176 q.x() = (
mat.coeff(2, 1) -
mat.coeff(1, 2)) *
t;
177 q.y() = (
mat.coeff(0, 2) -
mat.coeff(2, 0)) *
t;
178 q.z() = (
mat.coeff(1, 0) -
mat.coeff(0, 1)) *
t;
182 template<
typename Scalar>
185 template<
typename Matrix3,
typename QuaternionDerived>
186 static inline void run(Eigen::QuaternionBase<QuaternionDerived> &
q,
const Matrix3 & mat)
188 using pinocchio::math::sqrt;
195 Eigen::DenseIndex
i = 0;
196 if (
mat.coeff(1, 1) >
mat.coeff(0, 0))
198 if (
mat.coeff(2, 2) >
mat.coeff(
i,
i))
213 template<
typename D,
typename Matrix3>
214 void assignQuaternion(Eigen::QuaternionBase<D> & quat,
const Eigen::MatrixBase<Matrix3> & R)
217 quat.derived(),
R.derived());
228 template<
typename Quaternion>
230 const Eigen::QuaternionBase<Quaternion> & quat,
231 const typename Quaternion::Coefficients::RealScalar & prec =
232 Eigen::NumTraits<typename Quaternion::Coefficients::RealScalar>::dummy_precision())
242 template<
typename Quaternion>
243 inline void normalize(
const Eigen::QuaternionBase<Quaternion> & quat)
256 typename QuaternionIn1,
257 typename QuaternionIn2,
258 typename QuaternionOut>
261 const Eigen::QuaternionBase<QuaternionIn1> & quat0,
262 const Eigen::QuaternionBase<QuaternionIn2> & quat1,
263 const Eigen::QuaternionBase<QuaternionOut> &
res)
265 const Scalar one =
Scalar(1) - Eigen::NumTraits<Scalar>::epsilon();
266 const Scalar d = quat0.dot(quat1);
269 const Scalar theta = acos(absD);
270 const Scalar sinTheta = sin(theta);
280 const Scalar scale1_factor =
285 static_cast<Scalar>(sin((
u * theta)) / sinTheta)
290 scale0 * quat0.coeffs() + scale1 * quat1.coeffs();
296 #endif // #ifndef __pinocchio_math_quaternion_hpp__
void slerp(const Scalar &u, const Eigen::QuaternionBase< QuaternionIn1 > &quat0, const Eigen::QuaternionBase< QuaternionIn2 > &quat1, const Eigen::QuaternionBase< QuaternionOut > &res)
static void run(Scalar t, Eigen::QuaternionBase< QuaternionDerived > &q, const Matrix3 &mat)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > const Scalar & u
void uniformRandom(Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
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.
void normalize(const Eigen::QuaternionBase< Quaternion > &quat)
Normalize the input quaternion.
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)
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
void assignQuaternion(Eigen::QuaternionBase< D > &quat, const Eigen::MatrixBase< Matrix3 > &R)
static void run(Scalar t, Eigen::QuaternionBase< QuaternionDerived > &q, const Matrix3 &mat)
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(Eigen::QuaternionBase< QuaternionDerived > &q, const Matrix3 &mat)
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.
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
D1::Scalar angleBetweenQuaternions(const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2)
Compute the minimal angle between q1 and q2.
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
AD< Scalar > max(const AD< Scalar > &x, const AD< Scalar > &y)
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Main pinocchio namespace.
pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:12