normalize quaternion sign for planar joint (#3628)

Author: jsupratman13

moveit_core/robot_model/src/planar_joint_model.cpp

@@ -355,6 +355,10 @@ void PlanarJointModel::computeVariablePositions(const Eigen::Isometry3d& transf,
 
   ASSERT_ISOMETRY(transf)  // unsanitized input, could contain a non-isometry
   Eigen::Quaterniond q(transf.linear());
+  if (q.w() < 0.0)
+  {
+    q.coeffs() *= -1.0;
+  }
   // taken from Bullet
   double s_squared = 1.0 - (q.w() * q.w());
   if (s_squared < 10.0 * std::numeric_limits<double>::epsilon())

moveit_core/robot_model/test/test.cpp

@@ -172,6 +172,38 @@ 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);