Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Fix C++ Odometry ResetRotation missing a negation (#8949)

Browse Source 
The original implementation was `m_gyroOffset + (rotation -
m_pose.Rotation())`, which means the first `RotateBy` should be using
`-m_pose.Rotation()` (see `ResetPose()`). Java is already correct. This
also adds new tests to catch this particular error.
This commit is contained in:
Benjamin Hall
2026-06-04 20:03:22 -04:00
committed by GitHub
parent fdc6fd9cb1
commit a41679854b
3 changed files with 85 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp
View File

@@ -95,7 +95,7 @@ class WPILIB_DLLEXPORT Odometry {
* @param rotation The rotation to reset to.
*/
void ResetRotation(const Rotation2d& rotation) {
m_gyroOffset = m_gyroOffset.RotateBy(m_pose.Rotation()).RotateBy(rotation);
m_gyroOffset = m_gyroOffset.RotateBy(-m_pose.Rotation()).RotateBy(rotation);
m_pose = Pose2d{m_pose.Translation(), rotation};
m_previousAngle = rotation;
}

46
wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveOdometryTest.java
View File

@@ -101,6 +101,52 @@ class SwerveDriveOdometryTest {
() -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1));
}
@Test
void testResetTranslation() {
var modulePositions = new SwerveModulePosition[] {zero, zero, zero, zero};
var gyro = Rotation2d.fromDegrees(30.0);
m_odometry.resetPosition(
Rotation2d.kZero, modulePositions, new Pose2d(Translation2d.kZero, Rotation2d.kZero));
var initialPose = m_odometry.update(gyro, modulePositions);
assertAll(
() -> assertEquals(0.00, initialPose.getX(), 0.1),
() -> assertEquals(0.00, initialPose.getY(), 0.1),
() -> assertEquals(30.0, initialPose.getRotation().getDegrees(), 0.1));
m_odometry.resetTranslation(new Translation2d(0.5, 0));
var newPose = m_odometry.update(gyro, modulePositions);
assertAll(
() -> assertEquals(0.50, newPose.getX(), 0.1),
() -> assertEquals(0.00, newPose.getY(), 0.1),
() -> assertEquals(30.0, newPose.getRotation().getDegrees(), 0.1));
}
@Test
void testResetRotation() {
var modulePositions = new SwerveModulePosition[] {zero, zero, zero, zero};
var gyro = Rotation2d.fromDegrees(30.0);
m_odometry.resetPosition(
Rotation2d.kZero, modulePositions, new Pose2d(0.5, 0.0, Rotation2d.kZero));
var initialPose = m_odometry.update(gyro, modulePositions);
assertAll(
() -> assertEquals(0.50, initialPose.getX(), 0.1),
() -> assertEquals(0.00, initialPose.getY(), 0.1),
() -> assertEquals(30.0, initialPose.getRotation().getDegrees(), 0.1));
m_odometry.resetRotation(Rotation2d.kCCW_90deg);
var newPose = m_odometry.update(gyro, modulePositions);
assertAll(
() -> assertEquals(0.50, newPose.getX(), 0.1),
() -> assertEquals(0.00, newPose.getY(), 0.1),
() -> assertEquals(90.0, newPose.getRotation().getDegrees(), 0.1));
}
@Test
void testAccuracyFacingTrajectory() {
var kinematics =

38
wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
View File

@@ -73,6 +73,44 @@ TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon);
}
TEST_F(SwerveDriveOdometryTest, ResetTranslation) {
wpi::util::array<SwerveModulePosition, 4> wheelPositions{zero, zero, zero,
zero};
m_odometry.ResetPosition(0_deg, wheelPositions, Pose2d{});
auto pose = m_odometry.Update(30_deg, wheelPositions);
EXPECT_NEAR(0.0, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
EXPECT_NEAR(30.0, pose.Rotation().Degrees().value(), kEpsilon);
m_odometry.ResetTranslation({0.5_m, 0_m});
pose = m_odometry.Update(30_deg, wheelPositions);
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
EXPECT_NEAR(30.0, pose.Rotation().Degrees().value(), kEpsilon);
}
TEST_F(SwerveDriveOdometryTest, ResetRotation) {
wpi::util::array<SwerveModulePosition, 4> wheelPositions{zero, zero, zero,
zero};
m_odometry.ResetPosition(0_deg, wheelPositions, Pose2d{0.5_m, 0_m, 0_rad});
auto pose = m_odometry.Update(30_deg, wheelPositions);
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
EXPECT_NEAR(30.0, pose.Rotation().Degrees().value(), kEpsilon);
m_odometry.ResetRotation(90_deg);
pose = m_odometry.Update(30_deg, wheelPositions);
EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
EXPECT_NEAR(90.0, pose.Rotation().Degrees().value(), kEpsilon);
}
TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) {
SwerveDriveKinematics<4> kinematics{
Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m},