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

Co-authored-by: Ryan Blue <ryanzblue@gmail.com>

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

@@ -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;
 

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

@@ -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();

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

@@ -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();

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

@@ -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;
   }
 
   /**

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

@@ -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);

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

@@ -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 "