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

[wpimath] Rework odometry APIs to improve feature parity (#4645)

Browse Source 
Co-authored-by: Ryan Blue <ryanzblue@gmail.com>
This commit is contained in:
Jordan McMichael
2022-11-18 23:42:00 -05:00
committed by GitHub
parent e2d49181da
commit 902e8686d3
53 changed files with 266 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
View File

@@ -56,6 +56,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
* Constructs a DifferentialDrivePoseEstimator.
*
* @param gyroAngle The gyro angle of the robot.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
* @param initialPose The estimated initial pose.
* @param stateStdDevs Standard deviations of model states.
* Increase these numbers to trust your
@@ -79,7 +81,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
* @param nominalDt The period of the loop calling Update().
*/
DifferentialDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& initialPose,
const wpi::array<double, 5>& stateStdDevs,
const wpi::array<double, 3>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
@@ -103,14 +106,18 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
/**
* Resets the robot's position on the field.
*
* You NEED to reset your encoders to zero when calling this method. The
* IF leftDistance and rightDistance are unspecified,
* You NEED to reset your encoders (to zero). The
* gyroscope angle does not need to be reset here on the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
*
* @param pose The estimated pose of the robot on the field.
* @param gyroAngle The current gyro angle.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
* @param pose The estimated pose of the robot on the field.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle);
void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& pose);
/**
* Returns the pose of the robot at the current time as estimated by the

18
wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
View File

@@ -56,8 +56,8 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
* Constructs a MecanumDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param initialPose The starting pose estimate.
* @param wheelPositions The distance measured by each wheel.
* @param initialPose The starting pose estimate.
* @param kinematics A correctly-configured kinematics object
* for your drivetrain.
* @param stateStdDevs Standard deviations of model states.
@@ -82,9 +82,9 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
* loop.
*/
MecanumDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
MecanumDriveKinematics kinematics,
const Pose2d& initialPose, MecanumDriveKinematics kinematics,
const wpi::array<double, 7>& stateStdDevs,
const wpi::array<double, 5>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
@@ -108,15 +108,19 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
/**
* Resets the robot's position on the field.
*
* <p>The gyroscope angle does not need to be reset in the user's robot code.
* IF wheelPositions are unspecified,
* You NEED to reset your encoders (to zero).
*
* The gyroscope angle does not need to be reset in the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances measured at each wheel.
* @param pose The position on the field that your robot is at.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions);
void ResetPosition(const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
const Pose2d& pose);
/**
* Gets the pose of the robot at the current time as estimated by the Extended

26
wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
View File

@@ -61,9 +61,9 @@ class SwerveDrivePoseEstimator {
* Constructs a SwerveDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param initialPose The starting pose estimate.
* @param modulePositions The current distance and rotation
* measurements of the swerve modules.
* @param initialPose The starting pose estimate.
* @param kinematics A correctly-configured kinematics object
* for your drivetrain.
* @param stateStdDevs Standard deviations of model states.
@@ -86,9 +86,9 @@ class SwerveDrivePoseEstimator {
* loop.
*/
SwerveDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
const wpi::array<SwerveModulePosition, NumModules> modulePositions,
SwerveDriveKinematics<NumModules>& kinematics,
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose, SwerveDriveKinematics<NumModules>& kinematics,
const wpi::array<double, States>& stateStdDevs,
const wpi::array<double, Outputs>& localMeasurementStdDevs,
const wpi::array<double, 3>& visionMeasurementStdDevs,
@@ -138,17 +138,21 @@ class SwerveDrivePoseEstimator {
/**
* Resets the robot's position on the field.
*
* IF leftDistance and rightDistance are unspecified,
* You NEED to reset your encoders (to zero).
*
* The gyroscope angle does not need to be reset in the user's robot code.
* The library automatically takes care of offsetting the gyro angle.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param modulePositions The current distance and rotation measurements of
* the swerve modules.
* @param pose The position on the field that your robot is at.
*/
void ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
wpi::array<SwerveModulePosition, NumModules> modulePositions) {
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& pose) {
// Reset state estimate and error covariance
m_observer.Reset();
m_poseBuffer.Clear();
@@ -285,8 +289,8 @@ class SwerveDrivePoseEstimator {
*/
Pose2d Update(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModuleState, NumModules> moduleStates,
const wpi::array<SwerveModulePosition, NumModules> modulePositions) {
const wpi::array<SwerveModuleState, NumModules>& moduleStates,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
moduleStates, modulePositions);
}
@@ -306,8 +310,8 @@ class SwerveDrivePoseEstimator {
*/
Pose2d UpdateWithTime(
units::second_t currentTime, const Rotation2d& gyroAngle,
const wpi::array<SwerveModuleState, NumModules> moduleStates,
const wpi::array<SwerveModulePosition, NumModules> modulePositions) {
const wpi::array<SwerveModuleState, NumModules>& moduleStates,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
m_prevTime = currentTime;