From 64cda4d09998b1e4b042e102760891d2a3a18910 Mon Sep 17 00:00:00 2001 From: Bryce Roethel Date: Fri, 11 Oct 2024 01:05:49 -0400 Subject: [PATCH] [wpimath] SwerveDrivePoseEstimator: Fix stationary module emitting error when calculating angle in ToSwerveModuleStates (#7175) --- .../edu/wpi/first/math/kinematics/SwerveDriveKinematics.java | 2 +- .../native/include/frc/kinematics/SwerveDriveKinematics.inc | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) 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 a5205dcfc74..45c3f3bb235 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 @@ -167,7 +167,7 @@ public SwerveModuleState[] toSwerveModuleStates( double y = moduleStatesMatrix.get(i * 2 + 1, 0); double speed = Math.hypot(x, y); - Rotation2d angle = new Rotation2d(x, y); + Rotation2d angle = speed > 1e-6 ? new Rotation2d(x, y) : m_moduleHeadings[i]; moduleStates[i] = new SwerveModuleState(speed, angle); m_moduleHeadings[i] = angle; diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc index b9bf34016e3..8c5e18b3592 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc @@ -66,7 +66,8 @@ SwerveDriveKinematics::ToSwerveModuleStates( units::meters_per_second_t y{moduleStateMatrix(i * 2 + 1, 0)}; auto speed = units::math::hypot(x, y); - Rotation2d rotation{x.value(), y.value()}; + auto rotation = speed > 1e-6_mps ? Rotation2d{x.value(), y.value()} + : m_moduleHeadings[i]; moduleStates[i] = {speed, rotation}; m_moduleHeadings[i] = rotation;