[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/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>