[wpimath] Make swerve kinematics functions immutable

wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp

@@ -14,10 +14,9 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
   }
   chassisSpeeds = chassisSpeeds.Discretize(period);
 
-  auto states = m_kinematics.ToSwerveModuleStates(chassisSpeeds);
-  m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed);
+  auto [fl, fr, bl, br] = m_kinematics.DesaturateWheelSpeeds(
+      m_kinematics.ToSwerveModuleStates(chassisSpeeds), kMaxSpeed);
 
-  auto [fl, fr, bl, br] = states;
   m_frontLeft.SetDesiredState(fl);
   m_frontRight.SetDesiredState(fr);
   m_backLeft.SetDesiredState(bl);

wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp

@@ -50,25 +50,23 @@ void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
   frc::Rotation2d encoderRotation{
       units::radian_t{m_turningEncoder.GetDistance()}};
 
-  // Optimize the reference state to avoid spinning further than 90 degrees
-  referenceState.Optimize(encoderRotation);
-
-  // Scale speed by cosine of angle error. This scales down movement
+  // Optimize the reference state to avoid spinning further than 90 degrees,
+  // then scale speed by cosine of angle error. This scales down movement
   // perpendicular to the desired direction of travel that can occur when
   // modules change directions. This results in smoother driving.
-  referenceState.CosineScale(encoderRotation);
+  // Optimize the reference state to avoid spinning further than 90 degrees
+  auto state =
+      referenceState.Optimize(encoderRotation).CosineScale(encoderRotation);
 
   // Calculate the drive output from the drive PID controller.
   const auto driveOutput = m_drivePIDController.Calculate(
-      m_driveEncoder.GetRate(), referenceState.speed.value());
+      m_driveEncoder.GetRate(), state.speed.value());
 
-  const auto driveFeedforward =
-      m_driveFeedforward.Calculate(referenceState.speed);
+  const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed);
 
   // Calculate the turning motor output from the turning PID controller.
   const auto turnOutput = m_turningPIDController.Calculate(
-      units::radian_t{m_turningEncoder.GetDistance()},
-      referenceState.angle.Radians());
+      units::radian_t{m_turningEncoder.GetDistance()}, state.angle.Radians());
 
   const auto turnFeedforward = m_turnFeedforward.Calculate(
       m_turningPIDController.GetSetpoint().velocity);

wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp

@@ -19,10 +19,9 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
   }
   chassisSpeeds = chassisSpeeds.Discretize(period);
 
-  auto states = m_kinematics.ToSwerveModuleStates(chassisSpeeds);
-  m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed);
+  auto [fl, fr, bl, br] = m_kinematics.DesaturateWheelSpeeds(
+      m_kinematics.ToSwerveModuleStates(chassisSpeeds), kMaxSpeed);
 
-  auto [fl, fr, bl, br] = states;
   m_frontLeft.SetDesiredState(fl);
   m_frontRight.SetDesiredState(fr);
   m_backLeft.SetDesiredState(bl);

wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp

@@ -50,25 +50,22 @@ void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
   frc::Rotation2d encoderRotation{
       units::radian_t{m_turningEncoder.GetDistance()}};
 
-  // Optimize the reference state to avoid spinning further than 90 degrees
-  referenceState.Optimize(encoderRotation);
-
-  // Scale speed by cosine of angle error. This scales down movement
+  // Optimize the reference state to avoid spinning further than 90 degrees,
+  // then scale speed by cosine of angle error. This scales down movement
   // perpendicular to the desired direction of travel that can occur when
   // modules change directions. This results in smoother driving.
-  referenceState.CosineScale(encoderRotation);
+  auto state =
+      referenceState.Optimize(encoderRotation).CosineScale(encoderRotation);
 
   // Calculate the drive output from the drive PID controller.
   const auto driveOutput = m_drivePIDController.Calculate(
-      m_driveEncoder.GetRate(), referenceState.speed.value());
+      m_driveEncoder.GetRate(), state.speed.value());
 
