Restrict clamp due to singularity in Euler to Quaternion to near 90 and not above near 90

Author: Jonathan Marino

core_sim/src/transforms/transform_utils.cpp

@@ -22,7 +22,7 @@ Quaternion TransformUtils::ToQuaternion(float roll_rad, float pitch_rad,
 
   if (std::abs(pitch_rad) > TransformUtils::kEulerSingularityMinor &&
       std::abs(pitch_rad) < TransformUtils::kEulerSingularityMajor) {
-    // TODO Trigger a warning for clipped pitch angle
+    // TODO(edufford) Trigger a warning for clipped pitch angle
     if (std::abs(pitch_rad) < M_PI/2)
       pitch_rad = std::clamp(pitch_rad, -TransformUtils::kEulerSingularityMinor,
                            TransformUtils::kEulerSingularityMinor);

core_sim/test/gtest_transform_tree.cpp

@@ -185,11 +185,11 @@ TEST(TransformTree, ConvertCross2) {
   projectairsim::Quaternion quatA =
       projectairsim::TransformUtils::ToQuaternion(0.0f, 0.0f, 0.0f);
   projectairsim::Quaternion quatB =
-      projectairsim::TransformUtils::ToQuaternion(M_PI, 0.0f, 0.0f);
+      projectairsim::TransformUtils::ToQuaternion(0.0f, M_PI, 0.0f);
   projectairsim::Quaternion quatA2 =
       projectairsim::TransformUtils::ToQuaternion(0.0f, 0.0f, 0.0f);
   projectairsim::Quaternion quatB2 =
-      projectairsim::TransformUtils::ToQuaternion(M_PI, 0.0f, 0.0f);
+      projectairsim::TransformUtils::ToQuaternion(0.0f, M_PI, 0.0f);
   projectairsim::Quaternion quatAToGlobal = quatA * quatA2;
   projectairsim::Quaternion quatGlobalToB = quatB2.inverse() * quatB.inverse();
   projectairsim::Quaternion quatAInB =
@@ -201,7 +201,7 @@ TEST(TransformTree, ConvertCross2) {
   projectairsim::TransformTree transformtree;
   projectairsim::Vector3 vec3A = projectairsim::Vector3(1.0f, 2.0f, 3.0f);
   projectairsim::Vector3 vec3A2 = projectairsim::Vector3(4.0f, 5.0f, 6.0f);
-  projectairsim::Vector3 vec3B = projectairsim::Vector3(10.0f, -20.0f, -30.0f);
+  projectairsim::Vector3 vec3B = projectairsim::Vector3(-10.0f, 20.0f, -30.0f);
   projectairsim::Vector3 vec3B2 = projectairsim::Vector3(40.0f, 50.0f, 60.0f);
   projectairsim::Vector3 vec3AToGlobal = vec3A + vec3A2;
   projectairsim::Vector3 vec3BToB2Global = quatB2 * vec3B;

core_sim/test/gtest_transform_utils.cpp

@@ -26,6 +26,9 @@ TEST(TransformUtils, ToQuaternion) {
   constexpr float t1_roll = projectairsim::TransformUtils::kEulerSingularityMinor;
   constexpr float t1_pitch = projectairsim::TransformUtils::kEulerSingularityMinor;
   constexpr float t1_yaw = projectairsim::TransformUtils::kEulerSingularityMinor;
+  constexpr float t1_roll = projectairsim::TransformUtils::kEulerSingularityMinor;
+  constexpr float t1_pitch = projectairsim::TransformUtils::kEulerSingularityMinor;
+  constexpr float t1_yaw = projectairsim::TransformUtils::kEulerSingularityMinor;
   constexpr float a1_w = 0.7071075f;
   constexpr float a1_x = 0.000617057f;
   constexpr float a1_y = 0.7071054f;
@@ -60,9 +63,9 @@ TEST(TransformUtils, ToQuaternion) {
   constexpr float t1b_pitch = projectairsim::TransformUtils::kEulerSingularityMajor;
   constexpr float t1b_yaw = projectairsim::TransformUtils::kEulerSingularityMajor;
   constexpr float a1b_w = 0.7071075f;
-  constexpr float a1b_x = -0.00061708689f;
+  constexpr float a1b_x = -0.00061705709f;
   constexpr float a1b_y = 0.7071054f;
-  constexpr float a1b_z = -0.00061708689f;
+  constexpr float a1b_z = -0.00061705709f;
 
   q = projectairsim::TransformUtils::ToQuaternion(t1b_roll, t1b_pitch, t1b_yaw);
 
@@ -124,9 +127,9 @@ TEST(TransformUtils, ToQuaternion) {
   constexpr float t3b_roll = -projectairsim::TransformUtils::kEulerSingularityMajor;
   constexpr float t3b_pitch = -projectairsim::TransformUtils::kEulerSingularityMajor;
   constexpr float t3b_yaw = -projectairsim::TransformUtils::kEulerSingularityMajor;
-  constexpr float a3b_w = -0.0018512607f;
+  constexpr float a3b_w = -0.0018511415f;
   constexpr float a3b_x = -0.7071054f;
-  constexpr float a3b_y = 0.00061711669f;
+  constexpr float a3b_y = 0.00061705709f;
   constexpr float a3b_z = -0.7071054f;
 
   q = projectairsim::TransformUtils::ToQuaternion(t3b_roll, t3b_pitch, t3b_yaw);