[wpimath] Remove swerve wrappers for odometry and pose estimation, move wheel positions operations to kinematics (#6673)

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

@@ -15,7 +15,6 @@ import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer;
 import edu.wpi.first.math.kinematics.Kinematics;
 import edu.wpi.first.math.kinematics.Odometry;
-import edu.wpi.first.math.kinematics.WheelPositions;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import java.util.Map;
@@ -38,7 +37,7 @@ import java.util.Optional;
  *
  * @param <T> Wheel positions type.
  */
-public class PoseEstimator<T extends WheelPositions<T>> {
+public class PoseEstimator<T> {
   private final Kinematics<?, T> m_kinematics;
   private final Odometry<T> m_odometry;
   private final Matrix<N3, N1> m_q = new Matrix<>(Nat.N3(), Nat.N1());
@@ -150,11 +149,10 @@ public class PoseEstimator<T extends WheelPositions<T>> {
    * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
    * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
    *     don't use your own time source by calling {@link
-   *     PoseEstimator#updateWithTime(double,Rotation2d,WheelPositions)} then you must use a
-   *     timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same
-   *     epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you
-   *     should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or
-   *     sync the epochs.
+   *     PoseEstimator#updateWithTime(double,Rotation2d,Object)} then you must use a timestamp with
+   *     an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as {@link
+   *     edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you should use {@link
+   *     edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or sync the epochs.
    */
   public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
     // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip.
@@ -263,7 +261,8 @@ public class PoseEstimator<T extends WheelPositions<T>> {
     m_odometry.update(gyroAngle, wheelPositions);
     m_poseBuffer.addSample(
         currentTimeSeconds,
-        new InterpolationRecord(getEstimatedPosition(), gyroAngle, wheelPositions.copy()));
+        new InterpolationRecord(
+            getEstimatedPosition(), gyroAngle, m_kinematics.copy(wheelPositions)));
 
     return getEstimatedPosition();
   }
@@ -311,7 +310,7 @@ public class PoseEstimator<T extends WheelPositions<T>> {
         return endValue;
       } else {
         // Find the new wheel distances.
-        var wheelLerp = wheelPositions.interpolate(endValue.wheelPositions, t);
+        var wheelLerp = m_kinematics.interpolate(wheelPositions, endValue.wheelPositions, t);
 
         // Find the new gyro angle.
         var gyroLerp = gyroAngle.interpolate(endValue.gyroAngle, t);

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

@@ -10,7 +10,6 @@ import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
-import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
@@ -25,7 +24,7 @@ import edu.wpi.first.math.numbers.N3;
  * <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
  * want; if you never call it, then this class will behave as regular encoder odometry.
  */
-public class SwerveDrivePoseEstimator extends PoseEstimator<SwerveDriveWheelPositions> {
+public class SwerveDrivePoseEstimator extends PoseEstimator<SwerveModulePosition[]> {
   private final int m_numModules;
 
   /**
@@ -85,52 +84,10 @@ public class SwerveDrivePoseEstimator extends PoseEstimator<SwerveDriveWheelPosi
     m_numModules = modulePositions.length;
   }
 
-  /**
-   * Resets the robot's position on the field.
-   *
-   * <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 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(
-      Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) {
-    resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), poseMeters);
-  }
-
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
-   * loop.
-   *
-   * @param gyroAngle The current gyro angle.
-   * @param modulePositions The current distance measurements and rotations of the swerve modules.
-   * @return The estimated pose of the robot in meters.
-   */
-  public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
-    return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions));
-  }
-
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This should be called every
-   * loop.
-   *
-   * @param currentTimeSeconds Time at which this method was called, in seconds.
-   * @param gyroAngle The current gyroscope angle.
-   * @param modulePositions The current distance measurements and rotations of the swerve modules.
-   * @return The estimated pose of the robot in meters.
-   */
-  public Pose2d updateWithTime(
-      double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
-    return updateWithTime(
-        currentTimeSeconds, gyroAngle, new SwerveDriveWheelPositions(modulePositions));
-  }
-
   @Override
   public Pose2d updateWithTime(
-      double currentTimeSeconds, Rotation2d gyroAngle, SwerveDriveWheelPositions wheelPositions) {
-    if (wheelPositions.positions.length != m_numModules) {
+      double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] wheelPositions) {
+    if (wheelPositions.length != m_numModules) {
       throw new IllegalArgumentException(
           "Number of modules is not consistent with number of wheel locations provided in "
               + "constructor");

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

@@ -8,6 +8,7 @@ import static edu.wpi.first.units.Units.Meters;
 
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.proto.DifferentialDriveKinematicsProto;
 import edu.wpi.first.math.kinematics.struct.DifferentialDriveKinematicsStruct;
@@ -113,4 +114,19 @@ public class DifferentialDriveKinematics
         0,
         (rightDistanceMeters - leftDistanceMeters) / trackWidthMeters);
   }
+
+  @Override
+  public DifferentialDriveWheelPositions copy(DifferentialDriveWheelPositions positions) {
+    return new DifferentialDriveWheelPositions(positions.leftMeters, positions.rightMeters);
+  }
+
+  @Override
+  public DifferentialDriveWheelPositions interpolate(
+      DifferentialDriveWheelPositions startValue,
+      DifferentialDriveWheelPositions endValue,
+      double t) {
+    return new DifferentialDriveWheelPositions(
+        MathUtil.interpolate(startValue.leftMeters, endValue.leftMeters, t),
+        MathUtil.interpolate(startValue.rightMeters, endValue.rightMeters, t));
+  }
 }

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

@@ -5,6 +5,7 @@
 package edu.wpi.first.math.kinematics;
 
 import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.interpolation.Interpolator;
 
 /**
  * Helper class that converts a chassis velocity (dx and dtheta components) into wheel speeds. Robot
@@ -14,7 +15,7 @@ import edu.wpi.first.math.geometry.Twist2d;
  * @param <S> The type of the wheel speeds.
  * @param <P> The type of the wheel positions.
  */
-public interface Kinematics<S, P> {
+public interface Kinematics<S, P> extends Interpolator<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
@@ -44,4 +45,12 @@ public interface Kinematics<S, P> {
    * @return The resulting Twist2d in the robot's movement.
    */
   Twist2d toTwist2d(P start, P end);
+
+  /**
+   * Returns a copy of the wheel positions object.
+   *
+   * @param positions The wheel positions object to copy.
+   * @return A copy.
+   */
+  P copy(P positions);
 }

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

@@ -6,6 +6,7 @@ package edu.wpi.first.math.kinematics;
 
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.proto.MecanumDriveKinematicsProto;
@@ -255,4 +256,23 @@ public class MecanumDriveKinematics
   public Translation2d getRearRight() {
     return m_rearRightWheelMeters;
   }
+
+  @Override
+  public MecanumDriveWheelPositions copy(MecanumDriveWheelPositions positions) {
+    return new MecanumDriveWheelPositions(
+        positions.frontLeftMeters,
+        positions.frontRightMeters,
+        positions.rearLeftMeters,
+        positions.rearRightMeters);
+  }
+
+  @Override
+  public MecanumDriveWheelPositions interpolate(
+      MecanumDriveWheelPositions startValue, MecanumDriveWheelPositions endValue, double t) {
+    return new MecanumDriveWheelPositions(
+        MathUtil.interpolate(startValue.frontLeftMeters, endValue.frontLeftMeters, t),
+        MathUtil.interpolate(startValue.frontRightMeters, endValue.frontRightMeters, t),
+        MathUtil.interpolate(startValue.rearLeftMeters, endValue.rearLeftMeters, t),
+        MathUtil.interpolate(startValue.rearRightMeters, endValue.rearRightMeters, t));
+  }
 }

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

@@ -18,7 +18,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
  *
  * @param <T> Wheel positions type.
  */
-public class Odometry<T extends WheelPositions<T>> {
+public class Odometry<T> {
   private final Kinematics<?, T> m_kinematics;
   private Pose2d m_poseMeters;
 
@@ -43,7 +43,7 @@ public class Odometry<T extends WheelPositions<T>> {
     m_poseMeters = initialPoseMeters;
     m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
     m_previousAngle = m_poseMeters.getRotation();
-    m_previousWheelPositions = wheelPositions.copy();
+    m_previousWheelPositions = m_kinematics.copy(wheelPositions);
   }
 
   /**
@@ -60,7 +60,7 @@ public class Odometry<T extends WheelPositions<T>> {
     m_poseMeters = poseMeters;
     m_previousAngle = m_poseMeters.getRotation();
     m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousWheelPositions = wheelPositions.copy();
+    m_previousWheelPositions = m_kinematics.copy(wheelPositions);
   }
 
   /**
@@ -90,7 +90,7 @@ public class Odometry<T extends WheelPositions<T>> {
 
     var newPose = m_poseMeters.exp(twist);
 
-    m_previousWheelPositions = wheelPositions.copy();
+    m_previousWheelPositions = m_kinematics.copy(wheelPositions);
     m_previousAngle = angle;
     m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
 

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

@@ -39,26 +39,9 @@ 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.
  */
+@SuppressWarnings("overrides")
 public class SwerveDriveKinematics
-    implements Kinematics<SwerveDriveKinematics.SwerveDriveWheelStates, SwerveDriveWheelPositions> {
-  /** Wrapper class for swerve module states. */
-  public static class SwerveDriveWheelStates {
-    /** Swerve module states. */
-    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);
-      }
-    }
-  }
-
+    implements Kinematics<SwerveModuleState[], SwerveModulePosition[]> {
   private final SimpleMatrix m_inverseKinematics;
   private final SimpleMatrix m_forwardKinematics;
 
@@ -200,8 +183,8 @@ public class SwerveDriveKinematics
   }
 
   @Override
-  public SwerveDriveWheelStates toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
-    return new SwerveDriveWheelStates(toSwerveModuleStates(chassisSpeeds));
+  public SwerveModuleState[] toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+    return toSwerveModuleStates(chassisSpeeds);
   }
 
   /**
@@ -214,6 +197,7 @@ public class SwerveDriveKinematics
    *     passed into the constructor of this class.
    * @return The resulting chassis speed.
    */
+  @Override
   public ChassisSpeeds toChassisSpeeds(SwerveModuleState... moduleStates) {
     if (moduleStates.length != m_numModules) {
       throw new IllegalArgumentException(
@@ -235,11 +219,6 @@ 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
@@ -270,17 +249,14 @@ public class SwerveDriveKinematics
   }
 
   @Override
-  public Twist2d toTwist2d(SwerveDriveWheelPositions start, SwerveDriveWheelPositions end) {
-    if (start.positions.length != end.positions.length) {
+  public Twist2d toTwist2d(SwerveModulePosition[] start, SwerveModulePosition[] end) {
+    if (start.length != end.length) {
       throw new IllegalArgumentException("Inconsistent number of modules!");
     }
-    var newPositions = new SwerveModulePosition[start.positions.length];
-    for (int i = 0; i < start.positions.length; i++) {
-      var startModule = start.positions[i];
-      var endModule = end.positions[i];
+    var newPositions = new SwerveModulePosition[start.length];
+    for (int i = 0; i < start.length; i++) {
       newPositions[i] =
-          new SwerveModulePosition(
-              endModule.distanceMeters - startModule.distanceMeters, endModule.angle);
+          new SwerveModulePosition(end[i].distanceMeters - start[i].distanceMeters, end[i].angle);
     }
     return toTwist2d(newPositions);
   }
@@ -406,4 +382,26 @@ public class SwerveDriveKinematics
         attainableMaxTranslationalSpeed.in(MetersPerSecond),
         attainableMaxRotationalVelocity.in(RadiansPerSecond));
   }
+
+  @Override
+  public SwerveModulePosition[] copy(SwerveModulePosition[] positions) {
+    var newPositions = new SwerveModulePosition[positions.length];
+    for (int i = 0; i < positions.length; ++i) {
+      newPositions[i] = positions[i].copy();
+    }
+    return newPositions;
+  }
+
+  @Override
+  public SwerveModulePosition[] interpolate(
+      SwerveModulePosition[] startValue, SwerveModulePosition[] endValue, double t) {
+    if (endValue.length != startValue.length) {
+      throw new IllegalArgumentException("Inconsistent number of modules!");
+    }
+    var newPositions = new SwerveModulePosition[startValue.length];
+    for (int i = 0; i < startValue.length; ++i) {
+      newPositions[i] = startValue[i].interpolate(endValue[i], t);
+    }
+    return newPositions;
+  }
 }

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

@@ -17,7 +17,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 SwerveDriveOdometry extends Odometry<SwerveDriveWheelPositions> {
+public class SwerveDriveOdometry extends Odometry<SwerveModulePosition[]> {
   private final int m_numModules;
 
   /**
@@ -33,7 +33,7 @@ public class SwerveDriveOdometry extends Odometry<SwerveDriveWheelPositions> {
       Rotation2d gyroAngle,
       SwerveModulePosition[] modulePositions,
       Pose2d initialPose) {
-    super(kinematics, gyroAngle, new SwerveDriveWheelPositions(modulePositions), initialPose);
+    super(kinematics, gyroAngle, modulePositions, initialPose);
 
     m_numModules = modulePositions.length;
 
@@ -54,27 +54,10 @@ public class SwerveDriveOdometry extends Odometry<SwerveDriveWheelPositions> {
     this(kinematics, gyroAngle, modulePositions, Pose2d.kZero);
   }
 
-  /**
-   * 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.
-   *
-   * <p>Similarly, module positions do not need to be reset in user code.
-   *
-   * @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.
-   */
-  public void resetPosition(
-      Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) {
-    resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), pose);
-  }
-
   @Override
   public void resetPosition(
-      Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions, Pose2d pose) {
-    if (modulePositions.positions.length != m_numModules) {
+      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 "
               + "constructor");
@@ -82,25 +65,9 @@ public class SwerveDriveOdometry extends Odometry<SwerveDriveWheelPositions> {
     super.resetPosition(gyroAngle, modulePositions, pose);
   }
 
-  /**
-   * Updates the robot's position on the field using forward kinematics and integration of the pose
-   * over time. This method automatically calculates the current time to calculate period
-   * (difference between two timestamps). The period is used to calculate the change in distance
-   * from a velocity. This also takes in an angle parameter which is used instead of the angular
-   * rate that is calculated from forward kinematics.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param modulePositions The current position of all swerve modules. Please provide the positions
-   *     in the same order in which you instantiated your SwerveDriveKinematics.
-   * @return The new pose of the robot.
-   */
-  public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
-    return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions));
-  }
-
   @Override
-  public Pose2d update(Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions) {
-    if (modulePositions.positions.length != m_numModules) {
+  public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) {
+    if (modulePositions.length != m_numModules) {
       throw new IllegalArgumentException(
           "Number of modules is not consistent with number of wheel locations provided in "
               + "constructor");

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

@@ -1,63 +0,0 @@
-// 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;
-
-/** Represents the wheel positions for a swerve drive drivetrain. */
-public class SwerveDriveWheelPositions implements WheelPositions<SwerveDriveWheelPositions> {
-  /** The distances driven by the wheels. */
-  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));
-  }
-
-  @Override
-  public SwerveDriveWheelPositions copy() {
-    return new SwerveDriveWheelPositions(positions);
-  }
-
-  @Override
-  public SwerveDriveWheelPositions interpolate(SwerveDriveWheelPositions endValue, double t) {
-    if (endValue.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] = positions[i].interpolate(endValue.positions[i], t);
-    }
-    return new SwerveDriveWheelPositions(newPositions);
-  }
-}