-  const auto driveFeedforward =
-      m_driveFeedforward.Calculate(referenceState.speed);
+  const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed);
 
   // Calculate the turning motor output from the turning PID controller.
   const auto turnOutput = m_turningPIDController.Calculate(
-      units::radian_t{m_turningEncoder.GetDistance()},
-      referenceState.angle.Radians());
+      units::radian_t{m_turningEncoder.GetDistance()}, state.angle.Radians());
 
   const auto turnFeedforward = m_turnFeedforward.Calculate(
       m_turningPIDController.GetSetpoint().velocity);

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java

@@ -63,8 +63,9 @@ public void drive(
     }
     chassisSpeeds = chassisSpeeds.discretize(period);
 
-    var states = m_kinematics.toWheelSpeeds(chassisSpeeds);
-    SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeed);
+    var states =
+        SwerveDriveKinematics.desaturateWheelSpeeds(
+            m_kinematics.toWheelSpeeds(chassisSpeeds), kMaxSpeed);
 
     m_frontLeft.setDesiredState(states[0]);
     m_frontRight.setDesiredState(states[1]);

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java

@@ -110,25 +110,22 @@ public SwerveModulePosition getPosition() {
   public void setDesiredState(SwerveModuleState desiredState) {
     var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
 
-    // Optimize the reference state to avoid spinning further than 90 degrees
-    desiredState.optimize(encoderRotation);
-
-    // Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
+    // Optimize the reference state to avoid spinning further than 90 degrees, then
+    // scale speed by cosine of angle error. This scales down movement perpendicular to the desired
     // direction of travel that can occur when modules change directions. This results in smoother
     // driving.
-    desiredState.cosineScale(encoderRotation);
+    SwerveModuleState state = desiredState.optimize(encoderRotation).cosineScale(encoderRotation);
 
     // Calculate the drive output from the drive PID controller.
 
     final double driveOutput =
-        m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speed);
+        m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speed);
 
-    final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speed);
+    final double driveFeedforward = m_driveFeedforward.calculate(state.speed);
 
     // Calculate the turning motor output from the turning PID controller.
     final double turnOutput =
-        m_turningPIDController.calculate(
-            m_turningEncoder.getDistance(), desiredState.angle.getRadians());
+        m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians());
 
     final double turnFeedforward =
         m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java

@@ -74,8 +74,9 @@ public void drive(
 
     chassisSpeeds = chassisSpeeds.discretize(period);
 
-    var states = m_kinematics.toWheelSpeeds(chassisSpeeds);
-    SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeed);
+    var states =
+        SwerveDriveKinematics.desaturateWheelSpeeds(
+            m_kinematics.toWheelSpeeds(chassisSpeeds), kMaxSpeed);
 
     m_frontLeft.setDesiredState(states[0]);
     m_frontRight.setDesiredState(states[1]);

wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java

@@ -110,24 +110,21 @@ public SwerveModulePosition getPosition() {
   public void setDesiredState(SwerveModuleState desiredState) {
     var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
 
-    // Optimize the reference state to avoid spinning further than 90 degrees
-    desiredState.optimize(encoderRotation);
-
-    // Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
+    // Optimize the reference state to avoid spinning further than 90 degrees, then
+    // scale speed by cosine of angle error. This scales down movement perpendicular to the desired
     // direction of travel that can occur when modules change directions. This results in smoother
     // driving.
-    desiredState.cosineScale(encoderRotation);
+    SwerveModuleState state = desiredState.optimize(encoderRotation).cosineScale(encoderRotation);
 
     // Calculate the drive output from the drive PID controller.
     final double driveOutput =
-        m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speed);
+        m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speed);
 
-    final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speed);
+    final double driveFeedforward = m_driveFeedforward.calculate(state.speed);
 
     // Calculate the turning motor output from the turning PID controller.
     final double turnOutput =
-        m_turningPIDController.calculate(
-            m_turningEncoder.getDistance(), desiredState.angle.getRadians());
+        m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians());
 
     final double turnFeedforward =
         m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);

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

