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

[wpimath] Replace Pose2/3d.exp(Twist2/3d) with Pose2/3d.plus(Twist2/3d.exp()) (#8188)

Browse Source 
This better matches math notation.
This commit is contained in:
ninjadrknss
2025-08-30 11:37:09 -07:00
committed by GitHub
parent 55e52bc2b7
commit 96004f9bb5
51 changed files with 643 additions and 682 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
View File

@@ -271,7 +271,7 @@ public class PoseEstimator<T> {
}
// Step 4: Measure the twist between the old pose estimate and the vision pose.
var twist = visionSample.get().log(visionRobotPose);
var twist = visionRobotPose.minus(visionSample.get()).log();
// Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman
// gain matrix representing how much we trust vision measurements compared to our current pose.
@@ -282,7 +282,8 @@ public class PoseEstimator<T> {
new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0));
// Step 7: Calculate and record the vision update.
var visionUpdate = new VisionUpdate(visionSample.get().exp(scaledTwist), odometrySample.get());
var visionUpdate =
new VisionUpdate(visionSample.get().plus(scaledTwist.exp()), odometrySample.get());
m_visionUpdates.put(timestamp, visionUpdate);
// Step 8: Remove later vision measurements. (Matches previous behavior)

5
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
View File

@@ -284,7 +284,7 @@ public class PoseEstimator3d<T> {
}
// Step 4: Measure the twist between the old pose estimate and the vision pose.
var twist = visionSample.get().log(visionRobotPose);
var twist = visionRobotPose.minus(visionSample.get()).log();
// Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman
// gain matrix representing how much we trust vision measurements compared to our current pose.
@@ -303,7 +303,8 @@ public class PoseEstimator3d<T> {
k_times_twist.get(5, 0));
// Step 7: Calculate and record the vision update.
var visionUpdate = new VisionUpdate(visionSample.get().exp(scaledTwist), odometrySample.get());
var visionUpdate =
new VisionUpdate(visionSample.get().plus(scaledTwist.exp()), odometrySample.get());
m_visionUpdates.put(timestamp, visionUpdate);
// Step 8: Remove later vision measurements. (Matches previous behavior)

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

