[wpimath] Use immutable member functions in ChassisSpeeds (#7545)

wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java

@@ -104,7 +104,7 @@ public class HolonomicDriveController {
     m_rotationError = desiredHeading.minus(currentPose.getRotation());
 
     if (!m_enabled) {
-      return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());
+      return new ChassisSpeeds(xFF, yFF, thetaFF).toRobotRelative(currentPose.getRotation());
     }
 
     // Calculate feedback velocities (based on position error).
@@ -112,8 +112,8 @@ public class HolonomicDriveController {
     double yFeedback = m_yController.calculate(currentPose.getY(), trajectoryPose.getY());
 
     // Return next output.
-    return ChassisSpeeds.fromFieldRelativeSpeeds(
-        xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.getRotation());
+    return new ChassisSpeeds(xFF + xFeedback, yFF + yFeedback, thetaFF)
+        .toRobotRelative(currentPose.getRotation());
   }
 
   /**

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

@@ -6,7 +6,6 @@ package edu.wpi.first.math.kinematics;
 
 import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.RadiansPerSecond;
-import static edu.wpi.first.units.Units.Seconds;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -16,7 +15,6 @@ import edu.wpi.first.math.kinematics.proto.ChassisSpeedsProto;
 import edu.wpi.first.math.kinematics.struct.ChassisSpeedsStruct;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.LinearVelocity;
-import edu.wpi.first.units.measure.Time;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
 import java.util.Objects;
@@ -90,7 +88,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
   /**
    * Discretizes a continuous-time chassis speed.
    *
-   * <p>This function converts a continuous-time chassis speed into a discrete-time one such that
+   * <p>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).
@@ -98,19 +96,10 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
    * <p>This is useful for compensating for translational skew when translating and rotating a
    * swerve drivetrain.
    *
-   * @param vxMetersPerSecond Forward velocity.
-   * @param vyMetersPerSecond Sideways velocity.
-   * @param omegaRadiansPerSecond Angular velocity.
    * @param dtSeconds The duration of the timestep the speeds should be applied for.
    * @return Discretized ChassisSpeeds.
    */
-  public static ChassisSpeeds discretize(
-      double vxMetersPerSecond,
-      double vyMetersPerSecond,
-      double omegaRadiansPerSecond,
-      double dtSeconds) {
-    // Construct the desired pose after a timestep, relative to the current pose. The desired pose
-    // has decoupled translation and rotation.
+  public ChassisSpeeds discretize(double dtSeconds) {
     var desiredDeltaPose =
         new Pose2d(
             vxMetersPerSecond * dtSeconds,
@@ -126,70 +115,14 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
   }
 
   /**
-   * Discretizes a continuous-time chassis speed.
+   * Converts this field-relative set of speeds into a robot-relative ChassisSpeeds object.
    *
-   * <p>This function converts a 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).
-   *
-   * <p>This is useful for compensating for translational skew when translating and rotating a
-   * swerve drivetrain.
-   *
-   * @param vx Forward velocity.
-   * @param vy Sideways velocity.
-   * @param omega Angular velocity.
-   * @param dt The duration of the timestep the speeds should be applied for.
-   * @return Discretized ChassisSpeeds.
-   */
-  public static ChassisSpeeds discretize(
-      LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Time dt) {
-    return discretize(
-        vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), dt.in(Seconds));
-  }
-
-  /**
-   * Discretizes a continuous-time chassis speed.
-   *
-   * <p>This function converts a 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).
-   *
-   * <p>This is useful for compensating for translational skew when translating and rotating a
-   * swerve drivetrain.
-   *
-   * @param continuousSpeeds The continuous speeds.
-   * @param dtSeconds The duration of the timestep the speeds should be applied for.
-   * @return Discretized ChassisSpeeds.
-   */
-  public static ChassisSpeeds discretize(ChassisSpeeds continuousSpeeds, double dtSeconds) {
-    return discretize(
-        continuousSpeeds.vxMetersPerSecond,
-        continuousSpeeds.vyMetersPerSecond,
-        continuousSpeeds.omegaRadiansPerSecond,
-        dtSeconds);
-  }
-
-  /**
-   * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
-   * object.
-   *
-   * @param vxMetersPerSecond The component of speed in the x direction relative to the field.
-   *     Positive x is away from your alliance wall.
-   * @param vyMetersPerSecond The component of speed in the y direction relative to the field.
-   *     Positive y is to your left when standing behind the alliance wall.
-   * @param omegaRadiansPerSecond The angular rate of the robot.
    * @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.
    * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
    */
-  public static ChassisSpeeds fromFieldRelativeSpeeds(
-      double vxMetersPerSecond,
-      double vyMetersPerSecond,
-      double omegaRadiansPerSecond,
-      Rotation2d robotAngle) {
+  public ChassisSpeeds toRobotRelative(Rotation2d robotAngle) {
     // CW rotation into chassis frame
     var rotated =
         new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle.unaryMinus());
@@ -197,111 +130,19 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
   }
 
   /**
-   * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
-   * object.
+   * Converts this robot-relative set of speeds into a field-relative ChassisSpeeds object.
    *
-   * @param vx The component of speed in the x direction relative to the field. Positive x is away
-   *     from your alliance wall.
-   * @param vy The component of speed in the y direction relative to the field. Positive y is to
-   *     your left when standing behind the alliance wall.
-   * @param omega The angular rate of the robot.
-   * @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.
-   * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
-   */
-  public static ChassisSpeeds fromFieldRelativeSpeeds(
-      LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) {
-    return fromFieldRelativeSpeeds(
-        vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle);
-  }
-
-  /**
-   * Converts a user provided field-relative ChassisSpeeds object into a robot-relative
-   * ChassisSpeeds object.
-   *
-   * @param fieldRelativeSpeeds The ChassisSpeeds object representing the speeds in the field frame
-   *     of reference. Positive x is away from your alliance wall. Positive y is to your left when
-   *     standing behind the alliance wall.
-   * @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.
-   * @return ChassisSpeeds object representing the speeds in the robot's frame of reference.
-   */
-  public static ChassisSpeeds fromFieldRelativeSpeeds(
-      ChassisSpeeds fieldRelativeSpeeds, Rotation2d robotAngle) {
-    return fromFieldRelativeSpeeds(
-        fieldRelativeSpeeds.vxMetersPerSecond,
-        fieldRelativeSpeeds.vyMetersPerSecond,
-        fieldRelativeSpeeds.omegaRadiansPerSecond,
-        robotAngle);
-  }
-
-  /**
-   * Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
-   * object.
-   *
-   * @param vxMetersPerSecond The component of speed in the x direction relative to the robot.
-   *     Positive x is towards the robot's front.
-   * @param vyMetersPerSecond The component of speed in the y direction relative to the robot.
-   *     Positive y is towards the robot's left.
-   * @param omegaRadiansPerSecond The angular rate of the robot.
    * @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.
    * @return ChassisSpeeds object representing the speeds in the field's frame of reference.
    */
-  public static ChassisSpeeds fromRobotRelativeSpeeds(
-      double vxMetersPerSecond,
-      double vyMetersPerSecond,
-      double omegaRadiansPerSecond,
-      Rotation2d robotAngle) {
+  public ChassisSpeeds toFieldRelative(Rotation2d robotAngle) {
     // CCW rotation out of chassis frame
     var rotated = new Translation2d(vxMetersPerSecond, vyMetersPerSecond).rotateBy(robotAngle);
     return new ChassisSpeeds(rotated.getX(), rotated.getY(), omegaRadiansPerSecond);
   }
 
-  /**
-   * Converts a user provided robot-relative set of speeds into a field-relative ChassisSpeeds
-   * object.
-   *
-   * @param vx The component of speed in the x direction relative to the robot. Positive x is
-   *     towards the robot's front.
-   * @param vy The component of speed in the y direction relative to the robot. Positive y is
-   *     towards the robot's left.
-   * @param omega The angular rate of the robot.
-   * @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.
-   * @return ChassisSpeeds object representing the speeds in the field's frame of reference.
-   */
-  public static ChassisSpeeds fromRobotRelativeSpeeds(
-      LinearVelocity vx, LinearVelocity vy, AngularVelocity omega, Rotation2d robotAngle) {
-    return fromRobotRelativeSpeeds(
-        vx.in(MetersPerSecond), vy.in(MetersPerSecond), omega.in(RadiansPerSecond), robotAngle);
-  }
-
-  /**
-   * Converts a user provided robot-relative ChassisSpeeds object into a field-relative
-   * ChassisSpeeds object.
-   *
-   * @param robotRelativeSpeeds The ChassisSpeeds object representing the speeds in the robot frame
-   *     of reference. Positive x is towards the robot's front. Positive y is towards the robot's
-   *     left.
-   * @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.
-   * @return ChassisSpeeds object representing the speeds in the field's frame of reference.
-   */
-  public static ChassisSpeeds fromRobotRelativeSpeeds(
-      ChassisSpeeds robotRelativeSpeeds, Rotation2d robotAngle) {
-    return fromRobotRelativeSpeeds(
-        robotRelativeSpeeds.vxMetersPerSecond,
-        robotRelativeSpeeds.vyMetersPerSecond,
-        robotRelativeSpeeds.omegaRadiansPerSecond,
-        robotAngle);
-  }
-
   /**
    * Adds two ChassisSpeeds and returns the sum.
    *

wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java

@@ -83,23 +83,27 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
    * absolute threshold, while maintaining the ratio of speeds between wheels.
    *
    * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
+   * @return Desaturated MecanumDriveWheelSpeeds.
    */
-  public void desaturate(double attainableMaxSpeedMetersPerSecond) {
+  public MecanumDriveWheelSpeeds desaturate(double attainableMaxSpeedMetersPerSecond) {
     double realMaxSpeed =
         Math.max(Math.abs(frontLeftMetersPerSecond), Math.abs(frontRightMetersPerSecond));
     realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearLeftMetersPerSecond));
     realMaxSpeed = Math.max(realMaxSpeed, Math.abs(rearRightMetersPerSecond));
 
     if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
-      frontLeftMetersPerSecond =
-          frontLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
-      frontRightMetersPerSecond =
-          frontRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
-      rearLeftMetersPerSecond =
-          rearLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
-      rearRightMetersPerSecond =
-          rearRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
+      return new MecanumDriveWheelSpeeds(
+          frontLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond,
+          frontRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond,
+          rearLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond,
+          rearRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond);
     }
+
+    return new MecanumDriveWheelSpeeds(
+        frontLeftMetersPerSecond,
+        frontRightMetersPerSecond,
+        rearLeftMetersPerSecond,
+        rearRightMetersPerSecond);
   }
 
   /**
@@ -111,9 +115,10 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
    * absolute threshold, while maintaining the ratio of speeds between wheels.
    *
    * @param attainableMaxSpeed The absolute max speed that a wheel can reach.
+   * @return Desaturated MecanumDriveWheelSpeeds.
    */
-  public void desaturate(LinearVelocity attainableMaxSpeed) {
-    desaturate(attainableMaxSpeed.in(MetersPerSecond));
+  public MecanumDriveWheelSpeeds desaturate(LinearVelocity attainableMaxSpeed) {
+    return desaturate(attainableMaxSpeed.in(MetersPerSecond));
   }
 
   /**

wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java

@@ -52,8 +52,8 @@ public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
         new ChassisSpeeds(xdVelocity, ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
 
     // Get the wheel speeds and normalize them to within the max velocity.
-    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
-    wheelSpeeds.desaturate(m_maxSpeedMetersPerSecond);
+    var wheelSpeeds =
+        m_kinematics.toWheelSpeeds(chassisSpeeds).desaturate(m_maxSpeedMetersPerSecond);
 
     // Convert normalized wheel speeds back to chassis speeds
     var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);