PlanarJointModel DIFF_DRIVE interpolate does not normalize yaw

[jsupratman13]

Description

PlanarJointModel treats the yaw (theta) variable as unbounded but cyclic, using $-\pi$ ~ $\pi$ as a canonical normalization interval.
This behavior is enforced in the interpolate function when motion_model is HOLONOMIC, the state[2] is normalized back to $-\pi$ ~ $\pi$

moveit2/moveit_core/robot_model/src/planar_joint_model.cpp

Lines 227 to 234 in 5a8ef4d

	if (state[2] > M_PI)
	{
	state[2] -= 2.0 * M_PI;
	}
	else if (state[2] < -M_PI)
	{
	state[2] += 2.0 * M_PI;
	}

However, when motion_model is DIFF_DRIVE, the state[2] is not normalized back to $-\pi$ ~ $\pi$

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?

CycloneDDS

Steps to Reproduce

TEST(PlanarJointTest, InterpolateDiffDriveMayReturnNonCanonicalYaw)
{
  moveit::core::PlanarJointModel pjm("joint", 0, 0);
  pjm.setMotionModel(moveit::core::PlanarJointModel::DIFF_DRIVE);
  const double from[3] = {0.0, 0.0, -2.9};
  const double to[3]   = {0.0, 0.0,  3.0};
  double state[3];
  pjm.interpolate(from, to, 1.0, state);
  EXPECT_GE(state[2], -M_PI) << "theta=" << state[2];
  EXPECT_LE(state[2],  M_PI) << "theta=" << state[2];
}

Expected behavior

when motion_model is DIFF_DRIVE, state[2] should be normalized back to $-\pi$ ~ $\pi$

Actual behavior

state[2] is not normalized back to $-\pi$ ~ $\pi$

Backtrace or Console output

No response