Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
+613 −27
Open

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -131,7 +131,10 @@ public static <States extends Num, Outputs extends Num> 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<N3, N1> poseToVector(Pose2d pose) {
return VecBuilder.fill(pose.getX(), pose.getY(), pose.getRotation().getRadians());
}
Expand Down Expand Up @@ -180,7 +183,10 @@ public static <I extends Num> Matrix<I, N1> 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<N4, N1> poseTo4dVector(Pose2d pose) {
return VecBuilder.fill(
pose.getTranslation().getX(),
Expand All @@ -194,7 +200,10 @@ public static Matrix<N4, N1> 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<N3, N1> poseTo3dVector(Pose2d pose) {
return VecBuilder.fill(
pose.getTranslation().getX(),
Expand Down
40 changes: 40 additions & 0 deletions wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<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.
*
Expand Down Expand Up @@ -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<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.
Expand Down
50 changes: 50 additions & 0 deletions wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<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.
*
Expand Down Expand Up @@ -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<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.
*
Expand Down
48 changes: 48 additions & 0 deletions wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<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.
*
Expand Down Expand Up @@ -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<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.
*
Expand Down
27 changes: 27 additions & 0 deletions wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<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.
*
Expand Down
40 changes: 40 additions & 0 deletions wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<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;
Expand Down Expand Up @@ -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<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.
*
Expand Down
Loading
Loading