[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-28 02:11: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/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);
  }