@@ -249,81 +249,6 @@ public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, Str
return new Pose2d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot));
}
/**
* Obtain a new Pose2d from a (constant curvature) velocity.
*
* <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls
* Engineering in the FIRST Robotics Competition</a> section 10.2 "Pose exponential" for a
* derivation.
*
* <p>The twist is a change in pose in the robot's coordinate frame since the previous pose
* update. When the user runs exp() on the previous known field-relative pose with the argument
* being the twist, the user will receive the new field-relative pose.
*
* <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
* pose forward in time.
*
* @param twist The change in pose in the robot's coordinate frame since the previous pose update.
* For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5
* degrees since the previous pose update, the twist would be Twist2d(0.01, 0.0,
* Units.degreesToRadians(0.5)).
* @return The new pose of the robot.
*/
public Pose2d exp(Twist2d twist) {
double dx = twist.dx;
double dy = twist.dy;
double dtheta = twist.dtheta;
double sinTheta = Math.sin(dtheta);
double cosTheta = Math.cos(dtheta);
double s;
double c;
if (Math.abs(dtheta) < 1E-9) {
s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
c = 0.5 * dtheta;
} else {
s = sinTheta / dtheta;
c = (1 - cosTheta) / dtheta;
}
var transform =
new Transform2d(
new Translation2d(dx * s - dy * c, dx * c + dy * s),
new Rotation2d(cosTheta, sinTheta));
return this.plus(transform);
}
/**
* Returns a Twist2d that maps this pose to the end pose. If c is the output of {@code a.Log(b)},
* then {@code a.Exp(c)} would yield b.
*
* @param end The end pose for the transformation.
* @return The twist that maps this to end.
*/
public Twist2d log(Pose2d end) {
final var transform = end.relativeTo(this);
final var dtheta = transform.getRotation().getRadians();
final var halfDtheta = dtheta / 2.0;
final var cosMinusOne = transform.getRotation().getCos() - 1;
double halfThetaByTanOfHalfDtheta;
if (Math.abs(cosMinusOne) < 1E-9) {
halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else {
halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
}
Translation2d translationPart =
transform
.getTranslation()
.rotateBy(new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta))
.times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
}
/**
* Returns an affine transformation matrix representation of this pose.
*
@@ -393,9 +318,9 @@ public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, Str
} else if (t >= 1) {
return endValue;
} else {
var twist = this.log(endValue);
var twist = endValue.minus(this).log();
var scaledTwist = new Twist2d(twist.dx * t, twist.dy * t, twist.dtheta * t);
return this.exp(scaledTwist);
return this.plus(scaledTwist.exp());
}
}

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

@@ -16,7 +16,6 @@ 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;
@@ -285,82 +284,6 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
return new Pose3d(m_translation.rotateAround(point, rot), m_rotation.rotateBy(rot));
}
/**
* Obtain a new Pose3d from a (constant curvature) velocity.
*
* <p>The twist is a change in pose in the robot's coordinate frame since the previous pose
* update. When the user runs exp() on the previous known field-relative pose with the argument
* being the twist, the user will receive the new field-relative pose.
*
* <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
* pose forward in time.
*
* @param twist The change in pose in the robot's coordinate frame since the previous pose update.
* For example, if a non-holonomic robot moves forward 0.01 meters and changes angle by 0.5
* degrees since the previous pose update, the twist would be Twist3d(0.01, 0.0, 0.0, new new
* Rotation3d(0.0, 0.0, Units.degreesToRadians(0.5))).
* @return The new pose of the robot.
*/
public Pose3d exp(Twist3d twist) {
var quaternion = this.getRotation().getQuaternion();
double[] resultArray =
Pose3dJNI.exp(
this.getX(),
this.getY(),
this.getZ(),
quaternion.getW(),
quaternion.getX(),
quaternion.getY(),
quaternion.getZ(),
twist.dx,
twist.dy,
twist.dz,
twist.rx,
twist.ry,
twist.rz);
return new Pose3d(
resultArray[0],
resultArray[1],
resultArray[2],
new Rotation3d(
new Quaternion(resultArray[3], resultArray[4], resultArray[5], resultArray[6])));
}
/**
* Returns a Twist3d that maps this pose to the end pose. If c is the output of {@code a.Log(b)},
* then {@code a.Exp(c)} would yield b.
*
* @param end The end pose for the transformation.
* @return The twist that maps this to end.
*/
public Twist3d log(Pose3d end) {
var thisQuaternion = this.getRotation().getQuaternion();
var endQuaternion = end.getRotation().getQuaternion();
double[] resultArray =
Pose3dJNI.log(
this.getX(),
this.getY(),
this.getZ(),
thisQuaternion.getW(),
thisQuaternion.getX(),
thisQuaternion.getY(),
thisQuaternion.getZ(),
end.getX(),
end.getY(),
end.getZ(),
endQuaternion.getW(),
endQuaternion.getX(),
endQuaternion.getY(),
endQuaternion.getZ());
return new Twist3d(
resultArray[0],
resultArray[1],
resultArray[2],
resultArray[3],
resultArray[4],
resultArray[5]);
}
/**
* Returns an affine transformation matrix representation of this pose.
*
@@ -445,11 +368,11 @@ public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, Str
} else if (t >= 1) {
return endValue;
} else {
var twist = this.log(endValue);
var twist = endValue.minus(this).log();
var scaledTwist =
new Twist3d(
twist.dx * t, twist.dy * t, twist.dz * t, twist.rx * t, twist.ry * t, twist.rz * t);
return this.exp(scaledTwist);
return this.plus(scaledTwist.exp());
}
}

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

@@ -209,6 +209,33 @@ public class Transform2d implements ProtobufSerializable, StructSerializable {
return m_rotation;
}
/**
* Returns a Twist2d of the current transform (pose delta). If b is the output of {@code a.log()},
* then {@code b.exp()} would yield a.
*
* @return The twist that maps the current transform.
*/
public Twist2d log() {
final double dtheta = m_rotation.getRadians();
final double halfDtheta = dtheta / 2.0;
final double cosMinusOne = m_rotation.getCos() - 1;
double halfThetaByTanOfHalfDtheta;
if (Math.abs(cosMinusOne) < 1E-9) {
halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
} else {
halfThetaByTanOfHalfDtheta = -(halfDtheta * m_rotation.getSin()) / cosMinusOne;
}
Translation2d translationPart =
m_translation
.rotateBy(new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta))
.times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
}
/**
* Invert the transformation. This is useful for undoing a transformation.
*

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

@@ -11,6 +11,7 @@ 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.jni.Transform3dJNI;
import edu.wpi.first.math.numbers.N4;
import edu.wpi.first.units.measure.Distance;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
@@ -251,6 +252,32 @@ public class Transform3d implements ProtobufSerializable, StructSerializable {
return m_rotation;
}
/**
* Returns a Twist3d of the current transform (pose delta). If b is the output of {@code a.log()},
* then {@code b.exp()} would yield a.
*
* @return The twist that maps the current transform.
*/
public Twist3d log() {
var thisQuaternion = m_rotation.getQuaternion();
double[] resultArray =
Transform3dJNI.log(
this.getX(),
this.getY(),
this.getZ(),
thisQuaternion.getW(),
thisQuaternion.getX(),
thisQuaternion.getY(),
thisQuaternion.getZ());
return new Twist3d(
resultArray[0],
resultArray[1],
resultArray[2],
resultArray[3],
resultArray[4],
resultArray[5]);
}
/**
* Invert the transformation. This is useful for undoing a transformation.
*

33
wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
View File

@@ -42,6 +42,39 @@ public class Twist2d implements ProtobufSerializable, StructSerializable {
this.dtheta = dtheta;
}
/**
* Obtain a new Transform2d from a (constant curvature) velocity.
*
* <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls
* Engineering in the FIRST Robotics Competition</a> section 10.2 "Pose exponential" for a
* derivation.
*
* <p>The twist is a change in pose in the robot's coordinate frame since the previous pose
* update. When the user runs exp() on the twist, the user will receive the pose delta.
*
* <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
* pose forward in time.
*
* @return The pose delta of the robot.
*/
public Transform2d exp() {
double sinTheta = Math.sin(dtheta);
double cosTheta = Math.cos(dtheta);
double s;
double c;
if (Math.abs(dtheta) < 1E-9) {
s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
c = 0.5 * dtheta;
} else {
s = sinTheta / dtheta;
c = (1 - cosTheta) / dtheta;
}
return new Transform2d(
new Translation2d(dx * s - dy * c, dx * c + dy * s), new Rotation2d(cosTheta, sinTheta));
}
@Override
public String toString() {
return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);

