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

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

Browse Source 
Fixes #7429.
This commit is contained in:
Tyler Veness
2024-12-07 21:29:02 -08:00
committed by GitHub
parent c81bd0c909
commit 62a6a77bbf
32 changed files with 611 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.
*

46
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;
@@ -125,6 +129,37 @@ 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);
}
/**
* Constructs and returns a Rotation2d with the given radian value.
*
@@ -230,6 +265,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);