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

34
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java
View File

@@ -49,7 +49,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;
@@ -147,9 +149,22 @@ public class PoseEstimator<T> {
*/
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();
}
}
/**
@@ -159,9 +174,22 @@ public class PoseEstimator<T> {
*/
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();
}
}
/**

34
wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java
View File

@@ -57,7 +57,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;
@@ -159,9 +161,22 @@ public class PoseEstimator3d<T> {
*/
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();
}
}
/**
@@ -171,9 +186,22 @@ public class PoseEstimator3d<T> {
*/
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();
}
}
/**

54
wpimath/src/main/native/include/frc/estimator/PoseEstimator.h
View File

@@ -139,24 +139,70 @@ 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);
const 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
const VisionUpdate 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);
const 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
const VisionUpdate 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.
@@ -436,7 +482,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;

54
wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h
View File

@@ -148,24 +148,70 @@ 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);
const 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
const VisionUpdate 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);
const 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
const VisionUpdate 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.
@@ -451,7 +497,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;