Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03: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

34
wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
View File

@@ -30,10 +30,10 @@ import java.util.function.BiConsumer;
*
* <p>{@link DifferentialDrivePoseEstimator#update} should be called every robot loop (if your robot
* loops are faster than the default of 20 ms then you should change the {@link
* DifferentialDrivePoseEstimator#DifferentialDrivePoseEstimator(Rotation2d, Pose2d, Matrix, Matrix,
* Matrix, double) nominal delta time}.) {@link DifferentialDrivePoseEstimator#addVisionMeasurement}
* can be called as infrequently as you want; if you never call it then this class will behave
* exactly like regular encoder odometry.
* DifferentialDrivePoseEstimator#DifferentialDrivePoseEstimator(Rotation2d, double, double, Pose2d,
* Matrix, Matrix, Matrix, double) nominal delta time}.) {@link
* DifferentialDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you want;
* if you never call it then this class will behave exactly like regular encoder odometry.
*
* <p>The state-space system used internally has the following states (x), inputs (u), and outputs
* (y):
@@ -70,6 +70,8 @@ public class DifferentialDrivePoseEstimator {
* Constructs a DifferentialDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @param initialPoseMeters The starting pose estimate.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta, dist_l, dist_r]ᵀ,
@@ -83,12 +85,16 @@ public class DifferentialDrivePoseEstimator {
*/
public DifferentialDrivePoseEstimator(
Rotation2d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d initialPoseMeters,
Matrix<N5, N1> stateStdDevs,
Matrix<N3, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
this(
gyroAngle,
leftDistanceMeters,
rightDistanceMeters,
initialPoseMeters,
stateStdDevs,
localMeasurementStdDevs,
@@ -100,6 +106,8 @@ public class DifferentialDrivePoseEstimator {
* Constructs a DifferentialDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @param initialPoseMeters The starting pose estimate.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta, dist_l, dist_r]ᵀ,
@@ -114,6 +122,8 @@ public class DifferentialDrivePoseEstimator {
*/
public DifferentialDrivePoseEstimator(
Rotation2d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d initialPoseMeters,
Matrix<N5, N1> stateStdDevs,
Matrix<N3, N1> localMeasurementStdDevs,
@@ -155,7 +165,7 @@ public class DifferentialDrivePoseEstimator {
m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
m_observer.setXhat(fillStateVector(initialPoseMeters, 0.0, 0.0));
m_observer.setXhat(fillStateVector(initialPoseMeters, leftDistanceMeters, rightDistanceMeters));
}
/**
@@ -209,20 +219,24 @@ public class DifferentialDrivePoseEstimator {
/**
* Resets the robot's position on the field.
*
* <p>You NEED to reset your encoders (to zero) when calling this method.
*
* <p>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 poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param leftPositionMeters The distance traveled by the left encoder.
* @param rightPositionMeters The distance traveled by the right encoder.
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
public void resetPosition(
Rotation2d gyroAngle,
double leftPositionMeters,
double rightPositionMeters,
Pose2d poseMeters) {
// Reset state estimate and error covariance
m_observer.reset();
m_poseBuffer.clear();
m_observer.setXhat(fillStateVector(poseMeters, 0.0, 0.0));
m_observer.setXhat(fillStateVector(poseMeters, leftPositionMeters, rightPositionMeters));
m_prevTimeSeconds = -1;

16
wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
View File

@@ -32,7 +32,7 @@ import java.util.function.BiConsumer;
* <p>{@link MecanumDrivePoseEstimator#update} should be called every robot loop. If your loops are
* faster or slower than the default of 20 ms, then you should change the nominal delta time using
* the secondary constructor: {@link MecanumDrivePoseEstimator#MecanumDrivePoseEstimator(Rotation2d,
* Pose2d, MecanumDriveWheelPositions, MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}.
* MecanumDriveWheelPositions, Pose2d, MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}.
*
* <p>{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
* want; if you never call it, then this class will behave mostly like regular encoder odometry.
@@ -70,8 +70,8 @@ public class MecanumDrivePoseEstimator {
* Constructs a MecanumDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param wheelPositions The distances driven by each wheel.
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta, s_fl, s_fr, s_rl,
@@ -85,16 +85,16 @@ public class MecanumDrivePoseEstimator {
*/
public MecanumDrivePoseEstimator(
Rotation2d gyroAngle,
Pose2d initialPoseMeters,
MecanumDriveWheelPositions wheelPositions,
Pose2d initialPoseMeters,
MecanumDriveKinematics kinematics,
Matrix<N7, N1> stateStdDevs,
Matrix<N5, N1> localMeasurementStdDevs,
Matrix<N3, N1> visionMeasurementStdDevs) {
this(
gyroAngle,
initialPoseMeters,
wheelPositions,
initialPoseMeters,
kinematics,
stateStdDevs,
localMeasurementStdDevs,
@@ -106,8 +106,8 @@ public class MecanumDrivePoseEstimator {
* Constructs a MecanumDrivePoseEstimator.
*
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param wheelPositions The distances driven by each wheel.
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta, s_fl, s_fr, s_rl,
@@ -122,8 +122,8 @@ public class MecanumDrivePoseEstimator {
*/
public MecanumDrivePoseEstimator(
Rotation2d gyroAngle,
Pose2d initialPoseMeters,
MecanumDriveWheelPositions wheelPositions,
Pose2d initialPoseMeters,
MecanumDriveKinematics kinematics,
Matrix<N7, N1> stateStdDevs,
Matrix<N5, N1> localMeasurementStdDevs,
@@ -200,12 +200,12 @@ public class MecanumDrivePoseEstimator {
* <p>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 poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(
Pose2d poseMeters, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
// Reset state estimate and error covariance
m_observer.reset();
m_poseBuffer.clear();

14
wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
View File

@@ -34,7 +34,7 @@ import java.util.function.BiConsumer;
* <p>{@link SwerveDrivePoseEstimator#update} should be called every robot loop. If your loops are
* faster or slower than the default of 20 ms, then you should change the nominal delta time using
* the secondary constructor: {@link SwerveDrivePoseEstimator#SwerveDrivePoseEstimator(Nat, Nat,
* Nat, Rotation2d, Pose2d, SwerveModulePosition[], SwerveDriveKinematics, Matrix, Matrix, Matrix,
* Nat, Rotation2d, SwerveModulePosition[], Pose2d, SwerveDriveKinematics, Matrix, Matrix, Matrix,
* double)}.
*
* <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
@@ -96,8 +96,8 @@ public class SwerveDrivePoseEstimator<States extends Num, Inputs extends Num, Ou
Nat<Inputs> inputs,
Nat<Outputs> outputs,
Rotation2d gyroAngle,
Pose2d initialPoseMeters,
SwerveModulePosition[] modulePositions,
Pose2d initialPoseMeters,
SwerveDriveKinematics kinematics,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> localMeasurementStdDevs,
@@ -107,8 +107,8 @@ public class SwerveDrivePoseEstimator<States extends Num, Inputs extends Num, Ou
inputs,
outputs,
gyroAngle,
initialPoseMeters,
modulePositions,
initialPoseMeters,
kinematics,
stateStdDevs,
localMeasurementStdDevs,
@@ -123,8 +123,8 @@ public class SwerveDrivePoseEstimator<States extends Num, Inputs extends Num, Ou
* @param inputs The size of the input vector.
* @param outputs The size of the outputs vector.
* @param gyroAngle The current gyro angle.
* @param initialPoseMeters The starting pose estimate.
* @param modulePositions The current distance measurements and rotations of the swerve modules.
* @param initialPoseMeters The starting pose estimate.
* @param kinematics A correctly-configured kinematics object for your drivetrain.
* @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
* model's state estimates less. This matrix is in the form [x, y, theta, s_0, ... s_n]ᵀ, with
@@ -142,8 +142,8 @@ public class SwerveDrivePoseEstimator<States extends Num, Inputs extends Num, Ou
Nat<Inputs> inputs,
Nat<Outputs> outputs,
Rotation2d gyroAngle,
Pose2d initialPoseMeters,
SwerveModulePosition[] modulePositions,
Pose2d initialPoseMeters,
SwerveDriveKinematics kinematics,
Matrix<States, N1> stateStdDevs,
Matrix<Outputs, N1> localMeasurementStdDevs,
@@ -247,12 +247,12 @@ public class SwerveDrivePoseEstimator<States extends Num, Inputs extends Num, Ou
* <p>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 poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param modulePositions The current distance measurements and rotations of the swerve modules.
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(
Pose2d poseMeters, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) {
// Reset state estimate and error covariance
m_observer.reset();
m_poseBuffer.clear();

37
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
View File

@@ -33,42 +33,59 @@ public class DifferentialDriveOdometry {
* Constructs a DifferentialDriveOdometry object.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @param initialPoseMeters The starting position of the robot on the field.
*/
public DifferentialDriveOdometry(Rotation2d gyroAngle, Pose2d initialPoseMeters) {
public DifferentialDriveOdometry(
Rotation2d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d initialPoseMeters) {
m_poseMeters = initialPoseMeters;
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_previousAngle = initialPoseMeters.getRotation();
m_prevLeftDistance = leftDistanceMeters;
m_prevRightDistance = rightDistanceMeters;
MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
}
/**
* Constructs a DifferentialDriveOdometry object with the default pose at the origin.
* Constructs a DifferentialDriveOdometry object.
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
*/
public DifferentialDriveOdometry(Rotation2d gyroAngle) {
this(gyroAngle, new Pose2d());
public DifferentialDriveOdometry(
Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
this(gyroAngle, leftDistanceMeters, rightDistanceMeters, new Pose2d());
}
/**
* Resets the robot's position on the field.
*
* <p>You NEED to reset your encoders (to zero) when calling this method.
*
* <p>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 poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistanceMeters The distance traveled by the left encoder.
* @param rightDistanceMeters The distance traveled by the right encoder.
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
public void resetPosition(
Rotation2d gyroAngle,
double leftDistanceMeters,
double rightDistanceMeters,
Pose2d poseMeters) {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
m_prevLeftDistance = 0.0;
m_prevRightDistance = 0.0;
m_prevLeftDistance = leftDistanceMeters;
m_prevRightDistance = rightDistanceMeters;
}
/**

4
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
View File

@@ -70,12 +70,12 @@ public class MecanumDriveOdometry {
* <p>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 poseMeters The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The distances driven by each wheel.
* @param poseMeters The position on the field that your robot is at.
*/
public void resetPosition(
Pose2d poseMeters, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
m_poseMeters = poseMeters;
m_previousAngle = poseMeters.getRotation();
m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);

6
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
View File

@@ -77,12 +77,12 @@ public class SwerveDriveOdometry {
*
* <p>Similarly, module positions do not need to be reset in user code.
*
* @param pose The position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param modulePositions The wheel positions reported by each module.
* @param modulePositions The wheel positions reported by each module.,
* @param pose The position on the field that your robot is at.
*/
public void resetPosition(
Pose2d pose, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) {
if (modulePositions.length != m_numModules) {
throw new IllegalArgumentException(
"Number of modules is not consistent with number of wheel locations provided in "

13
wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
View File

@@ -12,7 +12,8 @@
using namespace frc;
DifferentialDrivePoseEstimator::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>& visionMeasurmentStdDevs,
@@ -41,7 +42,7 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
m_gyroOffset = initialPose.Rotation() - gyroAngle;
m_previousAngle = initialPose.Rotation();
m_observer.SetXhat(FillStateVector(initialPose, 0_m, 0_m));
m_observer.SetXhat(FillStateVector(initialPose, leftDistance, rightDistance));
}
void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
@@ -50,13 +51,15 @@ void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs);
}
void DifferentialDrivePoseEstimator::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle) {
void DifferentialDrivePoseEstimator::ResetPosition(const Rotation2d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance,
const Pose2d& pose) {
// Reset state estimate and error covariance
m_observer.Reset();
m_poseBuffer.Clear();
m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m));
m_observer.SetXhat(FillStateVector(pose, leftDistance, rightDistance));
m_prevTime = -1_s;

8
wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
View File

@@ -12,8 +12,8 @@
using namespace frc;
frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
const Rotation2d& gyroAngle, const Pose2d& initialPose,
const MecanumDriveWheelPositions& wheelPositions,
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose,
MecanumDriveKinematics kinematics,
const wpi::array<double, 7>& stateStdDevs,
const wpi::array<double, 5>& localMeasurementStdDevs,
@@ -67,8 +67,8 @@ void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
}
void frc::MecanumDrivePoseEstimator::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions) {
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& pose) {
// Reset state estimate and error covariance
m_observer.Reset();
m_poseBuffer.Clear();

7
wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
View File

@@ -9,8 +9,11 @@
using namespace frc;
DifferentialDriveOdometry::DifferentialDriveOdometry(
const Rotation2d& gyroAngle, const Pose2d& initialPose)
: m_pose(initialPose) {
const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& initialPose)
: m_pose(initialPose),
m_prevLeftDistance(leftDistance),
m_prevRightDistance(rightDistance) {
m_previousAngle = m_pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
wpi::math::MathSharedStore::ReportUsage(

4
wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
View File

@@ -10,7 +10,7 @@ using namespace frc;
MecanumDriveOdometry::MecanumDriveOdometry(
MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions, const Pose2d& initialPose)
const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose)
: m_kinematics(kinematics),
m_pose(initialPose),
m_previousWheelPositions(wheelPositions) {
@@ -22,7 +22,7 @@ MecanumDriveOdometry::MecanumDriveOdometry(
const Pose2d& MecanumDriveOdometry::Update(
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions) {
const MecanumDriveWheelPositions& wheelPositions) {
auto angle = gyroAngle + m_gyroOffset;
MecanumDriveWheelPositions wheelDeltas{

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;

15
wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
View File

@@ -27,24 +27,35 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry {
/**
* Constructs a DifferentialDriveOdometry object.
*
* IF leftDistance and rightDistance are unspecified,
* You NEED to reset your encoders (to zero).
*
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
* @param initialPose The starting position of the robot on the field.
*/
explicit DifferentialDriveOdometry(const Rotation2d& gyroAngle,
units::meter_t leftDistance,
units::meter_t rightDistance,
const Pose2d& initialPose = Pose2d{});
/**
* Resets the robot's position on the field.
*
* You NEED to reset your encoders (to zero) when calling this method.
* 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 position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param leftDistance The distance traveled by the left encoder.
* @param rightDistance The distance traveled by the right encoder.
*/
void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
units::meter_t rightDistance, const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;

17
wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
View File

@@ -33,10 +33,10 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
* @param wheelPositions The current distances measured by each wheel.
* @param initialPose The starting position of the robot on the field.
*/
explicit MecanumDriveOdometry(MecanumDriveKinematics kinematics,
const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions,
const Pose2d& initialPose = Pose2d{});
explicit MecanumDriveOdometry(
MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions& wheelPositions,
const Pose2d& initialPose = Pose2d{});
/**
* Resets the robot's position on the field.
@@ -44,12 +44,13 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
* 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 position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param wheelPositions The current distances measured by 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) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
@@ -74,7 +75,7 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
* @return The new pose of the robot.
*/
const Pose2d& Update(const Rotation2d& gyroAngle,
const MecanumDriveWheelPositions wheelPositions);
const MecanumDriveWheelPositions& wheelPositions);
private:
MecanumDriveKinematics m_kinematics;

11
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
View File

@@ -40,7 +40,7 @@ class SwerveDriveOdometry {
*/
SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose = Pose2d{});
/**
@@ -49,13 +49,14 @@ class SwerveDriveOdometry {
* 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 position on the field that your robot is at.
* @param gyroAngle The angle reported by the gyroscope.
* @param modulePositions The wheel positions reported by each module.
* @param pose The position on the field that your robot is at.
*/
void ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions);
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& pose);
/**
* Returns the position of the robot on the field.
@@ -80,7 +81,7 @@ class SwerveDriveOdometry {
*/
const Pose2d& Update(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions);
const wpi::array<SwerveModulePosition, NumModules>& modulePositions);
private:
SwerveDriveKinematics<NumModules> m_kinematics;

9
wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
View File

@@ -11,7 +11,7 @@ namespace frc {
template <size_t NumModules>
SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& initialPose)
: m_kinematics(kinematics),
m_pose(initialPose),
@@ -24,8 +24,9 @@ SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
template <size_t NumModules>
void SwerveDriveOdometry<NumModules>::ResetPosition(
const Pose2d& pose, const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions) {
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
const Pose2d& pose) {
m_pose = pose;
m_previousAngle = pose.Rotation();
m_gyroOffset = m_pose.Rotation() - gyroAngle;
@@ -35,7 +36,7 @@ void SwerveDriveOdometry<NumModules>::ResetPosition(
template <size_t NumModules>
const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
const Rotation2d& gyroAngle,
const wpi::array<SwerveModulePosition, NumModules> modulePositions) {
const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
auto moduleDeltas =
wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
for (size_t index = 0; index < modulePositions.size(); index++) {