Fixed fabs() use in quaternion interpolation

moveit_core/robot_model/src/floating_joint_model.cpp

@@ -136,19 +136,20 @@ void FloatingJointModel::interpolate(const double* from, const double* to, const
   state[1] = from[1] + (to[1] - from[1]) * t;
   state[2] = from[2] + (to[2] - from[2]) * t;
 
-  double dq = fabs(from[3] * to[3] + from[4] * to[4] + from[5] * to[5] + from[6] * to[6]);
-  double theta = (dq + std::numeric_limits<double>::epsilon() >= 1.0) ? 0.0 : acos(dq);
-  if (theta > std::numeric_limits<double>::epsilon())
+  // Check if the quaternions are significantly different
+  if (abs(from[3] - to[3]) + abs(from[4] - to[4]) + abs(from[5] - to[5]) + abs(from[6] - to[6]) >
+      std::numeric_limits<double>::epsilon())
   {
-    double d = 1.0 / sin(theta);
-    double s0 = sin((1.0 - t) * theta);
-    double s1 = sin(t * theta);
-    if (dq < 0)  // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
-      s1 = -s1;
-    state[3] = (from[3] * s0 + to[3] * s1) * d;
-    state[4] = (from[4] * s0 + to[4] * s1) * d;
-    state[5] = (from[5] * s0 + to[5] * s1) * d;
-    state[6] = (from[6] * s0 + to[6] * s1) * d;
+    // Note the ordering: Eigen takes w first!
+    Eigen::Quaterniond q1(from[6], from[3], from[4], from[5]);
+    Eigen::Quaterniond q2(to[6], to[3], to[4], to[5]);
+
+    Eigen::Quaterniond q = q1.slerp(t, q2);
+
+    state[3] = q.x();
+    state[4] = q.y();
+    state[5] = q.z();
+    state[6] = q.w();
   }
   else
   {

moveit_core/robot_model/test/test.cpp

@@ -128,6 +128,50 @@ TEST(SiblingAssociateLinks, SimpleYRobot)
   }
 }
 
+TEST(FloatingJointTest, interpolation_test)
+{
+  // Create a simple floating joint model with some dummy parameters (these are not used by the test)
+  moveit::core::FloatingJointModel fjm("joint", 0, 0);
+
+  // We set some bounds where the joint position's translation component is bounded between -1 and 1 in all
+  // dimensions. This is necessary, otherwise we just get (0,0,0) translations.
+  moveit::core::JointModel::Bounds bounds;
+  bounds = fjm.getVariableBounds();
+  bounds[0].min_position_ = -1.0;
+  bounds[0].max_position_ = 1.0;
+  bounds[1].min_position_ = -1.0;
+  bounds[1].max_position_ = 1.0;
+  bounds[2].min_position_ = -1.0;
+  bounds[2].max_position_ = 1.0;
+
+  double jv1[7];
+  double jv2[7];
+  double intp[7];
+  random_numbers::RandomNumberGenerator rng;
+
+  for (size_t i = 0; i < 1000; ++i)
+  {
+    // Randomize the joint settings.
+    fjm.getVariableRandomPositions(rng, jv1, bounds);
+    fjm.getVariableRandomPositions(rng, jv2, bounds);
+
+    // Pick a random interpolation value
+    double t = rng.uniformReal(0.0, 1.0);
+
+    // Apply the interpolation
+    fjm.interpolate(jv1, jv2, t, intp);
+
+    // Get the distances between the two joint configurations
+    double d1 = fjm.distance(jv1, intp);
+    double d2 = fjm.distance(jv2, intp);
+    double t_total = fjm.distance(jv1, jv2);
+
+    // Check that the resulting distances match with the interpolation value
+    EXPECT_NEAR(d1, t_total * t, 1e-6);
+    EXPECT_NEAR(d2, t_total * (1.0 - t), 1e-6);
+  }
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);