Fix angular distance calculation in floating joint model (backport #2538

) (#2544)

(cherry picked from commit 83ff55a)

Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
Co-authored-by: Sebastian Castro <sebas.a.castro@gmail.com>

moveit_core/robot_model/src/floating_joint_model.cpp

@@ -36,6 +36,7 @@
 /* Author: Ioan Sucan */
 
 #include <cmath>
+#include <Eigen/Geometry>
 #include <geometric_shapes/check_isometry.h>
 #include <limits>
 #include <moveit/robot_model/floating_joint_model.h>
@@ -121,16 +122,10 @@ double FloatingJointModel::distanceTranslation(const double* values1, const doub
 
 double FloatingJointModel::distanceRotation(const double* values1, const double* values2) const
 {
-  double dq =
-      fabs(values1[3] * values2[3] + values1[4] * values2[4] + values1[5] * values2[5] + values1[6] * values2[6]);
-  if (dq + std::numeric_limits<double>::epsilon() >= 1.0)
-  {
-    return 0.0;
-  }
-  else
-  {
-    return acos(dq);
-  }
+  // The values are in "xyzw" order but Eigen expects "wxyz".
+  const auto q1 = Eigen::Quaterniond(values1[6], values1[3], values1[4], values1[5]).normalized();
+  const auto q2 = Eigen::Quaterniond(values2[6], values2[3], values2[4], values2[5]).normalized();
+  return q2.angularDistance(q1);
 }
 
 void FloatingJointModel::interpolate(const double* from, const double* to, const double t, double* state) const

moveit_core/robot_trajectory/test/test_robot_trajectory.cpp

@@ -55,7 +55,7 @@ class RobotTrajectoryTestFixture : public testing::Test
   {
     robot_model_ = moveit::core::loadTestingRobotModel(robot_model_name_);
     robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
-    robot_state_->setToDefaultValues(arm_jmg_name_, arm_state_name_);
+    robot_state_->setToDefaultValues();
     robot_state_->setVariableVelocity(/*index*/ 0, /*value*/ 1.0);
     robot_state_->setVariableAcceleration(/*index*/ 0, /*value*/ -0.1);
     robot_state_->update();
@@ -67,7 +67,7 @@ class RobotTrajectoryTestFixture : public testing::Test
 
   void initTestTrajectory(robot_trajectory::RobotTrajectoryPtr& trajectory)
   {
-    // Init a traj
+    // Init a trajectory
     ASSERT_TRUE(robot_model_->hasJointModelGroup(arm_jmg_name_))
         << "Robot model does not have group: " << arm_jmg_name_;
 
@@ -79,7 +79,10 @@ class RobotTrajectoryTestFixture : public testing::Test
     double duration_from_previous = 0.1;
     std::size_t waypoint_count = 5;
     for (std::size_t ix = 0; ix < waypoint_count; ++ix)
+    {
       trajectory->addSuffixWayPoint(*robot_state_, duration_from_previous);
+    }
+
     // Quick check that getDuration is working correctly
     EXPECT_EQ(trajectory->getDuration(), duration_from_previous * waypoint_count)
         << "Generated trajectory duration incorrect";
@@ -528,6 +531,17 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryLength)
 {
   robot_trajectory::RobotTrajectoryPtr trajectory;
   initTestTrajectory(trajectory);
+  EXPECT_FLOAT_EQ(robot_trajectory::path_length(*trajectory), 0.0);
+
+  // modify joint values so the smoothness is nonzero
+  std::vector<double> positions;
+  for (size_t i = 0; i < trajectory->size(); ++i)
+  {
+    auto waypoint = trajectory->getWayPointPtr(i);
+    waypoint->copyJointGroupPositions(arm_jmg_name_, positions);
+    positions[0] += 0.01 * i;
+    waypoint->setJointGroupPositions(arm_jmg_name_, positions);
+  }
   EXPECT_GT(robot_trajectory::path_length(*trajectory), 0.0);
 }
 
@@ -536,6 +550,16 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectorySmoothness)
   robot_trajectory::RobotTrajectoryPtr trajectory;
   initTestTrajectory(trajectory);
 
+  // modify joint values so the smoothness is nonzero
+  std::vector<double> positions;
+  for (size_t i = 0; i < trajectory->size(); ++i)
+  {
+    auto waypoint = trajectory->getWayPointPtr(i);
+    waypoint->copyJointGroupPositions(arm_jmg_name_, positions);
+    positions[0] += 0.01 * i;
+    waypoint->setJointGroupPositions(arm_jmg_name_, positions);
+  }
+
   const auto smoothness = robot_trajectory::smoothness(*trajectory);
   ASSERT_TRUE(smoothness.has_value());
   EXPECT_GT(smoothness.value(), 0.0);
@@ -550,13 +574,28 @@ TEST_F(RobotTrajectoryTestFixture, RobotTrajectoryDensity)
   robot_trajectory::RobotTrajectoryPtr trajectory;
   initTestTrajectory(trajectory);
 
-  const auto density = robot_trajectory::waypoint_density(*trajectory);
+  // If trajectory has all equal state, and length zero, density should be null.
+  auto density = robot_trajectory::waypoint_density(*trajectory);
+  ASSERT_FALSE(density.has_value());
+
+  // modify joint values so the density is nonzero
+  std::vector<double> positions;
+  for (size_t i = 0; i < trajectory->size(); ++i)
+  {
+    auto waypoint = trajectory->getWayPointPtr(i);
+    waypoint->copyJointGroupPositions(arm_jmg_name_, positions);
+    positions[0] += 0.01 * i;
+    waypoint->setJointGroupPositions(arm_jmg_name_, positions);
+  }
+
+  density = robot_trajectory::waypoint_density(*trajectory);
   ASSERT_TRUE(density.has_value());
   EXPECT_GT(density.value(), 0.0);
 
   // Check for empty trajectory
   trajectory->clear();
-  EXPECT_FALSE(robot_trajectory::waypoint_density(*trajectory).has_value());
+  density = robot_trajectory::waypoint_density(*trajectory);
+  EXPECT_FALSE(density.has_value());
 }
 
 TEST_F(OneRobot, Unwind)