Invert second quaternion rather than first quaternion when interpolating.

Add unit test for interpolate.

* Inverting the second quaternion fits with the Eigen SLERP implementation.

src/joint-configuration.cc

@@ -199,12 +199,13 @@ namespace hpp {
       // theta is between 0 and M_PI/2.
       // sin(alpha*theta)/sin(theta) in 0 should be computed differently.
       if (fabs (angle) > 1e-6) {
+        const value_type sintheta_inv = 1 / sin (theta);
 	result.segment (index, 4) =
-	  (sin ((1-u)*theta)/sin (theta)) * invertor * q1.segment (index, 4) +
-	  (sin (u*theta)/sin (theta)) * q2.segment (index, 4);
+	  (sin ((1-u)*theta) * sintheta_inv)  * q1.segment (index, 4) +
+	  invertor * (sin (u*theta) * sintheta_inv) * q2.segment (index, 4);
       } else {
 	result.segment (index, 4) =
-	  (1-u) * invertor * q1.segment (index, 4) + u * q2.segment (index, 4);
+	  (1-u) * q1.segment (index, 4) + invertor * u * q2.segment (index, 4);
       }
     }
 

tests/test-configuration.cc

@@ -30,11 +30,14 @@
 #include <boost/test/output_test_stream.hpp>
 using boost::test_tools::output_test_stream;
 
+#include <Eigen/Geometry>
+
 #include <hpp/util/debug.hh>
 #include <hpp/model/configuration.hh>
 #include <hpp/model/object-factory.hh>
 
 using hpp::model::vector_t;
+using hpp::model::vectorIn_t;
 using hpp::model::Configuration_t;
 using hpp::model::JointPtr_t;
 using hpp::model::ObjectFactory;
@@ -47,6 +50,9 @@ using hpp::model::ConfigurationPtr_t;
 using hpp::model::size_type;
 using hpp::model::value_type;
 
+typedef Eigen::AngleAxis <value_type> AngleAxis_t;
+typedef Eigen::Quaternion <value_type> Quaternion_t;
+
 // Create a robot with various types of joints
 DevicePtr_t createRobot ()
 {
@@ -103,6 +109,52 @@ void shootRandomConfig (const DevicePtr_t& robot, Configuration_t& config)
   }
 }
 
+vector_t slerp (vectorIn_t v0, vectorIn_t v1, const value_type t) {
+  Quaternion_t q0(v0[0], v0[1], v0[2], v0[3]);
+  Quaternion_t q1(v1[0], v1[1], v1[2], v1[3]);
+  Quaternion_t q = q0.slerp (t, q1);
+  vector_t res(4);
+  res[0] = q.w(); res[1] = q.x(); res[2] = q.y(); res[3] = q.z();
+  return res;
+}
+
+AngleAxis_t aa (vectorIn_t v0, vectorIn_t v1) {
+  Quaternion_t q0(v0[0], v0[1], v0[2], v0[3]);
+  Quaternion_t q1(v1[0], v1[1], v1[2], v1[3]);
+  return AngleAxis_t (q0 * q1.conjugate ());
+}
+
+vector_t interpolation (DevicePtr_t robot, vectorIn_t q0, vectorIn_t q1, int n)
+{
+  const size_type rso3 = 4;
+  bool print = false;
+
+  vector_t q2 (q0);
+  value_type u = 0;
+  value_type step = (value_type)1 / (value_type)(n + 2);
+  vector_t angle1 (n+2), angle2(n+2);
+
+  if (print) std::cout << "HPP  : ";
+  for (int i = 0; i < n + 2; ++i) {
+    u = 0 + i * step; 
+    hpp::model::interpolate (robot, q0, q1, u, q2);
+    AngleAxis_t aa1 (aa (q0.segment<4>(rso3),q2.segment<4>(rso3)));
+    angle1[i] = aa1.angle ();
+    if (print) std::cout << aa1.angle () << ", ";
+  }
+  if (print) std::cout << "\n";
+  if (print) std::cout << "Eigen: ";
+  for (int i = 0; i < n + 2; ++i) {
+    u = 0 + i * step; 
+    vector_t eigen_slerp = slerp (q0.segment<4>(rso3), q1.segment<4>(rso3), u);
+    AngleAxis_t aa2 (aa (q0.segment<4>(rso3),eigen_slerp));
+    angle2[i] = aa2.angle ();
+    if (print) std::cout << aa2.angle () << ", ";
+  }
+  if (print) std::cout << "\n";
+  return angle1 - angle2;
+}
+
 BOOST_AUTO_TEST_CASE(difference_and_integrate)
 {
   DevicePtr_t robot = createRobot ();
@@ -138,3 +190,44 @@ BOOST_AUTO_TEST_CASE(difference_and_integrate)
         );
   }
 }
+
+BOOST_AUTO_TEST_CASE(interpolate)
+{
+  DevicePtr_t robot = createRobot ();
+  Configuration_t q0; q0.resize (robot->configSize ());
+  Configuration_t q1; q1.resize (robot->configSize ());
+  Configuration_t q2; q2.resize (robot->configSize ());
+  vector_t q1_minus_q0; q1_minus_q0.resize (robot->numberDof ());
+  const value_type eps_dist = robot->numberDof() * sqrt(Eigen::NumTraits<value_type>::epsilon());
+  value_type distance;
+  for (size_type i=0; i<10000; ++i) {
+    shootRandomConfig (robot, q0);
+    shootRandomConfig (robot, q1);
+
+    hpp::model::interpolate (robot, q0, q1, 0, q2);
+    distance = hpp::model::distance (robot, q0, q2);
+    BOOST_CHECK_MESSAGE (distance < eps_dist,
+        "\n(q0 + 0 * (q1 - q0)) is not equivalent to q0\n" <<
+        "q0=" << q0.transpose () << "\n" <<
+        "q1=" << q1.transpose () << "\n" <<
+        "q2=" << q2.transpose () << "\n" <<
+        "distance=" << distance
+        );
+
+    const size_type rso3 = 4;
+    vector_t errors = interpolation (robot, q0, q1, 4);
+    BOOST_CHECK_MESSAGE (errors.isZero (),
+        "The interpolation computed by HPP does not match the Eigen SLERP"
+        );
+
+    hpp::model::interpolate (robot, q0, q1, 1, q2);
+    distance = hpp::model::distance (robot, q1, q2);
+    BOOST_CHECK_MESSAGE (distance < eps_dist,
+        "\n(q0 + 1 * (q1 - q0)) is not equivalent to q1\n" <<
+        "q0=" << q0.transpose () << "\n" <<
+        "q1=" << q1.transpose () << "\n" <<
+        "q2=" << q2.transpose () << "\n" <<
+        "distance=" << distance
+        );
+  }
+}