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

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

@@ -36,7 +36,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 PoseEstimator {
  public:
   /**

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

@@ -8,7 +8,7 @@
 
 namespace frc {
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 PoseEstimator<WheelSpeeds, WheelPositions>::PoseEstimator(
     Kinematics<WheelSpeeds, WheelPositions>& kinematics,
     Odometry<WheelSpeeds, WheelPositions>& odometry,
@@ -22,7 +22,7 @@ PoseEstimator<WheelSpeeds, WheelPositions>::PoseEstimator(
   SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 void PoseEstimator<WheelSpeeds, WheelPositions>::SetVisionMeasurementStdDevs(
     const wpi::array<double, 3>& visionMeasurementStdDevs) {
   wpi::array<double, 3> r{wpi::empty_array};
@@ -42,7 +42,7 @@ void PoseEstimator<WheelSpeeds, WheelPositions>::SetVisionMeasurementStdDevs(
   }
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPosition(
     const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
     const Pose2d& pose) {
@@ -51,13 +51,13 @@ void PoseEstimator<WheelSpeeds, WheelPositions>::ResetPosition(
   m_poseBuffer.Clear();
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::GetEstimatedPosition()
     const {
   return m_odometry.GetPose();
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 std::optional<Pose2d> PoseEstimator<WheelSpeeds, WheelPositions>::SampleAt(
     units::second_t timestamp) const {
   // TODO Replace with std::optional::transform() in C++23
@@ -70,7 +70,7 @@ std::optional<Pose2d> PoseEstimator<WheelSpeeds, WheelPositions>::SampleAt(
   }
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
     const Pose2d& visionRobotPose, units::second_t timestamp) {
   // Step 0: If this measurement is old enough to be outside the pose buffer's
@@ -128,14 +128,14 @@ void PoseEstimator<WheelSpeeds, WheelPositions>::AddVisionMeasurement(
   }
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::Update(
     const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) {
   return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle,
                         wheelPositions);
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::UpdateWithTime(
     units::second_t currentTime, const Rotation2d& gyroAngle,
     const WheelPositions& wheelPositions) {
@@ -149,7 +149,7 @@ Pose2d PoseEstimator<WheelSpeeds, WheelPositions>::UpdateWithTime(
   return GetEstimatedPosition();
 }
 
-template <typename WheelSpeeds, WheelPositions WheelPositions>
+template <typename WheelSpeeds, typename WheelPositions>
 typename PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord
 PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord::Interpolate(
     Kinematics<WheelSpeeds, WheelPositions>& kinematics,
@@ -160,8 +160,8 @@ PoseEstimator<WheelSpeeds, WheelPositions>::InterpolationRecord::Interpolate(
     return endValue;
   } else {
     // Find the new wheel distance measurements.
-    WheelPositions wheels_lerp =
-        this->wheelPositions.Interpolate(endValue.wheelPositions, i);
+    WheelPositions wheels_lerp = kinematics.Interpolate(
+        this->wheelPositions, endValue.wheelPositions, i);
 
     // Find the new gyro angle.
     auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i);

wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h

@@ -14,7 +14,6 @@
 #include "frc/geometry/Rotation2d.h"
 #include "frc/kinematics/SwerveDriveKinematics.h"
 #include "frc/kinematics/SwerveDriveOdometry.h"
-#include "frc/kinematics/SwerveDriveWheelPositions.h"
 #include "units/time.h"
 
 namespace frc {
@@ -32,8 +31,8 @@ namespace frc {
  */
 template <size_t NumModules>
 class SwerveDrivePoseEstimator
-    : public PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
-                           SwerveDriveWheelPositions<NumModules>> {
+    : public PoseEstimator<wpi::array<SwerveModuleState, NumModules>,
+                           wpi::array<SwerveModulePosition, NumModules>> {
  public:
   /**
    * Constructs a SwerveDrivePoseEstimator with default standard deviations
@@ -83,69 +82,11 @@ class SwerveDrivePoseEstimator
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
       const Pose2d& initialPose, const wpi::array<double, 3>& stateStdDevs,
       const wpi::array<double, 3>& visionMeasurementStdDevs)
-      : PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
-                      SwerveDriveWheelPositions<NumModules>>(
+      : PoseEstimator<wpi::array<SwerveModuleState, NumModules>,
+                      wpi::array<SwerveModulePosition, NumModules>>(
             kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs),
         m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {}
 
-  /**
-   * Resets the robot's position on the field.
-   *
-   * The gyroscope angle does not need to be reset in 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 current distance and rotation measurements of
-   *                        the swerve modules.
-   * @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) {
-    PoseEstimator<
-        SwerveDriveWheelSpeeds<NumModules>,
-        SwerveDriveWheelPositions<NumModules>>::ResetPosition(gyroAngle,
-                                                              {modulePositions},
-                                                              pose);
-  }
-
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This
-   * should be called every loop.
-   *
-   * @param gyroAngle       The current gyro angle.
-   * @param modulePositions The current distance and rotation measurements of
-   *                        the swerve modules.
-   * @return The estimated robot pose in meters.
-   */
-  Pose2d Update(
-      const Rotation2d& gyroAngle,
-      const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
-    return PoseEstimator<
-        SwerveDriveWheelSpeeds<NumModules>,
-        SwerveDriveWheelPositions<NumModules>>::Update(gyroAngle,
-                                                       {modulePositions});
-  }
-
-  /**
-   * Updates the pose estimator with wheel encoder and gyro information. This
-   * should be called every loop.
-   *
-   * @param currentTime     Time at which this method was called, in seconds.
-   * @param gyroAngle       The current gyro angle.
-   * @param modulePositions The current distance traveled and rotations of
-   *                        the swerve modules.
-   * @return The estimated robot pose in meters.
-   */
-  Pose2d UpdateWithTime(
-      units::second_t currentTime, const Rotation2d& gyroAngle,
-      const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
-    return PoseEstimator<SwerveDriveWheelSpeeds<NumModules>,
-                         SwerveDriveWheelPositions<NumModules>>::
-        UpdateWithTime(currentTime, gyroAngle, {modulePositions});
-  }
-
  private:
   SwerveDriveOdometry<NumModules> m_odometryImpl;
 };

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