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 b30b5dfff2..82931d522b 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 @@ -38,50 +38,9 @@ public class CoordinateSystem { 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 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"); - } - - // 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)); + // The change of basis matrix should be a pure rotation. The Rotation3d + // constructor will verify this by checking for special orthogonality. + m_rotation = new Rotation3d(R); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 6daab9d8a7..4aed0ad162 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -13,7 +13,9 @@ import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.util.Units; import java.util.Objects; -/** A rotation in a 2D coordinate frame represented a point on the unit circle (cosine and sine). */ +/** + * A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine). + */ @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Rotation2d implements Interpolatable { diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index 957129481f..c3dc7b796a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -5,7 +5,9 @@ package edu.wpi.first.math.geometry; import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.Vector; @@ -14,7 +16,7 @@ import edu.wpi.first.math.numbers.N3; import java.util.Objects; import org.ejml.dense.row.factory.DecompositionFactory_DDRM; -/** A rotation in a 3D coordinate. */ +/** A rotation in a 3D coordinate frame represented by a quaternion. */ public class Rotation3d implements Interpolatable { private Quaternion m_q = new Quaternion(); @@ -77,6 +79,74 @@ public class Rotation3d implements Interpolatable { m_q = new Quaternion(Math.cos(angleRadians / 2.0), v.get(0, 0), v.get(1, 0), v.get(2, 0)); } + /** + * Constructs a Rotation3d from a rotation matrix. + * + * @param rotationMatrix The rotation matrix. + * @throws IllegalArgumentException if the rotation matrix isn't special orthogonal. + */ + public Rotation3d(Matrix rotationMatrix) { + final var R = rotationMatrix; + + // Require that the rotation matrix is special orthogonal. This is true if + // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1). + if (!R.times(R.transpose()).equals(Matrix.eye(Nat.N3()))) { + var builder = new StringBuilder("Rotation matrix isn't orthogonal\n\nR =\n"); + builder.append(R.getStorage().toString()).append('\n'); + + var msg = builder.toString(); + MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace()); + throw new IllegalArgumentException(msg); + } + if (R.det() != 1.0) { + var builder = + new StringBuilder("Rotation matrix is orthogonal but not special orthogonal\n\nR =\n"); + builder.append(R.getStorage().toString()).append('\n'); + + var msg = builder.toString(); + MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace()); + throw new IllegalArgumentException(msg); + } + + // Turn rotation matrix into a quaternion + // 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_q = new Quaternion(w, x, y, z); + } + /** * Constructs a Rotation3d that rotates the initial vector onto the final vector. * diff --git a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp index eaedfc608a..320f60a9ae 100644 --- a/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp +++ b/wpimath/src/main/native/cpp/geometry/CoordinateSystem.cpp @@ -23,50 +23,9 @@ CoordinateSystem::CoordinateSystem(const CoordinateAxis& positiveX, 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 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"); - } - - // 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}}; + // The change of basis matrix should be a pure rotation. The Rotation3d + // constructor will verify this by checking for special orthogonality. + m_rotation = Rotation3d{R}; } const CoordinateSystem& CoordinateSystem::NWU() { diff --git a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp index 77edf2d0dd..81bb8ab240 100644 --- a/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp +++ b/wpimath/src/main/native/cpp/geometry/Rotation3d.cpp @@ -9,8 +9,11 @@ #include #include "Eigen/Core" +#include "Eigen/LU" #include "Eigen/QR" +#include "frc/fmt/Eigen.h" #include "units/math.h" +#include "wpimath/MathShared.h" using namespace frc; @@ -45,6 +48,66 @@ Rotation3d::Rotation3d(const Vectord<3>& axis, units::radian_t angle) { m_q = Quaternion{units::math::cos(angle / 2.0), v(0), v(1), v(2)}; } +Rotation3d::Rotation3d(const Matrixd<3, 3>& rotationMatrix) { + const auto& R = rotationMatrix; + + // Require that the rotation matrix is special orthogonal. This is true if the + // matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1). + if (R * R.transpose() != Matrixd<3, 3>::Identity()) { + std::string msg = + fmt::format("Rotation matrix isn't orthogonal\n\nR =\n{}\n", R); + + wpi::math::MathSharedStore::ReportError(msg); + throw std::domain_error(msg); + } + if (R.determinant() != 1.0) { + std::string msg = fmt::format( + "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n{}\n", + R); + + wpi::math::MathSharedStore::ReportError(msg); + throw std::domain_error(msg); + } + + // Turn rotation matrix into a quaternion + // 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_q = Quaternion{w, x, y, z}; +} + Rotation3d::Rotation3d(const Vectord<3>& initial, const Vectord<3>& final) { double dot = initial.dot(final); double normProduct = initial.norm() * final.norm(); diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc index 840791f866..87d37ec425 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc +++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.inc @@ -4,8 +4,6 @@ #pragma once -#include - #include #include @@ -14,6 +12,7 @@ #include "drake/math/discrete_algebraic_riccati_equation.h" #include "frc/StateSpaceUtil.h" #include "frc/controller/LinearQuadraticRegulator.h" +#include "frc/fmt/Eigen.h" #include "frc/system/Discretization.h" #include "unsupported/Eigen/MatrixFunctions" #include "wpimath/MathShared.h" diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index aada0e25f2..ee5aeca8fc 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -14,7 +14,7 @@ namespace frc { /** - * A rotation in a 3D coordinate frame. + * A rotation in a 3D coordinate frame represented by a quaternion. */ class WPILIB_DLLEXPORT Rotation3d { public: @@ -51,6 +51,14 @@ class WPILIB_DLLEXPORT Rotation3d { */ Rotation3d(const Vectord<3>& axis, units::radian_t angle); + /** + * Constructs a Rotation3d from a rotation matrix. + * + * @param rotationMatrix The rotation matrix. + * @throws std::domain_error if the rotation matrix isn't special orthogonal. + */ + explicit Rotation3d(const Matrixd<3, 3>& rotationMatrix); + /** * Constructs a Rotation3d that rotates the initial vector onto the final * vector. diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java index e9ad4cddc6..c24179e5b4 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java @@ -7,7 +7,11 @@ package edu.wpi.first.math.geometry; import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertNotEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.util.Units; import org.junit.jupiter.api.Test; @@ -36,6 +40,32 @@ class Rotation3dTest { assertEquals(rot5, rot6); } + @Test + void testInitRotationMatrix() { + // No rotation + final var R1 = Matrix.eye(Nat.N3()); + final var rot1 = new Rotation3d(R1); + assertEquals(new Rotation3d(), rot1); + + // 90 degree CCW rotation around z-axis + final var R2 = new Matrix<>(Nat.N3(), Nat.N3()); + R2.assignBlock(0, 0, VecBuilder.fill(0.0, 1.0, 0.0)); + R2.assignBlock(0, 1, VecBuilder.fill(-1.0, 0.0, 0.0)); + R2.assignBlock(0, 2, VecBuilder.fill(0.0, 0.0, 1.0)); + final var rot2 = new Rotation3d(R2); + final var expected2 = new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0)); + assertEquals(expected2, rot2); + + // Matrix that isn't orthogonal + final var R3 = + new MatBuilder<>(Nat.N3(), Nat.N3()).fill(1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0); + assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R3)); + + // Matrix that's orthogonal but not special orthogonal + final var R4 = Matrix.eye(Nat.N3()).times(2.0); + assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R4)); + } + @Test void testInitTwoVector() { @SuppressWarnings("LocalVariableName") diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp index a92b7387a5..e37abeb2f4 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp @@ -7,6 +7,7 @@ #include #include +#include "frc/EigenCore.h" #include "frc/geometry/Rotation3d.h" #include "gtest/gtest.h" @@ -29,6 +30,30 @@ TEST(Rotation3dTest, InitAxisAngleAndRollPitchYaw) { EXPECT_EQ(rot5, rot6); } +TEST(Rotation3dTest, InitRotationMatrix) { + // No rotation + const Matrixd<3, 3> R1 = Matrixd<3, 3>::Identity(); + const Rotation3d rot1{R1}; + EXPECT_EQ(Rotation3d{}, rot1); + + // 90 degree CCW rotation around z-axis + Matrixd<3, 3> R2; + R2.block<3, 1>(0, 0) = Vectord<3>{0.0, 1.0, 0.0}; + R2.block<3, 1>(0, 1) = Vectord<3>{-1.0, 0.0, 0.0}; + R2.block<3, 1>(0, 2) = Vectord<3>{0.0, 0.0, 1.0}; + const Rotation3d rot2{R2}; + const Rotation3d expected2{0_deg, 0_deg, 90_deg}; + EXPECT_EQ(expected2, rot2); + + // Matrix that isn't orthogonal + const Matrixd<3, 3> R3{{1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}, {1.0, 0.0, 0.0}}; + EXPECT_THROW(Rotation3d{R3}, std::domain_error); + + // Matrix that's orthogonal but not special orthogonal + const Matrixd<3, 3> R4 = Matrixd<3, 3>::Identity() * 2.0; + EXPECT_THROW(Rotation3d{R4}, std::domain_error); +} + TEST(Rotation3dTest, InitTwoVector) { const Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};