Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add Reset methods to PoseEstimator (#6751)

Browse Source
This commit is contained in:
Jade
2024-07-09 09:26:08 +08:00
committed by GitHub
parent dc276b651b
commit cc02a948a0
3 changed files with 76 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -11,6 +11,7 @@ import edu.wpi.first.math.Nat;
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 class PoseEstimator<T> {
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.
*