From 8a030e0fa41c9beb1b4e418b886bffd6a945a280 Mon Sep 17 00:00:00 2001 From: Isaac Turner Date: Wed, 5 Jun 2024 13:08:40 +0800 Subject: [PATCH 1/2] [wpimath] Add reset methods to Odometry Resolves https://github.com/wpilibsuite/allwpilib/issues/5001 --- .../wpi/first/math/kinematics/Odometry.java | 32 ++++++++++++++++++ .../native/include/frc/kinematics/Odometry.h | 33 +++++++++++++++++++ 2 files changed, 65 insertions(+) diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java index 0b4dab7a950..66ce592957d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; /** * Class for odometry. Robot code should not use this directly- Instead, use the particular type for @@ -63,6 +64,37 @@ public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMet m_previousWheelPositions = m_kinematics.copy(wheelPositions); } + /** + * Resets the pose. + * + * @param poseMeters The pose to reset to. + */ + public void resetPose(Pose2d poseMeters) { + m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation())); + m_poseMeters = poseMeters; + m_previousAngle = m_poseMeters.getRotation(); + } + + /** + * Resets the translation of the pose. + * + * @param translation The translation to reset to. + */ + public void resetTranslation(Translation2d translation) { + m_poseMeters = new Pose2d(translation, m_poseMeters.getRotation()); + } + + /** + * Resets the rotation of the pose. + * + * @param rotation The rotation to reset to. + */ + public void resetRotation(Rotation2d rotation) { + m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation())); + m_poseMeters = new Pose2d(m_poseMeters.getTranslation(), rotation); + m_previousAngle = m_poseMeters.getRotation(); + } + /** * Returns the position of the robot on the field. * diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry.h b/wpimath/src/main/native/include/frc/kinematics/Odometry.h index 53066c7041a..c0daea0e3e0 100644 --- a/wpimath/src/main/native/include/frc/kinematics/Odometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/Odometry.h @@ -7,6 +7,8 @@ #include #include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Translation2d.h" #include "frc/kinematics/Kinematics.h" #include "frc/kinematics/WheelPositions.h" @@ -58,6 +60,37 @@ class WPILIB_DLLEXPORT Odometry { m_previousWheelPositions = wheelPositions; } + /** + * Resets the pose. + * + * @param pose The pose to reset to. + */ + void ResetPose(const Pose2d& pose) { + m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation()); + m_pose = pose; + m_previousAngle = pose.Rotation(); + } + + /** + * Resets the translation of the pose. + * + * @param translation The translation to reset to. + */ + void ResetTranslation(const Translation2d& translation) { + m_pose = Pose2d{translation, m_pose.Rotation()}; + } + + /** + * Resets the rotation of the pose. + * + * @param rotation The rotation to reset to. + */ + void ResetRotation(const Rotation2d& rotation) { + m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation()); + m_pose = Pose2d{m_pose.Translation(), rotation}; + m_previousAngle = rotation; + } + /** * Returns the position of the robot on the field. * @return The pose of the robot. From 15c818fe1de18c4f37bafead55dfef267c33d50a Mon Sep 17 00:00:00 2001 From: Jade Turner Date: Tue, 18 Jun 2024 17:05:59 +0800 Subject: [PATCH 2/2] [wpimath] Add Reset methods to PoseEstimator Requires https://github.com/wpilibsuite/allwpilib/pull/6702 Resolves https://github.com/wpilibsuite/allwpilib/issues/5001 --- .../first/math/estimator/PoseEstimator.java | 31 +++++++++++++++++++ .../include/frc/estimator/PoseEstimator.h | 22 +++++++++++++ .../include/frc/estimator/PoseEstimator.inc | 23 ++++++++++++++ 3 files changed, 76 insertions(+) diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java index 5f17c83fd7f..2674e2ceaed 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.Kinematics; @@ -125,6 +126,36 @@ public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMet m_poseEstimate = m_odometry.getPoseMeters(); } + /** + * Resets the robot's pose. + * + * @param pose The pose to reset to. + */ + public void resetPose(Pose2d pose) { + m_odometry.resetPose(pose); + m_odometryPoseBuffer.clear(); + } + + /** + * Resets the robot's translation. + * + * @param translation The pose to translation to. + */ + public void resetTranslation(Translation2d translation) { + m_odometry.resetTranslation(translation); + m_odometryPoseBuffer.clear(); + } + + /** + * Resets the robot's rotation. + * + * @param rotation The rotation to reset to. + */ + public void resetRotation(Rotation2d rotation) { + m_odometry.resetRotation(rotation); + m_odometryPoseBuffer.clear(); + } + /** * Gets the estimated robot pose. * diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h index cd0600f698b..da9f4f3ee68 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h @@ -15,6 +15,7 @@ #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Translation2d.h" #include "frc/interpolation/TimeInterpolatableBuffer.h" #include "frc/kinematics/Kinematics.h" #include "frc/kinematics/Odometry.h" @@ -87,6 +88,27 @@ class WPILIB_DLLEXPORT PoseEstimator { void ResetPosition(const Rotation2d& gyroAngle, const WheelPositions& wheelPositions, const Pose2d& pose); + /** + * Resets the robot's pose. + * + * @param pose The pose to reset to. + */ + void ResetPose(const Pose2d& pose); + + /** + * Resets the robot's translation. + * + * @param translation The pose to translation to. + */ + void ResetTranslation(const Translation2d& translation); + + /** + * Resets the robot's rotation. + * + * @param rotation The rotation to reset to. + */ + void ResetRotation(const Rotation2d& rotation); + /** * Gets the estimated robot pose. * diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc index f8bddca52d0..cb39dd26053 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc @@ -5,6 +5,9 @@ #pragma once #include "frc/estimator/PoseEstimator.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Translation2d.h" +#include "frc/kinematics/WheelPositions.h" namespace frc { @@ -53,6 +56,26 @@ void PoseEstimator::ResetPosition( m_poseEstimate = m_odometry.GetPose(); } +template +void PoseEstimator::ResetPose(const Pose2d& pose) { + m_odometry.ResetPose(pose); + m_odometryPoseBuffer.Clear(); +} + +template +void PoseEstimator::ResetTranslation( + const Translation2d& translation) { + m_odometry.ResetTranslation(translation); + m_odometryPoseBuffer.Clear(); +} + +template +void PoseEstimator::ResetRotation( + const Rotation2d& rotation) { + m_odometry.ResetTranslation(rotation); + m_odometryPoseBuffer.Clear(); +} + template Pose2d PoseEstimator::GetEstimatedPosition() const {