[wpimath] Change kinematics.ToTwist2d(end - start) to kinematics.ToTwist2d(start, end) (#5493)

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

@@ -300,14 +300,11 @@ public class PoseEstimator<T extends WheelPositions<T>> {
         // Find the new wheel distances.
         var wheelLerp = wheelPositions.interpolate(endValue.wheelPositions, t);
 
-        // Find the distance travelled between this measurement and the interpolated measurement.
-        var wheelDelta = wheelLerp.minus(wheelPositions);
-
         // Find the new gyro angle.
         var gyroLerp = gyroAngle.interpolate(endValue.gyroAngle, t);
 
-        // Create a twist to represent this change based on the interpolated sensor inputs.
-        Twist2d twist = m_kinematics.toTwist2d(wheelDelta);
+        // Create a twist to represent the change based on the interpolated sensor inputs.
+        Twist2d twist = m_kinematics.toTwist2d(wheelPositions, wheelLerp);
         twist.dtheta = gyroLerp.minus(gyroAngle).getRadians();
 
         return new InterpolationRecord(poseMeters.exp(twist), gyroLerp, wheelLerp);

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

@@ -63,8 +63,9 @@ public class DifferentialDriveKinematics
   }
 
   @Override
-  public Twist2d toTwist2d(DifferentialDriveWheelPositions wheelDeltas) {
-    return toTwist2d(wheelDeltas.leftMeters, wheelDeltas.rightMeters);
+  public Twist2d toTwist2d(
+      DifferentialDriveWheelPositions start, DifferentialDriveWheelPositions end) {
+    return toTwist2d(end.leftMeters - start.leftMeters, end.rightMeters - start.rightMeters);
   }
 
   /**

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

@@ -52,12 +52,6 @@ public class DifferentialDriveWheelPositions
     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) {

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

@@ -35,12 +35,13 @@ public interface Kinematics<S, P> {
   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.
+   * Performs forward kinematics to return the resulting Twist2d from the given change in wheel
+   * positions. 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.
+   * @param start The starting distances driven by the wheels.
+   * @param end The ending distances driven by the wheels.
    * @return The resulting Twist2d in the robot's movement.
    */
-  Twist2d toTwist2d(P wheelDeltas);
+  Twist2d toTwist2d(P start, P end);
 }

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

@@ -158,6 +158,27 @@ public class MecanumDriveKinematics
   }
 
   @Override
+  public Twist2d toTwist2d(MecanumDriveWheelPositions start, MecanumDriveWheelPositions end) {
+    var wheelDeltasVector = new SimpleMatrix(4, 1);
+    wheelDeltasVector.setColumn(
+        0,
+        0,
+        end.frontLeftMeters - start.frontLeftMeters,
+        end.frontRightMeters - start.frontRightMeters,
+        end.rearLeftMeters - start.rearLeftMeters,
+        end.rearRightMeters - start.rearRightMeters);
+    var twist = m_forwardKinematics.mult(wheelDeltasVector);
+    return new Twist2d(twist.get(0, 0), twist.get(1, 0), twist.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(
@@ -168,7 +189,6 @@ public class MecanumDriveKinematics
         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));
   }
 

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

@@ -73,15 +73,6 @@ public class MecanumDriveWheelPositions implements WheelPositions<MecanumDriveWh
         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(

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

@@ -81,11 +81,9 @@ public class Odometry<T extends WheelPositions<T>> {
    * @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);
+    var twist = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions);
     twist.dtheta = angle.minus(m_previousAngle).getRadians();
 
     var newPose = m_poseMeters.exp(twist);

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

@@ -261,8 +261,19 @@ public class SwerveDriveKinematics
   }
 
   @Override
-  public Twist2d toTwist2d(SwerveDriveWheelPositions wheelDeltas) {
-    return toTwist2d(wheelDeltas.positions);
+  public Twist2d toTwist2d(SwerveDriveWheelPositions start, SwerveDriveWheelPositions end) {
+    if (start.positions.length != end.positions.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];
+      newPositions[i] =
+          new SwerveModulePosition(
+              endModule.distanceMeters - startModule.distanceMeters, endModule.angle);
+    }
+    return toTwist2d(newPositions);
   }
 
   /**

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

@@ -6,7 +6,6 @@ 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;
@@ -43,30 +42,20 @@ public class SwerveDriveWheelPositions implements WheelPositions<SwerveDriveWhee
     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));
+    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);
   }
 }

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

@@ -73,19 +73,6 @@ public class SwerveModulePosition
     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). This is suitable for representing a module's motion between two timesteps,
-   * because the final angle describes the direction the module moved.
-   *
-   * @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(

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

@@ -13,12 +13,4 @@ public interface WheelPositions<T extends WheelPositions<T>> extends Interpolata
    * @return A copy.
    */
   T copy();
