From b33715db152ef11fa6ff6fe225fcdcf05ac971ad Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 7 May 2022 10:25:19 -0700 Subject: [PATCH] [wpimath] Add CoordinateSystem class (#4214) --- .../first/math/geometry/CoordinateAxis.java | 86 ++++++++++++ .../first/math/geometry/CoordinateSystem.java | 121 +++++++++++++++++ .../native/cpp/geometry/CoordinateAxis.cpp | 35 +++++ .../native/cpp/geometry/CoordinateSystem.cpp | 92 +++++++++++++ .../include/frc/geometry/CoordinateAxis.h | 73 ++++++++++ .../include/frc/geometry/CoordinateSystem.h | 71 ++++++++++ .../math/geometry/CoordinateSystemTest.java | 128 ++++++++++++++++++ .../cpp/geometry/CoordinateSystemTest.cpp | 77 +++++++++++ 8 files changed, 683 insertions(+) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java create mode 100644 wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java create mode 100644 wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp create mode 100644 wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp create mode 100644 wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h create mode 100644 wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h create mode 100644 wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java create mode 100644 wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java new file mode 100644 index 0000000000..19f4a3f083 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateAxis.java @@ -0,0 +1,86 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; +import edu.wpi.first.math.numbers.N3; + +/** A class representing a coordinate system axis within the NWU coordinate system. */ +public class CoordinateAxis { + final Vector m_axis; + + /** + * Constructs a coordinate system axis within the NWU coordinate system and normalizes it. + * + * @param x The x component. + * @param y The y component. + * @param z The z component. + */ + public CoordinateAxis(double x, double y, double z) { + double norm = Math.sqrt(x * x + y * y + z * z); + m_axis = VecBuilder.fill(x / norm, y / norm, z / norm); + } + + /** + * Returns a coordinate axis corresponding to +X in the NWU coordinate system. + * + * @return A coordinate axis corresponding to +X in the NWU coordinate system. + */ + @SuppressWarnings("MethodName") + public static CoordinateAxis N() { + return new CoordinateAxis(1.0, 0.0, 0.0); + } + + /** + * Returns a coordinate axis corresponding to -X in the NWU coordinate system. + * + * @return A coordinate axis corresponding to -X in the NWU coordinate system. + */ + @SuppressWarnings("MethodName") + public static CoordinateAxis S() { + return new CoordinateAxis(-1.0, 0.0, 0.0); + } + + /** + * Returns a coordinate axis corresponding to -Y in the NWU coordinate system. + * + * @return A coordinate axis corresponding to -Y in the NWU coordinate system. + */ + @SuppressWarnings("MethodName") + public static CoordinateAxis E() { + return new CoordinateAxis(0.0, -1.0, 0.0); + } + + /** + * Returns a coordinate axis corresponding to +Y in the NWU coordinate system. + * + * @return A coordinate axis corresponding to +Y in the NWU coordinate system. + */ + @SuppressWarnings("MethodName") + public static CoordinateAxis W() { + return new CoordinateAxis(0.0, 1.0, 0.0); + } + + /** + * Returns a coordinate axis corresponding to +Z in the NWU coordinate system. + * + * @return A coordinate axis corresponding to +Z in the NWU coordinate system. + */ + @SuppressWarnings("MethodName") + public static CoordinateAxis U() { + return new CoordinateAxis(0.0, 0.0, 1.0); + } + + /** + * Returns a coordinate axis corresponding to -Z in the NWU coordinate system. + * + * @return A coordinate axis corresponding to -Z in the NWU coordinate system. + */ + @SuppressWarnings("MethodName") + public static CoordinateAxis D() { + return new CoordinateAxis(0.0, 0.0, -1.0); + } +} 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 new file mode 100644 index 0000000000..93b57312b0 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/CoordinateSystem.java @@ -0,0 +1,121 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; + +/** A helper class that converts Pose3d objects between different standard coordinate frames. */ +public class CoordinateSystem { + // Rotation from this coordinate system to NWU coordinate system + private final Rotation3d m_rotation; + + /** + * Constructs a coordinate system with the given cardinal directions for each axis. + * + * @param positiveX The cardinal direction of the positive x-axis. + * @param positiveY The cardinal direction of the positive y-axis. + * @param positiveZ The cardinal direction of the positive z-axis. + * @throws IllegalArgumentException if the coordinate system isn't special orthogonal + */ + public CoordinateSystem( + CoordinateAxis positiveX, CoordinateAxis positiveY, CoordinateAxis positiveZ) { + // Construct a change of basis matrix from the source coordinate system to the + // NWU coordinate system. Each column vector in the change of basis matrix is + // one of the old basis vectors mapped to its representation in the new basis. + @SuppressWarnings("LocalVariableName") + var R = new Matrix<>(Nat.N3(), Nat.N3()); + R.assignBlock(0, 0, positiveX.m_axis); + R.assignBlock(0, 1, positiveY.m_axis); + 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 (!R.times(R.transpose()).equals(Matrix.eye(Nat.N3()))) { + throw new IllegalArgumentException("Coordinate system isn't special orthogonal"); + } + + // Turn change of basis matrix into a quaternion since it's a pure rotation + // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ + double trace = R.get(0, 0) + R.get(1, 1) + R.get(2, 2); + double w; + double x; + double y; + double z; + + if (trace > 0.0) { + double s = 0.5 / Math.sqrt(trace + 1.0); + w = 0.25 / s; + x = (R.get(2, 1) - R.get(1, 2)) * s; + y = (R.get(0, 2) - R.get(2, 0)) * s; + z = (R.get(1, 0) - R.get(0, 1)) * s; + } else { + if (R.get(0, 0) > R.get(1, 1) && R.get(0, 0) > R.get(2, 2)) { + double s = 2.0 * Math.sqrt(1.0 + R.get(0, 0) - R.get(1, 1) - R.get(2, 2)); + w = (R.get(2, 1) - R.get(1, 2)) / s; + x = 0.25 * s; + y = (R.get(0, 1) + R.get(1, 0)) / s; + z = (R.get(0, 2) + R.get(2, 0)) / s; + } else if (R.get(1, 1) > R.get(2, 2)) { + double s = 2.0 * Math.sqrt(1.0 + R.get(1, 1) - R.get(0, 0) - R.get(2, 2)); + w = (R.get(0, 2) - R.get(2, 0)) / s; + x = (R.get(0, 1) + R.get(1, 0)) / s; + y = 0.25 * s; + z = (R.get(1, 2) + R.get(2, 1)) / s; + } else { + double s = 2.0 * Math.sqrt(1.0 + R.get(2, 2) - R.get(0, 0) - R.get(1, 1)); + w = (R.get(1, 0) - R.get(0, 1)) / s; + x = (R.get(0, 2) + R.get(2, 0)) / s; + y = (R.get(1, 2) + R.get(2, 1)) / s; + z = 0.25 * s; + } + } + + m_rotation = new Rotation3d(new Quaternion(w, x, y, z)); + } + + /** + * Returns an instance of the NWU coordinate system. + * + * @return An instance of the NWU coordinate system. + */ + @SuppressWarnings("MethodName") + public static CoordinateSystem NWU() { + return new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.W(), CoordinateAxis.U()); + } + + /** + * Returns an instance of the EDN coordinate system. + * + * @return An instance of the EDN coordinate system. + */ + @SuppressWarnings("MethodName") + public static CoordinateSystem EDN() { + return new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.D(), CoordinateAxis.N()); + } + + /** + * Returns an instance of the NED coordinate system. + * + * @return An instance of the NED coordinate system. + */ + @SuppressWarnings("MethodName") + public static CoordinateSystem NED() { + return new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.D()); + } + + /** + * Converts the given pose from one coordinate system to another. + * + * @param pose The pose to convert. + * @param from The coordinate system the pose starts in. + * @param to The coordinate system to which to convert. + * @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))); + } +} diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp new file mode 100644 index 0000000000..b180b3c7ba --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/CoordinateAxis.cpp @@ -0,0 +1,35 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/geometry/CoordinateAxis.h" + +using namespace frc; + +CoordinateAxis::CoordinateAxis(double x, double y, double z) : m_axis{x, y, z} { + m_axis /= m_axis.norm(); +} + +CoordinateAxis CoordinateAxis::N() { + return CoordinateAxis{1.0, 0.0, 0.0}; +} + +CoordinateAxis CoordinateAxis::S() { + return CoordinateAxis{-1.0, 0.0, 0.0}; +} + +CoordinateAxis CoordinateAxis::E() { + return CoordinateAxis{0.0, -1.0, 0.0}; +} + +CoordinateAxis CoordinateAxis::W() { + return CoordinateAxis{0.0, 1.0, 0.0}; +} + +CoordinateAxis CoordinateAxis::U() { + return CoordinateAxis{0.0, 0.0, 1.0}; +} + +CoordinateAxis CoordinateAxis::D() { + return CoordinateAxis{0.0, 0.0, -1.0}; +} diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp new file mode 100644 index 0000000000..c05f927d4c --- /dev/null +++ b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp @@ -0,0 +1,92 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/geometry/CoordinateSystem.h" + +#include +#include +#include + +#include "Eigen/QR" + +using namespace frc; + +CoordinateSystem::CoordinateSystem(const CoordinateAxis& positiveX, + const CoordinateAxis& positiveY, + const CoordinateAxis& positiveZ) { + // Construct a change of basis matrix from the source coordinate system to the + // NWU coordinate system. Each column vector in the change of basis matrix is + // one of the old basis vectors mapped to its representation in the new basis. + Matrixd<3, 3> R; + R.block<3, 1>(0, 0) = positiveX.m_axis; + R.block<3, 1>(0, 1) = positiveY.m_axis; + 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 (R * R.transpose() != Matrixd<3, 3>::Identity()) { + throw std::domain_error("Coordinate system isn't special orthogonal"); + } + + // Turn change of basis matrix into a quaternion since it's a pure rotation + // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ + double trace = R(0, 0) + R(1, 1) + R(2, 2); + double w; + double x; + double y; + double z; + + if (trace > 0.0) { + double s = 0.5 / std::sqrt(trace + 1.0); + w = 0.25 / s; + x = (R(2, 1) - R(1, 2)) * s; + y = (R(0, 2) - R(2, 0)) * s; + z = (R(1, 0) - R(0, 1)) * s; + } else { + if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) { + double s = 2.0 * std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2)); + w = (R(2, 1) - R(1, 2)) / s; + x = 0.25 * s; + y = (R(0, 1) + R(1, 0)) / s; + z = (R(0, 2) + R(2, 0)) / s; + } else if (R(1, 1) > R(2, 2)) { + double s = 2.0 * std::sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2)); + w = (R(0, 2) - R(2, 0)) / s; + x = (R(0, 1) + R(1, 0)) / s; + y = 0.25 * s; + z = (R(1, 2) + R(2, 1)) / s; + } else { + double s = 2.0 * std::sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1)); + w = (R(1, 0) - R(0, 1)) / s; + x = (R(0, 2) + R(2, 0)) / s; + y = (R(1, 2) + R(2, 1)) / s; + z = 0.25 * s; + } + } + + m_rotation = Rotation3d{Quaternion{w, x, y, z}}; +} + +CoordinateSystem CoordinateSystem::NWU() { + return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::W(), + CoordinateAxis::U()}; +} + +CoordinateSystem CoordinateSystem::EDN() { + return CoordinateSystem{CoordinateAxis::E(), CoordinateAxis::D(), + CoordinateAxis::N()}; +} + +CoordinateSystem CoordinateSystem::NED() { + return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::E(), + CoordinateAxis::D()}; +} + +Pose3d CoordinateSystem::Convert(const Pose3d& pose, + const CoordinateSystem& from, + const CoordinateSystem& to) { + return pose.RelativeTo( + Pose3d{Translation3d{}, to.m_rotation - from.m_rotation}); +} diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h new file mode 100644 index 0000000000..1ac1a2c07a --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/CoordinateAxis.h @@ -0,0 +1,73 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "frc/EigenCore.h" +#include "frc/geometry/Pose3d.h" +#include "frc/geometry/Rotation3d.h" + +namespace frc { + +/** + * A class representing a coordinate system axis within the NWU coordinate + * system. + */ +class WPILIB_DLLEXPORT CoordinateAxis { + public: + /** + * Constructs a coordinate system axis within the NWU coordinate system and + * normalizes it. + * + * @param x The x component. + * @param y The y component. + * @param z The z component. + */ + CoordinateAxis(double x, double y, double z); + + CoordinateAxis(const CoordinateAxis&) = default; + CoordinateAxis& operator=(const CoordinateAxis&) = default; + + CoordinateAxis(CoordinateAxis&&) = default; + CoordinateAxis& operator=(CoordinateAxis&&) = default; + + /** + * Returns a coordinate axis corresponding to +X in the NWU coordinate system. + */ + static CoordinateAxis N(); + + /** + * Returns a coordinate axis corresponding to -X in the NWU coordinate system. + */ + static CoordinateAxis S(); + + /** + * Returns a coordinate axis corresponding to -Y in the NWU coordinate system. + */ + static CoordinateAxis E(); + + /** + * Returns a coordinate axis corresponding to +Y in the NWU coordinate system. + */ + static CoordinateAxis W(); + + /** + * Returns a coordinate axis corresponding to +Z in the NWU coordinate system. + */ + static CoordinateAxis U(); + + /** + * Returns a coordinate axis corresponding to -Z in the NWU coordinate system. + */ + static CoordinateAxis D(); + + private: + friend class CoordinateSystem; + + Vectord<3> m_axis; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h new file mode 100644 index 0000000000..9eab85e3b7 --- /dev/null +++ b/wpimath/src/main/native/include/frc/geometry/CoordinateSystem.h @@ -0,0 +1,71 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "frc/EigenCore.h" +#include "frc/geometry/CoordinateAxis.h" +#include "frc/geometry/Pose3d.h" +#include "frc/geometry/Rotation3d.h" + +namespace frc { + +/** + * A helper class that converts Pose3d objects between different standard + * coordinate frames. + */ +class WPILIB_DLLEXPORT CoordinateSystem { + public: + /** + * Constructs a coordinate system with the given cardinal directions for each + * axis. + * + * @param positiveX The cardinal direction of the positive x-axis. + * @param positiveY The cardinal direction of the positive y-axis. + * @param positiveZ The cardinal direction of the positive z-axis. + * @throws std::domain_error if the coordinate system isn't special orthogonal + */ + CoordinateSystem(const CoordinateAxis& positiveX, + const CoordinateAxis& positiveY, + const CoordinateAxis& positiveZ); + + CoordinateSystem(const CoordinateSystem&) = default; + CoordinateSystem& operator=(const CoordinateSystem&) = default; + CoordinateSystem(CoordinateSystem&&) = default; + CoordinateSystem& operator=(CoordinateSystem&&) = default; + + /** + * Returns an instance of the NWU coordinate system. + */ + static CoordinateSystem NWU(); + + /** + * Returns an instance of the EDN coordinate system. + */ + static CoordinateSystem EDN(); + + /** + * Returns an instance of the NED coordinate system. + */ + static CoordinateSystem NED(); + + /** + * Converts the given pose from one coordinate system to another. + * + * @param pose The pose to convert. + * @param from The coordinate system the pose starts in. + * @param to The coordinate system to which to convert. + * @return The given pose in the desired coordinate system. + */ + static Pose3d Convert(const Pose3d& pose, const CoordinateSystem& from, + const CoordinateSystem& to); + + private: + // Rotation from this coordinate system to NWU coordinate system + Rotation3d m_rotation; +}; + +} // namespace frc 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 new file mode 100644 index 0000000000..6b06f0789f --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java @@ -0,0 +1,128 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.geometry; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import edu.wpi.first.math.util.Units; +import org.junit.jupiter.api.Test; + +class CoordinateSystemTest { + @Test + void testEDNtoNWU() { + var edn1 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d()); + var nwu1 = + new Pose3d( + 3.0, + -1.0, + -2.0, + new Rotation3d(-Units.degreesToRadians(90.0), 0.0, -Units.degreesToRadians(90.0))); + + 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 = + new Pose3d( + 3.0, + -1.0, + -2.0, + new Rotation3d(-Units.degreesToRadians(45.0), 0.0, -Units.degreesToRadians(90.0))); + + 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 = + new Pose3d( + 3.0, + -1.0, + -2.0, + new Rotation3d(-Units.degreesToRadians(90.0), 0.0, -Units.degreesToRadians(135.0))); + + 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 = + new Pose3d( + 3.0, + -1.0, + -2.0, + new Rotation3d( + -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())); + } + + @Test + void testEDNtoNED() { + var edn1 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d()); + var ned1 = + new Pose3d( + 3.0, + 1.0, + 2.0, + new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0))); + + 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 = + new Pose3d( + 3.0, + 1.0, + 2.0, + new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0))); + + 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 = + new Pose3d( + 3.0, + 1.0, + 2.0, + new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0))); + + 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 = + 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())); + } +} diff --git a/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp new file mode 100644 index 0000000000..3783a8b15d --- /dev/null +++ b/wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp @@ -0,0 +1,77 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/geometry/CoordinateSystem.h" +#include "frc/geometry/Pose3d.h" +#include "gtest/gtest.h" + +using namespace frc; + +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}}; + + EXPECT_EQ(nwu1, CoordinateSystem::Convert(edn1, CoordinateSystem::EDN(), + CoordinateSystem::NWU())); + EXPECT_EQ(edn1, CoordinateSystem::Convert(nwu1, CoordinateSystem::NWU(), + CoordinateSystem::EDN())); + + 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}}; + + 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())); +} + +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}}; + + EXPECT_EQ(ned1, CoordinateSystem::Convert(edn1, CoordinateSystem::EDN(), + CoordinateSystem::NED())); + EXPECT_EQ(edn1, CoordinateSystem::Convert(ned1, CoordinateSystem::NED(), + CoordinateSystem::EDN())); + + 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}}; + + 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())); +}