From e222efaa0164e2ea4557676bbc68aaab45143731 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 7 Dec 2024 15:49:17 -0800 Subject: [PATCH 1/9] [wpimath] Add affine transformation constructors and getters to geometry API (#7430) Fixes #7429. --- .../edu/wpi/first/math/StateSpaceUtil.java | 9 ++++ .../edu/wpi/first/math/geometry/Pose2d.java | 40 +++++++++++++++ .../edu/wpi/first/math/geometry/Pose3d.java | 50 +++++++++++++++++++ .../wpi/first/math/geometry/Rotation2d.java | 48 ++++++++++++++++++ .../wpi/first/math/geometry/Rotation3d.java | 27 ++++++++++ .../wpi/first/math/geometry/Transform2d.java | 40 +++++++++++++++ .../wpi/first/math/geometry/Transform3d.java | 50 +++++++++++++++++++ .../first/math/geometry/Translation2d.java | 10 ++-- .../first/math/geometry/Translation3d.java | 10 ++-- .../main/native/include/frc/StateSpaceUtil.h | 21 +++++--- .../src/main/native/include/frc/ct_matrix.h | 19 +++++++ .../main/native/include/frc/geometry/Pose2d.h | 26 ++++++++++ .../main/native/include/frc/geometry/Pose3d.h | 32 ++++++++++++ .../native/include/frc/geometry/Rotation2d.h | 50 +++++++++++++++++++ .../native/include/frc/geometry/Rotation3d.h | 18 +++++++ .../native/include/frc/geometry/Transform2d.h | 27 ++++++++++ .../native/include/frc/geometry/Transform3d.h | 32 ++++++++++++ .../include/frc/geometry/Translation2d.h | 12 ++--- .../include/frc/geometry/Translation3d.h | 10 ++-- .../wpi/first/math/StateSpaceUtilTest.java | 1 + .../wpi/first/math/geometry/Pose2dTest.java | 8 +++ .../wpi/first/math/geometry/Pose3dTest.java | 16 ++++++ .../first/math/geometry/Rotation2dTest.java | 8 +++ .../first/math/geometry/Rotation3dTest.java | 12 +++++ .../first/math/geometry/Transform2dTest.java | 8 +++ .../first/math/geometry/Transform3dTest.java | 16 ++++++ .../test/native/cpp/geometry/Pose2dTest.cpp | 7 +++ .../test/native/cpp/geometry/Pose3dTest.cpp | 7 +++ .../native/cpp/geometry/Rotation2dTest.cpp | 7 +++ .../native/cpp/geometry/Rotation3dTest.cpp | 7 +++ .../native/cpp/geometry/Transform2dTest.cpp | 7 +++ .../native/cpp/geometry/Transform3dTest.cpp | 7 +++ 32 files changed, 615 insertions(+), 27 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java index 461d9a6f080..eb494308926 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java @@ -131,7 +131,10 @@ public static boolean isDetectable( * * @param pose A pose to convert to a vector. * @return The given pose in vector form, with the third element, theta, in radians. + * @deprecated Create the vector manually instead. If you were using this as an intermediate step + * for constructing affine transformations, use {@link Pose2d#toMatrix()} instead. */ + @Deprecated(forRemoval = true, since = "2025") public static Matrix poseToVector(Pose2d pose) { return VecBuilder.fill(pose.getX(), pose.getY(), pose.getRotation().getRadians()); } @@ -180,7 +183,10 @@ public static Matrix desaturateInputVector( * * @param pose A pose to convert to a vector. * @return The given pose in as a 4x1 vector of x, y, cos(theta), and sin(theta). + * @deprecated Create the vector manually instead. If you were using this as an intermediate step + * for constructing affine transformations, use {@link Pose2d#toMatrix()} instead. */ + @Deprecated(forRemoval = true, since = "2025") public static Matrix poseTo4dVector(Pose2d pose) { return VecBuilder.fill( pose.getTranslation().getX(), @@ -194,7 +200,10 @@ public static Matrix poseTo4dVector(Pose2d pose) { * * @param pose A pose to convert to a vector. * @return The given pose in vector form, with the third element, theta, in radians. + * @deprecated Create the vector manually instead. If you were using this as an intermediate step + * for constructing affine transformations, use {@link Pose2d#toMatrix()} instead. */ + @Deprecated(forRemoval = true, since = "2025") public static Matrix poseTo3dVector(Pose2d pose) { return VecBuilder.fill( pose.getTranslation().getX(), diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index 89c404fddbd..0705d61ea7f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -10,9 +10,13 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.proto.Pose2dProto; import edu.wpi.first.math.geometry.struct.Pose2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -79,6 +83,20 @@ public Pose2d(Distance x, Distance y, Rotation2d rotation) { this(x.in(Meters), y.in(Meters), rotation); } + /** + * Constructs a pose with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws IllegalArgumentException if the affine transformation matrix is invalid. + */ + public Pose2d(Matrix matrix) { + m_translation = new Translation2d(matrix.get(0, 2), matrix.get(1, 2)); + m_rotation = new Rotation2d(matrix.block(2, 2, 0, 0)); + if (matrix.get(2, 0) != 0.0 || matrix.get(2, 1) != 0.0 || matrix.get(2, 2) != 1.0) { + throw new IllegalArgumentException("Affine transformation matrix is invalid"); + } + } + /** * Transforms the pose by the given transformation and returns the new transformed pose. * @@ -295,6 +313,28 @@ public Twist2d log(Pose2d end) { return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta); } + /** + * Returns an affine transformation matrix representation of this pose. + * + * @return An affine transformation matrix representation of this pose. + */ + public Matrix toMatrix() { + var vec = m_translation.toVector(); + var mat = m_rotation.toMatrix(); + return MatBuilder.fill( + Nat.N3(), + Nat.N3(), + mat.get(0, 0), + mat.get(0, 1), + vec.get(0), + mat.get(1, 0), + mat.get(1, 1), + vec.get(1), + 0.0, + 0.0, + 1.0); + } + /** * Returns the nearest Pose2d from a list of poses. If two or more poses in the list have the same * distance from this pose, return the one with the closest rotation component. diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index ada3e9059db..02c2b9ba4eb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -10,10 +10,14 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.proto.Pose3dProto; import edu.wpi.first.math.geometry.struct.Pose3dStruct; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.jni.Pose3dJNI; +import edu.wpi.first.math.numbers.N4; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -79,6 +83,23 @@ public Pose3d(Distance x, Distance y, Distance z, Rotation3d rotation) { this(x.in(Meters), y.in(Meters), z.in(Meters), rotation); } + /** + * Constructs a pose with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws IllegalArgumentException if the affine transformation matrix is invalid. + */ + public Pose3d(Matrix matrix) { + m_translation = new Translation3d(matrix.get(0, 3), matrix.get(1, 3), matrix.get(2, 3)); + m_rotation = new Rotation3d(matrix.block(3, 3, 0, 0)); + if (matrix.get(3, 0) != 0.0 + || matrix.get(3, 1) != 0.0 + || matrix.get(3, 2) != 0.0 + || matrix.get(3, 3) != 1.0) { + throw new IllegalArgumentException("Affine transformation matrix is invalid"); + } + } + /** * Constructs a 3D pose from a 2D pose in the X-Y plane. * @@ -326,6 +347,35 @@ public Twist3d log(Pose3d end) { resultArray[5]); } + /** + * Returns an affine transformation matrix representation of this pose. + * + * @return An affine transformation matrix representation of this pose. + */ + public Matrix toMatrix() { + var vec = m_translation.toVector(); + var mat = m_rotation.toMatrix(); + return MatBuilder.fill( + Nat.N4(), + Nat.N4(), + mat.get(0, 0), + mat.get(0, 1), + mat.get(0, 2), + vec.get(0), + mat.get(1, 0), + mat.get(1, 1), + mat.get(1, 2), + vec.get(1), + mat.get(2, 0), + mat.get(2, 1), + mat.get(2, 2), + vec.get(2), + 0.0, + 0.0, + 0.0, + 1.0); + } + /** * Returns a Pose2d representing this Pose3d projected into the X-Y plane. * 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 49cf4c3c7d0..693a7f2bfc2 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 @@ -10,11 +10,15 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +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.geometry.proto.Rotation2dProto; import edu.wpi.first.math.geometry.struct.Rotation2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.util.protobuf.ProtobufSerializable; @@ -133,6 +137,39 @@ public Rotation2d(Angle angle) { this(angle.in(Radians)); } + /** + * Constructs a Rotation2d from a rotation matrix. + * + * @param rotationMatrix The rotation matrix. + * @throws IllegalArgumentException if the rotation matrix isn't special orthogonal. + */ + public Rotation2d(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()).minus(Matrix.eye(Nat.N2())).normF() > 1e-9) { + var msg = "Rotation matrix isn't orthogonal\n\nR =\n" + R.getStorage().toString() + '\n'; + MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace()); + throw new IllegalArgumentException(msg); + } + if (Math.abs(R.det() - 1.0) > 1e-9) { + var msg = + "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n" + + R.getStorage().toString() + + '\n'; + MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace()); + throw new IllegalArgumentException(msg); + } + + // R = [cosθ −sinθ] + // [sinθ cosθ] + m_cos = R.get(0, 0); + m_sin = R.get(1, 0); + + m_value = Math.atan2(m_sin, m_cos); + } + /** * Constructs and returns a Rotation2d with the given radian value. * @@ -238,6 +275,17 @@ public Rotation2d rotateBy(Rotation2d other) { m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos); } + /** + * Returns matrix representation of this rotation. + * + * @return Matrix representation of this rotation. + */ + public Matrix toMatrix() { + // R = [cosθ −sinθ] + // [sinθ cosθ] + return MatBuilder.fill(Nat.N2(), Nat.N2(), m_cos, -m_sin, m_sin, m_cos); + } + /** * Returns the measure of the Rotation2d. * 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 8883e665389..7d134774957 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 @@ -10,6 +10,7 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; @@ -458,6 +459,32 @@ public Angle getMeasureZ() { return Radians.of(getZ()); } + /** + * Returns rotation matrix representation of this rotation. + * + * @return Rotation matrix representation of this rotation. + */ + public Matrix toMatrix() { + double w = m_q.getW(); + double x = m_q.getX(); + double y = m_q.getY(); + double z = m_q.getZ(); + + // https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix + return MatBuilder.fill( + Nat.N3(), + Nat.N3(), + 1.0 - 2.0 * (y * y + z * z), + 2.0 * (x * y - w * z), + 2.0 * (x * z + w * y), + 2.0 * (x * y + w * z), + 1.0 - 2.0 * (x * x + z * z), + 2.0 * (y * z - w * x), + 2.0 * (x * z - w * y), + 2.0 * (y * z + w * x), + 1.0 - 2.0 * (x * x + y * y)); + } + /** * Returns the axis in the axis-angle representation of this rotation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java index 79dc16939e9..7a4cf8f4f9a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java @@ -6,8 +6,12 @@ import static edu.wpi.first.units.Units.Meters; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.proto.Transform2dProto; import edu.wpi.first.math.geometry.struct.Transform2dStruct; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -78,6 +82,20 @@ public Transform2d(Distance x, Distance y, Rotation2d rotation) { this(x.in(Meters), y.in(Meters), rotation); } + /** + * Constructs a transform with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws IllegalArgumentException if the affine transformation matrix is invalid. + */ + public Transform2d(Matrix matrix) { + m_translation = new Translation2d(matrix.get(0, 2), matrix.get(1, 2)); + m_rotation = new Rotation2d(matrix.block(2, 2, 0, 0)); + if (matrix.get(2, 0) != 0.0 || matrix.get(2, 1) != 0.0 || matrix.get(2, 2) != 1.0) { + throw new IllegalArgumentException("Affine transformation matrix is invalid"); + } + } + /** Constructs the identity transform -- maps an initial pose to itself. */ public Transform2d() { m_translation = Translation2d.kZero; @@ -160,6 +178,28 @@ public Distance getMeasureY() { return m_translation.getMeasureY(); } + /** + * Returns an affine transformation matrix representation of this transformation. + * + * @return An affine transformation matrix representation of this transformation. + */ + public Matrix toMatrix() { + var vec = m_translation.toVector(); + var mat = m_rotation.toMatrix(); + return MatBuilder.fill( + Nat.N3(), + Nat.N3(), + mat.get(0, 0), + mat.get(0, 1), + vec.get(0), + mat.get(1, 0), + mat.get(1, 1), + vec.get(1), + 0.0, + 0.0, + 1.0); + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java index 8332b607c20..d993d861001 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java @@ -6,8 +6,12 @@ import static edu.wpi.first.units.Units.Meters; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.proto.Transform3dProto; import edu.wpi.first.math.geometry.struct.Transform3dStruct; +import edu.wpi.first.math.numbers.N4; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -80,6 +84,23 @@ public Transform3d(Distance x, Distance y, Distance z, Rotation3d rotation) { this(x.in(Meters), y.in(Meters), z.in(Meters), rotation); } + /** + * Constructs a transform with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws IllegalArgumentException if the affine transformation matrix is invalid. + */ + public Transform3d(Matrix matrix) { + m_translation = new Translation3d(matrix.get(0, 3), matrix.get(1, 3), matrix.get(2, 3)); + m_rotation = new Rotation3d(matrix.block(3, 3, 0, 0)); + if (matrix.get(3, 0) != 0.0 + || matrix.get(3, 1) != 0.0 + || matrix.get(3, 2) != 0.0 + || matrix.get(3, 3) != 1.0) { + throw new IllegalArgumentException("Affine transformation matrix is invalid"); + } + } + /** Constructs the identity transform -- maps an initial pose to itself. */ public Transform3d() { m_translation = Translation3d.kZero; @@ -192,6 +213,35 @@ public Distance getMeasureZ() { return m_translation.getMeasureZ(); } + /** + * Returns an affine transformation matrix representation of this transformation. + * + * @return An affine transformation matrix representation of this transformation. + */ + public Matrix toMatrix() { + var vec = m_translation.toVector(); + var mat = m_rotation.toMatrix(); + return MatBuilder.fill( + Nat.N4(), + Nat.N4(), + mat.get(0, 0), + mat.get(0, 1), + mat.get(0, 2), + vec.get(0), + mat.get(1, 0), + mat.get(1, 1), + mat.get(1, 2), + vec.get(1), + mat.get(2, 0), + mat.get(2, 1), + mat.get(2, 2), + vec.get(2), + 0.0, + 0.0, + 0.0, + 1.0); + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index e3857eb14e6..9b4af815c72 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -88,10 +88,10 @@ public Translation2d(Distance x, Distance y) { } /** - * Constructs a Translation2d from the provided translation vector's X and Y components. The - * values are assumed to be in meters. + * Constructs a Translation2d from a 2D translation vector. The values are assumed to be in + * meters. * - * @param vector The translation vector to represent. + * @param vector The translation vector. */ public Translation2d(Vector vector) { this(vector.get(0), vector.get(1)); @@ -148,9 +148,9 @@ public Distance getMeasureY() { } /** - * Returns a vector representation of this translation. + * Returns a 2D translation vector representation of this translation. * - * @return A Vector representation of this translation. + * @return A 2D translation vector representation of this translation. */ public Vector toVector() { return VecBuilder.fill(m_x, m_y); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 48a70c7f45a..20cede3d3da 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -104,10 +104,10 @@ public Translation3d(Translation2d translation) { } /** - * Constructs a Translation3d from the provided translation vector's X, Y, and Z components. The - * values are assumed to be in meters. + * Constructs a Translation3d from a 3D translation vector. The values are assumed to be in + * meters. * - * @param vector The translation vector to represent. + * @param vector The translation vector. */ public Translation3d(Vector vector) { this(vector.get(0), vector.get(1), vector.get(2)); @@ -184,9 +184,9 @@ public Distance getMeasureZ() { } /** - * Returns a vector representation of this translation. + * Returns a 2D translation vector representation of this translation. * - * @return A Vector representation of this translation. + * @return A 2D translation vector representation of this translation. */ public Vector toVector() { return VecBuilder.fill(m_x, m_y, m_z); diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h index 785255d6748..1e8ae2a1081 100644 --- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -206,9 +206,12 @@ Vectord MakeWhiteNoiseVector(const std::array& stdDevs) { * @param pose The pose that is being represented. * * @return The vector. + * @deprecated Create the vector manually instead. If you were using this as an + * intermediate step for constructing affine transformations, use + * Pose2d.ToMatrix() instead. */ -WPILIB_DLLEXPORT -constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) { +[[deprecated("Use Pose2d.ToMatrix() instead.")]] +WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) { return Eigen::Vector3d{{pose.Translation().X().value(), pose.Translation().Y().value(), pose.Rotation().Radians().value()}}; @@ -220,9 +223,12 @@ constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) { * @param pose The pose that is being represented. * * @return The vector. + * @deprecated Create the vector manually instead. If you were using this as an + * intermediate step for constructing affine transformations, use + * Pose2d.ToMatrix() instead. */ -WPILIB_DLLEXPORT -constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) { +[[deprecated("Use Pose2d.ToMatrix() instead.")]] +WPILIB_DLLEXPORT constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) { return Eigen::Vector4d{{pose.Translation().X().value(), pose.Translation().Y().value(), pose.Rotation().Cos(), pose.Rotation().Sin()}}; @@ -311,9 +317,12 @@ bool IsDetectable(const Matrixd& A, * @param pose The pose that is being represented. * * @return The vector. + * @deprecated Create the vector manually instead. If you were using this as an + * intermediate step for constructing affine transformations, use + * Pose2d.ToMatrix() instead. */ -WPILIB_DLLEXPORT -constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) { +[[deprecated("Use Pose2d.ToMatrix() instead.")]] +WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) { return Eigen::Vector3d{ {pose.X().value(), pose.Y().value(), pose.Rotation().Radians().value()}}; } diff --git a/wpimath/src/main/native/include/frc/ct_matrix.h b/wpimath/src/main/native/include/frc/ct_matrix.h index 7b6f9056e7c..ce339db276e 100644 --- a/wpimath/src/main/native/include/frc/ct_matrix.h +++ b/wpimath/src/main/native/include/frc/ct_matrix.h @@ -308,6 +308,23 @@ class ct_matrix { (*this)(0) * rhs(1) - rhs(0) * (*this)(1)}}; } + /** + * Constexpr version of Eigen's 2x2 matrix determinant member function. + * + * @return Determinant of matrix. + */ + constexpr Scalar determinant() const + requires(Rows == 2 && Cols == 2) + { + // |a b| + // |c d| = ad - bc + Scalar a = (*this)(0, 0); + Scalar b = (*this)(0, 1); + Scalar c = (*this)(1, 0); + Scalar d = (*this)(1, 1); + return a * d - b * c; + } + /** * Constexpr version of Eigen's 3x3 matrix determinant member function. * @@ -364,7 +381,9 @@ using ct_vector = ct_matrix; template using ct_row_vector = ct_matrix; +using ct_matrix2d = ct_matrix; using ct_matrix3d = ct_matrix; +using ct_vector2d = ct_vector; using ct_vector3d = ct_vector; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index 016de906245..0664161d6d3 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h @@ -53,6 +53,21 @@ class WPILIB_DLLEXPORT Pose2d { constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation) : m_translation{x, y}, m_rotation{std::move(rotation)} {} + /** + * Constructs a pose with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws std::domain_error if the affine transformation matrix is invalid. + */ + constexpr explicit Pose2d(const Eigen::Matrix3d& matrix) + : m_translation{Eigen::Vector2d{{matrix(0, 2)}, {matrix(1, 2)}}}, + m_rotation{Eigen::Matrix2d{{matrix(0, 0), matrix(0, 1)}, + {matrix(1, 0), matrix(1, 1)}}} { + if (matrix(2, 0) != 0.0 || matrix(2, 1) != 0.0 || matrix(2, 2) != 1.0) { + throw std::domain_error("Affine transformation matrix is invalid"); + } + } + /** * Transforms the pose by the given transformation and returns the new * transformed pose. @@ -202,6 +217,17 @@ class WPILIB_DLLEXPORT Pose2d { */ constexpr Twist2d Log(const Pose2d& end) const; + /** + * Returns an affine transformation matrix representation of this pose. + */ + constexpr Eigen::Matrix3d ToMatrix() const { + auto vec = m_translation.ToVector(); + auto mat = m_rotation.ToMatrix(); + return Eigen::Matrix3d{{mat(0, 0), mat(0, 1), vec(0)}, + {mat(1, 0), mat(1, 1), vec(1)}, + {0.0, 0.0, 1.0}}; + } + /** * Returns the nearest Pose2d from a collection of poses * @param poses The collection of poses. diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index dc6a7416fd0..1f377a8ce71 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -4,6 +4,7 @@ #pragma once +#include #include #include @@ -54,6 +55,25 @@ class WPILIB_DLLEXPORT Pose3d { Rotation3d rotation) : m_translation{x, y, z}, m_rotation{std::move(rotation)} {} + /** + * Constructs a pose with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws std::domain_error if the affine transformation matrix is invalid. + */ + constexpr explicit Pose3d(const Eigen::Matrix4d& matrix) + : m_translation{Eigen::Vector3d{ + {matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}}, + m_rotation{ + Eigen::Matrix3d{{matrix(0, 0), matrix(0, 1), matrix(0, 2)}, + {matrix(1, 0), matrix(1, 1), matrix(1, 2)}, + {matrix(2, 0), matrix(2, 1), matrix(2, 2)}}} { + if (matrix(3, 0) != 0.0 || matrix(3, 1) != 0.0 || matrix(3, 2) != 0.0 || + matrix(3, 3) != 1.0) { + throw std::domain_error("Affine transformation matrix is invalid"); + } + } + /** * Constructs a 3D pose from a 2D pose in the X-Y plane. * @@ -218,6 +238,18 @@ class WPILIB_DLLEXPORT Pose3d { */ constexpr Twist3d Log(const Pose3d& end) const; + /** + * Returns an affine transformation matrix representation of this pose. + */ + constexpr Eigen::Matrix4d ToMatrix() const { + auto vec = m_translation.ToVector(); + auto mat = m_rotation.ToMatrix(); + return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)}, + {mat(1, 0), mat(1, 1), mat(1, 2), vec(1)}, + {mat(2, 0), mat(2, 1), mat(2, 2), vec(2)}, + {0.0, 0.0, 0.0, 1.0}}; + } + /** * Returns a Pose2d representing this Pose3d projected into the X-Y plane. */ diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h index a8899304f23..4251cabe0ad 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h @@ -5,12 +5,16 @@ #pragma once #include +#include +#include +#include #include #include #include #include +#include "frc/ct_matrix.h" #include "units/angle.h" #include "wpimath/MathShared.h" @@ -66,6 +70,43 @@ class WPILIB_DLLEXPORT Rotation2d { m_value = units::radian_t{gcem::atan2(m_sin, m_cos)}; } + /** + * Constructs a Rotation2d from a rotation matrix. + * + * @param rotationMatrix The rotation matrix. + * @throws std::domain_error if the rotation matrix isn't special orthogonal. + */ + constexpr explicit Rotation2d(const Eigen::Matrix2d& rotationMatrix) { + auto impl = + [](const Matrix2d& R) -> std::pair { + // 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() - Matrix2d::Identity()).norm() > 1e-9) { + throw std::domain_error("Rotation matrix isn't orthogonal"); + } + if (gcem::abs(R.determinant() - 1.0) > 1e-9) { + throw std::domain_error( + "Rotation matrix is orthogonal but not special orthogonal"); + } + + // R = [cosθ −sinθ] + // [sinθ cosθ] + return {R(0, 0), R(1, 0)}; + }; + + if (std::is_constant_evaluated()) { + auto cossin = impl(ct_matrix2d{rotationMatrix}); + m_cos = std::get<0>(cossin); + m_sin = std::get<1>(cossin); + } else { + auto cossin = impl(rotationMatrix); + m_cos = std::get<0>(cossin); + m_sin = std::get<1>(cossin); + } + + m_value = units::radian_t{gcem::atan2(m_sin, m_cos)}; + } + /** * Adds two rotations together, with the result being bounded between -π and * π. @@ -154,6 +195,15 @@ class WPILIB_DLLEXPORT Rotation2d { Cos() * other.Sin() + Sin() * other.Cos()}; } + /** + * Returns matrix representation of this rotation. + */ + constexpr Eigen::Matrix2d ToMatrix() const { + // R = [cosθ −sinθ] + // [sinθ cosθ] + return Eigen::Matrix2d{{m_cos, -m_sin}, {m_sin, m_cos}}; + } + /** * Returns the radian value of the rotation. * diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index f27d1d94131..47aa165b7c4 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -403,6 +403,24 @@ class WPILIB_DLLEXPORT Rotation3d { return units::radian_t{2.0 * gcem::atan2(norm, m_q.W())}; } + /** + * Returns rotation matrix representation of this rotation. + */ + constexpr Eigen::Matrix3d ToMatrix() const { + double w = m_q.W(); + double x = m_q.X(); + double y = m_q.Y(); + double z = m_q.Z(); + + // https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix + return Eigen::Matrix3d{{1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - w * z), + 2.0 * (x * z + w * y)}, + {2.0 * (x * y + w * z), 1.0 - 2.0 * (x * x + z * z), + 2.0 * (y * z - w * x)}, + {2.0 * (x * z - w * y), 2.0 * (y * z + w * x), + 1.0 - 2.0 * (x * x + y * y)}}; + } + /** * Returns a Rotation2d representing this Rotation3d projected into the X-Y * plane. diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h index abf5c1ff4de..88c00550d04 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h @@ -49,6 +49,21 @@ class WPILIB_DLLEXPORT Transform2d { constexpr Transform2d(units::meter_t x, units::meter_t y, Rotation2d rotation) : m_translation{x, y}, m_rotation{std::move(rotation)} {} + /** + * Constructs a pose with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws std::domain_error if the affine transformation matrix is invalid. + */ + constexpr explicit Transform2d(const Eigen::Matrix3d& matrix) + : m_translation{Eigen::Vector2d{{matrix(0, 2)}, {matrix(1, 2)}}}, + m_rotation{Eigen::Matrix2d{{matrix(0, 0), matrix(0, 1)}, + {matrix(1, 0), matrix(1, 1)}}} { + if (matrix(2, 0) != 0.0 || matrix(2, 1) != 0.0 || matrix(2, 2) != 1.0) { + throw std::domain_error("Affine transformation matrix is invalid"); + } + } + /** * Constructs the identity transform -- maps an initial pose to itself. */ @@ -75,6 +90,18 @@ class WPILIB_DLLEXPORT Transform2d { */ constexpr units::meter_t Y() const { return m_translation.Y(); } + /** + * Returns an affine transformation matrix representation of this + * transformation. + */ + constexpr Eigen::Matrix3d ToMatrix() const { + auto vec = m_translation.ToVector(); + auto mat = m_rotation.ToMatrix(); + return Eigen::Matrix3d{{mat(0, 0), mat(0, 1), vec(0)}, + {mat(1, 0), mat(1, 1), vec(1)}, + {0.0, 0.0, 1.0}}; + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h index 4716b7181e5..bd6ca9c41e0 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h @@ -51,6 +51,25 @@ class WPILIB_DLLEXPORT Transform3d { Rotation3d rotation) : m_translation{x, y, z}, m_rotation{std::move(rotation)} {} + /** + * Constructs a transform with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws std::domain_error if the affine transformation matrix is invalid. + */ + constexpr explicit Transform3d(const Eigen::Matrix4d& matrix) + : m_translation{Eigen::Vector3d{ + {matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}}, + m_rotation{ + Eigen::Matrix3d{{matrix(0, 0), matrix(0, 1), matrix(0, 2)}, + {matrix(1, 0), matrix(1, 1), matrix(1, 2)}, + {matrix(2, 0), matrix(2, 1), matrix(2, 2)}}} { + if (matrix(3, 0) != 0.0 || matrix(3, 1) != 0.0 || matrix(3, 2) != 0.0 || + matrix(3, 3) != 1.0) { + throw std::domain_error("Affine transformation matrix is invalid"); + } + } + /** * Constructs the identity transform -- maps an initial pose to itself. */ @@ -95,6 +114,19 @@ class WPILIB_DLLEXPORT Transform3d { */ constexpr units::meter_t Z() const { return m_translation.Z(); } + /** + * Returns an affine transformation matrix representation of this + * transformation. + */ + constexpr Eigen::Matrix4d ToMatrix() const { + auto vec = m_translation.ToVector(); + auto mat = m_rotation.ToMatrix(); + return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)}, + {mat(1, 0), mat(1, 1), mat(1, 2), vec(1)}, + {mat(2, 0), mat(2, 1), mat(2, 2), vec(2)}, + {0.0, 0.0, 0.0, 1.0}}; + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index a8a26b8bff5..aa6c4753ef7 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h @@ -54,13 +54,13 @@ class WPILIB_DLLEXPORT Translation2d { : m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {} /** - * Constructs a Translation2d from the provided translation vector's X and Y - * components. The values are assumed to be in meters. + * Constructs a Translation2d from a 2D translation vector. The values are + * assumed to be in meters. * - * @param vector The translation vector to represent. + * @param vector The translation vector. */ constexpr explicit Translation2d(const Eigen::Vector2d& vector) - : m_x{units::meter_t{vector(0)}}, m_y{units::meter_t{vector(1)}} {} + : m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {} /** * Calculates the distance between two translations in 2D space. @@ -90,9 +90,9 @@ class WPILIB_DLLEXPORT Translation2d { constexpr units::meter_t Y() const { return m_y; } /** - * Returns a vector representation of this translation. + * Returns a 2D translation vector representation of this translation. * - * @return A Vector representation of this translation. + * @return A 2D translation vector representation of this translation. */ constexpr Eigen::Vector2d ToVector() const { return Eigen::Vector2d{{m_x.value(), m_y.value()}}; diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index fb3194b8ced..c16fc56a5b8 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -56,10 +56,10 @@ class WPILIB_DLLEXPORT Translation3d { } /** - * Constructs a Translation3d from the provided translation vector's X, Y, and - * Z components. The values are assumed to be in meters. + * Constructs a Translation3d from a 3D translation vector. The values are + * assumed to be in meters. * - * @param vector The translation vector to represent. + * @param vector The translation vector. */ constexpr explicit Translation3d(const Eigen::Vector3d& vector) : m_x{units::meter_t{vector.x()}}, @@ -114,9 +114,9 @@ class WPILIB_DLLEXPORT Translation3d { constexpr units::meter_t Z() const { return m_z; } /** - * Returns a vector representation of this translation. + * Returns a 3D translation vector representation of this translation. * - * @return A Vector representation of this translation. + * @return A 3D translation vector representation of this translation. */ constexpr Eigen::Vector3d ToVector() const { return Eigen::Vector3d{{m_x.value(), m_y.value(), m_z.value()}}; diff --git a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java index 4883e6f5ce0..1715d93dc69 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java @@ -153,6 +153,7 @@ void testMatrixExp() { } @Test + @SuppressWarnings("removal") void testPoseToVector() { Pose2d pose = new Pose2d(1, 2, new Rotation2d(3)); var vector = StateSpaceUtil.poseToVector(pose); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java index 0844e1321e9..81f81509929 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java @@ -86,6 +86,14 @@ void testInequality() { assertNotEquals(one, two); } + @Test + void testToMatrix() { + var before = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(20.0)); + var after = new Pose2d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testMinus() { var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java index b333d24592f..00c63eae080 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java @@ -169,6 +169,22 @@ void testMinus() { () -> assertEquals(0.0, transform.getRotation().getZ(), kEpsilon)); } + @Test + void testToMatrix() { + var before = + new Pose3d( + 1.0, + 2.0, + 3.0, + new Rotation3d( + Units.degreesToRadians(20.0), + Units.degreesToRadians(30.0), + Units.degreesToRadians(40.0))); + var after = new Pose3d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testToPose2d() { var pose = diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java index b75182edb7e..6eaa2a3f98c 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java @@ -100,6 +100,14 @@ void testInequality() { assertNotEquals(rot1, rot2); } + @Test + void testToMatrix() { + var before = Rotation2d.fromDegrees(20.0); + var after = new Rotation2d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testInterpolate() { // 50 + (70 - 50) * 0.5 = 60 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 43b7e9b4c5b..3e96a1cb3df 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 @@ -344,6 +344,18 @@ void testInequality() { assertNotEquals(rot1, rot2); } + @Test + void testToMatrix() { + var before = + new Rotation3d( + Units.degreesToRadians(10.0), + Units.degreesToRadians(20.0), + Units.degreesToRadians(30.0)); + var after = new Rotation3d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testInterpolate() { final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java index 5817f4e2825..19d4b4f396f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java @@ -22,6 +22,14 @@ void testNewWithMeasures() { assertEquals(Math.PI / 4, transform.getRotation().getRadians(), kEpsilon); } + @Test + void testToMatrix() { + var before = new Transform2d(1.0, 2.0, Rotation2d.fromDegrees(20.0)); + var after = new Transform2d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testInverse() { var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java index 51d5f071fc3..dc251512fd1 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java @@ -11,6 +11,22 @@ import org.junit.jupiter.api.Test; class Transform3dTest { + @Test + void testToMatrix() { + var before = + new Transform3d( + 1.0, + 2.0, + 3.0, + new Rotation3d( + Units.degreesToRadians(20.0), + Units.degreesToRadians(30.0), + Units.degreesToRadians(40.0))); + var after = new Transform3d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testInverse() { var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp index bd5a9aa8a28..9a038590b1a 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp @@ -141,6 +141,13 @@ TEST(Pose2dTest, Nearest) { .value()); } +TEST(Pose2dTest, ToMatrix) { + Pose2d before{1_m, 2_m, 20_deg}; + Pose2d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Pose2dTest, Constexpr) { constexpr Pose2d defaultConstructed; constexpr Pose2d translationRotation{Translation2d{0_m, 1_m}, diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp index 88acb1fb897..f0168e95435 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp @@ -112,6 +112,13 @@ TEST(Pose3dTest, Minus) { EXPECT_NEAR(0.0, transform.Rotation().Z().value(), 1e-9); } +TEST(Pose3dTest, ToMatrix) { + Pose3d before{1_m, 2_m, 3_m, Rotation3d{10_deg, 20_deg, 30_deg}}; + Pose3d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Pose3dTest, ToPose2d) { Pose3d pose{1_m, 2_m, 3_m, Rotation3d{20_deg, 30_deg, 40_deg}}; Pose2d expected{1_m, 2_m, 40_deg}; diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp index 65d3016d18d..00065df514f 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -78,6 +78,13 @@ TEST(Rotation2dTest, Inequality) { EXPECT_NE(rot1, rot2); } +TEST(Rotation2dTest, ToMatrix) { + Rotation2d before{20_deg}; + Rotation2d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Rotation2dTest, Constexpr) { constexpr Rotation2d defaultCtor; constexpr Rotation2d radianCtor{5_rad}; diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp index 16c033723d1..6088bdd7590 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp @@ -307,6 +307,13 @@ TEST(Rotation3dTest, Inequality) { EXPECT_NE(rot1, rot2); } +TEST(Rotation3dTest, ToMatrix) { + Rotation3d before{10_deg, 20_deg, 30_deg}; + Rotation3d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Rotation3dTest, Interpolate) { const Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; const Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp index 49c9466201c..5a562405cc6 100644 --- a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp @@ -13,6 +13,13 @@ using namespace frc; +TEST(Transform2dTest, ToMatrix) { + Transform2d before{1_m, 2_m, 20_deg}; + Transform2d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Transform2dTest, Inverse) { const Pose2d initial{1_m, 2_m, 45_deg}; const Transform2d transform{{5_m, 0_m}, 5_deg}; diff --git a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp index 8a6bbc039f0..9aa7eda331e 100644 --- a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp @@ -13,6 +13,13 @@ using namespace frc; +TEST(Transform3dTest, ToMatrix) { + Transform3d before{1_m, 2_m, 3_m, Rotation3d{10_deg, 20_deg, 30_deg}}; + Transform3d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Transform3dTest, Inverse) { Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; From 9a1b4245fa13a31049a35aa164fb5d54dbba792d Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 7 Dec 2024 18:55:06 -0800 Subject: [PATCH 2/9] [build] Restore Windows constexpr mutex define on wpiutil (#7515) --- wpiutil/build.gradle | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/wpiutil/build.gradle b/wpiutil/build.gradle index 5191e17e56e..9846a49cc70 100644 --- a/wpiutil/build.gradle +++ b/wpiutil/build.gradle @@ -184,6 +184,12 @@ nativeUtils.exportsConfigs { } } +nativeUtils.platformConfigs.each { + if (it.name.contains('windows')) { + it.cppCompiler.args.add("/D_DISABLE_CONSTEXPR_MUTEX_CONSTRUCTOR") + } +} + cppHeadersZip { def thirdpartyIncDirs = [ 'src/main/native/thirdparty/argparse/include', From 6dbff902fad9dfb796da258578fc250c69265e32 Mon Sep 17 00:00:00 2001 From: Thad House Date: Sat, 7 Dec 2024 20:31:10 -0800 Subject: [PATCH 3/9] [build] Remove athena completely from build (#7517) --- shared/config.gradle | 1 - 1 file changed, 1 deletion(-) diff --git a/shared/config.gradle b/shared/config.gradle index a4eb784ba23..a5e70c4c676 100644 --- a/shared/config.gradle +++ b/shared/config.gradle @@ -7,7 +7,6 @@ if (project.hasProperty('ciDebugOnly')) { } nativeUtils.addWpiNativeUtils() -nativeUtils.withCrossRoboRIO() nativeUtils.withCrossLinuxArm32() nativeUtils.withCrossLinuxArm64() nativeUtils.withCrossSystemCore() From c497e4ec227be7165751a1dd7388c65ce21d8407 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 7 Dec 2024 20:32:16 -0800 Subject: [PATCH 4/9] [wpimath] Fix SimpleFeedforward overload set (#7516) --- .../command/MecanumControllerCommand.java | 11 ++++------- .../first/wpilibj2/command/RamseteCommand.java | 5 ++--- .../subsystems/DriveSubsystem.java | 4 ++-- .../math/controller/SimpleMotorFeedforward.java | 16 +--------------- .../frc/controller/SimpleMotorFeedforward.h | 16 ---------------- .../controller/SimpleMotorFeedforwardTest.java | 8 +++----- .../DifferentialDriveVoltageConstraintTest.java | 8 ++++---- .../math/trajectory/ExponentialProfileTest.java | 2 +- 8 files changed, 17 insertions(+), 53 deletions(-) diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java index 70978c63f12..440018817c2 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java @@ -368,19 +368,16 @@ public void execute() { if (m_usePID) { final double frontLeftFeedforward = - m_feedforward.calculateWithVelocities( - m_prevFrontLeftSpeedSetpoint, frontLeftSpeedSetpoint); + m_feedforward.calculate(m_prevFrontLeftSpeedSetpoint, frontLeftSpeedSetpoint); final double rearLeftFeedforward = - m_feedforward.calculateWithVelocities(m_prevRearLeftSpeedSetpoint, rearLeftSpeedSetpoint); + m_feedforward.calculate(m_prevRearLeftSpeedSetpoint, rearLeftSpeedSetpoint); final double frontRightFeedforward = - m_feedforward.calculateWithVelocities( - m_prevFrontRightSpeedSetpoint, frontRightSpeedSetpoint); + m_feedforward.calculate(m_prevFrontRightSpeedSetpoint, frontRightSpeedSetpoint); final double rearRightFeedforward = - m_feedforward.calculateWithVelocities( - m_prevRearRightSpeedSetpoint, rearRightSpeedSetpoint); + m_feedforward.calculate(m_prevRearRightSpeedSetpoint, rearRightSpeedSetpoint); frontLeftOutput = frontLeftFeedforward diff --git a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java index 88aae9cca16..23ec61c6227 100644 --- a/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java +++ b/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java @@ -181,11 +181,10 @@ public void execute() { double rightOutput; if (m_usePID) { - double leftFeedforward = - m_feedforward.calculateWithVelocities(m_prevLeftSpeedSetpoint, leftSpeedSetpoint); + double leftFeedforward = m_feedforward.calculate(m_prevLeftSpeedSetpoint, leftSpeedSetpoint); double rightFeedforward = - m_feedforward.calculateWithVelocities(m_prevRightSpeedSetpoint, rightSpeedSetpoint); + m_feedforward.calculate(m_prevRightSpeedSetpoint, rightSpeedSetpoint); leftOutput = leftFeedforward diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java index b8947ce6da9..3c3179dcc03 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java @@ -96,12 +96,12 @@ public void setDriveStates( m_leftLeader.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, currentLeft.position, - m_feedforward.calculateWithVelocities(currentLeft.velocity, nextLeft.velocity) + m_feedforward.calculate(currentLeft.velocity, nextLeft.velocity) / RobotController.getBatteryVoltage()); m_rightLeader.setSetpoint( ExampleSmartMotorController.PIDMode.kPosition, currentRight.position, - m_feedforward.calculateWithVelocities(currentLeft.velocity, nextLeft.velocity) + m_feedforward.calculate(currentLeft.velocity, nextLeft.velocity) / RobotController.getBatteryVoltage()); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java index b7603a6bfbe..e9e6a3f22d9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java @@ -123,20 +123,6 @@ public double getDt() { return m_dt; } - /** - * Calculates the feedforward from the gains and setpoints assuming continuous control. - * - * @param velocity The velocity setpoint. - * @param acceleration The acceleration setpoint. - * @return The computed feedforward. - * @deprecated Use {@link #calculateWithVelocities(double, double)} instead. - */ - @SuppressWarnings("removal") - @Deprecated(forRemoval = true, since = "2025") - public double calculate(double velocity, double acceleration) { - return ks * Math.signum(velocity) + kv * velocity + ka * acceleration; - } - /** * Calculates the feedforward from the gains and velocity setpoint assuming continuous control * (acceleration is assumed to be zero). @@ -157,7 +143,7 @@ public double calculate(double velocity) { * @param nextVelocity The next velocity setpoint. * @return The computed feedforward. */ - public double calculateWithVelocities(double currentVelocity, double nextVelocity) { + public double calculate(double currentVelocity, double nextVelocity) { // See wpimath/algorithms.md#Simple_motor_feedforward for derivation if (ka == 0.0) { return ks * Math.signum(nextVelocity) + kv * nextVelocity; diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index 87099047f5b..4aeea81d2c3 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -67,22 +67,6 @@ class SimpleMotorFeedforward { } } - /** - * Calculates the feedforward from the gains and setpoints assuming continuous - * control. - * - * @param velocity The velocity setpoint. - * @param acceleration The acceleration setpoint. - * @return The computed feedforward, in volts. - * @deprecated Use the current/next velocity overload instead. - */ - [[deprecated("Use the current/next velocity overload instead.")]] - constexpr units::volt_t Calculate( - units::unit_t velocity, - units::unit_t acceleration) const { - return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration; - } - /** * Calculates the feedforward from the gains and velocity setpoint assuming * discrete control. Use this method when the velocity setpoint does not diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java index 3c0a74835a2..799cfe8bac0 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java @@ -34,19 +34,17 @@ void testCalculate() { double nextVelocity = 3.0; // rad/s assertEquals( - 37.52499583432516 + 0.5, - simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity), - 0.002); + 37.52499583432516 + 0.5, simpleMotor.calculate(currentVelocity, nextVelocity), 0.002); assertEquals( plantInversion.calculate(r, nextR).get(0, 0) + Ks, - simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity), + simpleMotor.calculate(currentVelocity, nextVelocity), 0.002); // These won't match exactly. It's just an approximation to make sure they're // in the same ballpark. assertEquals( plantInversion.calculate(r, nextR).get(0, 0) + Ks, - simpleMotor.calculateWithVelocities(currentVelocity, nextVelocity), + simpleMotor.calculate(currentVelocity, nextVelocity), 2.0); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java index 52af40e89b2..11a47bd71c5 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java @@ -51,25 +51,25 @@ void testDifferentialDriveVoltageConstraint() { assertAll( () -> assertTrue( - feedforward.calculateWithVelocities( + feedforward.calculate( wheelSpeeds.leftMetersPerSecond, wheelSpeeds.leftMetersPerSecond + dt * acceleration) <= maxVoltage + 0.05), () -> assertTrue( - feedforward.calculateWithVelocities( + feedforward.calculate( wheelSpeeds.leftMetersPerSecond, wheelSpeeds.leftMetersPerSecond + dt * acceleration) >= -maxVoltage - 0.05), () -> assertTrue( - feedforward.calculateWithVelocities( + feedforward.calculate( wheelSpeeds.rightMetersPerSecond, wheelSpeeds.rightMetersPerSecond + dt * acceleration) <= maxVoltage + 0.05), () -> assertTrue( - feedforward.calculateWithVelocities( + feedforward.calculate( wheelSpeeds.rightMetersPerSecond, wheelSpeeds.rightMetersPerSecond + dt * acceleration) >= -maxVoltage - 0.05)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java index 2f4ffac2f2a..a71bfefe436 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/ExponentialProfileTest.java @@ -43,7 +43,7 @@ private static void assertNear( private static ExponentialProfile.State checkDynamics( ExponentialProfile profile, ExponentialProfile.State current, ExponentialProfile.State goal) { var next = profile.calculate(kDt, current, goal); - var signal = feedforward.calculateWithVelocities(current.velocity, next.velocity); + var signal = feedforward.calculate(current.velocity, next.velocity); assertTrue(Math.abs(signal) < constraints.maxInput + 1e-9); From 38a239b5314b4c11838f1250662e3be730becc45 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sat, 7 Dec 2024 21:15:49 -0800 Subject: [PATCH 5/9] [ci] Add labeler action to auto-label PRs (#7520) --- .github/labeler.yml | 59 +++++++++++++++++++++++++++++++++++ .github/workflows/labeler.yml | 12 +++++++ 2 files changed, 71 insertions(+) create mode 100644 .github/labeler.yml create mode 100644 .github/workflows/labeler.yml diff --git a/.github/labeler.yml b/.github/labeler.yml new file mode 100644 index 00000000000..c91083a5ba1 --- /dev/null +++ b/.github/labeler.yml @@ -0,0 +1,59 @@ +'2027': +- base-branch: '2027' +'component: apriltag': +- changed-files: + - any-glob-to-any-file: apriltag/** +'component: command-based': +- changed-files: + - any-glob-to-any-file: wpilibNewCommands/** +'component: cscore': +- changed-files: + - any-glob-to-any-file: cscore/** +'component: datalogtool': +- changed-files: + - any-glob-to-any-file: datalogtool/** +'component: epilogue': +- changed-files: + - any-glob-to-any-file: epilogue-*/** +'component: examples': +- changed-files: + - any-glob-to-any-file: wpilib*Examples/** +'component: examples': +- changed-files: + - any-glob-to-any-file: wpilib*Examples/** +'component: glass': +- changed-files: + - any-glob-to-any-file: glass/** +'component: hal': +- changed-files: + - any-glob-to-any-file: hal/** +'component: ntcore': +- changed-files: + - any-glob-to-any-file: ntcore/** +'component: outlineviewer': +- changed-files: + - any-glob-to-any-file: outlineviewer/** +'component: sysid': +- changed-files: + - any-glob-to-any-file: sysid/** +'component: teamnumbersetter': +- changed-files: + - any-glob-to-any-file: roborioteamnumbersetter/** +'component: wpilibc': +- changed-files: + - any-glob-to-any-file: wpilibc/** +'component: wpilibj': +- changed-files: + - any-glob-to-any-file: wpilibj/** +'component: wpimath': +- changed-files: + - any-glob-to-any-file: wpimath/** +'component: wpinet': +- changed-files: + - any-glob-to-any-file: wpinet/** +'component: wpiunits': +- changed-files: + - any-glob-to-any-file: wpiunits/** +'component: wpiutil': +- changed-files: + - any-glob-to-any-file: wpiutil/** diff --git a/.github/workflows/labeler.yml b/.github/workflows/labeler.yml new file mode 100644 index 00000000000..e57cd86e2b3 --- /dev/null +++ b/.github/workflows/labeler.yml @@ -0,0 +1,12 @@ +name: "Pull Request Labeler" +on: +- pull_request_target + +jobs: + labeler: + permissions: + contents: read + pull-requests: write + runs-on: ubuntu-latest + steps: + - uses: actions/labeler@v5 From c81bd0c909397398e5536ce0e631b56901eb4df6 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 7 Dec 2024 21:20:48 -0800 Subject: [PATCH 6/9] [ci] Upgrade to Ubuntu 24.04 (#7496) --- .github/workflows/bazel.yml | 6 +++--- .github/workflows/cmake-android.yml | 4 ++-- .github/workflows/cmake.yml | 6 +++--- .github/workflows/comment-command.yml | 10 ++++++---- .github/workflows/documentation.yml | 2 +- .github/workflows/gradle.yml | 16 +++++++-------- .github/workflows/lint-format.yml | 28 +++++++++++++++------------ .github/workflows/pregenerate.yml | 2 +- .github/workflows/sanitizers.yml | 8 ++++---- .github/workflows/sentinel-build.yml | 10 +++++----- .github/workflows/tools.yml | 10 +++++----- .github/workflows/upstream-utils.yml | 2 +- 12 files changed, 55 insertions(+), 49 deletions(-) diff --git a/.github/workflows/bazel.yml b/.github/workflows/bazel.yml index b85222db9b2..9a8caf0fa85 100644 --- a/.github/workflows/bazel.yml +++ b/.github/workflows/bazel.yml @@ -56,8 +56,8 @@ jobs: fail-fast: false matrix: include: - - { name: "Linux (native)", os: ubuntu-22.04, action: "test", config: "--config=linux", } - - { name: "Linux (roborio)", os: ubuntu-22.04, action: "build", config: "--config=roborio", } + - { name: "Linux (native)", os: ubuntu-24.04, action: "test", config: "--config=linux", } + - { name: "Linux (roborio)", os: ubuntu-24.04, action: "build", config: "--config=roborio", } name: "${{ matrix.name }}" runs-on: ${{ matrix.os }} steps: @@ -75,7 +75,7 @@ jobs: buildifier: name: "buildifier" - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - name: Set up Go 1.15.x uses: actions/setup-go@v5 diff --git a/.github/workflows/cmake-android.yml b/.github/workflows/cmake-android.yml index 46e2b09aab6..fc850f278d4 100644 --- a/.github/workflows/cmake-android.yml +++ b/.github/workflows/cmake-android.yml @@ -16,10 +16,10 @@ jobs: fail-fast: false matrix: include: - - os: ubuntu-22.04 + - os: ubuntu-24.04 name: Android Arm64 abi: arm64-v8a - - os: ubuntu-22.04 + - os: ubuntu-24.04 name: Android X64 abi: "x86_64" diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index 921a10bbdb4..4f8edc4fcb1 100644 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -16,9 +16,9 @@ jobs: fail-fast: false matrix: include: - - os: ubuntu-22.04 + - os: ubuntu-24.04 name: Linux - container: wpilib/roborio-cross-ubuntu:2025-22.04 + container: wpilib/roborio-cross-ubuntu:2025-24.04 flags: "--preset with-java-and-sccache -DCMAKE_BUILD_TYPE=Release -DWITH_EXAMPLES=ON" - os: macOS-14 name: macOS @@ -36,7 +36,7 @@ jobs: steps: - name: Install dependencies (Linux) if: runner.os == 'Linux' - run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv4.5-java libprotobuf-dev protobuf-compiler ninja-build + run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv-java libprotobuf-dev protobuf-compiler ninja-build - name: Install dependencies (macOS) if: runner.os == 'macOS' diff --git a/.github/workflows/comment-command.yml b/.github/workflows/comment-command.yml index 09c2428dece..7ee4d75869f 100644 --- a/.github/workflows/comment-command.yml +++ b/.github/workflows/comment-command.yml @@ -6,7 +6,7 @@ on: jobs: format: if: github.event.issue.pull_request && startsWith(github.event.comment.body, '/format') - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - name: React Rocket uses: actions/github-script@v7 @@ -43,9 +43,11 @@ jobs: distribution: 'temurin' java-version: 17 - name: Install wpiformat - run: pip3 install wpiformat==2024.50 + run: | + python -m venv ${{ runner.temp }}/wpiformat + ${{ runner.temp }}/wpiformat/bin/pip3 install wpiformat==2024.50 - name: Run wpiformat - run: wpiformat + run: ${{ runner.temp }}/wpiformat/bin/wpiformat - name: Run spotlessApply run: ./gradlew spotlessApply - name: Commit @@ -59,7 +61,7 @@ jobs: pregen: if: github.event.issue.pull_request && startsWith(github.event.comment.body, '/pregen') - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - name: React Rocket uses: actions/github-script@v7 diff --git a/.github/workflows/documentation.yml b/.github/workflows/documentation.yml index 0ace70927e4..1c900c754a5 100644 --- a/.github/workflows/documentation.yml +++ b/.github/workflows/documentation.yml @@ -12,7 +12,7 @@ env: jobs: publish: name: "Documentation - Publish" - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 if: github.repository == 'wpilibsuite/allwpilib' && (github.ref == 'refs/heads/main' || (startsWith(github.ref, 'refs/tags/v') && !contains(github.ref, '2027'))) concurrency: ci-docs-publish steps: diff --git a/.github/workflows/gradle.yml b/.github/workflows/gradle.yml index ee255f8a695..167966b8e2e 100644 --- a/.github/workflows/gradle.yml +++ b/.github/workflows/gradle.yml @@ -12,20 +12,20 @@ jobs: fail-fast: false matrix: include: - - container: wpilib/systemcore-cross-ubuntu:2025-22.04 + - container: wpilib/systemcore-cross-ubuntu:2025-24.04 artifact-name: SystemCore build-options: "-Ponlylinuxsystemcore" - - container: wpilib/raspbian-cross-ubuntu:bookworm-22.04 + - container: wpilib/raspbian-cross-ubuntu:bookworm-24.04 artifact-name: Arm32 build-options: "-Ponlylinuxarm32" - - container: wpilib/aarch64-cross-ubuntu:bookworm-22.04 + - container: wpilib/aarch64-cross-ubuntu:bookworm-24.04 artifact-name: Arm64 build-options: "-Ponlylinuxarm64" - - container: wpilib/ubuntu-base:22.04 + - container: wpilib/ubuntu-base:24.04 artifact-name: Linux build-options: "-Ponlylinuxx86-64" name: "Build - ${{ matrix.artifact-name }}" - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - name: Free Disk Space uses: jlumbroso/free-disk-space@main @@ -171,7 +171,7 @@ jobs: build-documentation: name: "Build - Documentation" - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - uses: actions/checkout@v4 with: @@ -196,7 +196,7 @@ jobs: combine: name: Combine needs: [build-docker, build-host, build-documentation] - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - name: Free Disk Space if: | @@ -274,7 +274,7 @@ jobs: strategy: matrix: repo: ['SmartDashboard', 'PathWeaver', 'Shuffleboard', 'RobotBuilder'] - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - uses: peter-evans/repository-dispatch@v3 if: | diff --git a/.github/workflows/lint-format.yml b/.github/workflows/lint-format.yml index 9b75d4ff895..88f730f56f7 100644 --- a/.github/workflows/lint-format.yml +++ b/.github/workflows/lint-format.yml @@ -13,7 +13,7 @@ concurrency: jobs: wpiformat: name: "wpiformat" - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - uses: actions/checkout@v4 with: @@ -27,9 +27,11 @@ jobs: with: python-version: '3.12' - name: Install wpiformat - run: pip3 install wpiformat==2024.50 + run: | + python -m venv ${{ runner.temp }}/wpiformat + ${{ runner.temp }}/wpiformat/bin/pip3 install wpiformat==2024.50 - name: Run - run: wpiformat + run: ${{ runner.temp }}/wpiformat/bin/wpiformat - name: Check output run: git --no-pager diff --exit-code HEAD - name: Generate diff @@ -50,8 +52,8 @@ jobs: tidy: name: "clang-tidy" - runs-on: ubuntu-22.04 - container: wpilib/ubuntu-base:22.04 + runs-on: ubuntu-24.04 + container: wpilib/ubuntu-base:24.04 steps: - uses: actions/checkout@v4 with: @@ -66,22 +68,24 @@ jobs: with: python-version: '3.12' - name: Install wpiformat - run: pip3 install wpiformat==2024.50 + run: | + python -m venv ${{ runner.temp }}/wpiformat + ${{ runner.temp }}/wpiformat/bin/pip3 install wpiformat==2024.50 - name: Create compile_commands.json run: | ./gradlew generateCompileCommands -Ptoolchain-optional-roboRio ./.github/workflows/fix_compile_commands.py build/TargetedCompileCommands/linuxx86-64release/compile_commands.json ./.github/workflows/fix_compile_commands.py build/TargetedCompileCommands/linuxx86-64debug/compile_commands.json - name: List changed files - run: wpiformat -list-changed-files + run: ${{ runner.temp }}/wpiformat/bin/wpiformat -list-changed-files - name: Run clang-tidy release - run: wpiformat -no-format -tidy-changed -compile-commands=build/TargetedCompileCommands/linuxx86-64release -vv + run: ${{ runner.temp }}/wpiformat/bin/wpiformat -no-format -tidy-changed -compile-commands=build/TargetedCompileCommands/linuxx86-64release -vv - name: Run clang-tidy debug - run: wpiformat -no-format -tidy-changed -compile-commands=build/TargetedCompileCommands/linuxx86-64debug -vv + run: ${{ runner.temp }}/wpiformat/bin/wpiformat -no-format -tidy-changed -compile-commands=build/TargetedCompileCommands/linuxx86-64debug -vv javaformat: name: "Java format" - runs-on: ubuntu-22.04 - container: wpilib/ubuntu-base:22.04 + runs-on: ubuntu-24.04 + container: wpilib/ubuntu-base:24.04 steps: - uses: actions/checkout@v4 with: @@ -113,7 +117,7 @@ jobs: documentation: name: "Documentation" - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - uses: actions/checkout@v4 with: diff --git a/.github/workflows/pregenerate.yml b/.github/workflows/pregenerate.yml index 12e09da6a58..cb8ed0c70cb 100644 --- a/.github/workflows/pregenerate.yml +++ b/.github/workflows/pregenerate.yml @@ -13,7 +13,7 @@ concurrency: jobs: update: name: "Update" - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - uses: actions/checkout@v4 with: diff --git a/.github/workflows/sanitizers.yml b/.github/workflows/sanitizers.yml index c32a3a05b8f..8640bcd5aff 100644 --- a/.github/workflows/sanitizers.yml +++ b/.github/workflows/sanitizers.yml @@ -29,11 +29,11 @@ jobs: ctest-env: "" ctest-flags: "" name: "${{ matrix.name }}" - runs-on: ubuntu-22.04 - container: wpilib/roborio-cross-ubuntu:2025-22.04 + runs-on: ubuntu-24.04 + container: wpilib/roborio-cross-ubuntu:2025-24.04 steps: - name: Install Dependencies - run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv4.5-java clang-14 libprotobuf-dev protobuf-compiler ninja-build + run: sudo apt-get update && sudo apt-get install -y libopencv-dev libopencv-java clang-18 libprotobuf-dev protobuf-compiler ninja-build - name: Install sccache uses: mozilla-actions/sccache-action@v0.0.5 @@ -41,7 +41,7 @@ jobs: - uses: actions/checkout@v4 - name: configure - run: mkdir build && cd build && cmake -G Ninja -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_C_COMPILER:FILEPATH=/usr/bin/clang-14 -DCMAKE_CXX_COMPILER:FILEPATH=/usr/bin/clang++-14 -DWITH_JAVA=OFF ${{ matrix.cmake-flags }} .. + run: mkdir build && cd build && cmake -G Ninja -DCMAKE_C_COMPILER_LAUNCHER=sccache -DCMAKE_CXX_COMPILER_LAUNCHER=sccache -DCMAKE_C_COMPILER:FILEPATH=/usr/bin/clang-18 -DCMAKE_CXX_COMPILER:FILEPATH=/usr/bin/clang++-18 -DWITH_JAVA=OFF ${{ matrix.cmake-flags }} .. env: SCCACHE_WEBDAV_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }} SCCACHE_WEBDAV_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }} diff --git a/.github/workflows/sentinel-build.yml b/.github/workflows/sentinel-build.yml index c75f577e1ba..866d54b9fb7 100644 --- a/.github/workflows/sentinel-build.yml +++ b/.github/workflows/sentinel-build.yml @@ -16,20 +16,20 @@ jobs: fail-fast: false matrix: include: - - container: wpilib/roborio-cross-ubuntu:2025-22.04 + - container: wpilib/roborio-cross-ubuntu:2025-24.04 artifact-name: Athena build-options: "-Ponlylinuxathena" - - container: wpilib/raspbian-cross-ubuntu:bookworm-22.04 + - container: wpilib/raspbian-cross-ubuntu:bookworm-24.04 artifact-name: Arm32 build-options: "-Ponlylinuxarm32" - - container: wpilib/aarch64-cross-ubuntu:bookworm-22.04 + - container: wpilib/aarch64-cross-ubuntu:bookworm-24.04 artifact-name: Arm64 build-options: "-Ponlylinuxarm64" - - container: wpilib/ubuntu-base:22.04 + - container: wpilib/ubuntu-base:24.04 artifact-name: Linux build-options: "-Ponlylinuxx86-64" name: "Build - ${{ matrix.artifact-name }}" - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - name: Free Disk Space uses: jlumbroso/free-disk-space@main diff --git a/.github/workflows/tools.yml b/.github/workflows/tools.yml index aa669a160c4..494cc575dff 100644 --- a/.github/workflows/tools.yml +++ b/.github/workflows/tools.yml @@ -12,7 +12,7 @@ env: jobs: build-artifacts: name: "Build - WPILib" - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 env: DISPLAY: ':10' steps: @@ -32,7 +32,7 @@ jobs: - name: Build WPILib with Gradle uses: addnab/docker-run-action@v3 with: - image: wpilib/systemcore-cross-ubuntu:2025-22.04 + image: wpilib/systemcore-cross-ubuntu:2025-24.04 options: -v ${{ github.workspace }}:/work -w /work -e GITHUB_REF -e CI -e DISPLAY run: df . && rm -f semicolon_delimited_script && ./gradlew -Pskiplinuxarm64 :wpilibc:publish :wpilibj:publish :wpilibNewCommands:publish :hal:publish :cameraserver:publish :ntcore:publish :cscore:publish :wpimath:publish :wpinet:publish :wpiutil:publish :apriltag:publish :wpiunits:publish :simulation:halsim_gui:publish :simulation:halsim_ds_socket:publish :fieldImages:publish :epilogue-processor:publish :epilogue-runtime:publish :thirdparty:googletest:publish -x test -x Javadoc -x doxygen --build-cache && cp -r /root/releases/maven/development /work - uses: actions/upload-artifact@v4 @@ -45,7 +45,7 @@ jobs: # Robotbuilder: # name: "Build - RobotBuilder" # needs: [build-artifacts] - # runs-on: ubuntu-22.04 + # runs-on: ubuntu-24.04 # env: # DISPLAY: ':10' # steps: @@ -92,7 +92,7 @@ jobs: Shuffleboard: name: "Build - Shuffleboard" needs: [build-artifacts] - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - uses: actions/checkout@v4 with: @@ -123,7 +123,7 @@ jobs: PathWeaver: name: "Build - PathWeaver" needs: [build-artifacts] - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - uses: actions/checkout@v4 with: diff --git a/.github/workflows/upstream-utils.yml b/.github/workflows/upstream-utils.yml index 587c7a6b3b6..fc8108e0133 100644 --- a/.github/workflows/upstream-utils.yml +++ b/.github/workflows/upstream-utils.yml @@ -13,7 +13,7 @@ concurrency: jobs: update: name: "Update" - runs-on: ubuntu-22.04 + runs-on: ubuntu-24.04 steps: - uses: actions/checkout@v4 with: From 62a6a77bbfc38984621be12293f6c4b4d984edf6 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 7 Dec 2024 21:29:02 -0800 Subject: [PATCH 7/9] [wpimath] Add affine transformation constructors and getters to geometry API (#7509) Fixes #7429. --- .../edu/wpi/first/math/StateSpaceUtil.java | 9 ++++ .../edu/wpi/first/math/geometry/Pose2d.java | 40 +++++++++++++++ .../edu/wpi/first/math/geometry/Pose3d.java | 50 +++++++++++++++++++ .../wpi/first/math/geometry/Rotation2d.java | 46 +++++++++++++++++ .../wpi/first/math/geometry/Rotation3d.java | 27 ++++++++++ .../wpi/first/math/geometry/Transform2d.java | 40 +++++++++++++++ .../wpi/first/math/geometry/Transform3d.java | 50 +++++++++++++++++++ .../first/math/geometry/Translation2d.java | 10 ++-- .../first/math/geometry/Translation3d.java | 10 ++-- .../main/native/include/frc/StateSpaceUtil.h | 21 +++++--- .../src/main/native/include/frc/ct_matrix.h | 19 +++++++ .../main/native/include/frc/geometry/Pose2d.h | 26 ++++++++++ .../main/native/include/frc/geometry/Pose3d.h | 32 ++++++++++++ .../native/include/frc/geometry/Rotation2d.h | 48 ++++++++++++++++++ .../native/include/frc/geometry/Rotation3d.h | 18 +++++++ .../native/include/frc/geometry/Transform2d.h | 27 ++++++++++ .../native/include/frc/geometry/Transform3d.h | 32 ++++++++++++ .../include/frc/geometry/Translation2d.h | 12 ++--- .../include/frc/geometry/Translation3d.h | 10 ++-- .../wpi/first/math/StateSpaceUtilTest.java | 1 + .../wpi/first/math/geometry/Pose2dTest.java | 8 +++ .../wpi/first/math/geometry/Pose3dTest.java | 16 ++++++ .../first/math/geometry/Rotation2dTest.java | 8 +++ .../first/math/geometry/Rotation3dTest.java | 12 +++++ .../first/math/geometry/Transform2dTest.java | 8 +++ .../first/math/geometry/Transform3dTest.java | 16 ++++++ .../test/native/cpp/geometry/Pose2dTest.cpp | 7 +++ .../test/native/cpp/geometry/Pose3dTest.cpp | 7 +++ .../native/cpp/geometry/Rotation2dTest.cpp | 7 +++ .../native/cpp/geometry/Rotation3dTest.cpp | 7 +++ .../native/cpp/geometry/Transform2dTest.cpp | 7 +++ .../native/cpp/geometry/Transform3dTest.cpp | 7 +++ 32 files changed, 611 insertions(+), 27 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java index 461d9a6f080..eb494308926 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java @@ -131,7 +131,10 @@ public static boolean isDetectable( * * @param pose A pose to convert to a vector. * @return The given pose in vector form, with the third element, theta, in radians. + * @deprecated Create the vector manually instead. If you were using this as an intermediate step + * for constructing affine transformations, use {@link Pose2d#toMatrix()} instead. */ + @Deprecated(forRemoval = true, since = "2025") public static Matrix poseToVector(Pose2d pose) { return VecBuilder.fill(pose.getX(), pose.getY(), pose.getRotation().getRadians()); } @@ -180,7 +183,10 @@ public static Matrix desaturateInputVector( * * @param pose A pose to convert to a vector. * @return The given pose in as a 4x1 vector of x, y, cos(theta), and sin(theta). + * @deprecated Create the vector manually instead. If you were using this as an intermediate step + * for constructing affine transformations, use {@link Pose2d#toMatrix()} instead. */ + @Deprecated(forRemoval = true, since = "2025") public static Matrix poseTo4dVector(Pose2d pose) { return VecBuilder.fill( pose.getTranslation().getX(), @@ -194,7 +200,10 @@ public static Matrix poseTo4dVector(Pose2d pose) { * * @param pose A pose to convert to a vector. * @return The given pose in vector form, with the third element, theta, in radians. + * @deprecated Create the vector manually instead. If you were using this as an intermediate step + * for constructing affine transformations, use {@link Pose2d#toMatrix()} instead. */ + @Deprecated(forRemoval = true, since = "2025") public static Matrix poseTo3dVector(Pose2d pose) { return VecBuilder.fill( pose.getTranslation().getX(), diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index 89c404fddbd..0705d61ea7f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -10,9 +10,13 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.proto.Pose2dProto; import edu.wpi.first.math.geometry.struct.Pose2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -79,6 +83,20 @@ public Pose2d(Distance x, Distance y, Rotation2d rotation) { this(x.in(Meters), y.in(Meters), rotation); } + /** + * Constructs a pose with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws IllegalArgumentException if the affine transformation matrix is invalid. + */ + public Pose2d(Matrix matrix) { + m_translation = new Translation2d(matrix.get(0, 2), matrix.get(1, 2)); + m_rotation = new Rotation2d(matrix.block(2, 2, 0, 0)); + if (matrix.get(2, 0) != 0.0 || matrix.get(2, 1) != 0.0 || matrix.get(2, 2) != 1.0) { + throw new IllegalArgumentException("Affine transformation matrix is invalid"); + } + } + /** * Transforms the pose by the given transformation and returns the new transformed pose. * @@ -295,6 +313,28 @@ public Twist2d log(Pose2d end) { return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta); } + /** + * Returns an affine transformation matrix representation of this pose. + * + * @return An affine transformation matrix representation of this pose. + */ + public Matrix toMatrix() { + var vec = m_translation.toVector(); + var mat = m_rotation.toMatrix(); + return MatBuilder.fill( + Nat.N3(), + Nat.N3(), + mat.get(0, 0), + mat.get(0, 1), + vec.get(0), + mat.get(1, 0), + mat.get(1, 1), + vec.get(1), + 0.0, + 0.0, + 1.0); + } + /** * Returns the nearest Pose2d from a list of poses. If two or more poses in the list have the same * distance from this pose, return the one with the closest rotation component. diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index ada3e9059db..02c2b9ba4eb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -10,10 +10,14 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.proto.Pose3dProto; import edu.wpi.first.math.geometry.struct.Pose3dStruct; import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.jni.Pose3dJNI; +import edu.wpi.first.math.numbers.N4; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -79,6 +83,23 @@ public Pose3d(Distance x, Distance y, Distance z, Rotation3d rotation) { this(x.in(Meters), y.in(Meters), z.in(Meters), rotation); } + /** + * Constructs a pose with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws IllegalArgumentException if the affine transformation matrix is invalid. + */ + public Pose3d(Matrix matrix) { + m_translation = new Translation3d(matrix.get(0, 3), matrix.get(1, 3), matrix.get(2, 3)); + m_rotation = new Rotation3d(matrix.block(3, 3, 0, 0)); + if (matrix.get(3, 0) != 0.0 + || matrix.get(3, 1) != 0.0 + || matrix.get(3, 2) != 0.0 + || matrix.get(3, 3) != 1.0) { + throw new IllegalArgumentException("Affine transformation matrix is invalid"); + } + } + /** * Constructs a 3D pose from a 2D pose in the X-Y plane. * @@ -326,6 +347,35 @@ public Twist3d log(Pose3d end) { resultArray[5]); } + /** + * Returns an affine transformation matrix representation of this pose. + * + * @return An affine transformation matrix representation of this pose. + */ + public Matrix toMatrix() { + var vec = m_translation.toVector(); + var mat = m_rotation.toMatrix(); + return MatBuilder.fill( + Nat.N4(), + Nat.N4(), + mat.get(0, 0), + mat.get(0, 1), + mat.get(0, 2), + vec.get(0), + mat.get(1, 0), + mat.get(1, 1), + mat.get(1, 2), + vec.get(1), + mat.get(2, 0), + mat.get(2, 1), + mat.get(2, 2), + vec.get(2), + 0.0, + 0.0, + 0.0, + 1.0); + } + /** * Returns a Pose2d representing this Pose3d projected into the X-Y plane. * 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 b633a45ef3c..54a4fbfcf0e 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 @@ -10,11 +10,15 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +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.geometry.proto.Rotation2dProto; import edu.wpi.first.math.geometry.struct.Rotation2dStruct; import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.util.protobuf.ProtobufSerializable; @@ -125,6 +129,37 @@ public Rotation2d(Angle angle) { this(angle.in(Radians)); } + /** + * Constructs a Rotation2d from a rotation matrix. + * + * @param rotationMatrix The rotation matrix. + * @throws IllegalArgumentException if the rotation matrix isn't special orthogonal. + */ + public Rotation2d(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()).minus(Matrix.eye(Nat.N2())).normF() > 1e-9) { + var msg = "Rotation matrix isn't orthogonal\n\nR =\n" + R.getStorage().toString() + '\n'; + MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace()); + throw new IllegalArgumentException(msg); + } + if (Math.abs(R.det() - 1.0) > 1e-9) { + var msg = + "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n" + + R.getStorage().toString() + + '\n'; + MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace()); + throw new IllegalArgumentException(msg); + } + + // R = [cosθ −sinθ] + // [sinθ cosθ] + m_cos = R.get(0, 0); + m_sin = R.get(1, 0); + } + /** * Constructs and returns a Rotation2d with the given radian value. * @@ -230,6 +265,17 @@ public Rotation2d rotateBy(Rotation2d other) { m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos); } + /** + * Returns matrix representation of this rotation. + * + * @return Matrix representation of this rotation. + */ + public Matrix toMatrix() { + // R = [cosθ −sinθ] + // [sinθ cosθ] + return MatBuilder.fill(Nat.N2(), Nat.N2(), m_cos, -m_sin, m_sin, m_cos); + } + /** * Returns the measure of the Rotation2d. * 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 8883e665389..7d134774957 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 @@ -10,6 +10,7 @@ import com.fasterxml.jackson.annotation.JsonCreator; import com.fasterxml.jackson.annotation.JsonIgnoreProperties; import com.fasterxml.jackson.annotation.JsonProperty; +import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Matrix; @@ -458,6 +459,32 @@ public Angle getMeasureZ() { return Radians.of(getZ()); } + /** + * Returns rotation matrix representation of this rotation. + * + * @return Rotation matrix representation of this rotation. + */ + public Matrix toMatrix() { + double w = m_q.getW(); + double x = m_q.getX(); + double y = m_q.getY(); + double z = m_q.getZ(); + + // https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix + return MatBuilder.fill( + Nat.N3(), + Nat.N3(), + 1.0 - 2.0 * (y * y + z * z), + 2.0 * (x * y - w * z), + 2.0 * (x * z + w * y), + 2.0 * (x * y + w * z), + 1.0 - 2.0 * (x * x + z * z), + 2.0 * (y * z - w * x), + 2.0 * (x * z - w * y), + 2.0 * (y * z + w * x), + 1.0 - 2.0 * (x * x + y * y)); + } + /** * Returns the axis in the axis-angle representation of this rotation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java index 79dc16939e9..7a4cf8f4f9a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java @@ -6,8 +6,12 @@ import static edu.wpi.first.units.Units.Meters; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.proto.Transform2dProto; import edu.wpi.first.math.geometry.struct.Transform2dStruct; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -78,6 +82,20 @@ public Transform2d(Distance x, Distance y, Rotation2d rotation) { this(x.in(Meters), y.in(Meters), rotation); } + /** + * Constructs a transform with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws IllegalArgumentException if the affine transformation matrix is invalid. + */ + public Transform2d(Matrix matrix) { + m_translation = new Translation2d(matrix.get(0, 2), matrix.get(1, 2)); + m_rotation = new Rotation2d(matrix.block(2, 2, 0, 0)); + if (matrix.get(2, 0) != 0.0 || matrix.get(2, 1) != 0.0 || matrix.get(2, 2) != 1.0) { + throw new IllegalArgumentException("Affine transformation matrix is invalid"); + } + } + /** Constructs the identity transform -- maps an initial pose to itself. */ public Transform2d() { m_translation = Translation2d.kZero; @@ -160,6 +178,28 @@ public Distance getMeasureY() { return m_translation.getMeasureY(); } + /** + * Returns an affine transformation matrix representation of this transformation. + * + * @return An affine transformation matrix representation of this transformation. + */ + public Matrix toMatrix() { + var vec = m_translation.toVector(); + var mat = m_rotation.toMatrix(); + return MatBuilder.fill( + Nat.N3(), + Nat.N3(), + mat.get(0, 0), + mat.get(0, 1), + vec.get(0), + mat.get(1, 0), + mat.get(1, 1), + vec.get(1), + 0.0, + 0.0, + 1.0); + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java index 8332b607c20..d993d861001 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java @@ -6,8 +6,12 @@ import static edu.wpi.first.units.Units.Meters; +import edu.wpi.first.math.MatBuilder; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; import edu.wpi.first.math.geometry.proto.Transform3dProto; import edu.wpi.first.math.geometry.struct.Transform3dStruct; +import edu.wpi.first.math.numbers.N4; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.util.protobuf.ProtobufSerializable; import edu.wpi.first.util.struct.StructSerializable; @@ -80,6 +84,23 @@ public Transform3d(Distance x, Distance y, Distance z, Rotation3d rotation) { this(x.in(Meters), y.in(Meters), z.in(Meters), rotation); } + /** + * Constructs a transform with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws IllegalArgumentException if the affine transformation matrix is invalid. + */ + public Transform3d(Matrix matrix) { + m_translation = new Translation3d(matrix.get(0, 3), matrix.get(1, 3), matrix.get(2, 3)); + m_rotation = new Rotation3d(matrix.block(3, 3, 0, 0)); + if (matrix.get(3, 0) != 0.0 + || matrix.get(3, 1) != 0.0 + || matrix.get(3, 2) != 0.0 + || matrix.get(3, 3) != 1.0) { + throw new IllegalArgumentException("Affine transformation matrix is invalid"); + } + } + /** Constructs the identity transform -- maps an initial pose to itself. */ public Transform3d() { m_translation = Translation3d.kZero; @@ -192,6 +213,35 @@ public Distance getMeasureZ() { return m_translation.getMeasureZ(); } + /** + * Returns an affine transformation matrix representation of this transformation. + * + * @return An affine transformation matrix representation of this transformation. + */ + public Matrix toMatrix() { + var vec = m_translation.toVector(); + var mat = m_rotation.toMatrix(); + return MatBuilder.fill( + Nat.N4(), + Nat.N4(), + mat.get(0, 0), + mat.get(0, 1), + mat.get(0, 2), + vec.get(0), + mat.get(1, 0), + mat.get(1, 1), + mat.get(1, 2), + vec.get(1), + mat.get(2, 0), + mat.get(2, 1), + mat.get(2, 2), + vec.get(2), + 0.0, + 0.0, + 0.0, + 1.0); + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index e3857eb14e6..9b4af815c72 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -88,10 +88,10 @@ public Translation2d(Distance x, Distance y) { } /** - * Constructs a Translation2d from the provided translation vector's X and Y components. The - * values are assumed to be in meters. + * Constructs a Translation2d from a 2D translation vector. The values are assumed to be in + * meters. * - * @param vector The translation vector to represent. + * @param vector The translation vector. */ public Translation2d(Vector vector) { this(vector.get(0), vector.get(1)); @@ -148,9 +148,9 @@ public Distance getMeasureY() { } /** - * Returns a vector representation of this translation. + * Returns a 2D translation vector representation of this translation. * - * @return A Vector representation of this translation. + * @return A 2D translation vector representation of this translation. */ public Vector toVector() { return VecBuilder.fill(m_x, m_y); diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 48a70c7f45a..20cede3d3da 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -104,10 +104,10 @@ public Translation3d(Translation2d translation) { } /** - * Constructs a Translation3d from the provided translation vector's X, Y, and Z components. The - * values are assumed to be in meters. + * Constructs a Translation3d from a 3D translation vector. The values are assumed to be in + * meters. * - * @param vector The translation vector to represent. + * @param vector The translation vector. */ public Translation3d(Vector vector) { this(vector.get(0), vector.get(1), vector.get(2)); @@ -184,9 +184,9 @@ public Distance getMeasureZ() { } /** - * Returns a vector representation of this translation. + * Returns a 2D translation vector representation of this translation. * - * @return A Vector representation of this translation. + * @return A 2D translation vector representation of this translation. */ public Vector toVector() { return VecBuilder.fill(m_x, m_y, m_z); diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h index 785255d6748..1e8ae2a1081 100644 --- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -206,9 +206,12 @@ Vectord MakeWhiteNoiseVector(const std::array& stdDevs) { * @param pose The pose that is being represented. * * @return The vector. + * @deprecated Create the vector manually instead. If you were using this as an + * intermediate step for constructing affine transformations, use + * Pose2d.ToMatrix() instead. */ -WPILIB_DLLEXPORT -constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) { +[[deprecated("Use Pose2d.ToMatrix() instead.")]] +WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) { return Eigen::Vector3d{{pose.Translation().X().value(), pose.Translation().Y().value(), pose.Rotation().Radians().value()}}; @@ -220,9 +223,12 @@ constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) { * @param pose The pose that is being represented. * * @return The vector. + * @deprecated Create the vector manually instead. If you were using this as an + * intermediate step for constructing affine transformations, use + * Pose2d.ToMatrix() instead. */ -WPILIB_DLLEXPORT -constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) { +[[deprecated("Use Pose2d.ToMatrix() instead.")]] +WPILIB_DLLEXPORT constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) { return Eigen::Vector4d{{pose.Translation().X().value(), pose.Translation().Y().value(), pose.Rotation().Cos(), pose.Rotation().Sin()}}; @@ -311,9 +317,12 @@ bool IsDetectable(const Matrixd& A, * @param pose The pose that is being represented. * * @return The vector. + * @deprecated Create the vector manually instead. If you were using this as an + * intermediate step for constructing affine transformations, use + * Pose2d.ToMatrix() instead. */ -WPILIB_DLLEXPORT -constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) { +[[deprecated("Use Pose2d.ToMatrix() instead.")]] +WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) { return Eigen::Vector3d{ {pose.X().value(), pose.Y().value(), pose.Rotation().Radians().value()}}; } diff --git a/wpimath/src/main/native/include/frc/ct_matrix.h b/wpimath/src/main/native/include/frc/ct_matrix.h index 7b6f9056e7c..ce339db276e 100644 --- a/wpimath/src/main/native/include/frc/ct_matrix.h +++ b/wpimath/src/main/native/include/frc/ct_matrix.h @@ -308,6 +308,23 @@ class ct_matrix { (*this)(0) * rhs(1) - rhs(0) * (*this)(1)}}; } + /** + * Constexpr version of Eigen's 2x2 matrix determinant member function. + * + * @return Determinant of matrix. + */ + constexpr Scalar determinant() const + requires(Rows == 2 && Cols == 2) + { + // |a b| + // |c d| = ad - bc + Scalar a = (*this)(0, 0); + Scalar b = (*this)(0, 1); + Scalar c = (*this)(1, 0); + Scalar d = (*this)(1, 1); + return a * d - b * c; + } + /** * Constexpr version of Eigen's 3x3 matrix determinant member function. * @@ -364,7 +381,9 @@ using ct_vector = ct_matrix; template using ct_row_vector = ct_matrix; +using ct_matrix2d = ct_matrix; using ct_matrix3d = ct_matrix; +using ct_vector2d = ct_vector; using ct_vector3d = ct_vector; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h index 016de906245..0664161d6d3 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h @@ -53,6 +53,21 @@ class WPILIB_DLLEXPORT Pose2d { constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation) : m_translation{x, y}, m_rotation{std::move(rotation)} {} + /** + * Constructs a pose with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws std::domain_error if the affine transformation matrix is invalid. + */ + constexpr explicit Pose2d(const Eigen::Matrix3d& matrix) + : m_translation{Eigen::Vector2d{{matrix(0, 2)}, {matrix(1, 2)}}}, + m_rotation{Eigen::Matrix2d{{matrix(0, 0), matrix(0, 1)}, + {matrix(1, 0), matrix(1, 1)}}} { + if (matrix(2, 0) != 0.0 || matrix(2, 1) != 0.0 || matrix(2, 2) != 1.0) { + throw std::domain_error("Affine transformation matrix is invalid"); + } + } + /** * Transforms the pose by the given transformation and returns the new * transformed pose. @@ -202,6 +217,17 @@ class WPILIB_DLLEXPORT Pose2d { */ constexpr Twist2d Log(const Pose2d& end) const; + /** + * Returns an affine transformation matrix representation of this pose. + */ + constexpr Eigen::Matrix3d ToMatrix() const { + auto vec = m_translation.ToVector(); + auto mat = m_rotation.ToMatrix(); + return Eigen::Matrix3d{{mat(0, 0), mat(0, 1), vec(0)}, + {mat(1, 0), mat(1, 1), vec(1)}, + {0.0, 0.0, 1.0}}; + } + /** * Returns the nearest Pose2d from a collection of poses * @param poses The collection of poses. diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h index dc6a7416fd0..1f377a8ce71 100644 --- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h @@ -4,6 +4,7 @@ #pragma once +#include #include #include @@ -54,6 +55,25 @@ class WPILIB_DLLEXPORT Pose3d { Rotation3d rotation) : m_translation{x, y, z}, m_rotation{std::move(rotation)} {} + /** + * Constructs a pose with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws std::domain_error if the affine transformation matrix is invalid. + */ + constexpr explicit Pose3d(const Eigen::Matrix4d& matrix) + : m_translation{Eigen::Vector3d{ + {matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}}, + m_rotation{ + Eigen::Matrix3d{{matrix(0, 0), matrix(0, 1), matrix(0, 2)}, + {matrix(1, 0), matrix(1, 1), matrix(1, 2)}, + {matrix(2, 0), matrix(2, 1), matrix(2, 2)}}} { + if (matrix(3, 0) != 0.0 || matrix(3, 1) != 0.0 || matrix(3, 2) != 0.0 || + matrix(3, 3) != 1.0) { + throw std::domain_error("Affine transformation matrix is invalid"); + } + } + /** * Constructs a 3D pose from a 2D pose in the X-Y plane. * @@ -218,6 +238,18 @@ class WPILIB_DLLEXPORT Pose3d { */ constexpr Twist3d Log(const Pose3d& end) const; + /** + * Returns an affine transformation matrix representation of this pose. + */ + constexpr Eigen::Matrix4d ToMatrix() const { + auto vec = m_translation.ToVector(); + auto mat = m_rotation.ToMatrix(); + return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)}, + {mat(1, 0), mat(1, 1), mat(1, 2), vec(1)}, + {mat(2, 0), mat(2, 1), mat(2, 2), vec(2)}, + {0.0, 0.0, 0.0, 1.0}}; + } + /** * Returns a Pose2d representing this Pose3d projected into the X-Y plane. */ diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h index f94d06216c3..2c3c94540ab 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h @@ -5,12 +5,16 @@ #pragma once #include +#include +#include +#include #include #include #include #include +#include "frc/ct_matrix.h" #include "units/angle.h" #include "wpimath/MathShared.h" @@ -59,6 +63,41 @@ class WPILIB_DLLEXPORT Rotation2d { } } + /** + * Constructs a Rotation2d from a rotation matrix. + * + * @param rotationMatrix The rotation matrix. + * @throws std::domain_error if the rotation matrix isn't special orthogonal. + */ + constexpr explicit Rotation2d(const Eigen::Matrix2d& rotationMatrix) { + auto impl = + [](const Matrix2d& R) -> std::pair { + // 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() - Matrix2d::Identity()).norm() > 1e-9) { + throw std::domain_error("Rotation matrix isn't orthogonal"); + } + if (gcem::abs(R.determinant() - 1.0) > 1e-9) { + throw std::domain_error( + "Rotation matrix is orthogonal but not special orthogonal"); + } + + // R = [cosθ −sinθ] + // [sinθ cosθ] + return {R(0, 0), R(1, 0)}; + }; + + if (std::is_constant_evaluated()) { + auto cossin = impl(ct_matrix2d{rotationMatrix}); + m_cos = std::get<0>(cossin); + m_sin = std::get<1>(cossin); + } else { + auto cossin = impl(rotationMatrix); + m_cos = std::get<0>(cossin); + m_sin = std::get<1>(cossin); + } + } + /** * Adds two rotations together, with the result being bounded between -π and * π. @@ -147,6 +186,15 @@ class WPILIB_DLLEXPORT Rotation2d { Cos() * other.Sin() + Sin() * other.Cos()}; } + /** + * Returns matrix representation of this rotation. + */ + constexpr Eigen::Matrix2d ToMatrix() const { + // R = [cosθ −sinθ] + // [sinθ cosθ] + return Eigen::Matrix2d{{m_cos, -m_sin}, {m_sin, m_cos}}; + } + /** * Returns the radian value of the rotation constrained within [-π, π]. * diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h index f27d1d94131..47aa165b7c4 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h @@ -403,6 +403,24 @@ class WPILIB_DLLEXPORT Rotation3d { return units::radian_t{2.0 * gcem::atan2(norm, m_q.W())}; } + /** + * Returns rotation matrix representation of this rotation. + */ + constexpr Eigen::Matrix3d ToMatrix() const { + double w = m_q.W(); + double x = m_q.X(); + double y = m_q.Y(); + double z = m_q.Z(); + + // https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix + return Eigen::Matrix3d{{1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - w * z), + 2.0 * (x * z + w * y)}, + {2.0 * (x * y + w * z), 1.0 - 2.0 * (x * x + z * z), + 2.0 * (y * z - w * x)}, + {2.0 * (x * z - w * y), 2.0 * (y * z + w * x), + 1.0 - 2.0 * (x * x + y * y)}}; + } + /** * Returns a Rotation2d representing this Rotation3d projected into the X-Y * plane. diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h index abf5c1ff4de..88c00550d04 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h @@ -49,6 +49,21 @@ class WPILIB_DLLEXPORT Transform2d { constexpr Transform2d(units::meter_t x, units::meter_t y, Rotation2d rotation) : m_translation{x, y}, m_rotation{std::move(rotation)} {} + /** + * Constructs a pose with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws std::domain_error if the affine transformation matrix is invalid. + */ + constexpr explicit Transform2d(const Eigen::Matrix3d& matrix) + : m_translation{Eigen::Vector2d{{matrix(0, 2)}, {matrix(1, 2)}}}, + m_rotation{Eigen::Matrix2d{{matrix(0, 0), matrix(0, 1)}, + {matrix(1, 0), matrix(1, 1)}}} { + if (matrix(2, 0) != 0.0 || matrix(2, 1) != 0.0 || matrix(2, 2) != 1.0) { + throw std::domain_error("Affine transformation matrix is invalid"); + } + } + /** * Constructs the identity transform -- maps an initial pose to itself. */ @@ -75,6 +90,18 @@ class WPILIB_DLLEXPORT Transform2d { */ constexpr units::meter_t Y() const { return m_translation.Y(); } + /** + * Returns an affine transformation matrix representation of this + * transformation. + */ + constexpr Eigen::Matrix3d ToMatrix() const { + auto vec = m_translation.ToVector(); + auto mat = m_rotation.ToMatrix(); + return Eigen::Matrix3d{{mat(0, 0), mat(0, 1), vec(0)}, + {mat(1, 0), mat(1, 1), vec(1)}, + {0.0, 0.0, 1.0}}; + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h index 4716b7181e5..bd6ca9c41e0 100644 --- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h @@ -51,6 +51,25 @@ class WPILIB_DLLEXPORT Transform3d { Rotation3d rotation) : m_translation{x, y, z}, m_rotation{std::move(rotation)} {} + /** + * Constructs a transform with the specified affine transformation matrix. + * + * @param matrix The affine transformation matrix. + * @throws std::domain_error if the affine transformation matrix is invalid. + */ + constexpr explicit Transform3d(const Eigen::Matrix4d& matrix) + : m_translation{Eigen::Vector3d{ + {matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}}, + m_rotation{ + Eigen::Matrix3d{{matrix(0, 0), matrix(0, 1), matrix(0, 2)}, + {matrix(1, 0), matrix(1, 1), matrix(1, 2)}, + {matrix(2, 0), matrix(2, 1), matrix(2, 2)}}} { + if (matrix(3, 0) != 0.0 || matrix(3, 1) != 0.0 || matrix(3, 2) != 0.0 || + matrix(3, 3) != 1.0) { + throw std::domain_error("Affine transformation matrix is invalid"); + } + } + /** * Constructs the identity transform -- maps an initial pose to itself. */ @@ -95,6 +114,19 @@ class WPILIB_DLLEXPORT Transform3d { */ constexpr units::meter_t Z() const { return m_translation.Z(); } + /** + * Returns an affine transformation matrix representation of this + * transformation. + */ + constexpr Eigen::Matrix4d ToMatrix() const { + auto vec = m_translation.ToVector(); + auto mat = m_rotation.ToMatrix(); + return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)}, + {mat(1, 0), mat(1, 1), mat(1, 2), vec(1)}, + {mat(2, 0), mat(2, 1), mat(2, 2), vec(2)}, + {0.0, 0.0, 0.0, 1.0}}; + } + /** * Returns the rotational component of the transformation. * diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h index a8a26b8bff5..aa6c4753ef7 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h @@ -54,13 +54,13 @@ class WPILIB_DLLEXPORT Translation2d { : m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {} /** - * Constructs a Translation2d from the provided translation vector's X and Y - * components. The values are assumed to be in meters. + * Constructs a Translation2d from a 2D translation vector. The values are + * assumed to be in meters. * - * @param vector The translation vector to represent. + * @param vector The translation vector. */ constexpr explicit Translation2d(const Eigen::Vector2d& vector) - : m_x{units::meter_t{vector(0)}}, m_y{units::meter_t{vector(1)}} {} + : m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {} /** * Calculates the distance between two translations in 2D space. @@ -90,9 +90,9 @@ class WPILIB_DLLEXPORT Translation2d { constexpr units::meter_t Y() const { return m_y; } /** - * Returns a vector representation of this translation. + * Returns a 2D translation vector representation of this translation. * - * @return A Vector representation of this translation. + * @return A 2D translation vector representation of this translation. */ constexpr Eigen::Vector2d ToVector() const { return Eigen::Vector2d{{m_x.value(), m_y.value()}}; diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h index fb3194b8ced..c16fc56a5b8 100644 --- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h +++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h @@ -56,10 +56,10 @@ class WPILIB_DLLEXPORT Translation3d { } /** - * Constructs a Translation3d from the provided translation vector's X, Y, and - * Z components. The values are assumed to be in meters. + * Constructs a Translation3d from a 3D translation vector. The values are + * assumed to be in meters. * - * @param vector The translation vector to represent. + * @param vector The translation vector. */ constexpr explicit Translation3d(const Eigen::Vector3d& vector) : m_x{units::meter_t{vector.x()}}, @@ -114,9 +114,9 @@ class WPILIB_DLLEXPORT Translation3d { constexpr units::meter_t Z() const { return m_z; } /** - * Returns a vector representation of this translation. + * Returns a 3D translation vector representation of this translation. * - * @return A Vector representation of this translation. + * @return A 3D translation vector representation of this translation. */ constexpr Eigen::Vector3d ToVector() const { return Eigen::Vector3d{{m_x.value(), m_y.value(), m_z.value()}}; diff --git a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java index 4883e6f5ce0..1715d93dc69 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java @@ -153,6 +153,7 @@ void testMatrixExp() { } @Test + @SuppressWarnings("removal") void testPoseToVector() { Pose2d pose = new Pose2d(1, 2, new Rotation2d(3)); var vector = StateSpaceUtil.poseToVector(pose); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java index 0844e1321e9..81f81509929 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java @@ -86,6 +86,14 @@ void testInequality() { assertNotEquals(one, two); } + @Test + void testToMatrix() { + var before = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(20.0)); + var after = new Pose2d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testMinus() { var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java index b333d24592f..00c63eae080 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java @@ -169,6 +169,22 @@ void testMinus() { () -> assertEquals(0.0, transform.getRotation().getZ(), kEpsilon)); } + @Test + void testToMatrix() { + var before = + new Pose3d( + 1.0, + 2.0, + 3.0, + new Rotation3d( + Units.degreesToRadians(20.0), + Units.degreesToRadians(30.0), + Units.degreesToRadians(40.0))); + var after = new Pose3d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testToPose2d() { var pose = diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java index c91df94a701..63ff7a06b0c 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java @@ -100,6 +100,14 @@ void testInequality() { assertNotEquals(rot1, rot2); } + @Test + void testToMatrix() { + var before = Rotation2d.fromDegrees(20.0); + var after = new Rotation2d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testInterpolate() { // 50 + (70 - 50) * 0.5 = 60 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 43b7e9b4c5b..3e96a1cb3df 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 @@ -344,6 +344,18 @@ void testInequality() { assertNotEquals(rot1, rot2); } + @Test + void testToMatrix() { + var before = + new Rotation3d( + Units.degreesToRadians(10.0), + Units.degreesToRadians(20.0), + Units.degreesToRadians(30.0)); + var after = new Rotation3d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testInterpolate() { final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java index 5817f4e2825..19d4b4f396f 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java @@ -22,6 +22,14 @@ void testNewWithMeasures() { assertEquals(Math.PI / 4, transform.getRotation().getRadians(), kEpsilon); } + @Test + void testToMatrix() { + var before = new Transform2d(1.0, 2.0, Rotation2d.fromDegrees(20.0)); + var after = new Transform2d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testInverse() { var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java index 51d5f071fc3..dc251512fd1 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java @@ -11,6 +11,22 @@ import org.junit.jupiter.api.Test; class Transform3dTest { + @Test + void testToMatrix() { + var before = + new Transform3d( + 1.0, + 2.0, + 3.0, + new Rotation3d( + Units.degreesToRadians(20.0), + Units.degreesToRadians(30.0), + Units.degreesToRadians(40.0))); + var after = new Transform3d(before.toMatrix()); + + assertEquals(before, after); + } + @Test void testInverse() { var zAxis = VecBuilder.fill(0.0, 0.0, 1.0); diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp index bd5a9aa8a28..9a038590b1a 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp @@ -141,6 +141,13 @@ TEST(Pose2dTest, Nearest) { .value()); } +TEST(Pose2dTest, ToMatrix) { + Pose2d before{1_m, 2_m, 20_deg}; + Pose2d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Pose2dTest, Constexpr) { constexpr Pose2d defaultConstructed; constexpr Pose2d translationRotation{Translation2d{0_m, 1_m}, diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp index 88acb1fb897..f0168e95435 100644 --- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp @@ -112,6 +112,13 @@ TEST(Pose3dTest, Minus) { EXPECT_NEAR(0.0, transform.Rotation().Z().value(), 1e-9); } +TEST(Pose3dTest, ToMatrix) { + Pose3d before{1_m, 2_m, 3_m, Rotation3d{10_deg, 20_deg, 30_deg}}; + Pose3d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Pose3dTest, ToPose2d) { Pose3d pose{1_m, 2_m, 3_m, Rotation3d{20_deg, 30_deg, 40_deg}}; Pose2d expected{1_m, 2_m, 40_deg}; diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp index 60d372cdc63..3150feeb93f 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp @@ -78,6 +78,13 @@ TEST(Rotation2dTest, Inequality) { EXPECT_NE(rot1, rot2); } +TEST(Rotation2dTest, ToMatrix) { + Rotation2d before{20_deg}; + Rotation2d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Rotation2dTest, Constexpr) { constexpr Rotation2d defaultCtor; constexpr Rotation2d radianCtor{5_rad}; diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp index 16c033723d1..6088bdd7590 100644 --- a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp @@ -307,6 +307,13 @@ TEST(Rotation3dTest, Inequality) { EXPECT_NE(rot1, rot2); } +TEST(Rotation3dTest, ToMatrix) { + Rotation3d before{10_deg, 20_deg, 30_deg}; + Rotation3d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Rotation3dTest, Interpolate) { const Eigen::Vector3d xAxis{1.0, 0.0, 0.0}; const Eigen::Vector3d yAxis{0.0, 1.0, 0.0}; diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp index 49c9466201c..5a562405cc6 100644 --- a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp @@ -13,6 +13,13 @@ using namespace frc; +TEST(Transform2dTest, ToMatrix) { + Transform2d before{1_m, 2_m, 20_deg}; + Transform2d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Transform2dTest, Inverse) { const Pose2d initial{1_m, 2_m, 45_deg}; const Transform2d transform{{5_m, 0_m}, 5_deg}; diff --git a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp index 8a6bbc039f0..9aa7eda331e 100644 --- a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp +++ b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp @@ -13,6 +13,13 @@ using namespace frc; +TEST(Transform3dTest, ToMatrix) { + Transform3d before{1_m, 2_m, 3_m, Rotation3d{10_deg, 20_deg, 30_deg}}; + Transform3d after{before.ToMatrix()}; + + EXPECT_EQ(before, after); +} + TEST(Transform3dTest, Inverse) { Eigen::Vector3d zAxis{0.0, 0.0, 1.0}; From 838c5fbcd739b3cb5ffd831858f241db69e9cef7 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sat, 7 Dec 2024 21:37:49 -0800 Subject: [PATCH 8/9] [ci] Fix labeler yaml (#7523) --- .github/labeler.yml | 3 --- 1 file changed, 3 deletions(-) diff --git a/.github/labeler.yml b/.github/labeler.yml index c91083a5ba1..f9ae9fa6dda 100644 --- a/.github/labeler.yml +++ b/.github/labeler.yml @@ -16,9 +16,6 @@ - changed-files: - any-glob-to-any-file: epilogue-*/** 'component: examples': -- changed-files: - - any-glob-to-any-file: wpilib*Examples/** -'component: examples': - changed-files: - any-glob-to-any-file: wpilib*Examples/** 'component: glass': From a7349f00ef19b5713cfd5026cf02f29b408f72d5 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sat, 7 Dec 2024 22:31:09 -0800 Subject: [PATCH 9/9] [wpimath] Fix duplicate Rotation2d constructor (#7524) --- .../native/include/frc/geometry/Rotation2d.h | 37 ------------------- 1 file changed, 37 deletions(-) diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h index 7cb7801e19f..2c3c94540ab 100644 --- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h +++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h @@ -98,43 +98,6 @@ class WPILIB_DLLEXPORT Rotation2d { } } - /** - * Constructs a Rotation2d from a rotation matrix. - * - * @param rotationMatrix The rotation matrix. - * @throws std::domain_error if the rotation matrix isn't special orthogonal. - */ - constexpr explicit Rotation2d(const Eigen::Matrix2d& rotationMatrix) { - auto impl = - [](const Matrix2d& R) -> std::pair { - // 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() - Matrix2d::Identity()).norm() > 1e-9) { - throw std::domain_error("Rotation matrix isn't orthogonal"); - } - if (gcem::abs(R.determinant() - 1.0) > 1e-9) { - throw std::domain_error( - "Rotation matrix is orthogonal but not special orthogonal"); - } - - // R = [cosθ −sinθ] - // [sinθ cosθ] - return {R(0, 0), R(1, 0)}; - }; - - if (std::is_constant_evaluated()) { - auto cossin = impl(ct_matrix2d{rotationMatrix}); - m_cos = std::get<0>(cossin); - m_sin = std::get<1>(cossin); - } else { - auto cossin = impl(rotationMatrix); - m_cos = std::get<0>(cossin); - m_sin = std::get<1>(cossin); - } - - m_value = units::radian_t{gcem::atan2(m_sin, m_cos)}; - } - /** * Adds two rotations together, with the result being bounded between -π and * π.