[wpimath] Position Delta Odometry for Swerve (#4493)

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

@@ -8,6 +8,7 @@ import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
 import java.util.Arrays;
 import java.util.Collections;
 import org.ejml.simple.SimpleMatrix;
@@ -185,6 +186,35 @@ public class SwerveDriveKinematics {
         chassisSpeedsVector.get(2, 0));
   }
 
+  /**
+   * 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
+   * data from the real-world speed and angle of each module on the robot.
+   *
+   * @param wheelDeltas The latest change in position of the modules (as a SwerveModulePosition
+   *     type) as measured from respective encoders and gyros. The order of the swerve module states
+   *     should be same as passed into the constructor of this class.
+   * @return The resulting Twist2d.
+   */
+  public Twist2d toTwist2d(SwerveModulePosition... wheelDeltas) {
+    if (wheelDeltas.length != m_numModules) {
+      throw new IllegalArgumentException(
+          "Number of modules is not consistent with number of wheel locations provided in "
+              + "constructor");
+    }
+    var moduleDeltaMatrix = new SimpleMatrix(m_numModules * 2, 1);
+
+    for (int i = 0; i < m_numModules; i++) {
+      var module = wheelDeltas[i];
+      moduleDeltaMatrix.set(i * 2, 0, module.distanceMeters * module.angle.getCos());
+      moduleDeltaMatrix.set(i * 2 + 1, module.distanceMeters * module.angle.getSin());
+    }
+
+    var chassisDeltaVector = m_forwardKinematics.mult(moduleDeltaMatrix);
+    return new Twist2d(
+        chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0));
+  }
+
   /**
    * Renormalizes the wheel speeds if any individual speed is above the specified maximum.
    *

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

@@ -8,8 +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;
-import edu.wpi.first.util.WPIUtilJNI;
 
 /**
  * Class for swerve drive odometry. Odometry allows you to track the robot's position on the field
@@ -22,24 +20,38 @@ import edu.wpi.first.util.WPIUtilJNI;
 public class SwerveDriveOdometry {
   private final SwerveDriveKinematics m_kinematics;
   private Pose2d m_poseMeters;
-  private double m_prevTimeSeconds = -1;
 
   private Rotation2d m_gyroOffset;
   private Rotation2d m_previousAngle;
+  private final int m_numModules;
+  private SwerveModulePosition[] m_previousModulePositions;
 
   /**
    * Constructs a SwerveDriveOdometry object.
    *
    * @param kinematics The swerve drive kinematics for your drivetrain.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param modulePositions The wheel positions reported by each module.
    * @param initialPose The starting position of the robot on the field.
    */
   public SwerveDriveOdometry(
-      SwerveDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPose) {
+      SwerveDriveKinematics kinematics,
+      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;
+
+    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);
+    }
+
     MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
   }
 
@@ -48,9 +60,13 @@ public class SwerveDriveOdometry {
    *
    * @param kinematics The swerve drive kinematics for your drivetrain.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param modulePositions The wheel positions reported by each module.
    */
-  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
-    this(kinematics, gyroAngle, new Pose2d());
+  public SwerveDriveOdometry(
+      SwerveDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      SwerveModulePosition... modulePositions) {
+    this(kinematics, gyroAngle, modulePositions, new Pose2d());
   }
 
   /**
@@ -61,11 +77,24 @@ public class SwerveDriveOdometry {
    *
    * @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.
    */
-  public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
+  public void resetPosition(
+      Pose2d pose, 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");
+    }
+
     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);
+    }
   }
 
   /**
@@ -77,40 +106,6 @@ public class SwerveDriveOdometry {
     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 the current time as a parameter 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 currentTimeSeconds The current time in seconds.
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param moduleStates The current state of all swerve modules. Please provide the states in the
-   *     same order in which you instantiated your SwerveDriveKinematics.
-   * @return The new pose of the robot.
-   */
-  public Pose2d updateWithTime(
-      double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
-    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
-    m_prevTimeSeconds = currentTimeSeconds;
-
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
-    var newPose =
-        m_poseMeters.exp(
-            new Twist2d(
-                chassisState.vxMetersPerSecond * period,
-                chassisState.vyMetersPerSecond * period,
-                angle.minus(m_previousAngle).getRadians()));
-
-    m_previousAngle = angle;
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-
-    return m_poseMeters;
-  }
-
   /**
    * 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
@@ -119,11 +114,40 @@ public class SwerveDriveOdometry {
    * rate that is calculated from forward kinematics.
    *
    * @param gyroAngle The angle reported by the gyroscope.
-   * @param moduleStates The current state of all swerve modules. Please provide the states in the
-   *     same order in which you instantiated your SwerveDriveKinematics.
+   * @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, SwerveModuleState... moduleStates) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
+  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");
+    }
+
+    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);
+    }
+
+    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);
+    for (int index = 0; index < m_numModules; index++) {
+      m_previousModulePositions[index] =
+          new SwerveModulePosition(
+              modulePositions[index].distanceMeters, modulePositions[index].angle);
+    }
+    return m_poseMeters;
   }
 }

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

@@ -0,0 +1,62 @@
+// 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.Rotation2d;
+import java.util.Objects;
+
+/** Represents the state of one swerve module. */
+public class SwerveModulePosition implements Comparable<SwerveModulePosition> {
+  /** Distance measured by the wheel of the module. */
+  public double distanceMeters;
+
+  /** Angle of the module. */
+  public Rotation2d angle = Rotation2d.fromDegrees(0);
+
+  /** Constructs a SwerveModulePosition with zeros for speed and angle. */
+  public SwerveModulePosition() {}
+
+  /**
+   * Constructs a SwerveModulePosition.
+   *
+   * @param distanceMeters The distance measured by the wheel of the module.
+   * @param angle The angle of the module.
+   */
+  public SwerveModulePosition(double distanceMeters, Rotation2d angle) {
+    this.distanceMeters = distanceMeters;
+    this.angle = angle;
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof SwerveModulePosition) {
+      return Double.compare(distanceMeters, ((SwerveModulePosition) obj).distanceMeters) == 0;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(distanceMeters);
+  }
+
+  /**
+   * Compares two swerve module positions. One swerve module is "greater" than the other if its
+   * speed is higher than the other.
+   *
+   * @param other The other swerve module.
+   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
+   */
+  @Override
+  public int compareTo(SwerveModulePosition other) {
+    return Double.compare(this.distanceMeters, other.distanceMeters);
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "SwerveModulePosition(Distance: %.2f m/s, Angle: %s)", distanceMeters, angle);
+  }
+}