Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-01 02:41:48 +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.
*

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

@@ -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.
*

23
wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc
View File

@@ -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<WheelSpeeds, WheelPositions>::ResetPosition(
m_poseEstimate = m_odometry.GetPose();
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPose(const Pose2d& pose) {
m_odometry.ResetPose(pose);
m_odometryPoseBuffer.Clear();
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetTranslation(
const Translation2d& translation) {
m_odometry.ResetTranslation(translation);
m_odometryPoseBuffer.Clear();
}
template <typename WheelSpeeds, typename WheelPositions>
void PoseEstimator<WheelSpeeds, WheelPositions>::ResetRotation(
const Rotation2d& rotation) {
m_odometry.ResetTranslation(rotation);
m_odometryPoseBuffer.Clear();
}
template <typename WheelSpeeds, typename WheelPositions>
Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
const {