diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
index 61cfce638ec..c64c9b3958a 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
@@ -129,12 +129,14 @@ public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
- var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
- if (fieldRelative) {
- chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d());
- }
- chassisSpeeds.discretize(periodSeconds);
- var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+ var mecanumDriveWheelSpeeds =
+ m_kinematics.toWheelSpeeds(
+ ChassisSpeeds.discretize(
+ fieldRelative
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(
+ xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+ : new ChassisSpeeds(xSpeed, ySpeed, rot),
+ periodSeconds));
mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
setSpeeds(mecanumDriveWheelSpeeds);
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
index de481094486..256824c03b6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
@@ -141,12 +141,14 @@ public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
- var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
- if (fieldRelative) {
- chassisSpeeds.toRobotRelativeSpeeds(m_poseEstimator.getEstimatedPosition().getRotation());
- }
- chassisSpeeds.discretize(periodSeconds);
- var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+ var mecanumDriveWheelSpeeds =
+ m_kinematics.toWheelSpeeds(
+ ChassisSpeeds.discretize(
+ fieldRelative
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(
+ xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation())
+ : new ChassisSpeeds(xSpeed, ySpeed, rot),
+ periodSeconds));
mecanumDriveWheelSpeeds.desaturate(kMaxSpeed);
setSpeeds(mecanumDriveWheelSpeeds);
}
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
index f622259b494..958e5a3e617 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
@@ -57,12 +57,14 @@ public Drivetrain() {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
- var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
- if (fieldRelative) {
- chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d());
- }
- chassisSpeeds.discretize(periodSeconds);
- var swerveModuleStates = m_kinematics.toWheelSpeeds(chassisSpeeds);
+ var swerveModuleStates =
+ m_kinematics.toSwerveModuleStates(
+ ChassisSpeeds.discretize(
+ fieldRelative
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(
+ xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+ : new ChassisSpeeds(xSpeed, ySpeed, rot),
+ periodSeconds));
SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
index c950e4c7c53..04ee2998df6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
@@ -118,12 +118,14 @@ public void resetOdometry(Pose2d pose) {
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
*/
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
- var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
- if (fieldRelative) {
- chassisSpeeds.toRobotRelativeSpeeds(m_gyro.getRotation2d());
- }
- chassisSpeeds.discretize(DriveConstants.kDrivePeriod);
- var swerveModuleStates = DriveConstants.kDriveKinematics.toWheelSpeeds(chassisSpeeds);
+ var swerveModuleStates =
+ DriveConstants.kDriveKinematics.toSwerveModuleStates(
+ ChassisSpeeds.discretize(
+ fieldRelative
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(
+ xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+ : new ChassisSpeeds(xSpeed, ySpeed, rot),
+ DriveConstants.kDrivePeriod));
SwerveDriveKinematics.desaturateWheelSpeeds(
swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
index a28dfcacc28..c232bb19a7b 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
@@ -36,11 +36,8 @@ public class Drivetrain {
new SwerveDriveKinematics(
m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
- /*
- * Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings.
- * The numbers used
- * below are robot specific, and should be tuned.
- */
+ /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
+ below are robot specific, and should be tuned. */
private final SwerveDrivePoseEstimator m_poseEstimator =
new SwerveDrivePoseEstimator(
m_kinematics,
@@ -69,12 +66,14 @@ public Drivetrain() {
*/
public void drive(
double xSpeed, double ySpeed, double rot, boolean fieldRelative, double periodSeconds) {
- var chassisSpeeds = new ChassisSpeeds(xSpeed, ySpeed, rot);
- if (fieldRelative) {
- chassisSpeeds.toRobotRelativeSpeeds(m_poseEstimator.getEstimatedPosition().getRotation());
- }
- chassisSpeeds.discretize(periodSeconds);
- var swerveModuleStates = m_kinematics.toWheelSpeeds(chassisSpeeds);
+ var swerveModuleStates =
+ m_kinematics.toSwerveModuleStates(
+ ChassisSpeeds.discretize(
+ fieldRelative
+ ? ChassisSpeeds.fromFieldRelativeSpeeds(
+ xSpeed, ySpeed, rot, m_poseEstimator.getEstimatedPosition().getRotation())
+ : new ChassisSpeeds(xSpeed, ySpeed, rot),
+ periodSeconds));
SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, kMaxSpeed);
m_frontLeft.setDesiredState(swerveModuleStates[0]);
m_frontRight.setDesiredState(swerveModuleStates[1]);
@@ -93,8 +92,7 @@ public void updateOdometry() {
m_backRight.getPosition()
});
- // Also apply vision measurements. We use 0.3 seconds in the past as an example
- // -- on
+ // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
// a real robot, this must be calculated based either on latency or timestamps.
m_poseEstimator.addVisionMeasurement(
ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
index c1c30d42d21..a243a52d859 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
@@ -102,10 +102,9 @@ public ChassisSpeeds calculate(
m_poseError = trajectoryPose.relativeTo(currentPose);
m_rotationError = desiredHeading.minus(currentPose.getRotation());
- ChassisSpeeds speeds = new ChassisSpeeds(xFF, yFF, thetaFF);
+
if (!m_enabled) {
- speeds.toRobotRelativeSpeeds(currentPose.getRotation());
- return speeds;
+ return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());
}
// Calculate feedback velocities (based on position error).
@@ -113,10 +112,8 @@ public ChassisSpeeds calculate(
double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY());
// Return next output.
- speeds.vxMetersPerSecond += xFeedback;
- speeds.vyMetersPerSecond += yFeedback;
- speeds.toRobotRelativeSpeeds(currentPose.getRotation());
- return speeds;
+ return ChassisSpeeds.fromFieldRelativeSpeeds(
+ xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.getRotation());
}
/**
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
index e0eb4077961..4c806a52cb7 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
@@ -103,9 +103,7 @@ public Twist2d toTwist2d(double dtSeconds) {
* @param omegaRadiansPerSecond Angular velocity.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
- * @deprecated Use instance method instead.
*/
- @Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds discretize(
double vxMetersPerSecond,
double vyMetersPerSecond,
@@ -143,9 +141,7 @@ public static ChassisSpeeds discretize(
* @param omega Angular velocity.
* @param dt The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
- * @deprecated Use instance method instead.
*/
- @Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds discretize(
LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Time dt) {
return discretize(
@@ -166,9 +162,7 @@ public static ChassisSpeeds discretize(
* @param continuousSpeeds The continuous speeds.
* @param dtSeconds The duration of the timestep the speeds should be applied for.
* @return Discretized ChassisSpeeds.
- * @deprecated Use instance method instead.
*/
- @Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dtSeconds) {
return discretize(
continuousSpeeds.vxMetersPerSecond,
@@ -177,36 +171,6 @@ public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dt
dtSeconds);
}
- /**
- * Discretizes a continuous-time chassis speed.
- *
- * This function converts this continuous-time chassis speed into a discrete-time one such that
- * when the discrete-time chassis speed is applied for one timestep, the robot moves as if the
- * velocity components are independent (i.e., the robot moves v_x * dt along the x-axis, v_y * dt
- * along the y-axis, and omega * dt around the z-axis).
- *
- *
This is useful for compensating for translational skew when translating and rotating a
- * swerve drivetrain.
- *
- * @param dtSeconds The duration of the timestep the speeds should be applied for.
- */
- public void discretize(double dtSeconds) {
- var desiredDeltaPose =
- new Pose2d(
- vxMetersPerSecond * dtSeconds,
- vyMetersPerSecond * dtSeconds,
- new Rotation2d(omegaRadiansPerSecond * dtSeconds));
-
- // 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);
-
- // Turn the chassis translation/rotation deltas into average velocities
- vxMetersPerSecond = twist.dx / dtSeconds;
- vyMetersPerSecond = twist.dy / dtSeconds;
- omegaRadiansPerSecond = twist.dtheta / dtSeconds;
- }
-
/**
* Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
* object.
@@ -220,9 +184,7 @@ public void discretize(double dtSeconds) {
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
- * @deprecated Use toRobotRelativeSpeeds instead.
*/
- @Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromFieldRelativeSpeeds(
double vxMetersPerSecond,
double vyMetersPerSecond,
@@ -247,9 +209,7 @@ public static ChassisSpeeds fromFieldRelativeSpeeds(
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
- * @deprecated Use toRobotRelativeSpeeds instead.
*/
- @Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromFieldRelativeSpeeds(
LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) {
return fromFieldRelativeSpeeds(
@@ -267,9 +227,7 @@ public static ChassisSpeeds fromFieldRelativeSpeeds(
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
- * @deprecated Use toRobotRelativeSpeeds instead.
*/
- @Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromFieldRelativeSpeeds(
ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) {
return fromFieldRelativeSpeeds(
@@ -279,21 +237,6 @@ public static ChassisSpeeds fromFieldRelativeSpeeds(
robotAngle);
}
- /**
- * Converts this field-relative set of speeds into a robot-relative ChassisSpeeds object.
- *
- * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
- * considered to be zero when it is facing directly away from your alliance station wall.
- * Remember that this should be CCW positive.
- */
- public void toRobotRelativeSpeeds(Rotation2d robotAngle) {
- // CW rotation into chassis frame
- var rotated =
- new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus());
- vxMetersPerSecond = rotated.getX();
- vyMetersPerSecond = rotated.getY();
- }
-
/**
* Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
* object.
@@ -307,9 +250,7 @@ public void toRobotRelativeSpeeds(Rotation2d robotAngle) {
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the field's frame of reference.
- * @deprecated Use toFieldRelativeSpeeds instead.
*/
- @Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromRobotRelativeSpeeds(
double vxMetersPerSecond,
double vyMetersPerSecond,
@@ -333,9 +274,7 @@ public static ChassisSpeeds fromRobotRelativeSpeeds(
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the field's frame of reference.
- * @deprecated Use toFieldRelativeSpeeds instead.
*/
- @Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromRobotRelativeSpeeds(
LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) {
return fromRobotRelativeSpeeds(
@@ -353,9 +292,7 @@ public static ChassisSpeeds fromRobotRelativeSpeeds(
* considered to be zero when it is facing directly away from your alliance station wall.
* Remember that this should be CCW positive.
* @return ChassisSpeeds object representing the speeds in the field's frame of reference.
- * @deprecated Use toFieldRelativeSpeeds instead.
*/
- @Deprecated(forRemoval = true, since = "2025")
public static ChassisSpeeds fromRobotRelativeSpeeds(
ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) {
return fromRobotRelativeSpeeds(
@@ -365,20 +302,6 @@ public static ChassisSpeeds fromRobotRelativeSpeeds(
robotAngle);
}
- /**
- * Converts this robot-relative set of speeds into a field-relative ChassisSpeeds object.
- *
- * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
- * considered to be zero when it is facing directly away from your alliance station wall.
- * Remember that this should be CCW positive.
- */
- public void toFieldRelativeSpeeds(Rotation2d robotAngle) {
- // CCW rotation out of chassis frame
- var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle);
- vxMetersPerSecond = rotated.getX();
- vyMetersPerSecond = rotated.getY();
- }
-
/**
* Adds two ChassisSpeeds and returns the sum.
*
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
index 5ed2e7f22a1..8f7bb6fd123 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
@@ -20,11 +20,10 @@ class ChassisSpeedsTest {
@Test
void testDiscretize() {
final var target = new ChassisSpeeds(1.0, 0.0, 0.5);
- final var speeds = new ChassisSpeeds(1.0, 0.0, 0.5);
final var duration = 1.0;
final var dt = 0.01;
- speeds.discretize(duration);
+ final var speeds = ChassisSpeeds.discretize(target, duration);
final var twist =
new Twist2d(
speeds.vxMetersPerSecond * dt,
@@ -62,8 +61,8 @@ void testMeasureConstructor() {
@Test
void testFromFieldRelativeSpeeds() {
- final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5);
- chassisSpeeds.toRobotRelativeSpeeds(Rotation2d.kCW_Pi_2);
+ final var chassisSpeeds =
+ ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.kCW_Pi_2);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
@@ -73,8 +72,8 @@ void testFromFieldRelativeSpeeds() {
@Test
void testFromRobotRelativeSpeeds() {
- final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5);
- chassisSpeeds.toFieldRelativeSpeeds(Rotation2d.fromDegrees(45.0));
+ final var chassisSpeeds =
+ ChassisSpeeds.fromRobotRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(45.0));
assertAll(
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vxMetersPerSecond, kEpsilon),