From d97a749d84ee3c7ffd6dc05c3accd0db86d5575e 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 a5205dcfc7..45c3f3bb23 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 class SwerveDriveKinematics 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 b9bf34016e..8c5e18b359 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;