From d1c7032decdbaa03dd5c6002dcacaf0213dcafa5 Mon Sep 17 00:00:00 2001 From: Claudius Tewari Date: Tue, 16 Feb 2021 18:04:38 -0800 Subject: [PATCH] [wpimath] Fix order of setting gyro offset in pose estimators (#3176) The gyro offset should be determined from the desired initial pose, not the current pose. This fix reflects the behavior of the odometry classes and the C++ holonomic pose estimators. --- .../first/wpilibj/estimator/DifferentialDrivePoseEstimator.java | 2 +- .../wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java | 2 +- .../wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java | 2 +- .../native/cpp/estimator/DifferentialDrivePoseEstimator.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java index 4d6319d7b0..eabb67ee38 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimator.java @@ -217,8 +217,8 @@ public class DifferentialDrivePoseEstimator { */ public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) { m_previousAngle = poseMeters.getRotation(); - m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle); m_observer.setXhat(fillStateVector(poseMeters, 0.0, 0.0)); + m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java index bf967ec2c0..6ff5264449 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimator.java @@ -182,8 +182,8 @@ public class MecanumDrivePoseEstimator { */ public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) { m_previousAngle = poseMeters.getRotation(); - m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle); m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters)); + m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java index 69cbfd15c8..34f7376ab6 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimator.java @@ -182,8 +182,8 @@ public class SwerveDrivePoseEstimator { */ public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) { m_previousAngle = poseMeters.getRotation(); - m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle); m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters)); + m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle); } /** diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index c5e50b6aae..3c00154fd4 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -56,8 +56,8 @@ void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs( void DifferentialDrivePoseEstimator::ResetPosition( const Pose2d& pose, const Rotation2d& gyroAngle) { m_previousAngle = pose.Rotation(); - m_gyroOffset = GetEstimatedPosition().Rotation() - gyroAngle; m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m)); + m_gyroOffset = GetEstimatedPosition().Rotation() - gyroAngle; } Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {