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>
34 template<
typename D1,
typename D2>
36 const Eigen::QuaternionBase<D1> &
q1,
const Eigen::QuaternionBase<D2> &
q2)
38 typedef typename D1::Scalar Scalar;
43 theta = internal::if_then_else(
57 template<
typename D1,
typename D2>
59 const Eigen::QuaternionBase<D1> &
q1,
60 const Eigen::QuaternionBase<D2> &
q2,
61 const typename D1::RealScalar &
prec =
62 Eigen::NumTraits<typename D1::Scalar>::dummy_precision())
64 return (
q1.coeffs().isApprox(
q2.coeffs(),
prec) ||
q1.coeffs().isApprox(-
q2.coeffs(),
prec));
92 typedef typename D::Scalar Scalar;
93 const Scalar
N2 = q.squaredNorm();
95 const Scalar epsilon =
sqrt(
sqrt(Eigen::NumTraits<Scalar>::epsilon()));
98 assert(static_leq::op(math::fabs(
static_cast<Scalar
>(
N2 - Scalar(1))), epsilon));
100 const Scalar
alpha = ((Scalar)3 -
N2) / Scalar(2);
101 PINOCCHIO_EIGEN_CONST_CAST(D, q).coeffs() *=
alpha;
104 Scalar(3) * math::pow(Scalar(1) - epsilon, ((Scalar)-Scalar(5)) / Scalar(2)) / Scalar(4);
106 math::fabs(
static_cast<Scalar
>(q.norm() - Scalar(1))),
108 M *
sqrt(
N2) * (
N2 - Scalar(1)) * (
N2 - Scalar(1)) / Scalar(2),
109 Eigen::NumTraits<Scalar>::dummy_precision())));
114 template<
typename Derived>
117 typedef typename Derived::Scalar Scalar;
143 struct quaternionbase_assign_impl;
145 template<Eigen::DenseIndex i>
146 struct quaternionbase_assign_impl_if_t_negative
148 template<
typename Scalar,
typename Matrix3,
typename QuaternionDerived>
150 run(Scalar
t, Eigen::QuaternionBase<QuaternionDerived> & q,
const Matrix3 &
mat)
152 using pinocchio::math::sqrt;
154 Eigen::DenseIndex
j = (
i + 1) % 3;
155 Eigen::DenseIndex
k = (
j + 1) % 3;
158 q.coeffs().coeffRef(
i) = Scalar(0.5) *
t;
161 q.coeffs().coeffRef(
j) = (
mat.coeff(
j,
i) +
mat.coeff(
i,
j)) *
t;
162 q.coeffs().coeffRef(
k) = (
mat.coeff(
k,
i) +
mat.coeff(
i,
k)) *
t;
166 struct quaternionbase_assign_impl_if_t_positive
168 template<
typename Scalar,
typename Matrix3,
typename QuaternionDerived>
170 run(Scalar t, Eigen::QuaternionBase<QuaternionDerived> & q,
const Matrix3 & mat)
172 using pinocchio::math::sqrt;
174 t = sqrt(t + Scalar(1.0));
175 q.w() = Scalar(0.5) * t;
177 q.x() = (mat.coeff(2, 1) - mat.coeff(1, 2)) * t;
178 q.y() = (mat.coeff(0, 2) - mat.coeff(2, 0)) * t;
179 q.z() = (mat.coeff(1, 0) - mat.coeff(0, 1)) * t;
183 template<
typename Scalar>
184 struct quaternionbase_assign_impl<Scalar, true>
186 template<
typename Matrix3,
typename QuaternionDerived>
187 static inline void run(Eigen::QuaternionBase<QuaternionDerived> & q,
const Matrix3 & mat)
189 using pinocchio::math::sqrt;
191 Scalar t = mat.trace();
193 quaternionbase_assign_impl_if_t_positive::run(t, q, mat);
196 Eigen::DenseIndex i = 0;
197 if (mat.coeff(1, 1) > mat.coeff(0, 0))
199 if (mat.coeff(2, 2) > mat.coeff(i, i))
203 quaternionbase_assign_impl_if_t_negative<0>::run(t, q, mat);
205 quaternionbase_assign_impl_if_t_negative<1>::run(t, q, mat);
207 quaternionbase_assign_impl_if_t_negative<2>::run(t, q, mat);
214 template<
typename D,
typename Matrix3>
215 void assignQuaternion(Eigen::QuaternionBase<D> & quat,
const Eigen::MatrixBase<Matrix3> & R)
217 internal::quaternionbase_assign_impl<typename Matrix3::Scalar>::run(
218 quat.derived(), R.derived());
229 template<
typename Quaternion>
231 const Eigen::QuaternionBase<Quaternion> &
quat,
232 const typename Quaternion::Coefficients::RealScalar &
prec)
244 template<
typename Quaternion>
247 typedef typename Quaternion::Coefficients::RealScalar RealScalar;
248 const RealScalar
prec = math::sqrt(Eigen::NumTraits<RealScalar>::epsilon());
257 template<
typename Quaternion>
276 const Eigen::QuaternionBase<QuaternionIn1> &
quat0,
277 const Eigen::QuaternionBase<QuaternionIn2> &
quat1,
278 const Eigen::QuaternionBase<QuaternionOut> & res)
280 const Scalar
one = Scalar(1) - Eigen::NumTraits<Scalar>::epsilon();
289 const Scalar
scale0 = if_then_else(
290 pinocchio::internal::GE,
absD,
one,
291 static_cast<Scalar
>(Scalar(1) - u),
296 if_then_else(pinocchio::internal::LT, d, Scalar(0), Scalar(-1), Scalar(1));
297 const Scalar
scale1 = if_then_else(
298 pinocchio::internal::GE,
absD,
one,
304 PINOCCHIO_EIGEN_CONST_CAST(
QuaternionOut, res.derived()).coeffs() =
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.
bool isNormalized(const Eigen::QuaternionBase< Quaternion > &quat, const typename Quaternion::Coefficients::RealScalar &prec)
Check whether the input quaternion is Normalized within the given precision.
void firstOrderNormalize(const Eigen::QuaternionBase< D > &q)
D1::Scalar angleBetweenQuaternions(const Eigen::QuaternionBase< D1 > &q1, const Eigen::QuaternionBase< D2 > &q2)
Compute the minimal angle between q1 and q2.
void slerp(const Scalar &u, const Eigen::QuaternionBase< QuaternionIn1 > &quat0, const Eigen::QuaternionBase< QuaternionIn2 > &quat1, const Eigen::QuaternionBase< QuaternionOut > &res)
void normalize(const Eigen::QuaternionBase< Quaternion > &quat)
Normalize the input quaternion.
void uniformRandom(Eigen::QuaternionBase< Derived > &q)
Uniformly random quaternion sphere.
Main pinocchio namespace.
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
void SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
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.