Segmentation fault when planning with differential drive planar joints

[jsupratman13]

Description

Segmentation fault occurs when planning motions with differential drive planar joints. Planning sometimes succeeds but from to time the process terminates with segfault.

[move_group-39] #9    Object "/opt/ros/jazzy/lib/libmoveit_planning_scene.so.2.12.3", at 0x7561e8dda100, in planning_scene::PlanningScene::isStateColliding(moveit::core::RobotState const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > con
st&, bool) const                                                                                                                                                                                                                                                                
[move_group-39] #8    Object "/opt/ros/jazzy/lib/libmoveit_collision_detection_fcl.so.2.12.3", at 0x7561e87c3032, in collision_detection::CollisionEnvFCL::checkSelfCollisionHelper(collision_detection::CollisionRequest const&, collision_detection::CollisionResult&, moveit:
:core::RobotState const&, collision_detection::AllowedCollisionMatrix const*) const
[move_group-39] #7    Object "/opt/ros/jazzy/lib/libmoveit_collision_detection_fcl.so.2.12.3", at 0x7561e87b3af1, in collision_detection::FCLObject::registerTo(fcl::BroadPhaseCollisionManager<double>*)
[move_group-39] #6  | Source "/usr/src/fcl-0.7.0-3build2/include/fcl/broadphase/broadphase_dynamic_AABB_tree-inl.h", line 772, in init
[move_group-39]     | Source "/usr/src/fcl-0.7.0-3build2/include/fcl/broadphase/detail/hierarchy_tree-inl.h", line 88, in init_0
[move_group-39]       Source "/usr/src/fcl-0.7.0-3build2/include/fcl/broadphase/detail/hierarchy_tree-inl.h", line 538, in registerObjects [0x7561e79437f3]
[move_group-39] #5    Source "/usr/src/fcl-0.7.0-3build2/include/fcl/broadphase/detail/hierarchy_tree-inl.h", line 446, in topdown_0 [0x7561e7947156]
[move_group-39] #4    Source "/usr/src/fcl-0.7.0-3build2/include/fcl/broadphase/detail/hierarchy_tree-inl.h", line 446, in topdown_0 [0x7561e7947156]
[move_group-39] #3    Source "/usr/src/fcl-0.7.0-3build2/include/fcl/broadphase/detail/hierarchy_tree-inl.h", line 446, in topdown_0 [0x7561e7947156]
[move_group-39] #2    Source "/usr/src/fcl-0.7.0-3build2/include/fcl/broadphase/detail/hierarchy_tree-inl.h", line 446, in topdown_0 [0x7561e7947156]
[move_group-39] #1    Source "/usr/src/fcl-0.7.0-3build2/include/fcl/broadphase/detail/hierarchy_tree-inl.h", line 454, in topdown_0 [0x7561e7947094]
[move_group-39] #0    Source "/usr/src/fcl-0.7.0-3build2/include/fcl/broadphase/detail/hierarchy_tree-inl.h", line 365, in bottomup [0x7561e7946c87]
[move_group-39] Segmentation fault (Address not mapped to object [(nil)])

Upon investigation, this issue is probably related to moveit/moveit#3457 on MoveIt (ROS1) side.
In the interpolate function, when the difference between from and to is extremely small (within floating-point precision) or when from equals to, subsequent arithmetic operations produce NaN values. These NaNs later propagate into planning computations that assume finite values, eventually leading to a segmentation fault.

ROS Distro

Jazzy

OS and version

Ubuntu 24.04

Source or binary build?

Binary

If binary, which release version?

No response

If source, which branch?

No response

Which RMW are you using?

None

Steps to Reproduce

I don't exactly have a best way to reproduce the crash but I wrote a test code as follows which demonstrate what the content of state would be after calling interpolate where from == to

TEST(PlanarJointTest, InterpolateDiffDriveNoNan)
{
  moveit::core::PlanarJointModel pjm("joint", 0, 0);
  pjm.setMotionModel(moveit::core::PlanarJointModel::DIFF_DRIVE);

  double from[3] = { 0.0, 0.0, M_PI / 2 };
  double to[3] = { 0.0, 0.0, M_PI / 2};
  double state[3];
  pjm.interpolate(from, to, 0.5, state);
  EXPECT_TRUE((std::isfinite(state[0]) && std::isfinite(state[1]) && std::isfinite(state[2])));
}

Expected behavior

Ideally, state should be non-Nan value (and therefore would not crash and plan as expected)

Actual behavior

state is a Nan value (and therefore segfaults at fcl)

Backtrace or Console output

No response