fix segfault when planning with differential drive planar joint

moveit_core/robot_model/src/planar_joint_model.cpp

@@ -236,6 +236,14 @@ void PlanarJointModel::interpolate(const double* from, const double* to, const d
   }
   else if (motion_model_ == DIFF_DRIVE)
   {
+    if (t <= 0.0)
+    {
+      state[0] = from[0];
+      state[1] = from[1];
+      state[2] = from[2];
+      return;
+    }
+
     double dx, dy, initial_turn, drive_angle, final_turn;
     computeTurnDriveTurnGeometry(from, to, min_translational_distance_, dx, dy, initial_turn, drive_angle, final_turn);
 
@@ -245,6 +253,14 @@ void PlanarJointModel::interpolate(const double* from, const double* to, const d
 
     double total_d = initial_d + drive_d + final_d;
 
+    if (total_d <= std::numeric_limits<double>::epsilon())
+    {
+      state[0] = to[0];
+      state[1] = to[1];
+      state[2] = to[2];
+      return;
+    }
+
     double initial_frac = initial_d / total_d;
     double drive_frac = drive_d / total_d;
     double final_frac = final_d / total_d;

moveit_core/robot_model/test/test.cpp

@@ -34,6 +34,7 @@
 
 /* Author: Ioan Sucan */
 
+#include <limits>
 #include <moveit/robot_model/robot_model.hpp>
 #include <urdf_parser/urdf_parser.h>
 #include <fstream>
@@ -204,6 +205,49 @@ TEST(PlanarJointTest, ComputeVariablePositionsNormalizeYaw)
   }
 }
 
+TEST(PlanarJointTest, InterpolateDiffDriveNoNan)
+{
+  // Create a simple planar joint model with some dummy parameters
+  moveit::core::PlanarJointModel pjm("joint", 0, 0);
+
+  // Set the motion model to DIFF_DRIVE
+  pjm.setMotionModel(moveit::core::PlanarJointModel::DIFF_DRIVE);
+
+  // Define a struct to hold test cases
+  struct FromToCase
+  {
+    std::string name;
+    double from[3];
+    double to[3];
+  };
+
+  // Define some test cases
+  const std::vector<FromToCase> test_cases = {
+    { "No movement", { 0.0, 0.0, 0.0 }, { 0.0, 0.0, 0.0 } },
+    { "Exact equal", { 0.5, -0.5, 0.2 }, { 0.5, -0.5, 0.2 } },
+    { "Small Delta", { 1.0, 1.0, 0.0 }, { 1.0 + 1e-10, 1.0 + 1e-10, 1e-10 } },
+    { "Straight line along X", { 0.0, 0.0, 0.0 }, { 0.1, 0.0, 0.0 } },
+    { "Straight line along Y", { 0.0, 0.0, M_PI / 2 }, { 0.0, 0.1, M_PI / 2 } },
+    { "180 degree turn in place", { 0.0, 0.0, 0.0 }, { 0.0, 0.0, M_PI } },
+    { "360 degree turn in place", { 0.0, 0.0, -M_PI }, { 0.0, 0.0, M_PI } },
+    { "Diagonal movement", { 0.0, 0.0, -M_PI / 4 }, { 0.1, -0.1, -M_PI / 4 } },
+    { "Complex move", { 0.5, 1.0, -M_PI / 2 }, { -0.7, -1.0, M_PI / 3 } }
+  };
+
+  // Test interpolation for each case and check for NaN values
+  for (const auto& test_case : test_cases)
+  {
+    for (double t : { 0.0, 0.5, 1.0 })
+    {
+      double state[3];
+      pjm.interpolate(test_case.from, test_case.to, t, state);
+      EXPECT_TRUE(std::isfinite(state[0])) << "Failed at test case: " << test_case.name << " at t=" << t;
+      EXPECT_TRUE(std::isfinite(state[1])) << "Failed at test case: " << test_case.name << " at t=" << t;
+      EXPECT_TRUE(std::isfinite(state[2])) << "Failed at test case: " << test_case.name << " at t=" << t;
+    }
+  }
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);