Skip to content

Commit

Permalink
[wpimath] Add affine transformation constructors and getters to geome…
Browse files Browse the repository at this point in the history 
…try API

Fixes wpilibsuite#7429.
  • Loading branch information
@calcmogul
calcmogul committed Nov 24, 2024
1 parent b7eb9fb commit 4d2c89e
Show file tree
Hide file tree
Showing 32 changed files with 604 additions and 27 deletions.
6 changes: 6 additions & 0 deletions wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,7 +131,9 @@ 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 Use {@link Pose2d#toMatrix()} instead.
*/
@Deprecated
public static Matrix<N3, N1> poseToVector(Pose2d pose) {
return VecBuilder.fill(pose.getX(), pose.getY(), pose.getRotation().getRadians());
}
Expand Down Expand Up @@ -180,7 +182,9 @@ 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 Use {@link Pose2d#toMatrix()} instead.
*/
@Deprecated
public static Matrix<N4, N1> poseTo4dVector(Pose2d pose) {
return VecBuilder.fill(
pose.getTranslation().getX(),
Expand All @@ -194,7 +198,9 @@ 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 Use {@link Pose2d#toMatrix()} instead.
*/
@Deprecated
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
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
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
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
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
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

0 comments on commit 4d2c89e

Please sign in to comment.