[wpimath] Add CoordinateSystem conversion for Transform3d (#4443)

I also refactored Pose3d's conversion implementation to use the Translation3d and Rotation3d conversions, thereby giving Translation3d and Rotation3d test coverage. No changes were made to the expected values of the Pose3d conversion tests. The expected values of the Transform3d conversion tests were copied from the Pose3d conversion tests without modification.
2026-07-05 03:21:42 +00:00 · 2022-10-01 21:09:04 -07:00
parent 38bb23eb18
commit b53b3526a2
5 changed files with 287 additions and 43 deletions
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java
@@ -10,7 +10,7 @@ import edu.wpi.first.math.util.Units;
 import org.junit.jupiter.api.Test;

 class CoordinateSystemTest {
-  private void checkConvert(
+  private void checkPose3dConvert(
      Pose3d poseFrom, Pose3d poseTo, CoordinateSystem coordFrom, CoordinateSystem coordTo) {
    // "from" to "to"
    assertEquals(
@@ -29,10 +29,34 @@ class CoordinateSystemTest {
    assertEquals(poseFrom, CoordinateSystem.convert(poseTo, coordTo, coordFrom));
  }

+  private void checkTransform3dConvert(
+      Transform3d transformFrom,
+      Transform3d transformTo,
+      CoordinateSystem coordFrom,
+      CoordinateSystem coordTo) {
+    // "from" to "to"
+    assertEquals(
+        transformTo.getTranslation(),
+        CoordinateSystem.convert(transformFrom.getTranslation(), coordFrom, coordTo));
+    assertEquals(
+        transformTo.getRotation(),
+        CoordinateSystem.convert(transformFrom.getRotation(), coordFrom, coordTo));
+    assertEquals(transformTo, CoordinateSystem.convert(transformFrom, coordFrom, coordTo));
+
+    // "to" to "from"
+    assertEquals(
+        transformFrom.getTranslation(),
+        CoordinateSystem.convert(transformTo.getTranslation(), coordTo, coordFrom));
+    assertEquals(
+        transformFrom.getRotation(),
+        CoordinateSystem.convert(transformTo.getRotation(), coordTo, coordFrom));
+    assertEquals(transformFrom, CoordinateSystem.convert(transformTo, coordTo, coordFrom));
+  }
+
  @Test
-  void testEDNtoNWU() {
+  void testPose3dEDNtoNWU() {
    // No rotation from EDN to NWU
-    checkConvert(
+    checkPose3dConvert(
        new Pose3d(1.0, 2.0, 3.0, new Rotation3d()),
        new Pose3d(
            3.0,
@@ -43,7 +67,7 @@ class CoordinateSystemTest {
        CoordinateSystem.NWU());

    // 45° roll from EDN to NWU
-    checkConvert(
+    checkPose3dConvert(
        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
        new Pose3d(
            3.0,
@@ -54,7 +78,7 @@ class CoordinateSystemTest {
        CoordinateSystem.NWU());

    // 45° pitch from EDN to NWU
-    checkConvert(
+    checkPose3dConvert(
        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
        new Pose3d(
            3.0,
@@ -65,7 +89,7 @@ class CoordinateSystemTest {
        CoordinateSystem.NWU());

    // 45° yaw from EDN to NWU
-    checkConvert(
+    checkPose3dConvert(
        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
        new Pose3d(
            3.0,
@@ -80,9 +104,9 @@ class CoordinateSystemTest {
  }

  @Test
-  void testEDNtoNED() {
+  void testPose3dEDNtoNED() {
    // No rotation from EDN to NED
-    checkConvert(
+    checkPose3dConvert(
        new Pose3d(1.0, 2.0, 3.0, new Rotation3d()),
        new Pose3d(
            3.0,
@@ -93,7 +117,7 @@ class CoordinateSystemTest {
        CoordinateSystem.NED());

    // 45° roll from EDN to NED
-    checkConvert(
+    checkPose3dConvert(
        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
        new Pose3d(
            3.0,
@@ -104,7 +128,7 @@ class CoordinateSystemTest {
        CoordinateSystem.NED());

    // 45° pitch from EDN to NED
-    checkConvert(
+    checkPose3dConvert(
        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
        new Pose3d(
            3.0,
@@ -115,7 +139,7 @@ class CoordinateSystemTest {
        CoordinateSystem.NED());

    // 45° yaw from EDN to NED
-    checkConvert(
+    checkPose3dConvert(
        new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
        new Pose3d(
            3.0,
@@ -128,4 +152,100 @@ class CoordinateSystemTest {
        CoordinateSystem.EDN(),
        CoordinateSystem.NED());
  }
+
+  @Test
+  void testTransform3dEDNtoNWU() {
+    // No rotation from EDN to NWU
+    checkTransform3dConvert(
+        new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()),
+        new Transform3d(
+            new Translation3d(3.0, -1.0, -2.0),
+            new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+
+    // 45° roll from EDN to NWU
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
+        new Transform3d(
+            new Translation3d(3.0, -1.0, -2.0),
+            new Rotation3d(Units.degreesToRadians(-45.0), 0.0, Units.degreesToRadians(-90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+
+    // 45° pitch from EDN to NWU
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
+        new Transform3d(
+            new Translation3d(3.0, -1.0, -2.0),
+            new Rotation3d(Units.degreesToRadians(-90.0), 0.0, Units.degreesToRadians(-135.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+
+    // 45° yaw from EDN to NWU
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
+        new Transform3d(
+            new Translation3d(3.0, -1.0, -2.0),
+            new Rotation3d(
+                Units.degreesToRadians(-90.0),
+                Units.degreesToRadians(45.0),
+                Units.degreesToRadians(-90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NWU());
+  }
+
+  @Test
+  void testTransform3dEDNtoNED() {
+    // No rotation from EDN to NED
+    checkTransform3dConvert(
+        new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()),
+        new Transform3d(
+            new Translation3d(3.0, 1.0, 2.0),
+            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+
+    // 45° roll from EDN to NED
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)),
+        new Transform3d(
+            new Translation3d(3.0, 1.0, 2.0),
+            new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+
+    // 45° pitch from EDN to NED
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)),
+        new Transform3d(
+            new Translation3d(3.0, 1.0, 2.0),
+            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+
+    // 45° yaw from EDN to NED
+    checkTransform3dConvert(
+        new Transform3d(
+            new Translation3d(1.0, 2.0, 3.0),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))),
+        new Transform3d(
+            new Translation3d(3.0, 1.0, 2.0),
+            new Rotation3d(
+                Units.degreesToRadians(90.0),
+                Units.degreesToRadians(-45.0),
+                Units.degreesToRadians(90.0))),
+        CoordinateSystem.EDN(),
+        CoordinateSystem.NED());
+  }
 }
--- a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp
@@ -4,13 +4,14 @@

 #include "frc/geometry/CoordinateSystem.h"
 #include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Transform3d.h"
 #include "gtest/gtest.h"

 using namespace frc;

-void CheckConvert(const Pose3d& poseFrom, const Pose3d& poseTo,
-                  const CoordinateSystem& coordFrom,
-                  const CoordinateSystem& coordTo) {
+void CheckPose3dConvert(const Pose3d& poseFrom, const Pose3d& poseTo,
+                        const CoordinateSystem& coordFrom,
+                        const CoordinateSystem& coordTo) {
  // "from" to "to"
  EXPECT_EQ(
      poseTo.Translation(),
@@ -28,46 +29,135 @@ void CheckConvert(const Pose3d& poseFrom, const Pose3d& poseTo,
  EXPECT_EQ(poseFrom, CoordinateSystem::Convert(poseTo, coordTo, coordFrom));
 }

-TEST(CoordinateSystemTest, EDNtoNWU) {
+void CheckTransform3dConvert(const Transform3d& transformFrom,
+                             const Transform3d& transformTo,
+                             const CoordinateSystem& coordFrom,
+                             const CoordinateSystem& coordTo) {
+  // "from" to "to"
+  EXPECT_EQ(transformTo.Translation(),
+            CoordinateSystem::Convert(transformFrom.Translation(), coordFrom,
+                                      coordTo));
+  EXPECT_EQ(
+      transformTo.Rotation(),
+      CoordinateSystem::Convert(transformFrom.Rotation(), coordFrom, coordTo));
+  EXPECT_EQ(transformTo,
+            CoordinateSystem::Convert(transformFrom, coordFrom, coordTo));
+
+  // "to" to "from"
+  EXPECT_EQ(
+      transformFrom.Translation(),
+      CoordinateSystem::Convert(transformTo.Translation(), coordTo, coordFrom));
+  EXPECT_EQ(
+      transformFrom.Rotation(),
+      CoordinateSystem::Convert(transformTo.Rotation(), coordTo, coordFrom));
+  EXPECT_EQ(transformFrom,
+            CoordinateSystem::Convert(transformTo, coordTo, coordFrom));
+}
+
+TEST(CoordinateSystemTest, Pose3dEDNtoNWU) {
  // No rotation from EDN to NWU
-  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());
+  CheckPose3dConvert(
+      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());

  // 45° roll from EDN to NWU
-  CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}},
-               Pose3d{3_m, -1_m, -2_m, Rotation3d{-45_deg, 0_deg, -90_deg}},
-               CoordinateSystem::EDN(), CoordinateSystem::NWU());
+  CheckPose3dConvert(
+      Pose3d{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}},
+      Pose3d{3_m, -1_m, -2_m, Rotation3d{-45_deg, 0_deg, -90_deg}},
+      CoordinateSystem::EDN(), CoordinateSystem::NWU());

  // 45° pitch from EDN to NWU
-  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());
+  CheckPose3dConvert(
+      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());

  // 45° yaw from EDN to NWU
-  CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}},
-               Pose3d{3_m, -1_m, -2_m, Rotation3d{-90_deg, 45_deg, -90_deg}},
-               CoordinateSystem::EDN(), CoordinateSystem::NWU());
+  CheckPose3dConvert(
+      Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}},
+      Pose3d{3_m, -1_m, -2_m, Rotation3d{-90_deg, 45_deg, -90_deg}},
+      CoordinateSystem::EDN(), CoordinateSystem::NWU());
 }

-TEST(CoordinateSystemTest, EDNtoNED) {
+TEST(CoordinateSystemTest, Pose3dEDNtoNED) {
  // No rotation from EDN to NED
-  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());
+  CheckPose3dConvert(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());

  // 45° roll from EDN to NED
-  CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}},
-               Pose3d{3_m, 1_m, 2_m, Rotation3d{135_deg, 0_deg, 90_deg}},
-               CoordinateSystem::EDN(), CoordinateSystem::NED());
+  CheckPose3dConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}},
+                     Pose3d{3_m, 1_m, 2_m, Rotation3d{135_deg, 0_deg, 90_deg}},
+                     CoordinateSystem::EDN(), CoordinateSystem::NED());

  // 45° pitch from EDN to NED
-  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());
+  CheckPose3dConvert(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());

  // 45° yaw from EDN to NED
-  CheckConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}},
-               Pose3d{3_m, 1_m, 2_m, Rotation3d{90_deg, -45_deg, 90_deg}},
-               CoordinateSystem::EDN(), CoordinateSystem::NED());
+  CheckPose3dConvert(Pose3d{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}},
+                     Pose3d{3_m, 1_m, 2_m, Rotation3d{90_deg, -45_deg, 90_deg}},
+                     CoordinateSystem::EDN(), CoordinateSystem::NED());
+}
+
+TEST(CoordinateSystemTest, Transform3dEDNtoNWU) {
+  // No rotation from EDN to NWU
+  CheckTransform3dConvert(
+      Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{}},
+      Transform3d{Translation3d{3_m, -1_m, -2_m},
+                  Rotation3d{-90_deg, 0_deg, -90_deg}},
+      CoordinateSystem::EDN(), CoordinateSystem::NWU());
+
+  // 45° roll from EDN to NWU
+  CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
+                                      Rotation3d{45_deg, 0_deg, 0_deg}},
+                          Transform3d{Translation3d{3_m, -1_m, -2_m},
+                                      Rotation3d{-45_deg, 0_deg, -90_deg}},
+                          CoordinateSystem::EDN(), CoordinateSystem::NWU());
+
+  // 45° pitch from EDN to NWU
+  CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
+                                      Rotation3d{0_deg, 45_deg, 0_deg}},
+                          Transform3d{Translation3d{3_m, -1_m, -2_m},
+                                      Rotation3d{-90_deg, 0_deg, -135_deg}},
+                          CoordinateSystem::EDN(), CoordinateSystem::NWU());
+
+  // 45° yaw from EDN to NWU
+  CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
+                                      Rotation3d{0_deg, 0_deg, 45_deg}},
+                          Transform3d{Translation3d{3_m, -1_m, -2_m},
+                                      Rotation3d{-90_deg, 45_deg, -90_deg}},
+                          CoordinateSystem::EDN(), CoordinateSystem::NWU());
+}
+
+TEST(CoordinateSystemTest, Transform3dEDNtoNED) {
+  // No rotation from EDN to NED
+  CheckTransform3dConvert(
+      Transform3d{Translation3d{1_m, 2_m, 3_m}, Rotation3d{}},
+      Transform3d{Translation3d{3_m, 1_m, 2_m},
+                  Rotation3d{90_deg, 0_deg, 90_deg}},
+      CoordinateSystem::EDN(), CoordinateSystem::NED());
+
+  // 45° roll from EDN to NED
+  CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
+                                      Rotation3d{45_deg, 0_deg, 0_deg}},
+                          Transform3d{Translation3d{3_m, 1_m, 2_m},
+                                      Rotation3d{135_deg, 0_deg, 90_deg}},
+                          CoordinateSystem::EDN(), CoordinateSystem::NED());
+
+  // 45° pitch from EDN to NED
+  CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
+                                      Rotation3d{0_deg, 45_deg, 0_deg}},
+                          Transform3d{Translation3d{3_m, 1_m, 2_m},
+                                      Rotation3d{90_deg, 0_deg, 135_deg}},
+                          CoordinateSystem::EDN(), CoordinateSystem::NED());
+
+  // 45° yaw from EDN to NED
+  CheckTransform3dConvert(Transform3d{Translation3d{1_m, 2_m, 3_m},
+                                      Rotation3d{0_deg, 0_deg, 45_deg}},
+                          Transform3d{Translation3d{3_m, 1_m, 2_m},
+                                      Rotation3d{90_deg, -45_deg, 90_deg}},
+                          CoordinateSystem::EDN(), CoordinateSystem::NED());
 }