@@ -261,21 +261,31 @@ public Twist2d toTwist2d(SwerveModulePosition[] start, SwerveModulePosition[] en
    * of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
    * discretization did not account for this translational skew.
    *
-   * @param moduleStates Reference to array of module states. The array will be mutated with the
-   *     normalized speeds!
+   * @param moduleStates The array of module states.
    * @param attainableMaxSpeed The absolute max speed in meters per second that a module can reach.
+   * @return The array of desaturated module states.
    */
-  public static void desaturateWheelSpeeds(
+  public static SwerveModuleState[] desaturateWheelSpeeds(
       SwerveModuleState[] moduleStates, double attainableMaxSpeed) {
     double realMaxSpeed = 0;
     for (SwerveModuleState moduleState : moduleStates) {
       realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speed));
     }
+    var states = new SwerveModuleState[moduleStates.length];
     if (realMaxSpeed > attainableMaxSpeed) {
-      for (SwerveModuleState moduleState : moduleStates) {
-        moduleState.speed = moduleState.speed / realMaxSpeed * attainableMaxSpeed;
+      for (int i = 0; i < states.length; i++) {
+        states[i] =
+            new SwerveModuleState(
+                moduleStates[i].speed / realMaxSpeed * attainableMaxSpeed, moduleStates[i].angle);
+      }
+    } else {
+      // Copy in the event someone wants to mutate the desaturated states but also wants the
+      // original states
+      for (int i = 0; i < states.length; i++) {
+        states[i] = new SwerveModuleState(moduleStates[i].speed, moduleStates[i].angle);
       }
     }
+    return states;
   }
 
   /**
@@ -290,13 +300,13 @@ public static void desaturateWheelSpeeds(
    * of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
    * discretization did not account for this translational skew.
    *
-   * @param moduleStates Reference to array of module states. The array will be mutated with the
-   *     normalized speeds!
+   * @param moduleStates The array of module states.
    * @param attainableMaxSpeed The absolute max speed in meters per second that a module can reach.
+   * @return The array of desaturated module states.
    */
-  public static void desaturateWheelSpeeds(
+  public static SwerveModuleState[] desaturateWheelSpeeds(
       SwerveModuleState[] moduleStates, LinearVelocity attainableMaxSpeed) {
-    desaturateWheelSpeeds(moduleStates, attainableMaxSpeed.in(MetersPerSecond));
+    return desaturateWheelSpeeds(moduleStates, attainableMaxSpeed.in(MetersPerSecond));
   }
 
   /**
@@ -312,17 +322,17 @@ public static void desaturateWheelSpeeds(
    * of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
    * discretization did not account for this translational skew.
    *
-   * @param moduleStates Reference to array of module states. The array will be mutated with the
-   *     normalized speeds!
+   * @param moduleStates The array of module states
    * @param desiredChassisSpeed The desired speed of the robot
    * @param attainableMaxModuleSpeed The absolute max speed in meters per second that a module can
    *     reach
    * @param attainableMaxTranslationalSpeed The absolute max speed in meters per second that your
    *     robot can reach while translating
    * @param attainableMaxRotationalVelocity The absolute max speed in radians per second the robot
    *     can reach while rotating
+   * @return The array of desaturated module states
    */
-  public static void desaturateWheelSpeeds(
+  public static SwerveModuleState[] desaturateWheelSpeeds(
       SwerveModuleState[] moduleStates,
       ChassisSpeeds desiredChassisSpeed,
       double attainableMaxModuleSpeed,
@@ -333,20 +343,27 @@ public static void desaturateWheelSpeeds(
       realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speed));
     }
 
+    var states = new SwerveModuleState[moduleStates.length];
     if (attainableMaxTranslationalSpeed == 0
         || attainableMaxRotationalVelocity == 0
         || realMaxSpeed == 0) {
-      return;
+      // Copy in the event someone wants to mutate the desaturated states but also wants the
+      // original states
+      for (int i = 0; i < states.length; i++) {
+        states[i] = new SwerveModuleState(moduleStates[i].speed, moduleStates[i].angle);
+      }
+      return states;
     }
     double translationalK =
         Math.hypot(desiredChassisSpeed.vx, desiredChassisSpeed.vy)
             / attainableMaxTranslationalSpeed;
     double rotationalK = Math.abs(desiredChassisSpeed.omega) / attainableMaxRotationalVelocity;
     double k = Math.max(translationalK, rotationalK);
     double scale = Math.min(k * attainableMaxModuleSpeed / realMaxSpeed, 1);
-    for (SwerveModuleState moduleState : moduleStates) {
-      moduleState.speed *= scale;
+    for (int i = 0; i < states.length; i++) {
+      states[i] = new SwerveModuleState(moduleStates[i].speed * scale, moduleStates[i].angle);
     }
+    return states;
   }
 
   /**
@@ -362,22 +379,22 @@ public static void desaturateWheelSpeeds(
    * of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
    * discretization did not account for this translational skew.
    *
-   * @param moduleStates Reference to array of module states. The array will be mutated with the
-   *     normalized speeds!
+   * @param moduleStates The array of module states
    * @param desiredChassisSpeed The desired speed of the robot
    * @param attainableMaxModuleSpeed The absolute max speed that a module can reach
    * @param attainableMaxTranslationalSpeed The absolute max speed that your robot can reach while
    *     translating
    * @param attainableMaxRotationalVelocity The absolute max speed the robot can reach while
    *     rotating
+   * @return The array of desaturated module states
    */
-  public static void desaturateWheelSpeeds(
+  public static SwerveModuleState[] desaturateWheelSpeeds(
       SwerveModuleState[] moduleStates,
       ChassisSpeeds desiredChassisSpeed,
       LinearVelocity attainableMaxModuleSpeed,
       LinearVelocity attainableMaxTranslationalSpeed,
       AngularVelocity attainableMaxRotationalVelocity) {
-    desaturateWheelSpeeds(
+    return desaturateWheelSpeeds(
         moduleStates,
         desiredChassisSpeed,
         attainableMaxModuleSpeed.in(MetersPerSecond),

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

@@ -88,34 +88,14 @@ public String toString() {
    * functionality, the furthest a wheel will ever rotate is 90 degrees.
    *
    * @param currentAngle The current module angle.
+   * @return The optimized module state.
    */
-  public void optimize(Rotation2d currentAngle) {
+  public SwerveModuleState optimize(Rotation2d currentAngle) {
     var delta = angle.minus(currentAngle);
     if (Math.abs(delta.getDegrees()) > 90.0) {
-      speed *= -1;
-      angle = angle.rotateBy(Rotation2d.kPi);
-    }
-  }
-
-  /**
-   * Minimize the change in heading the desired swerve module state would require by potentially
-   * reversing the direction the wheel spins. If this is used with the PIDController class's
-   * continuous input functionality, the furthest a wheel will ever rotate is 90 degrees.
-   *
-   * @param desiredState The desired state.
-   * @param currentAngle The current module angle.
-   * @return Optimized swerve module state.
-   * @deprecated Use the instance method instead.
-   */
-  @Deprecated
-  public static SwerveModuleState optimize(
-      SwerveModuleState desiredState, Rotation2d currentAngle) {
-    var delta = desiredState.angle.minus(currentAngle);
-    if (Math.abs(delta.getDegrees()) > 90.0) {
-      return new SwerveModuleState(
-          -desiredState.speed, desiredState.angle.rotateBy(Rotation2d.kPi));
+      return new SwerveModuleState(-speed, angle.rotateBy(Rotation2d.kPi));
     } else {
-      return new SwerveModuleState(desiredState.speed, desiredState.angle);
+      return new SwerveModuleState(speed, angle);
     }
   }
 
@@ -125,8 +105,9 @@ public static SwerveModuleState optimize(
    * driving.
    *
    * @param currentAngle The current module angle.
+   * @return The scaled module state.
    */
-  public void cosineScale(Rotation2d currentAngle) {
-    speed *= angle.minus(currentAngle).getCos();
+  public SwerveModuleState cosineScale(Rotation2d currentAngle) {
+    return new SwerveModuleState(speed * angle.minus(currentAngle).getCos(), angle);
   }
 }

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

@@ -49,8 +49,9 @@ public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
     var chassisSpeeds = new ChassisSpeeds(xdVelocity, ydVelocity, velocity * curvature);
 
     // Get the wheel speeds and normalize them to within the max velocity.
-    var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
-    SwerveDriveKinematics.desaturateWheelSpeeds(wheelSpeeds, m_maxSpeed);
+    var wheelSpeeds =
+        SwerveDriveKinematics.desaturateWheelSpeeds(
+            m_kinematics.toSwerveModuleStates(chassisSpeeds), m_maxSpeed);
 
     // Convert normalized wheel speeds back to chassis speeds
     var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);