diff --git a/src/math/quaternion.hpp b/src/math/quaternion.hpp
index aa98d53a7b2e9a699591a80f966cab219141a8b2..ecb3ddfcbe1091a3e6e8bbc5f0b4c4c29803731b 100644
--- a/src/math/quaternion.hpp
+++ b/src/math/quaternion.hpp
@@ -65,21 +65,63 @@ namespace se3
/// of the normalization function.
///
/// Only additions and multiplications are required. Neither square root nor
- /// division are used (except a division by 2).
+ /// division are used (except a division by 2). Let \f$ \delta = ||q||^2 - 1 \f$.
+ /// Using the following limited development:
+ /// \f[ \frac{1}{||q||} = (1 + \delta)^{-\frac{1}{2}} = 1 - \frac{\delta}{2} + \mathcal{O}(\delta^2) \f]
+ ///
+ /// The output is
+ /// \f[ q_{out} = q \times \frac{3 - ||q_{in}||^2}{2} \f]
+ ///
+ /// The output quaternion is guaranted to statisfy the following:
+ /// \f[ | ||q_{out}|| - 1 | \le \frac{M}{2} ||q_{in}|| ( ||q_{in}||^2 - 1 )^2 \f]
+ /// where \f$ M = \frac{3}{4} (1 - \epsilon)^{-\frac{5}{2}} \f$
+ /// and \f$ \epsilon \f$ is the maximum tolerance of \f$ ||q_{in}||^2 - 1 \f$.
///
/// \warning \f$ ||q||^2 - 1 \f$ should already be close to zero.
///
/// \note See
/// http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#title3
/// to know the reason why the argument is const.
- template <typename D> void
- firstOrderNormalize(const Eigen::QuaternionBase<D> & q)
+ template <typename D>
+ void firstOrderNormalize(const Eigen::QuaternionBase<D> & q)
{
- assert(std::fabs(q.norm() - 1) < 1e-2);
typedef typename D::Scalar Scalar;
- const Scalar alpha = ((Scalar)3 - q.squaredNorm()) / 2;
+ const Scalar N2 = q.squaredNorm();
+#ifndef NDEBUG
+ const Scalar epsilon = sqrt(sqrt(Eigen::NumTraits<Scalar>::epsilon()));
+ assert(std::fabs(N2-1.) <= epsilon);
+#endif
+ const Scalar alpha = ((Scalar)3 - N2) / 2;
const_cast <Eigen::QuaternionBase<D> &> (q).coeffs() *= alpha;
+#ifndef NDEBUG
+ const Scalar M = 3 * std::pow((Scalar)1.-epsilon, ((Scalar)-5)/2) / 4;
+ assert(std::fabs(q.norm() - 1) <=
+ std::max(M * sqrt(N2) * (N2 - 1)*(N2 - 1) / 2, Eigen::NumTraits<Scalar>::epsilon()));
+#endif
}
+ /// Uniformly random quaternion sphere.
+ template <typename D>
+ void uniformRandom (const Eigen::QuaternionBase<D> & q)
+ {
+ typedef typename D::Scalar Scalar;
+ typedef Eigen::QuaternionBase<D> Base;
+
+ // Rotational part
+ const Scalar u1 = (Scalar)rand() / RAND_MAX;
+ const Scalar u2 = (Scalar)rand() / RAND_MAX;
+ const Scalar u3 = (Scalar)rand() / RAND_MAX;
+
+ const Scalar mult1 = sqrt (1-u1);
+ const Scalar mult2 = sqrt (u1);
+
+ Scalar s2,c2; SINCOS(2.*PI*u2,&s2,&c2);
+ Scalar s3,c3; SINCOS(2.*PI*u3,&s3,&c3);
+
+ const_cast <Base &> (q).w() = mult1 * s2;
+ const_cast <Base &> (q).x() = mult1 * c2;
+ const_cast <Base &> (q).y() = mult2 * s3;
+ const_cast <Base &> (q).z() = mult2 * c3;
+ }
}
#endif //#ifndef __math_quaternion_hpp__
diff --git a/src/multibody/joint/joint-free-flyer.hpp b/src/multibody/joint/joint-free-flyer.hpp
index f5f9e444c2442cf95e27ebb7c0de4037810829a6..a58e019bc2d1f50bbfc3a88045004ac5cf7ede6c 100644
--- a/src/multibody/joint/joint-free-flyer.hpp
+++ b/src/multibody/joint/joint-free-flyer.hpp
@@ -213,7 +213,7 @@ namespace se3
typedef Eigen::Map<const Motion_t::Quaternion_t> ConstQuaternionMap_t;
ConstQuaternionMap_t quat(q_joint.template tail<4>().data());
- assert(std::fabs(quat.coeffs().norm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon()));
+ assert(std::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon()));
M.rotation(quat.matrix());
M.translation(q_joint.template head<3>());
@@ -314,7 +314,10 @@ namespace se3
ConfigVector_t random_impl() const
{
ConfigVector_t q(ConfigVector_t::Random());
- q.segment<4>(3).normalize();// /= q.segment<4>(3).norm();
+
+ typedef Eigen::Map<Motion_t::Quaternion_t> QuaternionMap_t;
+ uniformRandom(QuaternionMap_t(q.segment<4>(3).data()));
+
return q;
}
@@ -335,22 +338,9 @@ namespace se3
result[i] = lower_pos_limit[i] + (upper_pos_limit[i] - lower_pos_limit[i]) * (Scalar)(rand())/RAND_MAX;
}
- // Rotational part
- const Scalar u1 = (Scalar)rand() / RAND_MAX;
- const Scalar u2 = (Scalar)rand() / RAND_MAX;
- const Scalar u3 = (Scalar)rand() / RAND_MAX;
-
- const Scalar mult1 = sqrt (1-u1);
- const Scalar mult2 = sqrt (u1);
-
- Scalar s2,c2; SINCOS(2.*PI*u2,&s2,&c2);
- Scalar s3,c3; SINCOS(2.*PI*u3,&s3,&c3);
-
-
- result.segment<4>(3) << mult1 * s2,
- mult1 * c2,
- mult2 * s3,
- mult2 * c3;
+ typedef Eigen::Map<Motion_t::Quaternion_t> QuaternionMap_t;
+ uniformRandom(QuaternionMap_t(result.segment<4>(3).data()));
+
return result;
}
diff --git a/src/multibody/joint/joint-spherical.hpp b/src/multibody/joint/joint-spherical.hpp
index fdbf086c5a3d9e412cf443f081ee93ad368423f4..00e76ee0f7cb21d7e3645e29fb068659d7ddb6b4 100644
--- a/src/multibody/joint/joint-spherical.hpp
+++ b/src/multibody/joint/joint-spherical.hpp
@@ -356,31 +356,17 @@ namespace se3
ConfigVector_t random_impl() const
{
- ConfigVector_t q(ConfigVector_t::Random());
- q.normalize();
+ ConfigVector_t q;
+ typedef Eigen::Map<Motion_t::Quaternion_t> QuaternionMap_t;
+ uniformRandom(QuaternionMap_t(q.data()));
return q;
}
ConfigVector_t randomConfiguration_impl(const ConfigVector_t &, const ConfigVector_t &) const
{
ConfigVector_t result;
-
- // Rotational part
- const Scalar u1 = (Scalar)rand() / RAND_MAX;
- const Scalar u2 = (Scalar)rand() / RAND_MAX;
- const Scalar u3 = (Scalar)rand() / RAND_MAX;
-
- const Scalar mult1 = sqrt (1-u1);
- const Scalar mult2 = sqrt (u1);
-
- Scalar s2,c2; SINCOS(2.*PI*u2,&s2,&c2);
- Scalar s3,c3; SINCOS(2.*PI*u3,&s3,&c3);
-
- result << mult1 * s2,
- mult1 * c2,
- mult2 * s3,
- mult2 * c3;
-
+ typedef Eigen::Map<Motion_t::Quaternion_t> QuaternionMap_t;
+ uniformRandom(QuaternionMap_t(result.data()));
return result;
}