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

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