[wpimath] Refactor kinematics, odometry, and pose estimator (#5355)

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

@@ -16,7 +16,8 @@ import edu.wpi.first.math.geometry.Twist2d;
  * whereas forward kinematics converts left and right component velocities into a linear and angular
  * chassis speed.
  */
-public class DifferentialDriveKinematics {
+public class DifferentialDriveKinematics
+    implements Kinematics<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions> {
   public final double trackWidthMeters;
 
   /**
@@ -37,6 +38,7 @@ public class DifferentialDriveKinematics {
    * @param wheelSpeeds The left and right velocities.
    * @return The chassis speed.
    */
+  @Override
   public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
     return new ChassisSpeeds(
         (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2,
@@ -51,6 +53,7 @@ public class DifferentialDriveKinematics {
    *     chassis' speed.
    * @return The left and right velocities.
    */
+  @Override
   public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
     return new DifferentialDriveWheelSpeeds(
         chassisSpeeds.vxMetersPerSecond
@@ -59,6 +62,11 @@ public class DifferentialDriveKinematics {
             + trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
   }
 
+  @Override
+  public Twist2d toTwist2d(DifferentialDriveWheelPositions wheelDeltas) {
+    return toTwist2d(wheelDeltas.leftMeters, wheelDeltas.rightMeters);
+  }
+
   /**
    * Performs forward kinematics to return the resulting Twist2d from the given left and right side
    * distance deltas. This method is often used for odometry -- determining the robot's position on

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

@@ -8,7 +8,6 @@ import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Twist2d;
 
 /**
  * Class for differential drive odometry. Odometry allows you to track the robot's position on the
@@ -20,15 +19,7 @@ import edu.wpi.first.math.geometry.Twist2d;
  * <p>It is important that you reset your encoders to zero before using this class. Any subsequent
  * pose resets also require the encoders to be reset to zero.
  */
-public class DifferentialDriveOdometry {
-  private Pose2d m_poseMeters;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
-
-  private double m_prevLeftDistance;
-  private double m_prevRightDistance;
-
+public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPositions> {
   /**
    * Constructs a DifferentialDriveOdometry object.
    *
@@ -42,13 +33,11 @@ public class DifferentialDriveOdometry {
       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;
-
+    super(
+        new DifferentialDriveKinematics(1),
+        gyroAngle,
+        new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
+        initialPoseMeters);
     MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
   }
 
@@ -80,21 +69,10 @@ public class DifferentialDriveOdometry {
       double leftDistanceMeters,
       double rightDistanceMeters,
       Pose2d poseMeters) {
-    m_poseMeters = poseMeters;
-    m_previousAngle = poseMeters.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-
-    m_prevLeftDistance = leftDistanceMeters;
-    m_prevRightDistance = rightDistanceMeters;
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   *
-   * @return The pose of the robot (x and y are in meters).
-   */
-  public Pose2d getPoseMeters() {
-    return m_poseMeters;
+    super.resetPosition(
+        gyroAngle,
+        new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters),
+        poseMeters);
   }
 
   /**
@@ -109,22 +87,7 @@ public class DifferentialDriveOdometry {
    */
   public Pose2d update(
       Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
-    double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
-    double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
-
-    m_prevLeftDistance = leftDistanceMeters;
-    m_prevRightDistance = rightDistanceMeters;
-
-    double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var newPose =
-        m_poseMeters.exp(
-            new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
-
-    m_previousAngle = angle;
-
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-    return m_poseMeters;
+    return super.update(
+        gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters));
   }
 }

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

@@ -0,0 +1,68 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.MathUtil;
+import java.util.Objects;
+
+public class DifferentialDriveWheelPositions
+    implements WheelPositions<DifferentialDriveWheelPositions> {
+  /** Distance measured by the left side. */
+  public double leftMeters;
+
+  /** Distance measured by the right side. */
+  public double rightMeters;
+
+  /**
+   * Constructs a DifferentialDriveWheelPositions.
+   *
+   * @param leftMeters Distance measured by the left side.
+   * @param rightMeters Distance measured by the right side.
+   */
+  public DifferentialDriveWheelPositions(double leftMeters, double rightMeters) {
+    this.leftMeters = leftMeters;
+    this.rightMeters = rightMeters;
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof DifferentialDriveWheelPositions) {
+      DifferentialDriveWheelPositions other = (DifferentialDriveWheelPositions) obj;
+      return Math.abs(other.leftMeters - leftMeters) < 1E-9
+          && Math.abs(other.rightMeters - rightMeters) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(leftMeters, rightMeters);
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "DifferentialDriveWheelPositions(Left: %.2f m, Right: %.2f m", leftMeters, rightMeters);
+  }
+
+  @Override
+  public DifferentialDriveWheelPositions copy() {
+    return new DifferentialDriveWheelPositions(leftMeters, rightMeters);
+  }
+
+  @Override
+  public DifferentialDriveWheelPositions minus(DifferentialDriveWheelPositions other) {
+    return new DifferentialDriveWheelPositions(
+        this.leftMeters - other.leftMeters, this.rightMeters - other.rightMeters);
+  }
+
+  @Override
+  public DifferentialDriveWheelPositions interpolate(
+      DifferentialDriveWheelPositions endValue, double t) {
+    return new DifferentialDriveWheelPositions(
+        MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t),
+        MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t));
+  }
+}

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

@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.geometry.Twist2d;
+
+/**
+ * Helper class that converts a chassis velocity (dx and dtheta components) into wheel speeds. Robot
+ * code should not use this directly- Instead, use the particular type for your drivetrain (e.g.,
+ * {@link DifferentialDriveKinematics}).
+ *
+ * @param <S> The type of the wheel speeds.
+ * @param <P> The type of the wheel positions.
+ */
+public interface Kinematics<S, P> {
+  /**
+   * Performs forward kinematics to return the resulting chassis speed from the wheel speeds. This
+   * method is often used for odometry -- determining the robot's position on the field using data
+   * from the real-world speed of each wheel on the robot.
+   *
+   * @param wheelSpeeds The speeds of the wheels.
+   * @return The chassis speed.
+   */
+  ChassisSpeeds toChassisSpeeds(S wheelSpeeds);
+
+  /**
+   * Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This
+   * method is often used to convert joystick values into wheel speeds.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @return The wheel speeds.
+   */
+  S toWheelSpeeds(ChassisSpeeds chassisSpeeds);
+
+  /**
+   * Performs forward kinematics to return the resulting from the given wheel deltas. This method is
+   * often used for odometry -- determining the robot's position on the field using changes in the
+   * distance driven by each wheel on the robot.
+   *
+   * @param wheelDeltas The distances driven by each wheel.
+   * @return The resulting Twist2d in the robot's movement.
+   */
+  Twist2d toTwist2d(P wheelDeltas);
+}

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

@@ -30,7 +30,8 @@ import org.ejml.simple.SimpleMatrix;
  * <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
  * field using encoders and a gyro.
  */
-public class MecanumDriveKinematics {
+public class MecanumDriveKinematics
+    implements Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
   private final SimpleMatrix m_inverseKinematics;
   private final SimpleMatrix m_forwardKinematics;
 
@@ -125,6 +126,7 @@ public class MecanumDriveKinematics {
    * @param chassisSpeeds The desired chassis speed.
    * @return The wheel speeds.
    */
+  @Override
   public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
     return toWheelSpeeds(chassisSpeeds, new Translation2d());
   }
@@ -137,6 +139,7 @@ public class MecanumDriveKinematics {
    * @param wheelSpeeds The current mecanum drive wheel speeds.
    * @return The resulting chassis speed.
    */
+  @Override
   public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
     var wheelSpeedsVector = new SimpleMatrix(4, 1);
     wheelSpeedsVector.setColumn(
@@ -154,14 +157,7 @@ public class MecanumDriveKinematics {
         chassisSpeedsVector.get(2, 0));
   }
 
-  /**
-   * Performs forward kinematics to return the resulting Twist2d from the given wheel deltas. This
-   * method is often used for odometry -- determining the robot's position on the field using
-   * changes in the distance driven by each wheel on the robot.
-   *
-   * @param wheelDeltas The distances driven by each wheel.
-   * @return The resulting Twist2d.
-   */
+  @Override
   public Twist2d toTwist2d(MecanumDriveWheelPositions wheelDeltas) {
     var wheelDeltasVector = new SimpleMatrix(4, 1);
     wheelDeltasVector.setColumn(

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

@@ -16,14 +16,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
  * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
  * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
  */
-public class MecanumDriveOdometry {
-  private final MecanumDriveKinematics m_kinematics;
-  private Pose2d m_poseMeters;
-  private MecanumDriveWheelPositions m_previousWheelPositions;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
-
+public class MecanumDriveOdometry extends Odometry<MecanumDriveWheelPositions> {
   /**
    * Constructs a MecanumDriveOdometry object.
    *
@@ -37,16 +30,7 @@ public class MecanumDriveOdometry {
       Rotation2d gyroAngle,
       MecanumDriveWheelPositions wheelPositions,
       Pose2d initialPoseMeters) {
-    m_kinematics = kinematics;
-    m_poseMeters = initialPoseMeters;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPoseMeters.getRotation();
-    m_previousWheelPositions =
-        new MecanumDriveWheelPositions(
-            wheelPositions.frontLeftMeters,
-            wheelPositions.frontRightMeters,
-            wheelPositions.rearLeftMeters,
-            wheelPositions.rearRightMeters);
+    super(kinematics, gyroAngle, wheelPositions, initialPoseMeters);
     MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
   }
 
@@ -63,72 +47,4 @@ public class MecanumDriveOdometry {
       MecanumDriveWheelPositions wheelPositions) {
     this(kinematics, gyroAngle, wheelPositions, new Pose2d());
   }
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * <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 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(
-      Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) {
-    m_poseMeters = poseMeters;
-    m_previousAngle = poseMeters.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousWheelPositions =
-        new MecanumDriveWheelPositions(
-            wheelPositions.frontLeftMeters,
-            wheelPositions.frontRightMeters,
-            wheelPositions.rearLeftMeters,
-            wheelPositions.rearRightMeters);
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   *
-   * @return The pose of the robot (x and y are in meters).
-   */
-  public Pose2d getPoseMeters() {
-    return m_poseMeters;
-  }
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and integration of the pose
-   * over time. This method takes in an angle parameter which is used instead of the angular rate
-   * that is calculated from forward kinematics, in addition to the current distance measurement at
-   * each wheel.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelPositions The distances driven by each wheel.
-   * @return The new pose of the robot.
-   */
-  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var wheelDeltas =
-        new MecanumDriveWheelPositions(
-            wheelPositions.frontLeftMeters - m_previousWheelPositions.frontLeftMeters,
-            wheelPositions.frontRightMeters - m_previousWheelPositions.frontRightMeters,
-            wheelPositions.rearLeftMeters - m_previousWheelPositions.rearLeftMeters,
-            wheelPositions.rearRightMeters - m_previousWheelPositions.rearRightMeters);
-
-    var twist = m_kinematics.toTwist2d(wheelDeltas);
-    twist.dtheta = angle.minus(m_previousAngle).getRadians();
-    var newPose = m_poseMeters.exp(twist);
-
-    m_previousAngle = angle;
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-    m_previousWheelPositions =
-        new MecanumDriveWheelPositions(
-            wheelPositions.frontLeftMeters,
-            wheelPositions.frontRightMeters,
-            wheelPositions.rearLeftMeters,
-            wheelPositions.rearRightMeters);
-
-    return m_poseMeters;
-  }
 }

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

@@ -4,7 +4,10 @@
 
 package edu.wpi.first.math.kinematics;
 
-public class MecanumDriveWheelPositions {
+import edu.wpi.first.math.MathUtil;
+import java.util.Objects;
+
+public class MecanumDriveWheelPositions implements WheelPositions<MecanumDriveWheelPositions> {
   /** Distance measured by the front left wheel. */
   public double frontLeftMeters;
 
@@ -39,6 +42,23 @@ public class MecanumDriveWheelPositions {
     this.rearRightMeters = rearRightMeters;
   }
 
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof MecanumDriveWheelPositions) {
+      MecanumDriveWheelPositions other = (MecanumDriveWheelPositions) obj;
+      return Math.abs(other.frontLeftMeters - frontLeftMeters) < 1E-9
+          && Math.abs(other.frontRightMeters - frontRightMeters) < 1E-9
+          && Math.abs(other.rearLeftMeters - rearLeftMeters) < 1E-9
+          && Math.abs(other.rearRightMeters - rearRightMeters) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
+  }
+
   @Override
   public String toString() {
     return String.format(
@@ -46,4 +66,28 @@ public class MecanumDriveWheelPositions {
             + "Rear Left: %.2f m, Rear Right: %.2f m)",
         frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
   }
+
+  @Override
+  public MecanumDriveWheelPositions copy() {
+    return new MecanumDriveWheelPositions(
+        frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
+  }
+
+  @Override
+  public MecanumDriveWheelPositions minus(MecanumDriveWheelPositions other) {
+    return new MecanumDriveWheelPositions(
+        this.frontLeftMeters - other.frontLeftMeters,
+        this.frontRightMeters - other.frontRightMeters,
+        this.rearLeftMeters - other.rearLeftMeters,
+        this.rearRightMeters - other.rearRightMeters);
+  }
+
+  @Override
+  public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions endValue, double t) {
+    return new MecanumDriveWheelPositions(
+        MathUtil.interpolate(this.frontLeftMeters, endValue.frontLeftMeters, t),
+        MathUtil.interpolate(this.frontRightMeters, endValue.frontRightMeters, t),
+        MathUtil.interpolate(this.rearLeftMeters, endValue.rearLeftMeters, t),
+        MathUtil.interpolate(this.rearRightMeters, endValue.rearRightMeters, t));
+  }
 }

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

@@ -0,0 +1,99 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+/**
+ * Class for odometry. Robot code should not use this directly- Instead, use the particular type for
+ * your drivetrain (e.g., {@link DifferentialDriveOdometry}). Odometry allows you to track the
+ * robot's position on the field over the course of a match using readings from encoders and a
+ * gyroscope.
+ *
+ * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
+ * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
+ */
+public class Odometry<T extends WheelPositions<T>> {
+  private final Kinematics<?, T> m_kinematics;
+  private Pose2d m_poseMeters;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+  private T m_previousWheelPositions;
+
+  /**
+   * Constructs an Odometry object.
+   *
+   * @param kinematics The kinematics of the drivebase.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current encoder readings.
+   * @param initialPoseMeters The starting position of the robot on the field.
+   */
+  public Odometry(
+      Kinematics<?, T> kinematics,
+      Rotation2d gyroAngle,
+      T wheelPositions,
+      Pose2d initialPoseMeters) {
+    m_kinematics = kinematics;
+    m_poseMeters = initialPoseMeters;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = m_poseMeters.getRotation();
+    m_previousWheelPositions = wheelPositions.copy();
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <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 gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current encoder readings.
+   * @param poseMeters The position on the field that your robot is at.
+   */
+  public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) {
+    m_poseMeters = poseMeters;
+    m_previousAngle = m_poseMeters.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousWheelPositions = wheelPositions.copy();
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   *
+   * @return The pose of the robot (x and y are in meters).
+   */
+  public Pose2d getPoseMeters() {
+    return m_poseMeters;
+  }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and integration of the pose
+   * over time. This method takes in an angle parameter which is used instead of the angular rate
+   * that is calculated from forward kinematics, in addition to the current distance measurement at
+   * each wheel.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current encoder readings.
+   * @return The new pose of the robot.
+   */
+  public Pose2d update(Rotation2d gyroAngle, T wheelPositions) {
+    T wheelDeltas = wheelPositions.minus(m_previousWheelPositions);
+
+    var angle = gyroAngle.plus(m_gyroOffset);
+
+    var twist = m_kinematics.toTwist2d(wheelDeltas);
+    twist.dtheta = angle.minus(m_previousAngle).getRadians();
+
+    var newPose = m_poseMeters.exp(twist);
+
+    m_previousWheelPositions = wheelPositions.copy();
+    m_previousAngle = angle;
+    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+
+    return m_poseMeters;
+  }
+}

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

@@ -32,7 +32,24 @@ import org.ejml.simple.SimpleMatrix;
  * <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
  * field using encoders and a gyro.
  */
-public class SwerveDriveKinematics {
+public class SwerveDriveKinematics
+    implements Kinematics<SwerveDriveKinematics.SwerveDriveWheelStates, SwerveDriveWheelPositions> {
+  public static class SwerveDriveWheelStates {
+    public SwerveModuleState[] states;
+
+    /**
+     * Creates a new SwerveDriveWheelStates instance.
+     *
+     * @param states The swerve module states. This will be deeply copied.
+     */
+    public SwerveDriveWheelStates(SwerveModuleState[] states) {
+      this.states = new SwerveModuleState[states.length];
+      for (int i = 0; i < states.length; i++) {
+        this.states[i] = new SwerveModuleState(states[i].speedMetersPerSecond, states[i].angle);
+      }
+    }
+  }
+
   private final SimpleMatrix m_inverseKinematics;
   private final SimpleMatrix m_forwardKinematics;
 
@@ -159,6 +176,11 @@ public class SwerveDriveKinematics {
     return toSwerveModuleStates(chassisSpeeds, new Translation2d());
   }
 
+  @Override
+  public SwerveDriveWheelStates toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+    return new SwerveDriveWheelStates(toSwerveModuleStates(chassisSpeeds));
+  }
+
   /**
    * Performs forward kinematics to return the resulting chassis state from the given module states.
    * This method is often used for odometry -- determining the robot's position on the field using
@@ -190,6 +212,11 @@ public class SwerveDriveKinematics {
         chassisSpeedsVector.get(2, 0));
   }
 
+  @Override
+  public ChassisSpeeds toChassisSpeeds(SwerveDriveWheelStates wheelStates) {
+    return toChassisSpeeds(wheelStates.states);
+  }
+
   /**
    * Performs forward kinematics to return the resulting chassis state from the given module states.
    * This method is often used for odometry -- determining the robot's position on the field using
@@ -219,6 +246,11 @@ public class SwerveDriveKinematics {
         chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0));
   }
 
+  @Override
+  public Twist2d toTwist2d(SwerveDriveWheelPositions wheelDeltas) {
+    return toTwist2d(wheelDeltas.positions);
+  }
+
   /**
    * Renormalizes the wheel speeds if any individual speed is above the specified maximum.
    *

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

@@ -17,14 +17,8 @@ import edu.wpi.first.math.geometry.Rotation2d;
  * <p>Teams can use odometry during the autonomous period for complex tasks like path following.
  * Furthermore, odometry can be used for latency compensation when using computer-vision systems.
  */
-public class SwerveDriveOdometry {
-  private final SwerveDriveKinematics m_kinematics;
-  private Pose2d m_poseMeters;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
+public class SwerveDriveOdometry extends Odometry<SwerveDriveWheelPositions> {
   private final int m_numModules;
-  private SwerveModulePosition[] m_previousModulePositions;
 
   /**
    * Constructs a SwerveDriveOdometry object.
@@ -39,18 +33,9 @@ public class SwerveDriveOdometry {
       Rotation2d gyroAngle,
       SwerveModulePosition[] modulePositions,
       Pose2d initialPose) {
-    m_kinematics = kinematics;
-    m_poseMeters = initialPose;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPose.getRotation();
-    m_numModules = modulePositions.length;
+    super(kinematics, gyroAngle, new SwerveDriveWheelPositions(modulePositions), initialPose);
 
-    m_previousModulePositions = new SwerveModulePosition[m_numModules];
-    for (int index = 0; index < m_numModules; index++) {
-      m_previousModulePositions[index] =
-          new SwerveModulePosition(
-              modulePositions[index].distanceMeters, modulePositions[index].angle);
-    }
+    m_numModules = modulePositions.length;
 
     MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
   }
@@ -83,29 +68,18 @@ public class SwerveDriveOdometry {
    */
   public void resetPosition(
       Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) {
-    if (modulePositions.length != m_numModules) {
+    resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), pose);
+  }
+
+  @Override
+  public void resetPosition(
+      Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions, Pose2d pose) {
+    if (modulePositions.positions.length != m_numModules) {
       throw new IllegalArgumentException(
           "Number of modules is not consistent with number of wheel locations provided in "
               + "constructor");
     }
-
-    m_poseMeters = pose;
-    m_previousAngle = pose.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    for (int index = 0; index < m_numModules; index++) {
-      m_previousModulePositions[index] =
-          new SwerveModulePosition(
-              modulePositions[index].distanceMeters, modulePositions[index].angle);
-    }
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   *
-   * @return The pose of the robot (x and y are in meters).
-   */
-  public Pose2d getPoseMeters() {
-    return m_poseMeters;
+    super.resetPosition(gyroAngle, modulePositions, pose);
   }
 
   /**
@@ -121,32 +95,16 @@ public class SwerveDriveOdometry {
    * @return The new pose of the robot.
    */
   public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
-    if (modulePositions.length != m_numModules) {
+    return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions));
+  }
+
+  @Override
+  public Pose2d update(Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions) {
+    if (modulePositions.positions.length != m_numModules) {
       throw new IllegalArgumentException(
           "Number of modules is not consistent with number of wheel locations provided in "
               + "constructor");
     }
-
-    var moduleDeltas = new SwerveModulePosition[m_numModules];
-    for (int index = 0; index < m_numModules; index++) {
-      var current = modulePositions[index];
-      var previous = m_previousModulePositions[index];
-
-      moduleDeltas[index] =
-          new SwerveModulePosition(current.distanceMeters - previous.distanceMeters, current.angle);
-      previous.distanceMeters = current.distanceMeters;
-    }
-
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var twist = m_kinematics.toTwist2d(moduleDeltas);
-    twist.dtheta = angle.minus(m_previousAngle).getRadians();
-
-    var newPose = m_poseMeters.exp(twist);
-
-    m_previousAngle = angle;
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-
-    return m_poseMeters;
+    return super.update(gyroAngle, modulePositions);
   }
 }

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

@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import java.util.Arrays;
+import java.util.Objects;
+import java.util.function.BinaryOperator;
+
+public class SwerveDriveWheelPositions implements WheelPositions<SwerveDriveWheelPositions> {
+  public SwerveModulePosition[] positions;
+
+  /**
+   * Creates a new SwerveDriveWheelPositions instance.
+   *
+   * @param positions The swerve module positions. This will be deeply copied.
+   */
+  public SwerveDriveWheelPositions(SwerveModulePosition[] positions) {
+    this.positions = new SwerveModulePosition[positions.length];
+    for (int i = 0; i < positions.length; i++) {
+      this.positions[i] = positions[i].copy();
+    }
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof SwerveDriveWheelPositions) {
+      SwerveDriveWheelPositions other = (SwerveDriveWheelPositions) obj;
+      return Arrays.equals(this.positions, other.positions);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    // Cast to interpret positions as single argument, not array of the arguments
+    return Objects.hash((Object) positions);
+  }
+
+  @Override
+  public String toString() {
+    return String.format("SwerveDriveWheelPositions(%s)", Arrays.toString(positions));
+  }
+
+  private SwerveDriveWheelPositions generate(
+      SwerveDriveWheelPositions other, BinaryOperator<SwerveModulePosition> generator) {
+    if (other.positions.length != positions.length) {
+      throw new IllegalArgumentException("Inconsistent number of modules!");
+    }
+    var newPositions = new SwerveModulePosition[positions.length];
+    for (int i = 0; i < positions.length; i++) {
+      newPositions[i] = generator.apply(positions[i], other.positions[i]);
+    }
+    return new SwerveDriveWheelPositions(newPositions);
+  }
+
+  @Override
+  public SwerveDriveWheelPositions copy() {
+    return new SwerveDriveWheelPositions(positions);
+  }
+
+  @Override
+  public SwerveDriveWheelPositions minus(SwerveDriveWheelPositions other) {
+    return generate(other, (a, b) -> a.minus(b));
+  }
+
+  @Override
+  public SwerveDriveWheelPositions interpolate(SwerveDriveWheelPositions endValue, double t) {
+    return generate(endValue, (a, b) -> a.interpolate(b, t));
+  }
+}

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

@@ -4,11 +4,14 @@
 
 package edu.wpi.first.math.kinematics;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.interpolation.Interpolatable;
 import java.util.Objects;
 
 /** Represents the state of one swerve module. */
-public class SwerveModulePosition implements Comparable<SwerveModulePosition> {
+public class SwerveModulePosition
+    implements Comparable<SwerveModulePosition>, Interpolatable<SwerveModulePosition> {
   /** Distance measured by the wheel of the module. */
   public double distanceMeters;
 
@@ -60,4 +63,32 @@ public class SwerveModulePosition implements Comparable<SwerveModulePosition> {
     return String.format(
         "SwerveModulePosition(Distance: %.2f m, Angle: %s)", distanceMeters, angle);
   }
+
+  /**
+   * Returns a copy of this swerve module position.
+   *
+   * @return A copy.
+   */
+  public SwerveModulePosition copy() {
+    return new SwerveModulePosition(distanceMeters, angle);
+  }
+
+  /**
+   * Calculates the difference between two swerve module positions. The difference has a length
+   * equal to the difference in lengths and an angle equal to the ending angle (this module
+   * position's angle).
+   *
+   * @param other The swerve module position to subtract.
+   * @return The difference.
+   */
+  public SwerveModulePosition minus(SwerveModulePosition other) {
+    return new SwerveModulePosition(this.distanceMeters - other.distanceMeters, this.angle);
+  }
+
+  @Override
+  public SwerveModulePosition interpolate(SwerveModulePosition endValue, double t) {
+    return new SwerveModulePosition(
+        MathUtil.interpolate(this.distanceMeters, endValue.distanceMeters, t),
+        this.angle.interpolate(endValue.angle, t));
+  }
 }

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

@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.interpolation.Interpolatable;
+
+public interface WheelPositions<T extends WheelPositions<T>> extends Interpolatable<T> {
+  /**
+   * Returns a copy of this instance.
+   *
+   * @return A copy.
+   */
+  T copy();
+
+  /**
+   * Returns the difference with another set of wheel positions.
+   *
+   * @param other The other instance to compare to.
+   * @return The representation of how the wheels moved from other to this.
+   */
+  T minus(T other);
+}