26
wpimath/src/main/java/edu/wpi/first/math/geometry/Twist3d.java
View File

@@ -6,6 +6,7 @@ package edu.wpi.first.math.geometry;
import edu.wpi.first.math.geometry.proto.Twist3dProto;
import edu.wpi.first.math.geometry.struct.Twist3dStruct;
import edu.wpi.first.math.jni.Twist3dJNI;
import edu.wpi.first.util.protobuf.ProtobufSerializable;
import edu.wpi.first.util.struct.StructSerializable;
import java.util.Objects;
@@ -57,6 +58,31 @@ public class Twist3d implements ProtobufSerializable, StructSerializable {
this.rz = rz;
}
/**
* Obtain a new Transform3d from a (constant curvature) velocity.
*
* <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls
* Engineering in the FIRST Robotics Competition</a> section 10.2 "Pose exponential" for a
* derivation.
*
* <p>The twist is a change in pose in the robot's coordinate frame since the previous pose
* update. When the user runs exp() on the twist, the user will receive the pose delta.
*
* <p>"Exp" represents the pose exponential, which is solving a differential equation moving the
* pose forward in time.
*
* @return The pose delta of the robot.
*/
public Transform3d exp() {
double[] resultArray = Twist3dJNI.exp(dx, dy, dz, rx, ry, rz);
return new Transform3d(
resultArray[0],
resultArray[1],
resultArray[2],
new Rotation3d(
new Quaternion(resultArray[3], resultArray[4], resultArray[5], resultArray[6])));
}
@Override
public String toString() {
return String.format(

83
wpimath/src/main/java/edu/wpi/first/math/jni/Pose3dJNI.java
View File

@@ -1,83 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.jni;
/** Pose3d JNI. */
public final class Pose3dJNI extends WPIMathJNI {
/**
* Obtain a Pose3d from a (constant curvature) velocity.
*
* <p>The double array returned is of the form [dx, dy, dz, qx, qy, qz].
*
* @param poseX The pose's translational X component.
* @param poseY The pose's translational Y component.
* @param poseZ The pose's translational Z component.
* @param poseQw The pose quaternion's W component.
* @param poseQx The pose quaternion's X component.
* @param poseQy The pose quaternion's Y component.
* @param poseQz The pose quaternion's Z component.
* @param twistDx The twist's dx value.
* @param twistDy The twist's dy value.
* @param twistDz The twist's dz value.
* @param twistRx The twist's rx value.
* @param twistRy The twist's ry value.
* @param twistRz The twist's rz value.
* @return The new pose as a double array.
*/
public static native double[] exp(
double poseX,
double poseY,
double poseZ,
double poseQw,
double poseQx,
double poseQy,
double poseQz,
double twistDx,
double twistDy,
double twistDz,
double twistRx,
double twistRy,
double twistRz);
/**
* Returns a Twist3d that maps the starting pose to the end pose.
*
* <p>The double array returned is of the form [dx, dy, dz, rx, ry, rz].
*
* @param startX The starting pose's translational X component.
* @param startY The starting pose's translational Y component.
* @param startZ The starting pose's translational Z component.
* @param startQw The starting pose quaternion's W component.
* @param startQx The starting pose quaternion's X component.
* @param startQy The starting pose quaternion's Y component.
* @param startQz The starting pose quaternion's Z component.
* @param endX The ending pose's translational X component.
* @param endY The ending pose's translational Y component.
* @param endZ The ending pose's translational Z component.
* @param endQw The ending pose quaternion's W component.
* @param endQx The ending pose quaternion's X component.
* @param endQy The ending pose quaternion's Y component.
* @param endQz The ending pose quaternion's Z component.
* @return The twist that maps start to end as a double array.
*/
public static native double[] log(
double startX,
double startY,
double startZ,
double startQw,
double startQx,
double startQy,
double startQz,
double endX,
double endY,
double endZ,
double endQw,
double endQx,
double endQy,
double endQz);
/** Utility class. */
private Pose3dJNI() {}
}

34
wpimath/src/main/java/edu/wpi/first/math/jni/Transform3dJNI.java Normal file
View File

@@ -0,0 +1,34 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.jni;
/** Transform3d JNI. */
public final class Transform3dJNI extends WPIMathJNI {
/**
* Returns a Twist3d that maps the Transform3d (pose delta).
*
* <p>The double array returned is of the form [dx, dy, dz, rx, ry, rz].
*
* @param relX The transform's translational X component.
* @param relY The transform's translational Y component.
* @param relZ The transform's translational Z component.
* @param relQw The transform quaternion's W component.
* @param relQx The transform quaternion's X component.
* @param relQy The transform quaternion's Y component.
* @param relQz The transform quaternion's Z component.
* @return The twist that maps start to end as a double array.
*/
public static native double[] log(
double relX,
double relY,
double relZ,
double relQw,
double relQx,
double relQy,
double relQz);
/** Utility class. */
private Transform3dJNI() {}
}

32
wpimath/src/main/java/edu/wpi/first/math/jni/Twist3dJNI.java Normal file
View File

@@ -0,0 +1,32 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.jni;
/** Twist3d JNI. */
public final class Twist3dJNI extends WPIMathJNI {
/**
* Obtain a Transform3d from a (constant curvature) velocity.
*
* <p>The double array returned is of the form [dx, dy, dz, qw, qx, qy, qz].
*
* @param twistDx The twist's dx value.
* @param twistDy The twist's dy value.
* @param twistDz The twist's dz value.
* @param twistRx The twist's rx value.
* @param twistRy The twist's ry value.
* @param twistRz The twist's rz value.
* @return The new pose as a double array.
*/
public static native double[] exp(
double twistDx,
double twistDy,
double twistDz,
double twistRx,
double twistRy,
double twistRz);
/** Utility class. */
private Twist3dJNI() {}
}

6
wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
View File

@@ -7,8 +7,8 @@ package edu.wpi.first.math.kinematics;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.kinematics.proto.ChassisSpeedsProto;
@@ -101,11 +101,11 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
public ChassisSpeeds discretize(double dt) {
// Construct the desired pose after a timestep, relative to the current pose. The desired pose
// has decoupled translation and rotation.
var desiredDeltaPose = new Pose2d(vx * dt, vy * dt, new Rotation2d(omega * dt));
var desiredTransform = new Transform2d(vx * dt, vy * dt, new Rotation2d(omega * dt));
// Find the chassis translation/rotation deltas in the robot frame that move the robot from its
// current pose to the desired pose
var twist = Pose2d.kZero.log(desiredDeltaPose);
var twist = desiredTransform.log();
// Turn the chassis translation/rotation deltas into average velocities
return new ChassisSpeeds(twist.dx / dt, twist.dy / dt, twist.dtheta / dt);

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java
View File

@@ -117,7 +117,7 @@ public class Odometry<T> {
var twist = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
twist.dtheta = angle.minus(m_previousAngle).getRadians();
var newPose = m_pose.exp(twist);
var newPose = m_pose.plus(twist.exp());
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
m_previousAngle = angle;

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java
View File

@@ -134,7 +134,7 @@ public class Odometry3d<T> {
angle_difference.get(1),
angle_difference.get(2));
var newPose = m_pose.exp(twist);
var newPose = m_pose.plus(twist.exp());
m_kinematics.copyInto(wheelPositions, m_previousWheelPositions);
m_previousAngle = angle;

2
wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
View File

@@ -126,7 +126,7 @@ public final class SplineParameterizer {
throw new MalformedSplineException(kMalformedSplineExceptionMsg);
}
final var twist = start.get().pose.log(end.get().pose);
final var twist = (end.get().pose.minus(start.get().pose)).log();
if (Math.abs(twist.dy) > kMaxDy
|| Math.abs(twist.dx) > kMaxDx
|| Math.abs(twist.dtheta) > kMaxDtheta) {