diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index dc1b9e49f4..754015b7f1 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/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. * + *

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; } /** diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 33e126f687..4977b8285b 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -59,7 +59,7 @@ class SwerveDriveKinematics { */ template 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& 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 m_inverseKinematics; Eigen::HouseholderQR> m_forwardKinematics; wpi::array m_modules; + mutable wpi::array m_moduleStates; mutable Translation2d m_previousCoR; }; diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc index b549a87aa7..8e4b381fec 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc @@ -5,6 +5,7 @@ #pragma once #include +#include #include "frc/kinematics/SwerveDriveKinematics.h" #include "units/math.h" @@ -20,6 +21,15 @@ wpi::array SwerveDriveKinematics::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::ToSwerveModuleStates( Matrixd moduleStateMatrix = m_inverseKinematics * chassisSpeedsVector; - wpi::array 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::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 diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index 574e086272..db4b76b474 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/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); diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp index 55ebdcfd1f..a79b59bb64 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/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)};