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 2f58217493..0b851e3a91 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 @@ -55,7 +55,7 @@ public class SwerveDriveKinematics private final int m_numModules; private final Translation2d[] m_modules; - private SwerveModuleState[] m_moduleStates; + private Rotation2d[] m_moduleHeadings; private Translation2d m_prevCoR = new Translation2d(); /** @@ -74,8 +74,8 @@ public class SwerveDriveKinematics } m_numModules = moduleTranslationsMeters.length; m_modules = Arrays.copyOf(moduleTranslationsMeters, m_numModules); - m_moduleStates = new SwerveModuleState[m_numModules]; - Arrays.fill(m_moduleStates, new SwerveModuleState()); + m_moduleHeadings = new Rotation2d[m_numModules]; + Arrays.fill(m_moduleHeadings, new Rotation2d()); m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3); for (int i = 0; i < m_numModules; i++) { @@ -87,6 +87,21 @@ public class SwerveDriveKinematics MathSharedStore.reportUsage(MathUsageId.kKinematics_SwerveDrive, 1); } + /** + * Reset the internal swerve module headings. + * + * @param moduleHeadings The swerve module headings. The order of the module headings should be + * same as passed into the constructor of this class. + */ + public void resetHeadings(Rotation2d... moduleHeadings) { + if (moduleHeadings.length != m_numModules) { + throw new IllegalArgumentException( + "Number of headings is not consistent with number of module locations provided in " + + "constructor"); + } + m_moduleHeadings = Arrays.copyOf(moduleHeadings, 4); + } + /** * Performs inverse kinematics to return the module states from a desired chassis velocity. This * method is often used to convert joystick values into module speeds and angles. @@ -108,19 +123,18 @@ public class SwerveDriveKinematics * attainable max velocity. Use the {@link #desaturateWheelSpeeds(SwerveModuleState[], double) * DesaturateWheelSpeeds} function to rectify this issue. */ - @SuppressWarnings("PMD.MethodReturnsInternalArray") public SwerveModuleState[] toSwerveModuleStates( ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) { + var moduleStates = new SwerveModuleState[m_numModules]; + if (chassisSpeeds.vxMetersPerSecond == 0.0 && chassisSpeeds.vyMetersPerSecond == 0.0 && chassisSpeeds.omegaRadiansPerSecond == 0.0) { - SwerveModuleState[] newStates = new SwerveModuleState[m_numModules]; for (int i = 0; i < m_numModules; i++) { - newStates[i] = new SwerveModuleState(0.0, m_moduleStates[i].angle); + moduleStates[i] = new SwerveModuleState(0.0, m_moduleHeadings[i]); } - m_moduleStates = newStates; - return m_moduleStates; + return moduleStates; } if (!centerOfRotationMeters.equals(m_prevCoR)) { @@ -151,7 +165,6 @@ public class SwerveDriveKinematics var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector); - m_moduleStates = new SwerveModuleState[m_numModules]; for (int i = 0; i < m_numModules; i++) { double x = moduleStatesMatrix.get(i * 2, 0); double y = moduleStatesMatrix.get(i * 2 + 1, 0); @@ -159,10 +172,11 @@ public class SwerveDriveKinematics double speed = Math.hypot(x, y); Rotation2d angle = new Rotation2d(x, y); - m_moduleStates[i] = new SwerveModuleState(speed, angle); + moduleStates[i] = new SwerveModuleState(speed, angle); + m_moduleHeadings[i] = angle; } - return m_moduleStates; + return moduleStates; } /** diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 83edb4c6ee..63fe4c90b0 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -69,7 +69,7 @@ class SwerveDriveKinematics template ... ModuleTranslations> requires(sizeof...(ModuleTranslations) == NumModules) explicit SwerveDriveKinematics(ModuleTranslations&&... moduleTranslations) - : m_modules{moduleTranslations...}, m_moduleStates(wpi::empty_array) { + : m_modules{moduleTranslations...}, m_moduleHeadings(wpi::empty_array) { for (size_t i = 0; i < NumModules; i++) { // clang-format off m_inverseKinematics.template block<2, 3>(i * 2, 0) << @@ -86,7 +86,7 @@ class SwerveDriveKinematics explicit SwerveDriveKinematics( const wpi::array& modules) - : m_modules{modules}, m_moduleStates(wpi::empty_array) { + : m_modules{modules}, m_moduleHeadings(wpi::empty_array) { for (size_t i = 0; i < NumModules; i++) { // clang-format off m_inverseKinematics.template block<2, 3>(i * 2, 0) << @@ -103,6 +103,25 @@ class SwerveDriveKinematics SwerveDriveKinematics(const SwerveDriveKinematics&) = default; + /** + * Reset the internal swerve module headings. + * @param moduleHeadings The swerve module headings. The order of the module + * headings should be same as passed into the constructor of this class. + */ + template ... ModuleHeadings> + requires(sizeof...(ModuleHeadings) == NumModules) + void ResetHeadings(ModuleHeadings&&... moduleHeadings) { + return this->ResetHeadings( + wpi::array{moduleHeadings...}); + } + + /** + * Reset the internal swerve module headings. + * @param moduleHeadings The swerve module headings. The order of the module + * headings should be same as passed into the constructor of this class. + */ + void ResetHeadings(wpi::array moduleHeadings); + /** * Performs inverse kinematics to return the module states from a desired * chassis velocity. This method is often used to convert joystick values into @@ -270,7 +289,7 @@ class SwerveDriveKinematics mutable Matrixd m_inverseKinematics; Eigen::HouseholderQR> m_forwardKinematics; wpi::array m_modules; - mutable wpi::array m_moduleStates; + mutable wpi::array m_moduleHeadings; 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 e0ae222537..d29fad7c02 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc @@ -17,18 +17,28 @@ template SwerveDriveKinematics(ModuleTranslation, ModuleTranslations...) -> SwerveDriveKinematics<1 + sizeof...(ModuleTranslations)>; +template +void SwerveDriveKinematics::ResetHeadings( + wpi::array moduleHeadings) { + for (size_t i = 0; i < NumModules; i++) { + m_moduleHeadings[i] = moduleHeadings[i]; + } +} + template wpi::array SwerveDriveKinematics::ToSwerveModuleStates( const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation) const { + wpi::array moduleStates(wpi::empty_array); + 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; + moduleStates[i] = {0_mps, m_moduleHeadings[i]}; } - return m_moduleStates; + return moduleStates; } // We have a new center of rotation. We need to compute the matrix again. @@ -58,10 +68,11 @@ SwerveDriveKinematics::ToSwerveModuleStates( auto speed = units::math::hypot(x, y); Rotation2d rotation{x.value(), y.value()}; - m_moduleStates[i] = {speed, rotation}; + moduleStates[i] = {speed, rotation}; + m_moduleHeadings[i] = rotation; } - return m_moduleStates; + return 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 2f8b216a0a..f429a68d8c 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 @@ -119,6 +119,28 @@ class SwerveDriveKinematicsTest { () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)); } + @Test + void testResetWheelAngle() { + Rotation2d fl = new Rotation2d(0); + Rotation2d fr = new Rotation2d(Math.PI / 2); + Rotation2d bl = new Rotation2d(Math.PI); + Rotation2d br = new Rotation2d(3 * Math.PI / 2); + m_kinematics.resetHeadings(fl, fr, bl, br); + 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(0.0, moduleStates[0].angle.getDegrees(), kEpsilon), + () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon), + () -> assertEquals(180.0, moduleStates[2].angle.getDegrees(), kEpsilon), + () -> assertEquals(270.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 4e549b7c85..a8e41d6102 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp @@ -125,6 +125,25 @@ TEST_F(SwerveDriveKinematicsTest, ConserveWheelAngle) { EXPECT_NEAR(bl.angle.Degrees().value(), -135.0, kEpsilon); EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon); } +TEST_F(SwerveDriveKinematicsTest, ResetWheelAngle) { + Rotation2d fl = {0_deg}; + Rotation2d fr = {90_deg}; + Rotation2d bl = {180_deg}; + Rotation2d br = {270_deg}; + m_kinematics.ResetHeadings(fl, fr, bl, br); + auto [flMod, frMod, blMod, brMod] = + m_kinematics.ToSwerveModuleStates(ChassisSpeeds{}); + + EXPECT_NEAR(flMod.speed.value(), 0.0, kEpsilon); + EXPECT_NEAR(frMod.speed.value(), 0.0, kEpsilon); + EXPECT_NEAR(blMod.speed.value(), 0.0, kEpsilon); + EXPECT_NEAR(brMod.speed.value(), 0.0, kEpsilon); + + EXPECT_NEAR(flMod.angle.Degrees().value(), 0.0, kEpsilon); + EXPECT_NEAR(frMod.angle.Degrees().value(), 90.0, kEpsilon); + EXPECT_NEAR(blMod.angle.Degrees().value(), 180.0, kEpsilon); + EXPECT_NEAR(brMod.angle.Degrees().value(), 270.0, kEpsilon); +} TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) { SwerveModuleState fl{106.629_mps, 135_deg};