[wpimath] Make swerve and differential kinematics functions immutable (#8274)

Originally started with just swerve, but expanded to diff and mecanum (docs only) for parity across the drivetrains. Return value checks are applied when possible to make migration easier and to error loudly if people forget. --------- Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
2026-06-19 00:41:43 +00:00 · 2026-04-17 00:23:32 -04:00
parent 8c80cdcf28
commit 056d7bcbbe
34 changed files with 396 additions and 394 deletions
--- a/wpimath/BUILD.bazel
+++ b/wpimath/BUILD.bazel
@@ -236,6 +236,7 @@ wpilib_cc_shared_library(
 wpilib_jni_java_library(
    name = "wpimath-java",
    srcs = [":generated_java"] + glob(["src/main/java/**/*.java"]),
+    exported_plugins = ["//javacPlugin:plugin"],
    javacopts = [
        "-Xep:UnicodeInCode:OFF",
    ],
@@ -249,6 +250,7 @@ wpilib_jni_java_library(
    resources = glob(["src/main/proto/**"]),
    visibility = ["//visibility:public"],
    deps = [
+        "//wpiannotations",
        "//wpiunits:wpiunits-java",
        "//wpiutil:wpiutil-java",
        "@maven//:io_avaje_avaje_json_core",
--- a/wpimath/CMakeLists.txt
+++ b/wpimath/CMakeLists.txt
@@ -74,7 +74,13 @@ if(WITH_JAVA)
    add_jar(
        wpimath_jar
        ${JAVA_SOURCES}
-        INCLUDE_JARS wpiutil_jar wpiunits_jar ${EJML_JARS} ${AVAJE_JARS} ${QUICKBUF_JAR}
+        INCLUDE_JARS
+            wpiutil_jar
+            wpiunits_jar
+            wpiannotations_jar
+            ${EJML_JARS}
+            ${AVAJE_JARS}
+            ${QUICKBUF_JAR}
        OUTPUT_NAME wpimath
        OUTPUT_DIR ${WPILIB_BINARY_DIR}/${java_lib_dest}
        GENERATE_NATIVE_HEADERS wpimath_jni_headers
--- a/wpimath/build.gradle
+++ b/wpimath/build.gradle
@@ -93,6 +93,9 @@ dependencies {
    api libs.avaje.jsonb
    api libs.bundles.ejml
    api libs.quickbuf.runtime
+    implementation project(":wpiannotations")
+    annotationProcessor project(':javacPlugin')
+    testAnnotationProcessor project(':javacPlugin')
 }

 sourceSets.main.java.srcDir "${projectDir}/src/generated/main/java"
--- a/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelVelocities.java
+++ b/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelVelocities.java
@@ -6,6 +6,7 @@ package org.wpilib.math.kinematics;

 import static org.wpilib.units.Units.MetersPerSecond;

+import org.wpilib.annotation.NoDiscard;
 import org.wpilib.math.interpolation.Interpolatable;
 import org.wpilib.math.kinematics.proto.DifferentialDriveWheelVelocitiesProto;
 import org.wpilib.math.kinematics.struct.DifferentialDriveWheelVelocitiesStruct;
@@ -14,6 +15,7 @@ import org.wpilib.util.protobuf.ProtobufSerializable;
 import org.wpilib.util.struct.StructSerializable;

 /** Represents the wheel velocities for a differential drive drivetrain. */
+@NoDiscard
 public class DifferentialDriveWheelVelocities
    implements Interpolatable<DifferentialDriveWheelVelocities>,
        ProtobufSerializable,
@@ -66,14 +68,17 @@ public class DifferentialDriveWheelVelocities
   *
   * @param attainableMaxVelocity The absolute max velocity in meters per second that a wheel can
   *     reach.
+   * @return The desaturated DifferentialDriveWheelVelocities.
   */
-  public void desaturate(double attainableMaxVelocity) {
+  public DifferentialDriveWheelVelocities desaturate(double attainableMaxVelocity) {
    double realMaxVelocity = Math.max(Math.abs(left), Math.abs(right));

    if (realMaxVelocity > attainableMaxVelocity) {
-      left = left / realMaxVelocity * attainableMaxVelocity;
-      right = right / realMaxVelocity * attainableMaxVelocity;
+      return new DifferentialDriveWheelVelocities(
+          left / realMaxVelocity * attainableMaxVelocity,
+          right / realMaxVelocity * attainableMaxVelocity);
    }
+    return new DifferentialDriveWheelVelocities(left, right);
  }

  /**
@@ -86,9 +91,10 @@ public class DifferentialDriveWheelVelocities
   *
   * @param attainableMaxVelocity The absolute max velocity in meters per second that a wheel can
   *     reach.
+   * @return The desaturated DifferentialDriveWheelVelocities.
   */
-  public void desaturate(LinearVelocity attainableMaxVelocity) {
-    desaturate(attainableMaxVelocity.in(MetersPerSecond));
+  public DifferentialDriveWheelVelocities desaturate(LinearVelocity attainableMaxVelocity) {
+    return desaturate(attainableMaxVelocity.in(MetersPerSecond));
  }

  /**
--- a/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelVelocities.java
+++ b/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelVelocities.java
@@ -6,6 +6,7 @@ package org.wpilib.math.kinematics;

 import static org.wpilib.units.Units.MetersPerSecond;

+import org.wpilib.annotation.NoDiscard;
 import org.wpilib.math.interpolation.Interpolatable;
 import org.wpilib.math.kinematics.proto.MecanumDriveWheelVelocitiesProto;
 import org.wpilib.math.kinematics.struct.MecanumDriveWheelVelocitiesStruct;
@@ -14,6 +15,7 @@ import org.wpilib.util.protobuf.ProtobufSerializable;
 import org.wpilib.util.struct.StructSerializable;

 /** Represents the wheel velocities for a mecanum drive drivetrain. */
+@NoDiscard
 public class MecanumDriveWheelVelocities
    implements Interpolatable<MecanumDriveWheelVelocities>,
        ProtobufSerializable,
@@ -87,7 +89,7 @@ public class MecanumDriveWheelVelocities
   *
   * @param attainableMaxVelocity The absolute max velocity in meters per second that a wheel can
   *     reach.
-   * @return Desaturated MecanumDriveWheelVelocities.
+   * @return The desaturated MecanumDriveWheelVelocities.
   */
  public MecanumDriveWheelVelocities desaturate(double attainableMaxVelocity) {
    double realMaxVelocity = Math.max(Math.abs(frontLeft), Math.abs(frontRight));
@@ -114,7 +116,7 @@ public class MecanumDriveWheelVelocities
   * at-or-below the absolute threshold, while maintaining the ratio of velocities between wheels.
   *
   * @param attainableMaxVelocity The absolute max velocity that a wheel can reach.
-   * @return Desaturated MecanumDriveWheelVelocities.
+   * @return The desaturated MecanumDriveWheelVelocities.
   */
  public MecanumDriveWheelVelocities desaturate(LinearVelocity attainableMaxVelocity) {
    return desaturate(attainableMaxVelocity.in(MetersPerSecond));
--- a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java
+++ b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java
@@ -9,6 +9,7 @@ import static org.wpilib.units.Units.RadiansPerSecond;

 import java.util.Arrays;
 import org.ejml.simple.SimpleMatrix;
+import org.wpilib.annotation.NoDiscard;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.geometry.Translation2d;
 import org.wpilib.math.geometry.Twist2d;
@@ -261,22 +262,35 @@ public class SwerveDriveKinematics
   * direction of rotational velocity, which makes discretizing the chassis velocities inaccurate
   * because the discretization did not account for this translational skew.
   *
-   * @param moduleVelocities Reference to array of module states. The array will be mutated with the
-   *     normalized velocities!
+   * @param moduleVelocities The array of module velocities.
   * @param attainableMaxVelocity The absolute max velocity in meters per second that a module can
   *     reach.
+   * @return The array of desaturated module velocities.
   */
-  public static void desaturateWheelVelocities(
+  @NoDiscard
+  public static SwerveModuleVelocity[] desaturateWheelVelocities(
      SwerveModuleVelocity[] moduleVelocities, double attainableMaxVelocity) {
    double realMaxVelocity = 0;
    for (SwerveModuleVelocity moduleVelocity : moduleVelocities) {
      realMaxVelocity = Math.max(realMaxVelocity, Math.abs(moduleVelocity.velocity));
    }
+    var velocities = new SwerveModuleVelocity[moduleVelocities.length];
    if (realMaxVelocity > attainableMaxVelocity) {
-      for (SwerveModuleVelocity moduleVelocity : moduleVelocities) {
-        moduleVelocity.velocity = moduleVelocity.velocity / realMaxVelocity * attainableMaxVelocity;
+      for (int i = 0; i < velocities.length; i++) {
+        velocities[i] =
+            new SwerveModuleVelocity(
+                moduleVelocities[i].velocity / realMaxVelocity * attainableMaxVelocity,
+                moduleVelocities[i].angle);
+      }
+    } else {
+      // Copy in the event someone wants to mutate the desaturated velocities but also wants the
+      // original velocities
+      for (int i = 0; i < velocities.length; i++) {
+        velocities[i] =
+            new SwerveModuleVelocity(moduleVelocities[i].velocity, moduleVelocities[i].angle);
      }
    }
+    return velocities;
  }

  /**
@@ -291,14 +305,15 @@ public class SwerveDriveKinematics
   * direction of rotational velocity, which makes discretizing the chassis velocities inaccurate
   * because the discretization did not account for this translational skew.
   *
-   * @param moduleVelocities Reference to array of module states. The array will be mutated with the
-   *     normalized velocities!
+   * @param moduleVelocities The array of module velocities.
   * @param attainableMaxVelocity The absolute max velocity in meters per second that a module can
   *     reach.
+   * @return The array of desaturated module velocities.
   */
-  public static void desaturateWheelVelocities(
+  @NoDiscard
+  public static SwerveModuleVelocity[] desaturateWheelVelocities(
      SwerveModuleVelocity[] moduleVelocities, LinearVelocity attainableMaxVelocity) {
-    desaturateWheelVelocities(moduleVelocities, attainableMaxVelocity.in(MetersPerSecond));
+    return desaturateWheelVelocities(moduleVelocities, attainableMaxVelocity.in(MetersPerSecond));
  }

  /**
@@ -314,8 +329,7 @@ public class SwerveDriveKinematics
   * direction of rotational velocity, which makes discretizing the chassis velocities inaccurate
   * because the discretization did not account for this translational skew.
   *
-   * @param moduleVelocities Reference to array of module states. The array will be mutated with the
-   *     normalized velocities!
+   * @param moduleVelocities The array of module velocities
   * @param desiredChassisVelocity The desired velocity of the robot
   * @param attainableMaxModuleVelocity The absolute max velocity in meters per second that a module
   *     can reach
@@ -323,8 +337,10 @@ public class SwerveDriveKinematics
   *     your robot can reach while translating
   * @param attainableMaxRotationalVelocity The absolute max velocity in radians per second the
   *     robot can reach while rotating
+   * @return The array of desaturated module velocities
   */
-  public static void desaturateWheelVelocities(
+  @NoDiscard
+  public static SwerveModuleVelocity[] desaturateWheelVelocities(
      SwerveModuleVelocity[] moduleVelocities,
      ChassisVelocities desiredChassisVelocity,
      double attainableMaxModuleVelocity,
@@ -335,10 +351,17 @@ public class SwerveDriveKinematics
      realMaxVelocity = Math.max(realMaxVelocity, Math.abs(moduleVelocity.velocity));
    }

+    var velocities = new SwerveModuleVelocity[moduleVelocities.length];
    if (attainableMaxTranslationalVelocity == 0
        || attainableMaxRotationalVelocity == 0
        || realMaxVelocity == 0) {
-      return;
+      // Copy in the event someone wants to mutate the desaturated velocities but also wants the
+      // original velocities
+      for (int i = 0; i < velocities.length; i++) {
+        velocities[i] =
+            new SwerveModuleVelocity(moduleVelocities[i].velocity, moduleVelocities[i].angle);
+      }
+      return velocities;
    }
    double translationalK =
        Math.hypot(desiredChassisVelocity.vx, desiredChassisVelocity.vy)
@@ -346,9 +369,11 @@ public class SwerveDriveKinematics
    double rotationalK = Math.abs(desiredChassisVelocity.omega) / attainableMaxRotationalVelocity;
    double k = Math.max(translationalK, rotationalK);
    double scale = Math.min(k * attainableMaxModuleVelocity / realMaxVelocity, 1);
-    for (SwerveModuleVelocity moduleVelocity : moduleVelocities) {
-      moduleVelocity.velocity *= scale;
+    for (int i = 0; i < velocities.length; i++) {
+      velocities[i] =
+          new SwerveModuleVelocity(moduleVelocities[i].velocity * scale, moduleVelocities[i].angle);
    }
+    return velocities;
  }

  /**
@@ -364,22 +389,23 @@ public class SwerveDriveKinematics
   * direction of rotational velocity, which makes discretizing the chassis velocities inaccurate
   * because the discretization did not account for this translational skew.
   *
-   * @param moduleVelocities Reference to array of module states. The array will be mutated with the
-   *     normalized velocities!
+   * @param moduleVelocities The array of module velocities
   * @param desiredChassisVelocity The desired velocity of the robot
   * @param attainableMaxModuleVelocity The absolute max velocity that a module can reach
   * @param attainableMaxTranslationalVelocity The absolute max velocity that your robot can reach
   *     while translating
   * @param attainableMaxRotationalVelocity The absolute max velocity the robot can reach while
   *     rotating
+   * @return The array of desaturated module velocities
   */
-  public static void desaturateWheelVelocities(
+  @NoDiscard
+  public static SwerveModuleVelocity[] desaturateWheelVelocities(
      SwerveModuleVelocity[] moduleVelocities,
      ChassisVelocities desiredChassisVelocity,
      LinearVelocity attainableMaxModuleVelocity,
      LinearVelocity attainableMaxTranslationalVelocity,
      AngularVelocity attainableMaxRotationalVelocity) {
-    desaturateWheelVelocities(
+    return desaturateWheelVelocities(
        moduleVelocities,
        desiredChassisVelocity,
        attainableMaxModuleVelocity.in(MetersPerSecond),
--- a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleVelocity.java
+++ b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleVelocity.java
@@ -7,6 +7,7 @@ package org.wpilib.math.kinematics;
 import static org.wpilib.units.Units.MetersPerSecond;

 import java.util.Objects;
+import org.wpilib.annotation.NoDiscard;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.interpolation.Interpolatable;
 import org.wpilib.math.kinematics.proto.SwerveModuleVelocityProto;
@@ -16,6 +17,7 @@ import org.wpilib.util.protobuf.ProtobufSerializable;
 import org.wpilib.util.struct.StructSerializable;

 /** Represents the velocity of one swerve module. */
+@NoDiscard
 public class SwerveModuleVelocity
    implements Interpolatable<SwerveModuleVelocity>,
        Comparable<SwerveModuleVelocity>,
@@ -92,34 +94,14 @@ public class SwerveModuleVelocity
   * continuous input functionality, the furthest a wheel will ever rotate is 90 degrees.
   *
   * @param currentAngle The current module angle.
+   * @return The optimized SwerveModuleVelocity.
   */
-  public void optimize(Rotation2d currentAngle) {
+  public SwerveModuleVelocity optimize(Rotation2d currentAngle) {
    var delta = angle.minus(currentAngle);
    if (Math.abs(delta.getDegrees()) > 90.0) {
-      velocity *= -1;
-      angle = angle.rotateBy(Rotation2d.kPi);
-    }
-  }
-
-  /**
-   * Minimize the change in heading the desired swerve module velocity 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 desiredVelocity The desired velocity.
-   * @param currentAngle The current module angle.
-   * @return Optimized swerve module velocity.
-   * @deprecated Use the instance method instead.
-   */
-  @Deprecated
-  public static SwerveModuleVelocity optimize(
-      SwerveModuleVelocity desiredVelocity, Rotation2d currentAngle) {
-    var delta = desiredVelocity.angle.minus(currentAngle);
-    if (Math.abs(delta.getDegrees()) > 90.0) {
-      return new SwerveModuleVelocity(
-          -desiredVelocity.velocity, desiredVelocity.angle.rotateBy(Rotation2d.kPi));
+      return new SwerveModuleVelocity(-velocity, angle.rotateBy(Rotation2d.kPi));
    } else {
-      return new SwerveModuleVelocity(desiredVelocity.velocity, desiredVelocity.angle);
+      return new SwerveModuleVelocity(velocity, angle);
    }
  }

@@ -151,8 +133,9 @@ public class SwerveModuleVelocity
   * smoother driving.
   *
   * @param currentAngle The current module angle.
+   * @return The scaled SwerveModuleVelocity.
   */
-  public void cosineScale(Rotation2d currentAngle) {
-    velocity *= angle.minus(currentAngle).getCos();
+  public SwerveModuleVelocity cosineScale(Rotation2d currentAngle) {
+    return new SwerveModuleVelocity(velocity * angle.minus(currentAngle).getCos(), angle);
  }
 }
--- a/wpimath/src/main/java/org/wpilib/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
+++ b/wpimath/src/main/java/org/wpilib/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
@@ -44,8 +44,8 @@ public class DifferentialDriveKinematicsConstraint implements TrajectoryConstrai
    var chassisVelocities = new ChassisVelocities(velocity, 0, velocity * curvature);

    // Get the wheel velocities and normalize them to within the max velocity.
-    var wheelVelocities = m_kinematics.toWheelVelocities(chassisVelocities);
-    wheelVelocities.desaturate(m_maxVelocity);
+    var wheelVelocities =
+        m_kinematics.toWheelVelocities(chassisVelocities).desaturate(m_maxVelocity);

    // Return the new linear chassis velocity.
    return m_kinematics.toChassisVelocities(wheelVelocities).vx;
--- a/wpimath/src/main/java/org/wpilib/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
+++ b/wpimath/src/main/java/org/wpilib/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
@@ -50,8 +50,9 @@ public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
    var chassisVelocities = new ChassisVelocities(xdVelocity, ydVelocity, velocity * curvature);

    // Get the wheel velocities and normalize them to within the max velocity.
-    var wheelVelocities = m_kinematics.toSwerveModuleVelocities(chassisVelocities);
-    SwerveDriveKinematics.desaturateWheelVelocities(wheelVelocities, m_maxVelocity);
+    var wheelVelocities =
+        SwerveDriveKinematics.desaturateWheelVelocities(
+            m_kinematics.toSwerveModuleVelocities(chassisVelocities), m_maxVelocity);

    // Convert normalized wheel velocities back to chassis velocities
    var normVelocities = m_kinematics.toChassisVelocities(wheelVelocities);
--- a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp
+++ b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp
@@ -36,16 +36,19 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelVelocities {
   *
   * @param attainableMaxVelocity The absolute max velocity that a wheel can
   *     reach.
+   * @return The desaturated DifferentialDriveWheelVelocities.
   */
-  constexpr void Desaturate(
+  [[nodiscard]]
+  constexpr DifferentialDriveWheelVelocities Desaturate(
      wpi::units::meters_per_second_t attainableMaxVelocity) {
    auto realMaxVelocity = wpi::units::math::max(wpi::units::math::abs(left),
                                                 wpi::units::math::abs(right));

    if (realMaxVelocity > attainableMaxVelocity) {
-      left = left / realMaxVelocity * attainableMaxVelocity;
-      right = right / realMaxVelocity * attainableMaxVelocity;
+      return {left / realMaxVelocity * attainableMaxVelocity,
+              right / realMaxVelocity * attainableMaxVelocity};
    }
+    return *this;
  }

  /**
--- a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelVelocities.hpp
+++ b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelVelocities.hpp
@@ -49,8 +49,9 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelVelocities {
   *
   * @param attainableMaxVelocity The absolute max velocity that a wheel can
   *     reach.
-   * @return Desaturated MecanumDriveWheelVelocities.
+   * @return The desaturated MecanumDriveWheelVelocities.
   */
+  [[nodiscard]]
  constexpr MecanumDriveWheelVelocities Desaturate(
      wpi::units::meters_per_second_t attainableMaxVelocity) const {
    std::array<wpi::units::meters_per_second_t, 4> wheelVelocities{
--- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp
+++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp
@@ -168,7 +168,7 @@ class SwerveDriveKinematics
   *     module states are not normalized. Sometimes, a user input may cause one
   *     of the module velocities to go above the attainable max velocity. Use
   *     the DesaturateWheelVelocities(wpi::util::array<SwerveModuleVelocity,
-   *     NumModules>*, wpi::units::meters_per_second_t) function to rectify this
+   *     NumModules>, wpi::units::meters_per_second_t) function to rectify this
   *     issue. In addition, you can leverage the power of C++17 to directly
   *     assign the module states to variables:
   *
@@ -357,17 +357,18 @@ class SwerveDriveKinematics
   * chassis velocities inaccurate because the discretization did not account
   * for this translational skew.
   *
-   * @param moduleVelocities Reference to array of module states. The array will
-   *     be mutated with the normalized velocities!
+   * @param moduleVelocities The array of module velocities.
   * @param attainableMaxVelocity The absolute max velocity that a module can
   *     reach.
+   * @return The array of desaturated module velocities.
   */
-  static void DesaturateWheelVelocities(
-      wpi::util::array<SwerveModuleVelocity, NumModules>* moduleVelocities,
+  [[nodiscard]]
+  static wpi::util::array<SwerveModuleVelocity, NumModules>
+  DesaturateWheelVelocities(
+      wpi::util::array<SwerveModuleVelocity, NumModules> moduleVelocities,
      wpi::units::meters_per_second_t attainableMaxVelocity) {
-    auto& states = *moduleVelocities;
    auto realMaxVelocity = wpi::units::math::abs(
-        std::max_element(states.begin(), states.end(),
+        std::max_element(moduleVelocities.begin(), moduleVelocities.end(),
                         [](const auto& a, const auto& b) {
                           return wpi::units::math::abs(a.velocity) <
                                  wpi::units::math::abs(b.velocity);
@@ -375,11 +376,16 @@ class SwerveDriveKinematics
            ->velocity);

    if (realMaxVelocity > attainableMaxVelocity) {
-      for (auto& module : states) {
-        module.velocity =
-            module.velocity / realMaxVelocity * attainableMaxVelocity;
+      wpi::util::array<SwerveModuleVelocity, NumModules> velocities(
+          wpi::util::empty_array);
+      for (size_t i = 0; i < NumModules; ++i) {
+        velocities[i] = {moduleVelocities[i].velocity / realMaxVelocity *
+                             attainableMaxVelocity,
+                         moduleVelocities[i].angle};
      }
+      return velocities;
    }
+    return moduleVelocities;
  }

  /**
@@ -399,8 +405,7 @@ class SwerveDriveKinematics
   * chassis velocities inaccurate because the discretization did not account
   * for this translational skew.
   *
-   * @param moduleVelocities Reference to array of module states. The array will
-   *     be mutated with the normalized velocities!
+   * @param moduleVelocities The array of module velocities.
   * @param desiredChassisVelocity The desired velocity of the robot
   * @param attainableMaxModuleVelocity The absolute max velocity a module can
   *     reach
@@ -408,17 +413,18 @@ class SwerveDriveKinematics
   *     robot can reach while translating
   * @param attainableMaxRobotRotationVelocity The absolute max velocity the
   *     robot can reach while rotating
+   * @return The array of desaturated module velocities
   */
-  static void DesaturateWheelVelocities(
-      wpi::util::array<SwerveModuleVelocity, NumModules>* moduleVelocities,
+  [[nodiscard]]
+  static wpi::util::array<SwerveModuleVelocity, NumModules>
+  DesaturateWheelVelocities(
+      wpi::util::array<SwerveModuleVelocity, NumModules> moduleVelocities,
      ChassisVelocities desiredChassisVelocity,
      wpi::units::meters_per_second_t attainableMaxModuleVelocity,
      wpi::units::meters_per_second_t attainableMaxRobotTranslationVelocity,
      wpi::units::radians_per_second_t attainableMaxRobotRotationVelocity) {
-    auto& states = *moduleVelocities;
-
    auto realMaxVelocity = wpi::units::math::abs(
-        std::max_element(states.begin(), states.end(),
+        std::max_element(moduleVelocities.begin(), moduleVelocities.end(),
                         [](const auto& a, const auto& b) {
                           return wpi::units::math::abs(a.velocity) <
                                  wpi::units::math::abs(b.velocity);
@@ -428,7 +434,7 @@ class SwerveDriveKinematics
    if (attainableMaxRobotTranslationVelocity == 0_mps ||
        attainableMaxRobotRotationVelocity == 0_rad_per_s ||
        realMaxVelocity == 0_mps) {
-      return;
+      return moduleVelocities;
    }

    auto translationalK = wpi::units::math::hypot(desiredChassisVelocity.vx,
@@ -443,9 +449,13 @@ class SwerveDriveKinematics
    auto scale =
        wpi::units::math::min(k * attainableMaxModuleVelocity / realMaxVelocity,
                              wpi::units::scalar_t{1});
-    for (auto& module : states) {
-      module.velocity = module.velocity * scale;
+    wpi::util::array<SwerveModuleVelocity, NumModules> velocities(
+        wpi::util::empty_array);
+    for (size_t i = 0; i < NumModules; ++i) {
+      velocities[i] = {moduleVelocities[i].velocity * scale,
+                       moduleVelocities[i].angle};
    }
+    return velocities;
  }

  wpi::util::array<SwerveModulePosition, NumModules> Interpolate(
--- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleVelocity.hpp
+++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleVelocity.hpp
@@ -43,34 +43,15 @@ struct WPILIB_DLLEXPORT SwerveModuleVelocity {
   * furthest a wheel will ever rotate is 90 degrees.
   *
   * @param currentAngle The current module angle.
+   * @return The optimized SwerveModuleVelocity.
   */
-  constexpr void Optimize(const Rotation2d& currentAngle) {
+  [[nodiscard]]
+  constexpr SwerveModuleVelocity Optimize(const Rotation2d& currentAngle) {
    auto delta = angle - currentAngle;
    if (wpi::units::math::abs(delta.Degrees()) > 90_deg) {
-      velocity *= -1;
-      angle = angle + Rotation2d{180_deg};
-    }
-  }
-
-  /**
-   * Minimize the change in heading the desired swerve module velocity 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 desiredVelocity The desired velocity.
-   * @param currentAngle The current module angle.
-   */
-  [[deprecated("Use instance method instead.")]]
-  constexpr static SwerveModuleVelocity Optimize(
-      const SwerveModuleVelocity& desiredVelocity,
-      const Rotation2d& currentAngle) {
-    auto delta = desiredVelocity.angle - currentAngle;
-    if (wpi::units::math::abs(delta.Degrees()) > 90_deg) {
-      return {-desiredVelocity.velocity,
-              desiredVelocity.angle + Rotation2d{180_deg}};
+      return {-velocity, angle + Rotation2d{180_deg}};
    } else {
-      return {desiredVelocity.velocity, desiredVelocity.angle};
+      return {velocity, angle};
    }
  }

@@ -80,9 +61,11 @@ struct WPILIB_DLLEXPORT SwerveModuleVelocity {
   * modules change directions. This results in smoother driving.
   *
   * @param currentAngle The current module angle.
+   * @return The scaled SwerveModuleVelocity.
   */
-  constexpr void CosineScale(const Rotation2d& currentAngle) {
-    velocity *= (angle - currentAngle).Cos();
+  [[nodiscard]]
+  constexpr SwerveModuleVelocity CosineScale(const Rotation2d& currentAngle) {
+    return {velocity * (angle - currentAngle).Cos(), angle};
  }
 };
 }  // namespace wpi::math
--- a/wpimath/src/main/native/include/wpi/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.hpp
+++ b/wpimath/src/main/native/include/wpi/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.hpp
@@ -30,8 +30,8 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematicsConstraint
      const Pose2d& pose, wpi::units::curvature_t curvature,
      wpi::units::meters_per_second_t velocity) const override {
    auto wheelVelocities =
-        m_kinematics.ToWheelVelocities({velocity, 0_mps, velocity * curvature});
-    wheelVelocities.Desaturate(m_maxVelocity);
+        m_kinematics.ToWheelVelocities({velocity, 0_mps, velocity * curvature})
+            .Desaturate(m_maxVelocity);

    return m_kinematics.ToChassisVelocities(wheelVelocities).vx;
  }
--- a/wpimath/src/main/native/include/wpi/math/trajectory/constraint/SwerveDriveKinematicsConstraint.hpp
+++ b/wpimath/src/main/native/include/wpi/math/trajectory/constraint/SwerveDriveKinematicsConstraint.hpp
@@ -31,9 +31,9 @@ class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
    auto yVelocity = velocity * pose.Rotation().Sin();
    auto wheelVelocities = m_kinematics.ToSwerveModuleVelocities(
        {xVelocity, yVelocity, velocity * curvature});
-    m_kinematics.DesaturateWheelVelocities(&wheelVelocities, m_maxVelocity);

-    auto normVelocities = m_kinematics.ToChassisVelocities(wheelVelocities);
+    auto normVelocities = m_kinematics.ToChassisVelocities(
+        m_kinematics.DesaturateWheelVelocities(wheelVelocities, m_maxVelocity));

    return wpi::units::math::hypot(normVelocities.vx, normVelocities.vy);
  }
--- a/wpimath/src/main/python/semiwrap/SwerveDriveKinematics.yml
+++ b/wpimath/src/main/python/semiwrap/SwerveDriveKinematics.yml
@@ -58,23 +58,9 @@ classes:
          :
      DesaturateWheelVelocities:
        overloads:
-          wpi::util::array<SwerveModuleVelocity, NumModules>*, wpi::units::meters_per_second_t:
-            cpp_code: |
-              [](wpi::util::array<SwerveModuleVelocity, NumModules> moduleVelocities, wpi::units::meters_per_second_t attainableMaxVelocity) {
-                wpi::math::SwerveDriveKinematics<NumModules>::DesaturateWheelVelocities(&moduleVelocities, attainableMaxVelocity);
-                return moduleVelocities;
-              }
-          ? wpi::util::array<SwerveModuleVelocity, NumModules>*, ChassisVelocities, wpi::units::meters_per_second_t, wpi::units::meters_per_second_t, wpi::units::radians_per_second_t
-          : cpp_code: |
-              [](wpi::util::array<SwerveModuleVelocity, NumModules> moduleVelocities,
-                  ChassisVelocities currentChassisVelocity,
-                  wpi::units::meters_per_second_t attainableMaxModuleVelocity,
-                  wpi::units::meters_per_second_t attainableMaxRobotTranslationVelocity,
-                  wpi::units::radians_per_second_t attainableMaxRobotRotationVelocity) {
-                wpi::math::SwerveDriveKinematics<NumModules>::DesaturateWheelVelocities(&moduleVelocities, currentChassisVelocity, attainableMaxModuleVelocity, attainableMaxRobotTranslationVelocity, attainableMaxRobotRotationVelocity);
-                return moduleVelocities;
-              }
-
+          wpi::util::array<SwerveModuleVelocity, NumModules>, wpi::units::meters_per_second_t:
+          ? wpi::util::array<SwerveModuleVelocity, NumModules>, ChassisVelocities, wpi::units::meters_per_second_t, wpi::units::meters_per_second_t, wpi::units::radians_per_second_t
+          :
      Interpolate:
      GetModules:
      ToSwerveModuleAccelerations:
--- a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java
+++ b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java
@@ -350,15 +350,15 @@ class SwerveDriveKinematicsTest {
    SwerveModuleVelocity br = new SwerveModuleVelocity(7, Rotation2d.kZero);

    SwerveModuleVelocity[] arr = {fl, fr, bl, br};
-    SwerveDriveKinematics.desaturateWheelVelocities(arr, 5.5);
+    var desaturatedArr = SwerveDriveKinematics.desaturateWheelVelocities(arr, 5.5);

    double factor = 5.5 / 7.0;

    assertAll(
-        () -> assertEquals(5.0 * factor, arr[0].velocity, kEpsilon),
-        () -> assertEquals(6.0 * factor, arr[1].velocity, kEpsilon),
-        () -> assertEquals(4.0 * factor, arr[2].velocity, kEpsilon),
-        () -> assertEquals(7.0 * factor, arr[3].velocity, kEpsilon));
+        () -> assertEquals(5.0 * factor, desaturatedArr[0].velocity, kEpsilon),
+        () -> assertEquals(6.0 * factor, desaturatedArr[1].velocity, kEpsilon),
+        () -> assertEquals(4.0 * factor, desaturatedArr[2].velocity, kEpsilon),
+        () -> assertEquals(7.0 * factor, desaturatedArr[3].velocity, kEpsilon));
  }

  @Test
@@ -369,16 +369,17 @@ class SwerveDriveKinematicsTest {
    SwerveModuleVelocity br = new SwerveModuleVelocity(7, Rotation2d.kZero);

    SwerveModuleVelocity[] arr = {fl, fr, bl, br};
-    SwerveDriveKinematics.desaturateWheelVelocities(
-        arr, m_kinematics.toChassisVelocities(arr), 5.5, 5.5, 3.5);
+    var desaturatedArr =
+        SwerveDriveKinematics.desaturateWheelVelocities(
+            arr, m_kinematics.toChassisVelocities(arr), 5.5, 5.5, 3.5);

    double factor = 5.5 / 7.0;

    assertAll(
-        () -> assertEquals(5.0 * factor, arr[0].velocity, kEpsilon),
-        () -> assertEquals(6.0 * factor, arr[1].velocity, kEpsilon),
-        () -> assertEquals(4.0 * factor, arr[2].velocity, kEpsilon),
-        () -> assertEquals(7.0 * factor, arr[3].velocity, kEpsilon));
+        () -> assertEquals(5.0 * factor, desaturatedArr[0].velocity, kEpsilon),
+        () -> assertEquals(6.0 * factor, desaturatedArr[1].velocity, kEpsilon),
+        () -> assertEquals(4.0 * factor, desaturatedArr[2].velocity, kEpsilon),
+        () -> assertEquals(7.0 * factor, desaturatedArr[3].velocity, kEpsilon));
  }

  @Test
@@ -389,13 +390,13 @@ class SwerveDriveKinematicsTest {
    SwerveModuleVelocity br = new SwerveModuleVelocity(-2, Rotation2d.kZero);

    SwerveModuleVelocity[] arr = {fl, fr, bl, br};
-    SwerveDriveKinematics.desaturateWheelVelocities(arr, 1);
+    var desaturatedArr = SwerveDriveKinematics.desaturateWheelVelocities(arr, 1);

    assertAll(
-        () -> assertEquals(0.5, arr[0].velocity, kEpsilon),
-        () -> assertEquals(0.5, arr[1].velocity, kEpsilon),
-        () -> assertEquals(-1.0, arr[2].velocity, kEpsilon),
-        () -> assertEquals(-1.0, arr[3].velocity, kEpsilon));
+        () -> assertEquals(0.5, desaturatedArr[0].velocity, kEpsilon),
+        () -> assertEquals(0.5, desaturatedArr[1].velocity, kEpsilon),
+        () -> assertEquals(-1.0, desaturatedArr[2].velocity, kEpsilon),
+        () -> assertEquals(-1.0, desaturatedArr[3].velocity, kEpsilon));
  }

  @Test
--- a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleVelocityTest.java
+++ b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleVelocityTest.java
@@ -17,137 +17,137 @@ class SwerveModuleVelocityTest {
  void testOptimize() {
    var angleA = Rotation2d.fromDegrees(45);
    var refA = new SwerveModuleVelocity(-2.0, Rotation2d.kPi);
-    refA.optimize(angleA);
+    var optimizedA = refA.optimize(angleA);

    assertAll(
-        () -> assertEquals(2.0, refA.velocity, kEpsilon),
-        () -> assertEquals(0.0, refA.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(2.0, optimizedA.velocity, kEpsilon),
+        () -> assertEquals(0.0, optimizedA.angle.getDegrees(), kEpsilon));

    var angleB = Rotation2d.fromDegrees(-50);
    var refB = new SwerveModuleVelocity(4.7, Rotation2d.fromDegrees(41));
-    refB.optimize(angleB);
+    var optimizedB = refB.optimize(angleB);

    assertAll(
-        () -> assertEquals(-4.7, refB.velocity, kEpsilon),
-        () -> assertEquals(-139.0, refB.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-4.7, optimizedB.velocity, kEpsilon),
+        () -> assertEquals(-139.0, optimizedB.angle.getDegrees(), kEpsilon));
  }

  @Test
  void testNoOptimize() {
    var angleA = Rotation2d.kZero;
    var refA = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(89));
-    refA.optimize(angleA);
+    var optimizedA = refA.optimize(angleA);

    assertAll(
-        () -> assertEquals(2.0, refA.velocity, kEpsilon),
-        () -> assertEquals(89.0, refA.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(2.0, optimizedA.velocity, kEpsilon),
+        () -> assertEquals(89.0, optimizedA.angle.getDegrees(), kEpsilon));

    var angleB = Rotation2d.kZero;
    var refB = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(-2));
-    refB.optimize(angleB);
+    var optimizedB = refB.optimize(angleB);

    assertAll(
-        () -> assertEquals(-2.0, refB.velocity, kEpsilon),
-        () -> assertEquals(-2.0, refB.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-2.0, optimizedB.velocity, kEpsilon),
+        () -> assertEquals(-2.0, optimizedB.angle.getDegrees(), kEpsilon));
  }

  @Test
  void testCosineScale() {
    var angleA = Rotation2d.fromDegrees(0.0);
    var refA = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
-    refA.cosineScale(angleA);
+    var optimizedA = refA.cosineScale(angleA);

    assertAll(
-        () -> assertEquals(Math.sqrt(2.0), refA.velocity, kEpsilon),
-        () -> assertEquals(45.0, refA.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(Math.sqrt(2.0), optimizedA.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedA.angle.getDegrees(), kEpsilon));

    var angleB = Rotation2d.fromDegrees(45.0);
    var refB = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
-    refB.cosineScale(angleB);
+    var optimizedB = refB.cosineScale(angleB);

    assertAll(
-        () -> assertEquals(2.0, refB.velocity, kEpsilon),
-        () -> assertEquals(45.0, refB.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(2.0, optimizedB.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedB.angle.getDegrees(), kEpsilon));

    var angleC = Rotation2d.fromDegrees(-45.0);
    var refC = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
-    refC.cosineScale(angleC);
+    var optimizedC = refC.cosineScale(angleC);

    assertAll(
-        () -> assertEquals(0.0, refC.velocity, kEpsilon),
-        () -> assertEquals(45.0, refC.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(0.0, optimizedC.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedC.angle.getDegrees(), kEpsilon));

    var angleD = Rotation2d.fromDegrees(135.0);
    var refD = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
-    refD.cosineScale(angleD);
+    var optimizedD = refD.cosineScale(angleD);

    assertAll(
-        () -> assertEquals(0.0, refD.velocity, kEpsilon),
-        () -> assertEquals(45.0, refD.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(0.0, optimizedD.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedD.angle.getDegrees(), kEpsilon));

    var angleE = Rotation2d.fromDegrees(-135.0);
    var refE = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
-    refE.cosineScale(angleE);
+    var optimizedE = refE.cosineScale(angleE);

    assertAll(
-        () -> assertEquals(-2.0, refE.velocity, kEpsilon),
-        () -> assertEquals(45.0, refE.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-2.0, optimizedE.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedE.angle.getDegrees(), kEpsilon));

    var angleF = Rotation2d.fromDegrees(180.0);
    var refF = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
-    refF.cosineScale(angleF);
+    var optimizedF = refF.cosineScale(angleF);

    assertAll(
-        () -> assertEquals(-Math.sqrt(2.0), refF.velocity, kEpsilon),
-        () -> assertEquals(45.0, refF.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-Math.sqrt(2.0), optimizedF.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedF.angle.getDegrees(), kEpsilon));

    var angleG = Rotation2d.fromDegrees(0.0);
    var refG = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
-    refG.cosineScale(angleG);
+    var optimizedG = refG.cosineScale(angleG);

    assertAll(
-        () -> assertEquals(-Math.sqrt(2.0), refG.velocity, kEpsilon),
-        () -> assertEquals(45.0, refG.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-Math.sqrt(2.0), optimizedG.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedG.angle.getDegrees(), kEpsilon));

    var angleH = Rotation2d.fromDegrees(45.0);
    var refH = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
-    refH.cosineScale(angleH);
+    var optimizedH = refH.cosineScale(angleH);

    assertAll(
-        () -> assertEquals(-2.0, refH.velocity, kEpsilon),
-        () -> assertEquals(45.0, refH.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-2.0, optimizedH.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedH.angle.getDegrees(), kEpsilon));

    var angleI = Rotation2d.fromDegrees(-45.0);
    var refI = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
-    refI.cosineScale(angleI);
+    var optimizedI = refI.cosineScale(angleI);

    assertAll(
-        () -> assertEquals(0.0, refI.velocity, kEpsilon),
-        () -> assertEquals(45.0, refI.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(0.0, optimizedI.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedI.angle.getDegrees(), kEpsilon));

    var angleJ = Rotation2d.fromDegrees(135.0);
    var refJ = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
-    refJ.cosineScale(angleJ);
+    var optimizedJ = refJ.cosineScale(angleJ);

    assertAll(
-        () -> assertEquals(0.0, refJ.velocity, kEpsilon),
-        () -> assertEquals(45.0, refJ.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(0.0, optimizedJ.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedJ.angle.getDegrees(), kEpsilon));

    var angleK = Rotation2d.fromDegrees(-135.0);
    var refK = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
-    refK.cosineScale(angleK);
+    var optimizedK = refK.cosineScale(angleK);

    assertAll(
-        () -> assertEquals(2.0, refK.velocity, kEpsilon),
-        () -> assertEquals(45.0, refK.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(2.0, optimizedK.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedK.angle.getDegrees(), kEpsilon));

    var angleL = Rotation2d.fromDegrees(180.0);
    var refL = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
-    refL.cosineScale(angleL);
+    var optimizedL = refL.cosineScale(angleL);

    assertAll(
-        () -> assertEquals(Math.sqrt(2.0), refL.velocity, kEpsilon),
-        () -> assertEquals(45.0, refL.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(Math.sqrt(2.0), optimizedL.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedL.angle.getDegrees(), kEpsilon));
  }

  @Test
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
@@ -272,7 +272,7 @@ TEST_F(SwerveDriveKinematicsTest, Desaturate) {
  SwerveModuleVelocity state4{7.0_mps, 0_deg};

  wpi::util::array<SwerveModuleVelocity, 4> arr{state1, state2, state3, state4};
-  SwerveDriveKinematics<4>::DesaturateWheelVelocities(&arr, 5.5_mps);
+  arr = SwerveDriveKinematics<4>::DesaturateWheelVelocities(arr, 5.5_mps);

  double kFactor = 5.5 / 7.0;

@@ -289,8 +289,8 @@ TEST_F(SwerveDriveKinematicsTest, DesaturateSmooth) {
  SwerveModuleVelocity state4{7.0_mps, 0_deg};

  wpi::util::array<SwerveModuleVelocity, 4> arr{state1, state2, state3, state4};
-  SwerveDriveKinematics<4>::DesaturateWheelVelocities(
-      &arr, m_kinematics.ToChassisVelocities(arr), 5.5_mps, 5.5_mps,
+  arr = SwerveDriveKinematics<4>::DesaturateWheelVelocities(
+      arr, m_kinematics.ToChassisVelocities(arr), 5.5_mps, 5.5_mps,
      3.5_rad_per_s);

  double kFactor = 5.5 / 7.0;
@@ -308,7 +308,7 @@ TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeVelocity) {
  SwerveModuleVelocity state4{-2.0_mps, 0_deg};

  wpi::util::array<SwerveModuleVelocity, 4> arr{state1, state2, state3, state4};
-  SwerveDriveKinematics<4>::DesaturateWheelVelocities(&arr, 1.0_mps);
+  arr = SwerveDriveKinematics<4>::DesaturateWheelVelocities(arr, 1.0_mps);

  EXPECT_NEAR(arr[0].velocity.value(), 0.5, kEpsilon);
  EXPECT_NEAR(arr[1].velocity.value(), 0.5, kEpsilon);
--- a/wpimath/src/test/native/cpp/kinematics/SwerveModuleVelocityTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveModuleVelocityTest.cpp
@@ -13,119 +13,119 @@ static constexpr double kEpsilon = 1E-9;
 TEST(SwerveModuleVelocityTest, Optimize) {
  wpi::math::Rotation2d angleA{45_deg};
  wpi::math::SwerveModuleVelocity refA{-2_mps, 180_deg};
-  refA.Optimize(angleA);
+  auto optimizedA = refA.Optimize(angleA);

-  EXPECT_NEAR(refA.velocity.value(), 2.0, kEpsilon);
-  EXPECT_NEAR(refA.angle.Degrees().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.velocity.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.angle.Degrees().value(), 0.0, kEpsilon);

  wpi::math::Rotation2d angleB{-50_deg};
  wpi::math::SwerveModuleVelocity refB{4.7_mps, 41_deg};
-  refB.Optimize(angleB);
+  auto optimizedB = refB.Optimize(angleB);

-  EXPECT_NEAR(refB.velocity.value(), -4.7, kEpsilon);
-  EXPECT_NEAR(refB.angle.Degrees().value(), -139.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.velocity.value(), -4.7, kEpsilon);
+  EXPECT_NEAR(optimizedB.angle.Degrees().value(), -139.0, kEpsilon);
 }

 TEST(SwerveModuleVelocityTest, NoOptimize) {
  wpi::math::Rotation2d angleA{0_deg};
  wpi::math::SwerveModuleVelocity refA{2_mps, 89_deg};
-  refA.Optimize(angleA);
+  auto optimizedA = refA.Optimize(angleA);

-  EXPECT_NEAR(refA.velocity.value(), 2.0, kEpsilon);
-  EXPECT_NEAR(refA.angle.Degrees().value(), 89.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.velocity.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.angle.Degrees().value(), 89.0, kEpsilon);

  wpi::math::Rotation2d angleB{0_deg};
  wpi::math::SwerveModuleVelocity refB{-2_mps, -2_deg};
-  refB.Optimize(angleB);
+  auto optimizedB = refB.Optimize(angleB);

-  EXPECT_NEAR(refB.velocity.value(), -2.0, kEpsilon);
-  EXPECT_NEAR(refB.angle.Degrees().value(), -2.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.velocity.value(), -2.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.angle.Degrees().value(), -2.0, kEpsilon);
 }

 TEST(SwerveModuleVelocityTest, CosineScaling) {
  wpi::math::Rotation2d angleA{0_deg};
  wpi::math::SwerveModuleVelocity refA{2_mps, 45_deg};
-  refA.CosineScale(angleA);
+  auto optimizedA = refA.CosineScale(angleA);

-  EXPECT_NEAR(refA.velocity.value(), std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(refA.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.velocity.value(), std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(optimizedA.angle.Degrees().value(), 45.0, kEpsilon);

  wpi::math::Rotation2d angleB{45_deg};
  wpi::math::SwerveModuleVelocity refB{2_mps, 45_deg};
-  refB.CosineScale(angleB);
+  auto optimizedB = refB.CosineScale(angleB);

-  EXPECT_NEAR(refB.velocity.value(), 2.0, kEpsilon);
-  EXPECT_NEAR(refB.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.velocity.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.angle.Degrees().value(), 45.0, kEpsilon);

  wpi::math::Rotation2d angleC{-45_deg};
  wpi::math::SwerveModuleVelocity refC{2_mps, 45_deg};
-  refC.CosineScale(angleC);
+  auto optimizedC = refC.CosineScale(angleC);

-  EXPECT_NEAR(refC.velocity.value(), 0.0, kEpsilon);
-  EXPECT_NEAR(refC.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedC.velocity.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(optimizedC.angle.Degrees().value(), 45.0, kEpsilon);

  wpi::math::Rotation2d angleD{135_deg};
  wpi::math::SwerveModuleVelocity refD{2_mps, 45_deg};
-  refD.CosineScale(angleD);
+  auto optimizedD = refD.CosineScale(angleD);

-  EXPECT_NEAR(refD.velocity.value(), 0.0, kEpsilon);
-  EXPECT_NEAR(refD.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedD.velocity.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(optimizedD.angle.Degrees().value(), 45.0, kEpsilon);

  wpi::math::Rotation2d angleE{-135_deg};
  wpi::math::SwerveModuleVelocity refE{2_mps, 45_deg};
-  refE.CosineScale(angleE);
+  auto optimizedE = refE.CosineScale(angleE);

-  EXPECT_NEAR(refE.velocity.value(), -2.0, kEpsilon);
-  EXPECT_NEAR(refE.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedE.velocity.value(), -2.0, kEpsilon);
+  EXPECT_NEAR(optimizedE.angle.Degrees().value(), 45.0, kEpsilon);

  wpi::math::Rotation2d angleF{180_deg};
  wpi::math::SwerveModuleVelocity refF{2_mps, 45_deg};
-  refF.CosineScale(angleF);
+  auto optimizedF = refF.CosineScale(angleF);

-  EXPECT_NEAR(refF.velocity.value(), -std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(refF.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedF.velocity.value(), -std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(optimizedF.angle.Degrees().value(), 45.0, kEpsilon);

  wpi::math::Rotation2d angleG{0_deg};
  wpi::math::SwerveModuleVelocity refG{-2_mps, 45_deg};
-  refG.CosineScale(angleG);
+  auto optimizedG = refG.CosineScale(angleG);

-  EXPECT_NEAR(refG.velocity.value(), -std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(refG.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedG.velocity.value(), -std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(optimizedG.angle.Degrees().value(), 45.0, kEpsilon);

  wpi::math::Rotation2d angleH{45_deg};
  wpi::math::SwerveModuleVelocity refH{-2_mps, 45_deg};
-  refH.CosineScale(angleH);
+  auto optimizedH = refH.CosineScale(angleH);

-  EXPECT_NEAR(refH.velocity.value(), -2.0, kEpsilon);
-  EXPECT_NEAR(refH.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedH.velocity.value(), -2.0, kEpsilon);
+  EXPECT_NEAR(optimizedH.angle.Degrees().value(), 45.0, kEpsilon);

  wpi::math::Rotation2d angleI{-45_deg};
  wpi::math::SwerveModuleVelocity refI{-2_mps, 45_deg};
-  refI.CosineScale(angleI);
+  auto optimizedI = refI.CosineScale(angleI);

-  EXPECT_NEAR(refI.velocity.value(), 0.0, kEpsilon);
-  EXPECT_NEAR(refI.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedI.velocity.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(optimizedI.angle.Degrees().value(), 45.0, kEpsilon);

  wpi::math::Rotation2d angleJ{135_deg};
  wpi::math::SwerveModuleVelocity refJ{-2_mps, 45_deg};
-  refJ.CosineScale(angleJ);
+  auto optimizedJ = refJ.CosineScale(angleJ);

-  EXPECT_NEAR(refJ.velocity.value(), 0.0, kEpsilon);
-  EXPECT_NEAR(refJ.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedJ.velocity.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(optimizedJ.angle.Degrees().value(), 45.0, kEpsilon);

  wpi::math::Rotation2d angleK{-135_deg};
  wpi::math::SwerveModuleVelocity refK{-2_mps, 45_deg};
-  refK.CosineScale(angleK);
+  auto optimizedK = refK.CosineScale(angleK);

-  EXPECT_NEAR(refK.velocity.value(), 2.0, kEpsilon);
-  EXPECT_NEAR(refK.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedK.velocity.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedK.angle.Degrees().value(), 45.0, kEpsilon);

  wpi::math::Rotation2d angleL{180_deg};
  wpi::math::SwerveModuleVelocity refL{-2_mps, 45_deg};
-  refL.CosineScale(angleL);
+  auto optimizedL = refL.CosineScale(angleL);

-  EXPECT_NEAR(refL.velocity.value(), std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(refL.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedL.velocity.value(), std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(optimizedL.angle.Degrees().value(), 45.0, kEpsilon);
 }

 TEST(SwerveModuleVelocityTest, Equality) {
--- a/wpimath/src/test/python/kinematics/test_swerve_module_velocity.py
+++ b/wpimath/src/test/python/kinematics/test_swerve_module_velocity.py
@@ -9,119 +9,119 @@ kEpsilon = 1e-9
 def test_optimize():
    angle_a = Rotation2d.fromDegrees(45)
    ref_a = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(180))
-    ref_a.optimize(angle_a)
+    optimized_a = ref_a.optimize(angle_a)

-    assert ref_a.velocity == pytest.approx(2.0, abs=kEpsilon)
-    assert ref_a.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
+    assert optimized_a.velocity == pytest.approx(2.0, abs=kEpsilon)
+    assert optimized_a.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)

    angle_b = Rotation2d.fromDegrees(-50)
    ref_b = SwerveModuleVelocity(4.7, Rotation2d.fromDegrees(41))
-    ref_b.optimize(angle_b)
+    optimized_b = ref_b.optimize(angle_b)

-    assert ref_b.velocity == pytest.approx(-4.7, abs=kEpsilon)
-    assert ref_b.angle.degrees() == pytest.approx(-139.0, abs=kEpsilon)
+    assert optimized_b.velocity == pytest.approx(-4.7, abs=kEpsilon)
+    assert optimized_b.angle.degrees() == pytest.approx(-139.0, abs=kEpsilon)


 def test_no_optimize():
    angle_a = Rotation2d.fromDegrees(0)
    ref_a = SwerveModuleVelocity(2, Rotation2d.fromDegrees(89))
-    ref_a.optimize(angle_a)
+    optimized_a = ref_a.optimize(angle_a)

-    assert ref_a.velocity == pytest.approx(2.0, abs=kEpsilon)
-    assert ref_a.angle.degrees() == pytest.approx(89.0, abs=kEpsilon)
+    assert optimized_a.velocity == pytest.approx(2.0, abs=kEpsilon)
+    assert optimized_a.angle.degrees() == pytest.approx(89.0, abs=kEpsilon)

    angle_b = Rotation2d.fromDegrees(0)
    ref_b = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(-2))
-    ref_b.optimize(angle_b)
+    optimized_b = ref_b.optimize(angle_b)

-    assert ref_b.velocity == pytest.approx(-2.0, abs=kEpsilon)
-    assert ref_b.angle.degrees() == pytest.approx(-2.0, abs=kEpsilon)
+    assert optimized_b.velocity == pytest.approx(-2.0, abs=kEpsilon)
+    assert optimized_b.angle.degrees() == pytest.approx(-2.0, abs=kEpsilon)


 def test_cosine_scaling():
    angle_a = Rotation2d.fromDegrees(0)
    ref_a = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
-    ref_a.cosineScale(angle_a)
+    optimized_a = ref_a.cosineScale(angle_a)

-    assert ref_a.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
-    assert ref_a.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_a.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
+    assert optimized_a.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)

    angle_b = Rotation2d.fromDegrees(45)
    ref_b = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
-    ref_b.cosineScale(angle_b)
+    optimized_b = ref_b.cosineScale(angle_b)

-    assert ref_b.velocity == pytest.approx(2.0, abs=kEpsilon)
-    assert ref_b.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_b.velocity == pytest.approx(2.0, abs=kEpsilon)
+    assert optimized_b.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)

    angle_c = Rotation2d.fromDegrees(-45)
    ref_c = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
-    ref_c.cosineScale(angle_c)
+    optimized_c = ref_c.cosineScale(angle_c)

-    assert ref_c.velocity == pytest.approx(0.0, abs=kEpsilon)
-    assert ref_c.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_c.velocity == pytest.approx(0.0, abs=kEpsilon)
+    assert optimized_c.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)

    angle_d = Rotation2d.fromDegrees(135)
    ref_d = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
-    ref_d.cosineScale(angle_d)
+    optimized_d = ref_d.cosineScale(angle_d)

-    assert ref_d.velocity == pytest.approx(0.0, abs=kEpsilon)
-    assert ref_d.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_d.velocity == pytest.approx(0.0, abs=kEpsilon)
+    assert optimized_d.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)

    angle_e = Rotation2d.fromDegrees(-135)
    ref_e = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
-    ref_e.cosineScale(angle_e)
+    optimized_e = ref_e.cosineScale(angle_e)

-    assert ref_e.velocity == pytest.approx(-2.0, abs=kEpsilon)
-    assert ref_e.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_e.velocity == pytest.approx(-2.0, abs=kEpsilon)
+    assert optimized_e.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)

    angle_f = Rotation2d.fromDegrees(180)
    ref_f = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
-    ref_f.cosineScale(angle_f)
+    optimized_f = ref_f.cosineScale(angle_f)

-    assert ref_f.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
-    assert ref_f.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_f.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
+    assert optimized_f.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)

    angle_g = Rotation2d.fromDegrees(0)
    ref_g = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
-    ref_g.cosineScale(angle_g)
+    optimized_g = ref_g.cosineScale(angle_g)

-    assert ref_g.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
-    assert ref_g.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_g.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
+    assert optimized_g.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)

    angle_h = Rotation2d.fromDegrees(45)
    ref_h = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
-    ref_h.cosineScale(angle_h)
+    optimized_h = ref_h.cosineScale(angle_h)

-    assert ref_h.velocity == pytest.approx(-2.0, abs=kEpsilon)
-    assert ref_h.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_h.velocity == pytest.approx(-2.0, abs=kEpsilon)
+    assert optimized_h.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)

    angle_i = Rotation2d.fromDegrees(-45)
    ref_i = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
-    ref_i.cosineScale(angle_i)
+    optimized_i = ref_i.cosineScale(angle_i)

-    assert ref_i.velocity == pytest.approx(0.0, abs=kEpsilon)
-    assert ref_i.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_i.velocity == pytest.approx(0.0, abs=kEpsilon)
+    assert optimized_i.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)

    angle_j = Rotation2d.fromDegrees(135)
    ref_j = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
-    ref_j.cosineScale(angle_j)
+    optimized_j = ref_j.cosineScale(angle_j)

-    assert ref_j.velocity == pytest.approx(0.0, abs=kEpsilon)
-    assert ref_j.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_j.velocity == pytest.approx(0.0, abs=kEpsilon)
+    assert optimized_j.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)

    angle_k = Rotation2d.fromDegrees(-135)
    ref_k = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
-    ref_k.cosineScale(angle_k)
+    optimized_k = ref_k.cosineScale(angle_k)

-    assert ref_k.velocity == pytest.approx(2.0, abs=kEpsilon)
-    assert ref_k.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_k.velocity == pytest.approx(2.0, abs=kEpsilon)
+    assert optimized_k.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)

    angle_l = Rotation2d.fromDegrees(180)
    ref_l = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
-    ref_l.cosineScale(angle_l)
+    optimized_l = ref_l.cosineScale(angle_l)

-    assert ref_l.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
-    assert ref_l.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_l.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
+    assert optimized_l.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)


 def test_equality():