[wpimath] Add CoordinateSystem.convert() translation and rotation overloads (#4227)

wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java

@@ -39,8 +39,8 @@ public class CoordinateSystem {
     R.assignBlock(0, 2, positiveZ.m_axis);
 
     // Require that the change of basis matrix is special orthogonal. This is true
-    // if the axes used are orthogonal and normalized. The Axis class already
+    // if the axes used are orthogonal and normalized. The CoordinateAxis class
-    // normalizes itself, so we just need to check for orthogonality.
+    // already normalizes itself, so we just need to check for orthogonality.
     if (!R.times(R.transpose()).equals(Matrix.eye(Nat.N3()))) {
       throw new IllegalArgumentException("Coordinate system isn't special orthogonal");
     }
@@ -120,6 +120,32 @@ public class CoordinateSystem {
     return m_ned;
   }
 
+  /**
+   * Converts the given translation from one coordinate system to another.
+   *
+   * @param translation The translation to convert.
+   * @param from The coordinate system the pose starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given translation in the desired coordinate system.
+   */
+  public static Translation3d convert(
+      Translation3d translation, CoordinateSystem from, CoordinateSystem to) {
+    return translation.rotateBy(from.m_rotation.minus(to.m_rotation));
+  }
+
+  /**
+   * Converts the given rotation from one coordinate system to another.
+   *
+   * @param rotation The rotation to convert.
+   * @param from The coordinate system the rotation starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given rotation in the desired coordinate system.
+   */
+  public static Rotation3d convert(
+      Rotation3d rotation, CoordinateSystem from, CoordinateSystem to) {
+    return rotation.rotateBy(from.m_rotation.minus(to.m_rotation));
+  }
+
   /**
    * Converts the given pose from one coordinate system to another.
    *

wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp

@@ -24,8 +24,8 @@ CoordinateSystem::CoordinateSystem(const CoordinateAxis& positiveX,
   R.block<3, 1>(0, 2) = positiveZ.m_axis;
 
   // Require that the change of basis matrix is special orthogonal. This is true
-  // if the axes used are orthogonal and normalized. The Axis class already
+  // if the axes used are orthogonal and normalized. The CoordinateAxis class
-  // normalizes itself, so we just need to check for orthogonality.
+  // already normalizes itself, so we just need to check for orthogonality.
   if (R * R.transpose() != Matrixd<3, 3>::Identity()) {
     throw std::domain_error("Coordinate system isn't special orthogonal");
   }
@@ -87,6 +87,18 @@ const CoordinateSystem& CoordinateSystem::NED() {
   return instance;
 }
 
+Translation3d CoordinateSystem::Convert(const Translation3d& translation,
+                                        const CoordinateSystem& from,
+                                        const CoordinateSystem& to) {
+  return translation.RotateBy(from.m_rotation - to.m_rotation);
+}
+
+Rotation3d CoordinateSystem::Convert(const Rotation3d& rotation,
+                                     const CoordinateSystem& from,
+                                     const CoordinateSystem& to) {
+  return rotation.RotateBy(from.m_rotation - to.m_rotation);
+}
+
 Pose3d CoordinateSystem::Convert(const Pose3d& pose,
                                  const CoordinateSystem& from,
                                  const CoordinateSystem& to) {

wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h

@@ -10,6 +10,7 @@
 #include "frc/geometry/CoordinateAxis.h"
 #include "frc/geometry/Pose3d.h"
 #include "frc/geometry/Rotation3d.h"
+#include "frc/geometry/Translation3d.h"
 
 namespace frc {
 
@@ -58,6 +59,30 @@ class WPILIB_DLLEXPORT CoordinateSystem {
    */
   static const CoordinateSystem& NED();
 
+  /**
+   * Converts the given translation from one coordinate system to another.
+   *
+   * @param translation The translation to convert.
+   * @param from The coordinate system the translation starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given translation in the desired coordinate system.
+   */
+  static Translation3d Convert(const Translation3d& translation,
+                               const CoordinateSystem& from,
+                               const CoordinateSystem& to);
+
+  /**
+   * Converts the given rotation from one coordinate system to another.
+   *
+   * @param rotation The rotation to convert.
+   * @param from The coordinate system the rotation starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given rotation in the desired coordinate system.
+   */
+  static Rotation3d Convert(const Rotation3d& rotation,
+                            const CoordinateSystem& from,
+                            const CoordinateSystem& to);
+
   /**
    * Converts the given pose from one coordinate system to another.
    *

wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java

@@ -10,119 +10,124 @@ import edu.wpi.first.math.util.Units;
 import org.junit.jupiter.api.Test;
 
 class CoordinateSystemTest {
+  private void checkConvert(
+      Pose3d poseFrom, Pose3d poseTo, CoordinateSystem coordFrom, CoordinateSystem coordTo) {
+    // "from" to "to"
+    assertEquals(
+        poseTo.getTranslation(),
+        CoordinateSystem.convert(poseFrom.getTranslation(), coordFrom, coordTo));
+    assertEquals(
+        poseTo.getRotation(), CoordinateSystem.convert(poseFrom.getRotation(), coordFrom, coordTo));
+    assertEquals(poseTo, CoordinateSystem.convert(poseFrom, coordFrom, coordTo));
+
+    // "to" to "from"
+    assertEquals(
+        poseFrom.getTranslation(),
+        CoordinateSystem.convert(poseTo.getTranslation(), coordTo, coordFrom));
+    assertEquals(
+        poseFrom.getRotation(), CoordinateSystem.convert(poseTo.getRotation(), coordTo, coordFrom));
+    assertEquals(poseFrom, CoordinateSystem.convert(poseTo, coordTo, coordFrom));
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
   @Test
   void testEDNtoNWU() {
-    var edn1 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d());
+    // No rotation from EDN to NWU
-    var nwu1 =
+    checkConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d()),
         new Pose3d(
             3.0,
             -1.0,
             -2.0,
-            new Rotation3d(-Units.degreesToRadians(90.0), 0.0, -Units.degreesToRadians(90.0)));
+            new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
 
-    assertEquals(
+    // 45° roll from EDN to NWU
-        nwu1, CoordinateSystem.convert(edn1, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
+    checkConvert(
-    assertEquals(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
-        edn1, CoordinateSystem.convert(nwu1, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
-
-    var edn2 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0));
-    var nwu2 =
         new Pose3d(
             3.0,
             -1.0,
             -2.0,
-            new Rotation3d(-Units.degreesToRadians(45.0), 0.0, -Units.degreesToRadians(90.0)));
+            new Rotation3d(Units.degreesToRadians(-45.0), 0.0, Units.degreesToRadians(-90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
 
-    assertEquals(
+    // 45° pitch from EDN to NWU
-        nwu2, CoordinateSystem.convert(edn2, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
+    checkConvert(
-    assertEquals(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
-        edn2, CoordinateSystem.convert(nwu2, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
-
-    var edn3 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0));
-    var nwu3 =
         new Pose3d(
             3.0,
             -1.0,
             -2.0,
-            new Rotation3d(-Units.degreesToRadians(90.0), 0.0, -Units.degreesToRadians(135.0)));
+            new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-135.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
 
-    assertEquals(
+    // 45° yaw from EDN to NWU
-        nwu3, CoordinateSystem.convert(edn3, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
+    checkConvert(
-    assertEquals(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
-        edn3, CoordinateSystem.convert(nwu3, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
-
-    var edn4 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0)));
-    var nwu4 =
         new Pose3d(
             3.0,
             -1.0,
             -2.0,
             new Rotation3d(
-                -Units.degreesToRadians(90.0),
+                Units.degreesToRadians(-90.0),
                 Units.degreesToRadians(45.0),
-                -Units.degreesToRadians(90.0)));
+                Units.degreesToRadians(-90.0))),
-
+        CoordinateSystem.EDN(),
-    assertEquals(
+        CoordinateSystem.NWU());
-        nwu4, CoordinateSystem.convert(edn4, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
-    assertEquals(
-        edn4, CoordinateSystem.convert(nwu4, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
   }
 
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
   @Test
   void testEDNtoNED() {
-    var edn1 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d());
+    // No rotation from EDN to NED
-    var ned1 =
+    checkConvert(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d()),
         new Pose3d(
             3.0,
             1.0,
             2.0,
-            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0)));
+            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
 
-    assertEquals(
+    // 45° roll from EDN to NED
-        ned1, CoordinateSystem.convert(edn1, CoordinateSystem.EDN(), CoordinateSystem.NED()));
+    checkConvert(
-    assertEquals(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
-        edn1, CoordinateSystem.convert(ned1, CoordinateSystem.NED(), CoordinateSystem.EDN()));
-
-    var edn2 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0));
-    var ned2 =
         new Pose3d(
             3.0,
             1.0,
             2.0,
-            new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0)));
+            new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
 
-    assertEquals(
+    // 45° pitch from EDN to NED
-        ned2, CoordinateSystem.convert(edn2, CoordinateSystem.EDN(), CoordinateSystem.NED()));
+    checkConvert(
-    assertEquals(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
-        edn2, CoordinateSystem.convert(ned2, CoordinateSystem.NED(), CoordinateSystem.EDN()));
-
-    var edn3 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0));
-    var ned3 =
         new Pose3d(
             3.0,
             1.0,
             2.0,
-            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0)));
+            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
 
-    assertEquals(
+    // 45° yaw from EDN to NED
-        ned3, CoordinateSystem.convert(edn3, CoordinateSystem.EDN(), CoordinateSystem.NED()));
+    checkConvert(
-    assertEquals(
+        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
-        edn3, CoordinateSystem.convert(ned3, CoordinateSystem.NED(), CoordinateSystem.EDN()));
-
-    var edn4 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0)));
-    var ned4 =
         new Pose3d(
             3.0,
             1.0,
             2.0,
             new Rotation3d(
                 Units.degreesToRadians(90.0),
-                -Units.degreesToRadians(45.0),
+                Units.degreesToRadians(-45.0),
-                Units.degreesToRadians(90.0)));
+                Units.degreesToRadians(90.0))),
-
+        CoordinateSystem.EDN(),
-    assertEquals(
+        CoordinateSystem.NED());
-        ned4, CoordinateSystem.convert(edn4, CoordinateSystem.EDN(), CoordinateSystem.NED()));
-    assertEquals(
-        edn4, CoordinateSystem.convert(ned4, CoordinateSystem.NED(), CoordinateSystem.EDN()));
   }
 }

wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp

@@ -8,70 +8,66 @@
 
 using namespace frc;
 
+void CheckConvert(const Pose3d& poseFrom, const Pose3d& poseTo,
+                  const CoordinateSystem& coordFrom,
+                  const CoordinateSystem& coordTo) {
+  // "from" to "to"
+  EXPECT_EQ(
+      poseTo.Translation(),
+      CoordinateSystem::Convert(poseFrom.Translation(), coordFrom, coordTo));
+  EXPECT_EQ(poseTo.Rotation(),
+            CoordinateSystem::Convert(poseFrom.Rotation(), coordFrom, coordTo));
+  EXPECT_EQ(poseTo, CoordinateSystem::Convert(poseFrom, coordFrom, coordTo));
+
+  // "to" to "from"
+  EXPECT_EQ(
+      poseFrom.Translation(),
+      CoordinateSystem::Convert(poseTo.Translation(), coordTo, coordFrom));
+  EXPECT_EQ(poseFrom.Rotation(),
+            CoordinateSystem::Convert(poseTo.Rotation(), coordTo, coordFrom));
+  EXPECT_EQ(poseFrom, CoordinateSystem::Convert(poseTo, coordTo, coordFrom));
+}
+
 TEST(CoordinateSystemTest, EDNtoNWU) {
-  Pose3d edn1{1_m, 2_m, 3_m, Rotation3d{}};
+  // No rotation from EDN to NWU
-  Pose3d nwu1{3_m, -1_m, -2_m, Rotation3d{-90_deg, 0_deg, -90_deg}};
+  CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{}},
+               Pose3d{3_m, -1_m, -2_m, Rotation3d{-90_deg, 0_deg, -90_deg}},
+               CoordinateSystem::EDN(), CoordinateSystem::NWU());
 
-  EXPECT_EQ(nwu1, CoordinateSystem::Convert(edn1, CoordinateSystem::EDN(),
+  // 45° roll from EDN to NWU
-                                            CoordinateSystem::NWU()));
+  CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}},
-  EXPECT_EQ(edn1, CoordinateSystem::Convert(nwu1, CoordinateSystem::NWU(),
+               Pose3d{3_m, -1_m, -2_m, Rotation3d{-45_deg, 0_deg, -90_deg}},
-                                            CoordinateSystem::EDN()));
+               CoordinateSystem::EDN(), CoordinateSystem::NWU());
 
-  Pose3d edn2{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}};
+  // 45° pitch from EDN to NWU
-  Pose3d nwu2{3_m, -1_m, -2_m, Rotation3d{-45_deg, 0_deg, -90_deg}};
+  CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 45_deg, 0_deg}},
+               Pose3d{3_m, -1_m, -2_m, Rotation3d{-90_deg, 0_deg, -135_deg}},
+               CoordinateSystem::EDN(), CoordinateSystem::NWU());
 
-  EXPECT_EQ(nwu2, CoordinateSystem::Convert(edn2, CoordinateSystem::EDN(),
+  // 45° yaw from EDN to NWU
-                                            CoordinateSystem::NWU()));
+  CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}},
-  EXPECT_EQ(edn2, CoordinateSystem::Convert(nwu2, CoordinateSystem::NWU(),
+               Pose3d{3_m, -1_m, -2_m, Rotation3d{-90_deg, 45_deg, -90_deg}},
-                                            CoordinateSystem::EDN()));
+               CoordinateSystem::EDN(), CoordinateSystem::NWU());
-
-  Pose3d edn3{1_m, 2_m, 3_m, Rotation3d{0_deg, 45_deg, 0_deg}};
-  Pose3d nwu3{3_m, -1_m, -2_m, Rotation3d{-90_deg, 0_deg, -135_deg}};
-
-  EXPECT_EQ(nwu3, CoordinateSystem::Convert(edn3, CoordinateSystem::EDN(),
-                                            CoordinateSystem::NWU()));
-  EXPECT_EQ(edn3, CoordinateSystem::Convert(nwu3, CoordinateSystem::NWU(),
-                                            CoordinateSystem::EDN()));
-
-  Pose3d edn4{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}};
-  Pose3d nwu4{3_m, -1_m, -2_m, Rotation3d{-90_deg, 45_deg, -90_deg}};
-
-  EXPECT_EQ(nwu4, CoordinateSystem::Convert(edn4, CoordinateSystem::EDN(),
-                                            CoordinateSystem::NWU()));
-  EXPECT_EQ(edn4, CoordinateSystem::Convert(nwu4, CoordinateSystem::NWU(),
-                                            CoordinateSystem::EDN()));
 }
 
 TEST(CoordinateSystemTest, EDNtoNED) {
-  Pose3d edn1{1_m, 2_m, 3_m, Rotation3d{}};
+  // No rotation from EDN to NED
-  Pose3d ned1{3_m, 1_m, 2_m, Rotation3d{90_deg, 0_deg, 90_deg}};
+  CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{}},
+               Pose3d{3_m, 1_m, 2_m, Rotation3d{90_deg, 0_deg, 90_deg}},
+               CoordinateSystem::EDN(), CoordinateSystem::NED());
 
-  EXPECT_EQ(ned1, CoordinateSystem::Convert(edn1, CoordinateSystem::EDN(),
+  // 45° roll from EDN to NED
-                                            CoordinateSystem::NED()));
+  CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}},
-  EXPECT_EQ(edn1, CoordinateSystem::Convert(ned1, CoordinateSystem::NED(),
+               Pose3d{3_m, 1_m, 2_m, Rotation3d{135_deg, 0_deg, 90_deg}},
-                                            CoordinateSystem::EDN()));
+               CoordinateSystem::EDN(), CoordinateSystem::NED());
 
-  Pose3d edn2{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}};
+  // 45° pitch from EDN to NED
-  Pose3d ned2{3_m, 1_m, 2_m, Rotation3d{135_deg, 0_deg, 90_deg}};
+  CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 45_deg, 0_deg}},
+               Pose3d{3_m, 1_m, 2_m, Rotation3d{90_deg, 0_deg, 135_deg}},
+               CoordinateSystem::EDN(), CoordinateSystem::NED());
 
-  EXPECT_EQ(ned2, CoordinateSystem::Convert(edn2, CoordinateSystem::EDN(),
+  // 45° yaw from EDN to NED
-                                            CoordinateSystem::NED()));
+  CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}},
-  EXPECT_EQ(edn2, CoordinateSystem::Convert(ned2, CoordinateSystem::NED(),
+               Pose3d{3_m, 1_m, 2_m, Rotation3d{90_deg, -45_deg, 90_deg}},
-                                            CoordinateSystem::EDN()));
+               CoordinateSystem::EDN(), CoordinateSystem::NED());
-
-  Pose3d edn3{1_m, 2_m, 3_m, Rotation3d{0_deg, 45_deg, 0_deg}};
-  Pose3d ned3{3_m, 1_m, 2_m, Rotation3d{90_deg, 0_deg, 135_deg}};
-
-  EXPECT_EQ(ned3, CoordinateSystem::Convert(edn3, CoordinateSystem::EDN(),
-                                            CoordinateSystem::NED()));
-  EXPECT_EQ(edn3, CoordinateSystem::Convert(ned3, CoordinateSystem::NED(),
-                                            CoordinateSystem::EDN()));
-
-  Pose3d edn4{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}};
-  Pose3d ned4{3_m, 1_m, 2_m, Rotation3d{90_deg, -45_deg, 90_deg}};
-
-  EXPECT_EQ(ned4, CoordinateSystem::Convert(edn4, CoordinateSystem::EDN(),
-                                            CoordinateSystem::NED()));
-  EXPECT_EQ(edn4, CoordinateSystem::Convert(ned4, CoordinateSystem::NED(),
-                                            CoordinateSystem::EDN()));
 }