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 ResetTranslation and ResetRotation in PoseEstimator and PoseEstimator3d causing the robot to teleport (#8285)

Browse Source 
Fixes https://github.com/wpilibsuite/allwpilib/issues/8284.

If we have vision updates at the time of the `Reset*` call, we can
correct the translation/rotation of the new odometry pose by adding a
new vision update where:
- `ResetTranslation`: the translation is hard-coded to the new
translation, and the rotation components are set to those of the latest
vision update (prior to clearing the map).
- `ResetRotation`: the rotation is hard-coded to the new rotation, and
the translation components are set to those of the latest vision update
(prior to clearing the map).
This commit is contained in:
Benjamin Hall
2025-12-13 18:53:53 -05:00
committed by GitHub
parent baa6379267
commit 3dc334c1ee
8 changed files with 275 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

26
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
View File

@@ -430,10 +430,19 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
EXPECT_DOUBLE_EQ(
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Add a vision measurement with a different translation
estimator.AddVisionMeasurement(frc::Pose2d{3_m, 0_m, frc::Rotation2d{}},
wpi::math::MathSharedStore::GetTimestamp());
EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
0, estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset rotation
estimator.ResetRotation(frc::Rotation2d{90_deg});
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
@@ -446,19 +455,30 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) {
modulePosition, modulePosition});
}
EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Add a vision measurement with a different rotation
estimator.AddVisionMeasurement(
frc::Pose2d{2.5_m, 1_m, frc::Rotation2d{180_deg}},
wpi::math::MathSharedStore::GetTimestamp());
EXPECT_DOUBLE_EQ(2.5, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi * 3.0 / 4,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m});
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value());
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value());
EXPECT_DOUBLE_EQ(
std::numbers::pi / 2,
std::numbers::pi * 3.0 / 4,
estimator.GetEstimatedPosition().Rotation().Radians().value());
// Test reset pose