[wpimath] Position Delta Odometry for Mecanum (#4514)

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

@@ -7,6 +7,7 @@ package edu.wpi.first.math.kinematics;
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Twist2d;
 import org.ejml.simple.SimpleMatrix;
 
 /**
@@ -153,6 +154,28 @@ 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.
+   */
+  public Twist2d toTwist2d(MecanumDriveWheelPositions wheelDeltas) {
+    var wheelDeltasVector = new SimpleMatrix(4, 1);
+    wheelDeltasVector.setColumn(
+        0,
+        0,
+        wheelDeltas.frontLeftMeters,
+        wheelDeltas.frontRightMeters,
+        wheelDeltas.rearLeftMeters,
+        wheelDeltas.rearRightMeters);
+    var twist = m_forwardKinematics.mult(wheelDeltasVector);
+
+    return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.get(2, 0));
+  }
+
   /**
    * Construct inverse kinematics matrix from wheel locations.
    *

wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.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 mecanum drive odometry. Odometry allows you to track the robot's position on the field
@@ -21,7 +19,7 @@ import edu.wpi.first.util.WPIUtilJNI;
 public class MecanumDriveOdometry {
   private final MecanumDriveKinematics m_kinematics;
   private Pose2d m_poseMeters;
-  private double m_prevTimeSeconds = -1;
+  private MecanumDriveWheelPositions m_previousWheelPositions;
 
   private Rotation2d m_gyroOffset;
   private Rotation2d m_previousAngle;
@@ -31,14 +29,24 @@ public class MecanumDriveOdometry {
    *
    * @param kinematics The mecanum drive kinematics for your drivetrain.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The distances driven by each wheel.
    * @param initialPoseMeters The starting position of the robot on the field.
    */
   public MecanumDriveOdometry(
-      MecanumDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPoseMeters) {
+      MecanumDriveKinematics kinematics,
+      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);
     MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
   }
 
@@ -47,9 +55,13 @@ public class MecanumDriveOdometry {
    *
    * @param kinematics The mecanum drive kinematics for your drivetrain.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The distances driven by each wheel.
    */
-  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
-    this(kinematics, gyroAngle, new Pose2d());
+  public MecanumDriveOdometry(
+      MecanumDriveKinematics kinematics,
+      Rotation2d gyroAngle,
+      MecanumDriveWheelPositions wheelPositions) {
+    this(kinematics, gyroAngle, wheelPositions, new Pose2d());
   }
 
   /**
@@ -60,11 +72,19 @@ public class MecanumDriveOdometry {
    *
    * @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.
    */
-  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+  public void resetPosition(
+      Pose2d poseMeters, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
     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);
   }
 
   /**
@@ -78,48 +98,37 @@ public class MecanumDriveOdometry {
 
   /**
    * 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.
+   * 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 currentTimeSeconds The current time in seconds.
    * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelSpeeds The current wheel speeds.
+   * @param wheelPositions The distances driven by each wheel.
    * @return The new pose of the robot.
    */
-  public Pose2d updateWithTime(
-      double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
-    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
-    m_prevTimeSeconds = currentTimeSeconds;
-
+  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) {
     var angle = gyroAngle.plus(m_gyroOffset);
 
-    var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
-    var newPose =
-        m_poseMeters.exp(
-            new Twist2d(
-                chassisState.vxMetersPerSecond * period,
-                chassisState.vyMetersPerSecond * period,
-                angle.minus(m_previousAngle).getRadians()));
+    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;
   }
-
-  /**
-   * 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 wheelSpeeds The current wheel speeds.
-   * @return The new pose of the robot.
-   */
-  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds);
-  }
 }

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

@@ -0,0 +1,49 @@
+// 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;
+
+public class MecanumDriveWheelPositions {
+  /** Distance measured by the front left wheel. */
+  public double frontLeftMeters;
+
+  /** Distance measured by the front right wheel. */
+  public double frontRightMeters;
+
+  /** Distance measured by the rear left wheel. */
+  public double rearLeftMeters;
+
+  /** Distance measured by the rear right wheel. */
+  public double rearRightMeters;
+
+  /** Constructs a MecanumDriveWheelPositions with zeros for all member fields. */
+  public MecanumDriveWheelPositions() {}
+
+  /**
+   * Constructs a MecanumDriveWheelPositions.
+   *
+   * @param frontLeftMeters Distance measured by the front left wheel.
+   * @param frontRightMeters Distance measured by the front right wheel.
+   * @param rearLeftMeters Distance measured by the rear left wheel.
+   * @param rearRightMeters Distance measured by the rear right wheel.
+   */
+  public MecanumDriveWheelPositions(
+      double frontLeftMeters,
+      double frontRightMeters,
+      double rearLeftMeters,
+      double rearRightMeters) {
+    this.frontLeftMeters = frontLeftMeters;
+    this.frontRightMeters = frontRightMeters;
+    this.rearLeftMeters = rearLeftMeters;
+    this.rearRightMeters = rearRightMeters;
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "MecanumDriveWheelPositions(Front Left: %.2f m, Front Right: %.2f m, "
+            + "Rear Left: %.2f m, Rear Right: %.2f m)",
+        frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
+  }
+}