-
-  /**
-   * Returns a representation of how the wheels moved from other to this.
-   *
-   * @param other The other instance to compare to.
-   * @return The representation of how the wheels moved from other to this.
-   */
-  T minus(T other);
 }

wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp

@@ -49,6 +49,21 @@ ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
           units::radians_per_second_t{chassisSpeedsVector(2)}};
 }
 
+Twist2d MecanumDriveKinematics::ToTwist2d(
+    const MecanumDriveWheelPositions& start,
+    const MecanumDriveWheelPositions& end) const {
+  Vectord<4> wheelDeltasVector{
+      end.frontLeft.value() - start.frontLeft.value(),
+      end.frontRight.value() - start.frontRight.value(),
+      end.rearLeft.value() - start.rearLeft.value(),
+      end.rearRight.value() - start.rearRight.value()};
+
+  Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
+
+  return {units::meter_t{twistVector(0)}, units::meter_t{twistVector(1)},
+          units::radian_t{twistVector(2)}};
+}
+
 Twist2d MecanumDriveKinematics::ToTwist2d(
     const MecanumDriveWheelPositions& wheelDeltas) const {
   Vectord<4> wheelDeltasVector{
@@ -57,8 +72,8 @@ Twist2d MecanumDriveKinematics::ToTwist2d(
 
   Eigen::Vector3d twistVector = m_forwardKinematics.solve(wheelDeltasVector);
 
-  return {units::meter_t{twistVector(0)},  // NOLINT
-          units::meter_t{twistVector(1)}, units::radian_t{twistVector(2)}};
+  return {units::meter_t{twistVector(0)}, units::meter_t{twistVector(1)},
+          units::radian_t{twistVector(2)}};
 }
 
 void MecanumDriveKinematics::SetInverseKinematics(Translation2d fl,

wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc

@@ -128,7 +128,6 @@ Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::UpdateWithTime(
     const WheelPositions& wheelPositions) {
   m_odometry.Update(gyroAngle, wheelPositions);
 
-  // Copy?
   WheelPositions internalWheelPositions = wheelPositions;
 
   m_poseBuffer.AddSample(
@@ -151,16 +150,12 @@ PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord::Interpolate(
     WheelPositions wheels_lerp =
         this->wheelPositions.Interpolate(endValue.wheelPositions, i);
 
-    // Find the distance between this measurement and the
-    // interpolated measurement.
-    WheelPositions wheels_delta = wheels_lerp - this->wheelPositions;
-
     // Find the new gyro angle.
     auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);
 
-    // Create a twist to represent this changed based on the interpolated
+    // Create a twist to represent the change based on the interpolated
     // sensor inputs.
-    auto twist = kinematics.ToTwist2d(wheels_delta);
+    auto twist = kinematics.ToTwist2d(this->wheelPositions, wheels_lerp);
     twist.dtheta = (gyro - gyroAngle).Radians();
 
     return {pose.Exp(twist), gyro, wheels_lerp};

wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h

@@ -83,9 +83,9 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics
             (rightDistance - leftDistance) / trackWidth * 1_rad};
   }
 
-  Twist2d ToTwist2d(
-      const DifferentialDriveWheelPositions& wheelDeltas) const override {
-    return ToTwist2d(wheelDeltas.left, wheelDeltas.right);
+  Twist2d ToTwist2d(const DifferentialDriveWheelPositions& start,
+                    const DifferentialDriveWheelPositions& end) const override {
+    return ToTwist2d(end.left - start.left, end.right - start.right);
   }
 
   units::meter_t trackWidth;

wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h

@@ -42,11 +42,6 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions {
    */
   bool operator!=(const DifferentialDriveWheelPositions& other) const = default;
 
-  DifferentialDriveWheelPositions operator-(
-      const DifferentialDriveWheelPositions& other) const {
-    return {left - other.left, right - other.right};
-  }
-
   DifferentialDriveWheelPositions Interpolate(
       const DifferentialDriveWheelPositions& endValue, double t) const {
     return {wpi::Lerp(left, endValue.left, t),

wpimath/src/main/native/include/frc/kinematics/Kinematics.h

@@ -47,14 +47,16 @@ class WPILIB_DLLEXPORT Kinematics {
 
   /**
    * 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.
+   * change in wheel positions. 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.
+   * @param start The starting distances driven by the wheels.
+   * @param end The ending distances driven by the wheels.
    *
    * @return The resulting Twist2d in the robot's movement.
    */
-  virtual Twist2d ToTwist2d(const WheelPositions& wheelDeltas) const = 0;
+  virtual Twist2d ToTwist2d(const WheelPositions& start,
+                            const WheelPositions& end) const = 0;
 };
 }  // namespace frc

wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h

@@ -123,6 +123,9 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics
   ChassisSpeeds ToChassisSpeeds(
       const MecanumDriveWheelSpeeds& wheelSpeeds) const override;
 
+  Twist2d ToTwist2d(const MecanumDriveWheelPositions& start,
+                    const MecanumDriveWheelPositions& end) const override;
+
   /**
    * Performs forward kinematics to return the resulting Twist2d from the
    * given wheel position deltas. This method is often used for odometry --
@@ -133,8 +136,7 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics
    *
    * @return The resulting chassis speed.
    */
-  Twist2d ToTwist2d(
-      const MecanumDriveWheelPositions& wheelDeltas) const override;
+  Twist2d ToTwist2d(const MecanumDriveWheelPositions& wheelDeltas) const;
 
  private:
   mutable Matrixd<4, 3> m_inverseKinematics;

wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h

@@ -51,12 +51,6 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
    */
   bool operator!=(const MecanumDriveWheelPositions& other) const = default;
 
-  MecanumDriveWheelPositions operator-(
-      const MecanumDriveWheelPositions& other) const {
-    return {frontLeft - other.frontLeft, frontRight - other.frontRight,
-            rearLeft - other.rearLeft, rearRight - other.rearRight};
-  }
-
   MecanumDriveWheelPositions Interpolate(
       const MecanumDriveWheelPositions& endValue, double t) const {
     return {wpi::Lerp(frontLeft, endValue.frontLeft, t),

wpimath/src/main/native/include/frc/kinematics/Odometry.inc

@@ -24,9 +24,7 @@ const Pose2d& Odometry<WheelSpeeds, WheelPositions>::Update(
     const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
   auto angle = gyroAngle + m_gyroOffset;
 
-  auto wheelDeltas = wheelPositions - m_previousWheelPositions;
-
-  auto twist = m_kinematics.ToTwist2d(wheelDeltas);
+  auto twist = m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions);
   twist.dtheta = (angle - m_previousAngle).Radians();
 
   auto newPose = m_pose.Exp(twist);

wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h

@@ -234,8 +234,16 @@ class SwerveDriveKinematics
       wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const;
 
   Twist2d ToTwist2d(
-      const SwerveDriveWheelPositions<NumModules>& wheelDeltas) const override {
-    return ToTwist2d(wheelDeltas.positions);
+      const SwerveDriveWheelPositions<NumModules>& start,
+      const SwerveDriveWheelPositions<NumModules>& end) const override {
+    auto result =
+        wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
+    for (size_t i = 0; i < NumModules; i++) {
+      auto startModule = start.positions[i];
+      auto endModule = end.positions[i];
+      result[i] = {endModule.distance - startModule.distance, endModule.angle};
+    }
+    return ToTwist2d(result);
   }
 
   /**

wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h

@@ -38,16 +38,6 @@ struct WPILIB_DLLEXPORT SwerveDriveWheelPositions {
    */
   bool operator!=(const SwerveDriveWheelPositions& other) const = default;
 
-  SwerveDriveWheelPositions<NumModules> operator-(
-      const SwerveDriveWheelPositions<NumModules>& other) const {
-    auto result =
-        wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
-    for (size_t i = 0; i < NumModules; i++) {
-      result[i] = positions[i] - other.positions[i];
-    }
-    return {result};
-  }
-
   SwerveDriveWheelPositions<NumModules> Interpolate(
       const SwerveDriveWheelPositions<NumModules>& endValue, double t) const {
     auto result =

wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h

@@ -35,20 +35,6 @@ struct WPILIB_DLLEXPORT SwerveModulePosition {
    */
   bool operator==(const SwerveModulePosition& other) const;
 
-  /**
-   * 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). This is suitable
-   * for representing a module's motion between two timesteps, because the final
-   * angle describes the direction the module moved.
-   *
-   * @param other The other swerve module position to subtract.
-   * @return The difference.
-   */
-  SwerveModulePosition operator-(const SwerveModulePosition& other) const {
-    return {distance - other.distance, angle};
-  }
-
   SwerveModulePosition Interpolate(const SwerveModulePosition& endValue,
                                    double t) const {
     return {wpi::Lerp(distance, endValue.distance, t),

wpimath/src/main/native/include/frc/kinematics/WheelPositions.h

@@ -10,7 +10,6 @@ namespace frc {
 template <typename T>
 concept WheelPositions =
     std::copy_constructible<T> && requires(T a, T b, double t) {
-      { a - b } -> std::convertible_to<T>;
       { a.Interpolate(b, t) } -> std::convertible_to<T>;
     };
 }  // namespace frc