From 0d9956273cbfa28a6d59102012b9bfa08db80e82 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 18 May 2022 20:41:15 -0700 Subject: [PATCH] [wpimath] Add CoordinateSystem.convert() translation and rotation overloads (#4227) --- .../first/math/geometry/CoordinateSystem.java | 30 +++- .../native/cpp/geometry/CoordinateSystem.cpp | 16 +- .../include/frc/geometry/CoordinateSystem.h | 25 ++++ .../math/geometry/CoordinateSystemTest.java | 137 +++++++++--------- .../cpp/geometry/CoordinateSystemTest.cpp | 108 +++++++------- 5 files changed, 190 insertions(+), 126 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 449ba7438e..b30b5dfff2 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 @@ -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 - // normalizes itself, so we just need to check for orthogonality. + // if the axes used are orthogonal and normalized. The CoordinateAxis class + // 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. * diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp index f4d200c92e..eaedfc608a 100644 --- a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp +++ b/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 - // normalizes itself, so we just need to check for orthogonality. + // if the axes used are orthogonal and normalized. The CoordinateAxis class + // 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) { diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h index 1062aa5eae..e2643ac457 100644 --- a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h +++ b/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. * 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 6b06f0789f..ea06604cb7 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,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()); - var nwu1 = + // No rotation from EDN to NWU + 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( - nwu1, CoordinateSystem.convert(edn1, CoordinateSystem.EDN(), CoordinateSystem.NWU())); - assertEquals( - 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 = + // 45° roll from EDN to NWU + checkConvert( + new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)), 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( - nwu2, CoordinateSystem.convert(edn2, CoordinateSystem.EDN(), CoordinateSystem.NWU())); - assertEquals( - 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 = + // 45° pitch from EDN to NWU + checkConvert( + new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)), 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( - nwu3, CoordinateSystem.convert(edn3, CoordinateSystem.EDN(), CoordinateSystem.NWU())); - assertEquals( - 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 = + // 45° yaw from EDN to NWU + checkConvert( + new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))), 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))); - - assertEquals( - nwu4, CoordinateSystem.convert(edn4, CoordinateSystem.EDN(), CoordinateSystem.NWU())); - assertEquals( - edn4, CoordinateSystem.convert(nwu4, CoordinateSystem.NWU(), CoordinateSystem.EDN())); + Units.degreesToRadians(-90.0))), + CoordinateSystem.EDN(), + CoordinateSystem.NWU()); } + @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert") @Test void testEDNtoNED() { - var edn1 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d()); - var ned1 = + // No rotation from EDN to NED + 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( - ned1, CoordinateSystem.convert(edn1, CoordinateSystem.EDN(), CoordinateSystem.NED())); - assertEquals( - 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 = + // 45° roll from EDN to NED + checkConvert( + new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0)), 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( - ned2, CoordinateSystem.convert(edn2, CoordinateSystem.EDN(), CoordinateSystem.NED())); - assertEquals( - 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 = + // 45° pitch from EDN to NED + checkConvert( + new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0)), 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( - ned3, CoordinateSystem.convert(edn3, CoordinateSystem.EDN(), CoordinateSystem.NED())); - assertEquals( - 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 = + // 45° yaw from EDN to NED + checkConvert( + new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0))), new Pose3d( 3.0, 1.0, 2.0, new Rotation3d( Units.degreesToRadians(90.0), - -Units.degreesToRadians(45.0), - Units.degreesToRadians(90.0))); - - assertEquals( - ned4, CoordinateSystem.convert(edn4, CoordinateSystem.EDN(), CoordinateSystem.NED())); - assertEquals( - edn4, CoordinateSystem.convert(ned4, CoordinateSystem.NED(), CoordinateSystem.EDN())); + 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 3783a8b15d..e28805c195 100644 --- a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp +++ b/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{}}; - Pose3d nwu1{3_m, -1_m, -2_m, Rotation3d{-90_deg, 0_deg, -90_deg}}; + // 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()); - EXPECT_EQ(nwu1, CoordinateSystem::Convert(edn1, CoordinateSystem::EDN(), - CoordinateSystem::NWU())); - EXPECT_EQ(edn1, CoordinateSystem::Convert(nwu1, CoordinateSystem::NWU(), - CoordinateSystem::EDN())); + // 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()); - Pose3d edn2{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}}; - Pose3d nwu2{3_m, -1_m, -2_m, Rotation3d{-45_deg, 0_deg, -90_deg}}; + // 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()); - EXPECT_EQ(nwu2, CoordinateSystem::Convert(edn2, CoordinateSystem::EDN(), - CoordinateSystem::NWU())); - EXPECT_EQ(edn2, CoordinateSystem::Convert(nwu2, CoordinateSystem::NWU(), - CoordinateSystem::EDN())); - - 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())); + // 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()); } TEST(CoordinateSystemTest, EDNtoNED) { - Pose3d edn1{1_m, 2_m, 3_m, Rotation3d{}}; - Pose3d ned1{3_m, 1_m, 2_m, Rotation3d{90_deg, 0_deg, 90_deg}}; + // 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()); - EXPECT_EQ(ned1, CoordinateSystem::Convert(edn1, CoordinateSystem::EDN(), - CoordinateSystem::NED())); - EXPECT_EQ(edn1, CoordinateSystem::Convert(ned1, CoordinateSystem::NED(), - CoordinateSystem::EDN())); + // 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()); - Pose3d edn2{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}}; - Pose3d ned2{3_m, 1_m, 2_m, Rotation3d{135_deg, 0_deg, 90_deg}}; + // 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()); - EXPECT_EQ(ned2, CoordinateSystem::Convert(edn2, CoordinateSystem::EDN(), - CoordinateSystem::NED())); - EXPECT_EQ(edn2, CoordinateSystem::Convert(ned2, CoordinateSystem::NED(), - CoordinateSystem::EDN())); - - 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())); + // 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()); }