[wpimath] Conserve previously calculated swerve module angles when updating states for stationary ChassisSpeeds (#4208)

* Calculated swerve module states now stored in a member variable
* If ChassisSpeeds(0, 0, 0) is converted to module speeds, the
previously calculated module angle will be conserved, with forward speed
set to 0
* New tests added

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

@@ -38,6 +38,7 @@ public class SwerveDriveKinematics {
 
   private final int m_numModules;
   private final Translation2d[] m_modules;
+  private final SwerveModuleState[] m_moduleStates;
   private Translation2d m_prevCoR = new Translation2d();
 
   /**
@@ -54,6 +55,8 @@ public class SwerveDriveKinematics {
     }
     m_numModules = wheelsMeters.length;
     m_modules = Arrays.copyOf(wheelsMeters, m_numModules);
+    m_moduleStates = new SwerveModuleState[m_numModules];
+    Arrays.fill(m_moduleStates, new SwerveModuleState());
     m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
 
     for (int i = 0; i < m_numModules; i++) {
@@ -74,6 +77,9 @@ public class SwerveDriveKinematics {
    * argument is defaulted to that use case. However, if you wish to change the center of rotation
    * for evasive maneuvers, vision alignment, or for any other use case, you can do so.
    *
+   * <p>In the case that the desired chassis speeds are zero (i.e. the robot will be stationary),
+   * the previously calculated module angle will be maintained.
+   *
    * @param chassisSpeeds The desired chassis speed.
    * @param centerOfRotationMeters The center of rotation. For example, if you set the center of
    *     rotation at one corner of the robot and provide a chassis speed that only has a dtheta
@@ -83,9 +89,19 @@ public class SwerveDriveKinematics {
    *     attainable max velocity. Use the {@link #desaturateWheelSpeeds(SwerveModuleState[], double)
    *     DesaturateWheelSpeeds} function to rectify this issue.
    */
-  @SuppressWarnings("LocalVariableName")
+  @SuppressWarnings({"LocalVariableName", "PMD.MethodReturnsInternalArray"})
   public SwerveModuleState[] toSwerveModuleStates(
       ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
+    if (chassisSpeeds.vxMetersPerSecond == 0.0
+        && chassisSpeeds.vyMetersPerSecond == 0.0
+        && chassisSpeeds.omegaRadiansPerSecond == 0.0) {
+      for (int i = 0; i < m_numModules; i++) {
+        m_moduleStates[i].speedMetersPerSecond = 0.0;
+      }
+
+      return m_moduleStates;
+    }
+
     if (!centerOfRotationMeters.equals(m_prevCoR)) {
       for (int i = 0; i < m_numModules; i++) {
         m_inverseKinematics.setRow(
@@ -113,7 +129,6 @@ public class SwerveDriveKinematics {
         chassisSpeeds.omegaRadiansPerSecond);
 
     var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
-    SwerveModuleState[] moduleStates = new SwerveModuleState[m_numModules];
 
     for (int i = 0; i < m_numModules; i++) {
       double x = moduleStatesMatrix.get(i * 2, 0);
@@ -122,10 +137,10 @@ public class SwerveDriveKinematics {
       double speed = Math.hypot(x, y);
       Rotation2d angle = new Rotation2d(x, y);
 
-      moduleStates[i] = new SwerveModuleState(speed, angle);
+      m_moduleStates[i] = new SwerveModuleState(speed, angle);
     }
 
-    return moduleStates;
+    return m_moduleStates;
   }
 
   /**

wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h

@@ -59,7 +59,7 @@ class SwerveDriveKinematics {
    */
   template <typename... Wheels>
   explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
-      : m_modules{wheel, wheels...} {
+      : m_modules{wheel, wheels...}, m_moduleStates(wpi::empty_array) {
     static_assert(sizeof...(wheels) >= 1,
                   "A swerve drive requires at least two modules");
 
@@ -79,7 +79,7 @@ class SwerveDriveKinematics {
 
   explicit SwerveDriveKinematics(
       const wpi::array<Translation2d, NumModules>& wheels)
-      : m_modules{wheels} {
+      : m_modules{wheels}, m_moduleStates(wpi::empty_array) {
     for (size_t i = 0; i < NumModules; i++) {
       // clang-format off
       m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
@@ -107,6 +107,9 @@ class SwerveDriveKinematics {
    * However, if you wish to change the center of rotation for evasive
    * maneuvers, vision alignment, or for any other use case, you can do so.
    *
+   * In the case that the desired chassis speeds are zero (i.e. the robot will
+   * be stationary), the previously calculated module angle will be maintained.
+   *
    * @param chassisSpeeds The desired chassis speed.
    * @param centerOfRotation The center of rotation. For example, if you set the
    * center of rotation at one corner of the robot and provide a chassis speed
@@ -182,6 +185,7 @@ class SwerveDriveKinematics {
   mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
   Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;
   wpi::array<Translation2d, NumModules> m_modules;
+  mutable wpi::array<SwerveModuleState, NumModules> m_moduleStates;
 
   mutable Translation2d m_previousCoR;
 };

wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc

@@ -5,6 +5,7 @@
 #pragma once
 
 #include <algorithm>
+#include <utility>
 
 #include "frc/kinematics/SwerveDriveKinematics.h"
 #include "units/math.h"
@@ -20,6 +21,15 @@ wpi::array<SwerveModuleState, NumModules>
 SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
     const ChassisSpeeds& chassisSpeeds,
     const Translation2d& centerOfRotation) const {
+  if (chassisSpeeds.vx == 0_mps && chassisSpeeds.vy == 0_mps &&
+      chassisSpeeds.omega == 0_rad_per_s) {
+    for (size_t i = 0; i < NumModules; i++) {
+      m_moduleStates[i].speed = 0_mps;
+    }
+
+    return m_moduleStates;
+  }
+
   // We have a new center of rotation. We need to compute the matrix again.
   if (centerOfRotation != m_previousCoR) {
     for (size_t i = 0; i < NumModules; i++) {
@@ -39,7 +49,6 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
 
   Matrixd<NumModules * 2, 1> moduleStateMatrix =
       m_inverseKinematics * chassisSpeedsVector;
-  wpi::array<SwerveModuleState, NumModules> moduleStates{wpi::empty_array};
 
   for (size_t i = 0; i < NumModules; i++) {
     units::meters_per_second_t x{moduleStateMatrix(i * 2, 0)};
@@ -48,10 +57,10 @@ SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
     auto speed = units::math::hypot(x, y);
     Rotation2d rotation{x.value(), y.value()};
 
-    moduleStates[i] = {speed, rotation};
+    m_moduleStates[i] = {speed, rotation};
   }
 
-  return moduleStates;
+  return m_moduleStates;
 }
 
 template <size_t NumModules>

wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java

@@ -77,6 +77,25 @@ class SwerveDriveKinematicsTest {
         () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
   }
 
+  @Test
+  void testConserveWheelAngle() {
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
+    m_kinematics.toSwerveModuleStates(speeds);
+    var moduleStates = m_kinematics.toSwerveModuleStates(new ChassisSpeeds());
+
+    // Robot is stationary, but module angles are preserved.
+
+    assertAll(
+        () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon));
+  }
+
   @Test
   void testTurnInPlaceInverseKinematics() {
     ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);

wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp

@@ -89,6 +89,23 @@ TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
   EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
 }
 
+TEST_F(SwerveDriveKinematicsTest, ConserveWheelAngle) {
+  ChassisSpeeds speeds{0_mps, 0_mps,
+                       units::radians_per_second_t(2 * wpi::numbers::pi)};
+  m_kinematics.ToSwerveModuleStates(speeds);
+  auto [fl, fr, bl, br] = m_kinematics.ToSwerveModuleStates(ChassisSpeeds{});
+
+  EXPECT_NEAR(fl.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(fr.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(bl.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(br.speed.value(), 0.0, kEpsilon);
+
+  EXPECT_NEAR(fl.angle.Degrees().value(), 135.0, kEpsilon);
+  EXPECT_NEAR(fr.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(bl.angle.Degrees().value(), -135.0, kEpsilon);
+  EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
+}
+
 TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
   SwerveModuleState fl{106.629_mps, Rotation2d(135_deg)};
   SwerveModuleState fr{106.629_mps, Rotation2d(45_deg)};