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

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

@@ -88,6 +88,13 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics
     return ToTwist2d(end.left - start.left, end.right - start.right);
   }
 
+  DifferentialDriveWheelPositions Interpolate(
+      const DifferentialDriveWheelPositions& start,
+      const DifferentialDriveWheelPositions& end, double t) const override {
+    return {wpi::Lerp(start.left, end.left, t),
+            wpi::Lerp(start.right, end.right, t)};
+  }
+
   /// Differential drive trackwidth.
   units::meter_t trackWidth;
 };

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

@@ -20,6 +20,7 @@ namespace frc {
  * forward kinematics converts wheel speeds into chassis speed.
  */
 template <typename WheelSpeeds, typename WheelPositions>
+  requires std::copy_constructible<WheelPositions>
 class WPILIB_DLLEXPORT Kinematics {
  public:
   /**
@@ -58,5 +59,18 @@ class WPILIB_DLLEXPORT Kinematics {
    */
   virtual Twist2d ToTwist2d(const WheelPositions& start,
                             const WheelPositions& end) const = 0;
+
+  /**
+   * Performs interpolation between two values.
+   *
+   * @param start The value to start at.
+   * @param end The value to end at.
+   * @param t How far between the two values to interpolate. This should be
+   * bounded to [0, 1].
+   * @return The interpolated value.
+   */
+  virtual WheelPositions Interpolate(const WheelPositions& start,
+                                     const WheelPositions& end,
+                                     double t) const = 0;
 };
 }  // namespace frc

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

@@ -166,6 +166,15 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics
    */
   const Translation2d& GetRearRight() const { return m_rearRightWheel; }
 
+  MecanumDriveWheelPositions Interpolate(
+      const MecanumDriveWheelPositions& start,
+      const MecanumDriveWheelPositions& end, double t) const override {
+    return {wpi::Lerp(start.frontLeft, end.frontLeft, t),
+            wpi::Lerp(start.frontRight, end.frontRight, t),
+            wpi::Lerp(start.rearLeft, end.rearLeft, t),
+            wpi::Lerp(start.rearRight, end.rearRight, t)};
+  }
+
  private:
   mutable Matrixd<4, 3> m_inverseKinematics;
   Eigen::HouseholderQR<Matrixd<4, 3>> m_forwardKinematics;

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

