From 60e3e2bb94979be1535157a85458a8d000edf19f Mon Sep 17 00:00:00 2001 From: jsupratman13 Date: Mon, 19 Jan 2026 11:59:18 +0900 Subject: [PATCH 1/3] normalize rotation --- moveit_core/robot_model/src/planar_joint_model.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_core/robot_model/src/planar_joint_model.cpp b/moveit_core/robot_model/src/planar_joint_model.cpp index 57a0b3109a..28f87aab3e 100644 --- a/moveit_core/robot_model/src/planar_joint_model.cpp +++ b/moveit_core/robot_model/src/planar_joint_model.cpp @@ -271,6 +271,7 @@ void PlanarJointModel::interpolate(const double* from, const double* to, const d state[1] = to[1]; state[2] = drive_angle + final_turn * percent; } + normalizeRotation(state); } } From 13f2051bd6982727a74f9816bc56a1be01e583bc Mon Sep 17 00:00:00 2001 From: jsupratman13 Date: Mon, 19 Jan 2026 12:14:07 +0900 Subject: [PATCH 2/3] added test code --- moveit_core/robot_model/test/test.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/moveit_core/robot_model/test/test.cpp b/moveit_core/robot_model/test/test.cpp index cf3016185d..1aed129dd2 100644 --- a/moveit_core/robot_model/test/test.cpp +++ b/moveit_core/robot_model/test/test.cpp @@ -204,6 +204,26 @@ TEST(PlanarJointTest, ComputeVariablePositionsNormalizeYaw) } } +TEST(PlanarJointTest, InterpolateNormalizesYaw) +{ + // Create a simple planar joint model with some dummy parameters + moveit::core::PlanarJointModel pjm("joint", 0, 0); + + // Test for normalization + const double from[3] = {0.0, 0.0, -2.9}; + const double to[3] = {0.0, 0.0, 3.0}; + + // Check that yaw value is normalized between [-pi, pi] + for (auto model : {moveit::core::PlanarJointModel::HOLONOMIC, moveit::core::PlanarJointModel::DIFF_DRIVE}) + { + pjm.setMotionModel(model); + double state[3]; + pjm.interpolate(from, to, 1.0, state); + EXPECT_GE(state[2], -M_PI) << "model=" << model << " theta=" << state[2]; + EXPECT_LE(state[2], M_PI) << "model=" << model << " theta=" << state[2]; + } +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From 075979e6e81c475c26b5a207f217b9b210ab45f5 Mon Sep 17 00:00:00 2001 From: jsupratman13 Date: Mon, 19 Jan 2026 12:21:51 +0900 Subject: [PATCH 3/3] fix format --- moveit_core/robot_model/test/test.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/moveit_core/robot_model/test/test.cpp b/moveit_core/robot_model/test/test.cpp index 1aed129dd2..25e71f4b55 100644 --- a/moveit_core/robot_model/test/test.cpp +++ b/moveit_core/robot_model/test/test.cpp @@ -210,11 +210,11 @@ TEST(PlanarJointTest, InterpolateNormalizesYaw) moveit::core::PlanarJointModel pjm("joint", 0, 0); // Test for normalization - const double from[3] = {0.0, 0.0, -2.9}; - const double to[3] = {0.0, 0.0, 3.0}; + const double from[3] = { 0.0, 0.0, -2.9 }; + const double to[3] = { 0.0, 0.0, 3.0 }; // Check that yaw value is normalized between [-pi, pi] - for (auto model : {moveit::core::PlanarJointModel::HOLONOMIC, moveit::core::PlanarJointModel::DIFF_DRIVE}) + for (auto model : { moveit::core::PlanarJointModel::HOLONOMIC, moveit::core::PlanarJointModel::DIFF_DRIVE }) { pjm.setMotionModel(model); double state[3];