added unit test

Author: jsupratman13

moveit_core/robot_model/test/test.cpp

@@ -172,6 +172,36 @@ TEST(FloatingJointTest, interpolation_test)
   }
 }
 
+TEST(PlanarJointTest, ComputeVariablePositionsNormalizeYaw)
+{
+  // Create a simple planar joint model with some dummy parameters
+  moveit::core::PlanarJointModel pjm("joint", 0, 0);
+
+  // Test various angles for normalization
+  std::vector<double> test_angles = {0.0, M_PI/2, -M_PI/2, M_PI, -M_PI, 3.0, -3.0,4.1, -4.1};
+  for (double angle : test_angles)
+  {
+    // Create a transform with the given yaw angle
+    Eigen::Isometry3d transform = Eigen::Isometry3d::Identity();
+    Eigen::AngleAxisd q = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ());
+    transform.linear() = q.toRotationMatrix();
+
+    // Compute the variable positions from the transform
+    double joint_values[3];
+    pjm.computeVariablePositions(transform, joint_values);
+
+    // Check that the x and y values are zero (since we didn't set any translation)
+    EXPECT_NEAR(joint_values[0], 0.0, 1e-6);
+    EXPECT_NEAR(joint_values[1], 0.0, 1e-6);
+
+    // The yaw value should be normalized to the range [-pi, pi]
+    double normalized_angle = angle;
+    while (normalized_angle > M_PI) normalized_angle -= 2.0 * M_PI;
+    while (normalized_angle < -M_PI) normalized_angle += 2.0 * M_PI;
+    EXPECT_NEAR(joint_values[2], normalized_angle, 1e-6);
+  }
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);