Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add affine transformation constructors and getters to geometry API (#7430)

Browse Source 
Fixes #7429.
This commit is contained in:
Tyler Veness
2024-12-07 15:49:17 -08:00
committed by GitHub
parent f772bb141d
commit e222efaa01
32 changed files with 615 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
View File

@@ -131,7 +131,10 @@ public final class StateSpaceUtil {
*
* @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<N3, N1> poseToVector(Pose2d pose) {
return VecBuilder.fill(pose.getX(), pose.getY(), pose.getRotation().getRadians());
}
@@ -180,7 +183,10 @@ public final class StateSpaceUtil {
*
* @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<N4, N1> poseTo4dVector(Pose2d pose) {
return VecBuilder.fill(
pose.getTranslation().getX(),
@@ -194,7 +200,10 @@ public final class StateSpaceUtil {
*
* @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<N3, N1> poseTo3dVector(Pose2d pose) {
return VecBuilder.fill(
pose.getTranslation().getX(),

40
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
View File

@@ -10,9 +10,13 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect;
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 class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, Str
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<N3, N3> 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 class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, Str
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<N3, N3> 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.

50
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -10,10 +10,14 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect;
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 class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
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<N4, N4> 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 class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
resultArray[5]);
}
/**
* Returns an affine transformation matrix representation of this pose.
*
* @return An affine transformation matrix representation of this pose.
*/
public Matrix<N4, N4> 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.
*

48
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
View File

@@ -10,11 +10,15 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect;
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 class Rotation2d
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<N2, N2> 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 class Rotation2d
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<N2, N2> 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.
*

27
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
View File

@@ -10,6 +10,7 @@ import com.fasterxml.jackson.annotation.JsonAutoDetect;
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 class Rotation3d
return Radians.of(getZ());
}
/**
* Returns rotation matrix representation of this rotation.
*
* @return Rotation matrix representation of this rotation.
*/
public Matrix<N3, N3> 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.
*

40
wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
View File

@@ -6,8 +6,12 @@ package edu.wpi.first.math.geometry;
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 class Transform2d implements ProtobufSerializable, StructSerializable {
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<N3, N3> 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 class Transform2d implements ProtobufSerializable, StructSerializable {
return m_translation.getMeasureY();
}
/**
* Returns an affine transformation matrix representation of this transformation.
*
* @return An affine transformation matrix representation of this transformation.
*/
public Matrix<N3, N3> 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.
*

50
wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
View File

@@ -6,8 +6,12 @@ package edu.wpi.first.math.geometry;
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 class Transform3d implements ProtobufSerializable, StructSerializable {
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<N4, N4> 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 class Transform3d implements ProtobufSerializable, StructSerializable {
return m_translation.getMeasureZ();
}
/**
* Returns an affine transformation matrix representation of this transformation.
*
* @return An affine transformation matrix representation of this transformation.
*/
public Matrix<N4, N4> 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.
*

10
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
View File

@@ -88,10 +88,10 @@ public class Translation2d
}
/**
* 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<N2> vector) {
this(vector.get(0), vector.get(1));
@@ -148,9 +148,9 @@ public class Translation2d
}
/**
* 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<N2> toVector() {
return VecBuilder.fill(m_x, m_y);

10
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
View File

@@ -104,10 +104,10 @@ public class 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.
*/
public Translation3d(Vector<N3> vector) {
this(vector.get(0), vector.get(1), vector.get(2));
@@ -184,9 +184,9 @@ public class Translation3d
}
/**
* 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<N3> toVector() {
return VecBuilder.fill(m_x, m_y, m_z);

21
wpimath/src/main/native/include/frc/StateSpaceUtil.h
View File

@@ -206,9 +206,12 @@ Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& 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<States, States>& 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()}};
}

19
wpimath/src/main/native/include/frc/ct_matrix.h
View File

@@ -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<Scalar, Rows, 1>;
template <typename Scalar, int Cols>
using ct_row_vector = ct_matrix<Scalar, 1, Cols>;
using ct_matrix2d = ct_matrix<double, 2, 2>;
using ct_matrix3d = ct_matrix<double, 3, 3>;
using ct_vector2d = ct_vector<double, 2>;
using ct_vector3d = ct_vector<double, 3>;
} // namespace frc

26
wpimath/src/main/native/include/frc/geometry/Pose2d.h
View File

@@ -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.

32
wpimath/src/main/native/include/frc/geometry/Pose3d.h
View File

@@ -4,6 +4,7 @@
#pragma once
#include <stdexcept>
#include <type_traits>
#include <utility>
@@ -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.
*/

50
wpimath/src/main/native/include/frc/geometry/Rotation2d.h
View File

@@ -5,12 +5,16 @@
#pragma once
#include <type_traits>
#include <utility>
#include <Eigen/Core>
#include <Eigen/LU>
#include <gcem.hpp>
#include <wpi/StackTrace.h>
#include <wpi/SymbolExports.h>
#include <wpi/json_fwd.h>
#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 =
[]<typename Matrix2d>(const Matrix2d& R) -> std::pair<double, double> {
// 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.
*

18
wpimath/src/main/native/include/frc/geometry/Rotation3d.h
View File

@@ -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.

27
wpimath/src/main/native/include/frc/geometry/Transform2d.h
View File

@@ -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.
*

32
wpimath/src/main/native/include/frc/geometry/Transform3d.h
View File

@@ -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.
*

12
wpimath/src/main/native/include/frc/geometry/Translation2d.h
View File

@@ -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()}};

10
wpimath/src/main/native/include/frc/geometry/Translation3d.h
View File

@@ -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()}};

1
wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
View File

@@ -153,6 +153,7 @@ class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
}
@Test
@SuppressWarnings("removal")
void testPoseToVector() {
Pose2d pose = new Pose2d(1, 2, new Rotation2d(3));
var vector = StateSpaceUtil.poseToVector(pose);

8
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
View File

@@ -86,6 +86,14 @@ class Pose2dTest {
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));

16
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
View File

@@ -169,6 +169,22 @@ class Pose3dTest {
() -> 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 =

8
wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
View File

@@ -100,6 +100,14 @@ class Rotation2dTest {
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

12
wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
View File

@@ -344,6 +344,18 @@ class Rotation3dTest {
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);

8
wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
View File

@@ -22,6 +22,14 @@ class Transform2dTest {
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));

16
wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java
View File

@@ -11,6 +11,22 @@ import edu.wpi.first.math.util.Units;
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);

7
wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
View File

@@ -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},

7
wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
View File

@@ -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};

7
wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
View File

@@ -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};

7
wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
View File

@@ -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};

7
wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
View File

@@ -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};

7
wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
View File

@@ -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};