From b53b3526a27877d71883fe944e89577a08f9e0ff Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 1 Oct 2022 21:09:04 -0700 Subject: [PATCH] [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. --- .../first/math/geometry/CoordinateSystem.java | 17 +- .../native/cpp/geometry/CoordinateSystem.cpp | 11 +- .../include/frc/geometry/CoordinateSystem.h | 12 ++ .../math/geometry/CoordinateSystemTest.java | 142 +++++++++++++++-- .../cpp/geometry/CoordinateSystemTest.cpp | 148 ++++++++++++++---- 5 files changed, 287 insertions(+), 43 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java index 57c64ec190..57331771df 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java @@ -110,6 +110,21 @@ public class CoordinateSystem { * @return The given pose in the desired coordinate system. */ public static Pose3d convert(Pose3d pose, CoordinateSystem from, CoordinateSystem to) { - return pose.relativeTo(new Pose3d(new Translation3d(), to.m_rotation.minus(from.m_rotation))); + return new Pose3d( + convert(pose.getTranslation(), from, to), convert(pose.getRotation(), from, to)); + } + + /** + * Converts the given transform from one coordinate system to another. + * + * @param transform The transform to convert. + * @param from The coordinate system the transform starts in. + * @param to The coordinate system to which to convert. + * @return The given transform in the desired coordinate system. + */ + public static Transform3d convert( + Transform3d transform, CoordinateSystem from, CoordinateSystem to) { + return new Transform3d( + convert(transform.getTranslation(), from, to), convert(transform.getRotation(), from, to)); } } diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp index 320f60a9ae..d1ee93d417 100644 --- a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp +++ b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp @@ -61,6 +61,13 @@ Rotation3d CoordinateSystem::Convert(const Rotation3d& rotation, Pose3d CoordinateSystem::Convert(const Pose3d& pose, const CoordinateSystem& from, const CoordinateSystem& to) { - return pose.RelativeTo( - Pose3d{Translation3d{}, to.m_rotation - from.m_rotation}); + return Pose3d{Convert(pose.Translation(), from, to), + Convert(pose.Rotation(), from, to)}; +} + +Transform3d CoordinateSystem::Convert(const Transform3d& transform, + const CoordinateSystem& from, + const CoordinateSystem& to) { + return Transform3d{Convert(transform.Translation(), from, to), + Convert(transform.Rotation(), from, to)}; } diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h index e2643ac457..232455ff86 100644 --- a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h +++ b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h @@ -94,6 +94,18 @@ class WPILIB_DLLEXPORT CoordinateSystem { static Pose3d Convert(const Pose3d& pose, const CoordinateSystem& from, const CoordinateSystem& to); + /** + * Converts the given transform from one coordinate system to another. + * + * @param transform The transform to convert. + * @param from The coordinate system the transform starts in. + * @param to The coordinate system to which to convert. + * @return The given transform in the desired coordinate system. + */ + static Transform3d Convert(const Transform3d& transform, + const CoordinateSystem& from, + const CoordinateSystem& to); + private: // Rotation from this coordinate system to NWU coordinate system Rotation3d m_rotation; diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java index cd19fbb05c..68babdd813 100644 --- 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()); + } } diff --git a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp index e28805c195..fc44fa52ed 100644 --- 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()); }