@@ -24,7 +24,7 @@ namespace frc {
  * @tparam WheelSpeeds Wheel speeds type.
  * @tparam WheelPositions Wheel positions type.
  */
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 class WPILIB_DLLEXPORT Odometry {
  public:
   /**

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

@@ -7,7 +7,7 @@
 #include "frc/kinematics/Odometry.h"
 
 namespace frc {
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 Odometry<WheelSpeeds, WheelPositions>::Odometry(
     const Kinematics<WheelSpeeds, WheelPositions>& kinematics,
     const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
@@ -19,7 +19,7 @@ Odometry<WheelSpeeds, WheelPositions>::Odometry(
   m_gyroOffset = m_pose.Rotation() - gyroAngle;
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 const Pose2d& Odometry<WheelSpeeds, WheelPositions>::Update(
     const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
   auto angle = gyroAngle + m_gyroOffset;

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

@@ -17,7 +17,6 @@
 #include "frc/geometry/Twist2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
 #include "frc/kinematics/Kinematics.h"
-#include "frc/kinematics/SwerveDriveWheelPositions.h"
 #include "frc/kinematics/SwerveModulePosition.h"
 #include "frc/kinematics/SwerveModuleState.h"
 #include "units/velocity.h"
@@ -25,9 +24,6 @@
 
 namespace frc {
 
-template <size_t NumModules>
-using SwerveDriveWheelSpeeds = wpi::array<SwerveModuleState, NumModules>;
-
 /**
  * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
  * into individual module states (speed and angle).
@@ -52,8 +48,8 @@ using SwerveDriveWheelSpeeds = wpi::array<SwerveModuleState, NumModules>;
  */
 template <size_t NumModules>
 class SwerveDriveKinematics
-    : public Kinematics<SwerveDriveWheelSpeeds<NumModules>,
-                        SwerveDriveWheelPositions<NumModules>> {
+    : public Kinematics<wpi::array<SwerveModuleState, NumModules>,
+                        wpi::array<SwerveModulePosition, NumModules>> {
  public:
   /**
    * Constructs a swerve drive kinematics object. This takes in a variable
@@ -157,7 +153,7 @@ class SwerveDriveKinematics
       const ChassisSpeeds& chassisSpeeds,
       const Translation2d& centerOfRotation = Translation2d{}) const;
 
-  SwerveDriveWheelSpeeds<NumModules> ToWheelSpeeds(
+  wpi::array<SwerveModuleState, NumModules> ToWheelSpeeds(
       const ChassisSpeeds& chassisSpeeds) const override {
     return ToSwerveModuleStates(chassisSpeeds);
   }
@@ -234,14 +230,12 @@ class SwerveDriveKinematics
       wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const;
 
   Twist2d ToTwist2d(
-      const SwerveDriveWheelPositions<NumModules>& start,
-      const SwerveDriveWheelPositions<NumModules>& end) const override {
+      const wpi::array<SwerveModulePosition, NumModules>& start,
+      const wpi::array<SwerveModulePosition, 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};
+      result[i] = {end[i].distance - start[i].distance, end[i].angle};
     }
     return ToTwist2d(result);
   }
@@ -293,6 +287,18 @@ class SwerveDriveKinematics
       units::meters_per_second_t attainableMaxRobotTranslationSpeed,
       units::radians_per_second_t attainableMaxRobotRotationSpeed);
 
+  wpi::array<SwerveModulePosition, NumModules> Interpolate(
+      const wpi::array<SwerveModulePosition, NumModules>& start,
+      const wpi::array<SwerveModulePosition, NumModules>& end,
+      double t) const override {
+    auto result =
+        wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
+    for (size_t i = 0; i < NumModules; ++i) {
+      result[i] = start[i].Interpolate(end[i], t);
+    }
+    return {result};
+  }
+
  private:
   mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
   Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;

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

@@ -13,7 +13,6 @@
 
 #include "Odometry.h"
 #include "SwerveDriveKinematics.h"
-#include "SwerveDriveWheelPositions.h"
 #include "SwerveModulePosition.h"
 #include "SwerveModuleState.h"
 #include "frc/geometry/Pose2d.h"
@@ -32,8 +31,8 @@ namespace frc {
  */
 template <size_t NumModules>
 class SwerveDriveOdometry
-    : public Odometry<SwerveDriveWheelSpeeds<NumModules>,
-                      SwerveDriveWheelPositions<NumModules>> {
+    : public Odometry<wpi::array<SwerveModuleState, NumModules>,
+                      wpi::array<SwerveModulePosition, NumModules>> {
  public:
   /**
    * Constructs a SwerveDriveOdometry object.
@@ -48,49 +47,6 @@ class SwerveDriveOdometry
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
       const Pose2d& initialPose = Pose2d{});
 
-  /**
-   * Resets the robot's position on the field.
-   *
-   * 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.
-   *
-   * @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.
-   */
-  void ResetPosition(
-      const Rotation2d& gyroAngle,
-      const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
-      const Pose2d& pose) {
-    Odometry<
-        SwerveDriveWheelSpeeds<NumModules>,
-        SwerveDriveWheelPositions<NumModules>>::ResetPosition(gyroAngle,
-                                                              {modulePositions},
-                                                              pose);
-  }
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. 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.
-   */
-  const Pose2d& Update(
-      const Rotation2d& gyroAngle,
-      const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
-    return Odometry<
-        SwerveDriveWheelSpeeds<NumModules>,
-        SwerveDriveWheelPositions<NumModules>>::Update(gyroAngle,
-                                                       {modulePositions});
-  }
-
  private:
   SwerveDriveKinematics<NumModules> m_kinematicsImpl;
 };

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

@@ -13,9 +13,9 @@ SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
     SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
     const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
     const Pose2d& initialPose)
-    : Odometry<SwerveDriveWheelSpeeds<NumModules>,
-               SwerveDriveWheelPositions<NumModules>>(
-          m_kinematicsImpl, gyroAngle, {modulePositions}, initialPose),
+    : Odometry<wpi::array<SwerveModuleState, NumModules>,
+               wpi::array<SwerveModulePosition, NumModules>>(
+          m_kinematicsImpl, gyroAngle, modulePositions, initialPose),
       m_kinematicsImpl(kinematics) {
   wpi::math::MathSharedStore::ReportUsage(
       wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);

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

@@ -1,51 +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.
-
-#pragma once
-
-#include <wpi/MathExtras.h>
-#include <wpi/SymbolExports.h>
-#include <wpi/array.h>
-
-#include "frc/kinematics/SwerveModulePosition.h"
-
-namespace frc {
-/**
- * Represents the wheel positions for a swerve drive drivetrain.
- */
-template <size_t NumModules>
-struct WPILIB_DLLEXPORT SwerveDriveWheelPositions {
-  /**
-   * The distances driven by the wheels.
-   */
-  wpi::array<SwerveModulePosition, NumModules> positions;
-
-  /**
-   * Checks equality between this SwerveDriveWheelPositions and another object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are equal.
-   */
-  bool operator==(const SwerveDriveWheelPositions& other) const = default;
-
-  /**
-   * Checks inequality between this SwerveDriveWheelPositions and another
-   * object.
-   *
-   * @param other The other object.
-   * @return Whether the two objects are not equal.
-   */
-  bool operator!=(const SwerveDriveWheelPositions& other) const = default;
-
-  SwerveDriveWheelPositions<NumModules> Interpolate(
-      const SwerveDriveWheelPositions<NumModules>& endValue, double t) const {
-    auto result =
-        wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
-    for (size_t i = 0; i < NumModules; i++) {
-      result[i] = positions[i].Interpolate(endValue.positions[i], t);
-    }
-    return {result};
-  }
-};
-}  // namespace frc