Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11: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

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},