Skip to content

[wpimath] Fix ResetTranslation and ResetRotation in PoseEstimator and PoseEstimator3d causing the robot to teleport #8285

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 8 commits into
base: main
Choose a base branch
from
+275 −24
Open

[wpimath] Fix ResetTranslation and ResetRotation in PoseEstimator and PoseEstimator3d causing the robot to teleport #8285

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
2e45d07
Fix `ResetTranslation` and `ResetRotation` in `PoseEstimator` and `Po…
bhall-ctre Oct 10, 2025
236400e
Fix typo
bhall-ctre Oct 10, 2025
4e3ab99
Attempt to fix maybe-uninitialized warning on Linux
bhall-ctre Oct 10, 2025
9ecbc18
Silence false positive maybe-unused warning
bhall-ctre Oct 10, 2025
e680f81
Update comment on m_visionUpdates
bhall-ctre Nov 2, 2025
8f57b2a
Fix usage of const to be on the left
bhall-ctre Nov 3, 2025
d33cb7a
Update SwerveDrivePoseEstimator unit tests to test with vision
bhall-ctre Nov 23, 2025
f33906b
Fix formatting
bhall-ctre Nov 23, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 31 additions & 3 deletions wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,9 @@ public class PoseEstimator<T> {
TimeInterpolatableBuffer.createBuffer(kBufferDuration);
// Maps timestamps to vision updates
// Always contains one entry before the oldest entry in m_odometryPoseBuffer, unless there have
// been no vision measurements after the last reset
// been no vision measurements after the last reset. May contain one entry while
// m_odometryPoseBuffer is empty to correct for translation/rotation after a call to
// ResetRotation/ResetTranslation.
private final NavigableMap<Double, VisionUpdate> m_visionUpdates = new TreeMap<>();

private Pose2d m_poseEstimate;
Expand Down Expand Up @@ -145,9 +147,22 @@ public void resetPose(Pose2d pose) {
*/
public void resetTranslation(Translation2d translation) {
m_odometry.resetTranslation(translation);

final var latestVisionUpdate = m_visionUpdates.lastEntry();
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();

if (latestVisionUpdate != null) {
// apply vision compensation to the pose rotation
final var visionUpdate =
new VisionUpdate(
new Pose2d(translation, latestVisionUpdate.getValue().visionPose.getRotation()),
new Pose2d(translation, latestVisionUpdate.getValue().odometryPose.getRotation()));
m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
} else {
m_poseEstimate = m_odometry.getPoseMeters();
}
}

/**
Expand All @@ -157,9 +172,22 @@ public void resetTranslation(Translation2d translation) {
*/
public void resetRotation(Rotation2d rotation) {
m_odometry.resetRotation(rotation);

final var latestVisionUpdate = m_visionUpdates.lastEntry();
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();

if (latestVisionUpdate != null) {
// apply vision compensation to the pose translation
final var visionUpdate =
new VisionUpdate(
new Pose2d(latestVisionUpdate.getValue().visionPose.getTranslation(), rotation),
new Pose2d(latestVisionUpdate.getValue().odometryPose.getTranslation(), rotation));
m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
} else {
m_poseEstimate = m_odometry.getPoseMeters();
}
}

/**
Expand Down
34 changes: 31 additions & 3 deletions wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,9 @@ public class PoseEstimator3d<T> {
TimeInterpolatableBuffer.createBuffer(kBufferDuration);
// Maps timestamps to vision updates
// Always contains one entry before the oldest entry in m_odometryPoseBuffer, unless there have
// been no vision measurements after the last reset
// been no vision measurements after the last reset. May contain one entry while
// m_odometryPoseBuffer is empty to correct for translation/rotation after a call to
// ResetRotation/ResetTranslation.
private final NavigableMap<Double, VisionUpdate> m_visionUpdates = new TreeMap<>();

private Pose3d m_poseEstimate;
Expand Down Expand Up @@ -157,9 +159,22 @@ public void resetPose(Pose3d pose) {
*/
public void resetTranslation(Translation3d translation) {
m_odometry.resetTranslation(translation);

final var latestVisionUpdate = m_visionUpdates.lastEntry();
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();

if (latestVisionUpdate != null) {
// apply vision compensation to the pose rotation
final var visionUpdate =
new VisionUpdate(
new Pose3d(translation, latestVisionUpdate.getValue().visionPose.getRotation()),
new Pose3d(translation, latestVisionUpdate.getValue().odometryPose.getRotation()));
m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
} else {
m_poseEstimate = m_odometry.getPoseMeters();
}
}

/**
Expand All @@ -169,9 +184,22 @@ public void resetTranslation(Translation3d translation) {
*/
public void resetRotation(Rotation3d rotation) {
m_odometry.resetRotation(rotation);

final var latestVisionUpdate = m_visionUpdates.lastEntry();
m_odometryPoseBuffer.clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.getPoseMeters();

if (latestVisionUpdate != null) {
// apply vision compensation to the pose translation
final var visionUpdate =
new VisionUpdate(
new Pose3d(latestVisionUpdate.getValue().visionPose.getTranslation(), rotation),
new Pose3d(latestVisionUpdate.getValue().odometryPose.getTranslation(), rotation));
m_visionUpdates.put(latestVisionUpdate.getKey(), visionUpdate);
m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters());
} else {
m_poseEstimate = m_odometry.getPoseMeters();
}
}

/**
Expand Down
52 changes: 49 additions & 3 deletions wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -136,24 +136,68 @@ class WPILIB_DLLEXPORT PoseEstimator {
*
* @param translation The pose to translation to.
*/
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif // defined(__GNUC__) && !defined(__clang__)
void ResetTranslation(const Translation2d& translation) {
m_odometry.ResetTranslation(translation);

std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
m_visionUpdates.empty() ? std::nullopt
: std::optional{*m_visionUpdates.crbegin()};
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();

if (latestVisionUpdate) {
// apply vision compensation to the pose rotation
VisionUpdate const visionUpdate{
Pose2d{translation, latestVisionUpdate->second.visionPose.Rotation()},
Pose2d{translation,
latestVisionUpdate->second.odometryPose.Rotation()}};
m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
} else {
m_poseEstimate = m_odometry.GetPose();
}
}
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic pop
#endif // defined(__GNUC__) && !defined(__clang__)

/**
* Resets the robot's rotation.
*
* @param rotation The rotation to reset to.
*/
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif // defined(__GNUC__) && !defined(__clang__)
void ResetRotation(const Rotation2d& rotation) {
m_odometry.ResetRotation(rotation);

std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
m_visionUpdates.empty() ? std::nullopt
: std::optional{*m_visionUpdates.crbegin()};
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();

if (latestVisionUpdate) {
// apply vision compensation to the pose translation
VisionUpdate const visionUpdate{
Pose2d{latestVisionUpdate->second.visionPose.Translation(), rotation},
Pose2d{latestVisionUpdate->second.odometryPose.Translation(),
rotation}};
m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
} else {
m_poseEstimate = m_odometry.GetPose();
}
}
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic pop
#endif // defined(__GNUC__) && !defined(__clang__)

/**
* Gets the estimated robot pose.
Expand Down Expand Up @@ -431,7 +475,9 @@ class WPILIB_DLLEXPORT PoseEstimator {
TimeInterpolatableBuffer<Pose2d> m_odometryPoseBuffer{kBufferDuration};
// Maps timestamps to vision updates
// Always contains one entry before the oldest entry in m_odometryPoseBuffer,
// unless there have been no vision measurements after the last reset
// unless there have been no vision measurements after the last reset. May
// contain one entry while m_odometryPoseBuffer is empty to correct for
// translation/rotation after a call to ResetRotation/ResetTranslation.
std::map<units::second_t, VisionUpdate> m_visionUpdates;

Pose2d m_poseEstimate;
Expand Down
52 changes: 49 additions & 3 deletions wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -145,24 +145,68 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
*
* @param translation The pose to translation to.
*/
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif // defined(__GNUC__) && !defined(__clang__)
void ResetTranslation(const Translation3d& translation) {
m_odometry.ResetTranslation(translation);

std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
m_visionUpdates.empty() ? std::nullopt
: std::optional{*m_visionUpdates.crbegin()};
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();

if (latestVisionUpdate) {
// apply vision compensation to the pose rotation
VisionUpdate const visionUpdate{
Pose3d{translation, latestVisionUpdate->second.visionPose.Rotation()},
Pose3d{translation,
latestVisionUpdate->second.odometryPose.Rotation()}};
m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
} else {
m_poseEstimate = m_odometry.GetPose();
}
}
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic pop
#endif // defined(__GNUC__) && !defined(__clang__)

/**
* Resets the robot's rotation.
*
* @param rotation The rotation to reset to.
*/
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif // defined(__GNUC__) && !defined(__clang__)
void ResetRotation(const Rotation3d& rotation) {
m_odometry.ResetRotation(rotation);

std::optional<std::pair<units::second_t, VisionUpdate>> latestVisionUpdate =
m_visionUpdates.empty() ? std::nullopt
: std::optional{*m_visionUpdates.crbegin()};
m_odometryPoseBuffer.Clear();
m_visionUpdates.clear();
m_poseEstimate = m_odometry.GetPose();

if (latestVisionUpdate) {
// apply vision compensation to the pose translation
VisionUpdate const visionUpdate{
Pose3d{latestVisionUpdate->second.visionPose.Translation(), rotation},
Pose3d{latestVisionUpdate->second.odometryPose.Translation(),
rotation}};
m_visionUpdates[latestVisionUpdate->first] = visionUpdate;
m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose());
} else {
m_poseEstimate = m_odometry.GetPose();
}
}
#if defined(__GNUC__) && !defined(__clang__)
#pragma GCC diagnostic pop
#endif // defined(__GNUC__) && !defined(__clang__)

/**
* Gets the estimated robot pose.
Expand Down Expand Up @@ -442,7 +486,9 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
TimeInterpolatableBuffer<Pose3d> m_odometryPoseBuffer{kBufferDuration};
// Maps timestamps to vision updates
// Always contains one entry before the oldest entry in m_odometryPoseBuffer,
// unless there have been no vision measurements after the last reset
// unless there have been no vision measurements after the last reset. May
// contain one entry while m_odometryPoseBuffer is empty to correct for
// translation/rotation after a call to ResetRotation/ResetTranslation.
std::map<units::second_t, VisionUpdate> m_visionUpdates;

Pose3d m_poseEstimate;
Expand Down
Loading