Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-26 01:51:41 +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

37
wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java
View File

@@ -8,6 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
@@ -509,11 +510,23 @@ class SwerveDrivePoseEstimator3dTest {
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
// Add a vision measurement with a different translation
estimator.addVisionMeasurement(
new Pose3d(3, 0, 0, Rotation3d.kZero), MathSharedStore.getTimestamp());
assertAll(
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
// Test reset rotation
estimator.resetRotation(new Rotation3d(Rotation2d.kCCW_Pi_2));
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
@@ -533,7 +546,7 @@ class SwerveDrivePoseEstimator3dTest {
}
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
@@ -542,6 +555,22 @@ class SwerveDrivePoseEstimator3dTest {
assertEquals(
Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
// Add a vision measurement with a different rotation
estimator.addVisionMeasurement(
new Pose3d(2.5, 1, 0, new Rotation3d(Rotation2d.kPi)), MathSharedStore.getTimestamp());
assertAll(
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
() ->
assertEquals(
Math.PI * 3.0 / 4,
estimator.getEstimatedPosition().getRotation().getZ(),
kEpsilon));
// Test reset translation
estimator.resetTranslation(new Translation3d(-1, -1, -1));
@@ -553,7 +582,9 @@ class SwerveDrivePoseEstimator3dTest {
() -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon),
() ->
assertEquals(
Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon));
Math.PI * 3.0 / 4,
estimator.getEstimatedPosition().getRotation().getZ(),
kEpsilon));
// Test reset pose
estimator.resetPose(Pose3d.kZero);

30
wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
View File

@@ -8,6 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.MathSharedStore;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
@@ -481,11 +482,21 @@ class SwerveDrivePoseEstimatorTest {
() ->
assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
// Add a vision measurement with a different translation
estimator.addVisionMeasurement(
new Pose2d(3, 0, Rotation2d.kZero), MathSharedStore.getTimestamp());
assertAll(
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon));
// Test reset rotation
estimator.resetRotation(Rotation2d.kCCW_Pi_2);
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
@@ -504,7 +515,7 @@ class SwerveDrivePoseEstimatorTest {
}
assertAll(
() -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
@@ -512,6 +523,19 @@ class SwerveDrivePoseEstimatorTest {
estimator.getEstimatedPosition().getRotation().getRadians(),
kEpsilon));
// Add a vision measurement with a different rotation
estimator.addVisionMeasurement(
new Pose2d(2.5, 1, Rotation2d.kPi), MathSharedStore.getTimestamp());
assertAll(
() -> assertEquals(2.5, estimator.getEstimatedPosition().getX(), kEpsilon),
() -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
Math.PI * 3.0 / 4,
estimator.getEstimatedPosition().getRotation().getRadians(),
kEpsilon));
// Test reset translation
estimator.resetTranslation(new Translation2d(-1, -1));
@@ -520,7 +544,7 @@ class SwerveDrivePoseEstimatorTest {
() -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon),
() ->
assertEquals(
Math.PI / 2,
Math.PI * 3.0 / 4,
estimator.getEstimatedPosition().getRotation().getRadians(),
kEpsilon));

30
wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp
View File

@@ -453,10 +453,21 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
// Add a vision measurement with a different translation
estimator.AddVisionMeasurement(frc::Pose3d(3_m, 0_m, 0_m, frc::Rotation3d{}),
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().Z().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset rotation
estimator.ResetRotation(frc::Rotation3d{0_deg, 0_deg, 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(0, estimator.GetEstimatedPosition().Z().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
@@ -471,7 +482,7 @@ TEST(SwerveDrivePoseEstimator3dTest, 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(0, estimator.GetEstimatedPosition().Z().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
@@ -479,6 +490,19 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(std::numbers::pi / 2,
estimator.GetEstimatedPosition().Rotation().Z().value());
// Add a vision measurement with a different rotation
estimator.AddVisionMeasurement(
frc::Pose3d(2.5_m, 1_m, 0_m, frc::Rotation3d{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(0, estimator.GetEstimatedPosition().Z().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
EXPECT_DOUBLE_EQ(std::numbers::pi * 3.0 / 4,
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset translation
estimator.ResetTranslation(frc::Translation3d{-1_m, -1_m, -1_m});
@@ -487,7 +511,7 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) {
EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Z().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value());
EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value());
EXPECT_DOUBLE_EQ(std::numbers::pi / 2,
EXPECT_DOUBLE_EQ(std::numbers::pi * 3.0 / 4,
estimator.GetEstimatedPosition().Rotation().Z().value());
// Test reset pose

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