[wpimath] Refactor kinematics, odometry, and pose estimator (#5355)

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

@@ -8,7 +8,9 @@
 
 #include "frc/geometry/Twist2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
 #include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "frc/kinematics/Kinematics.h"
 #include "units/angle.h"
 #include "units/length.h"
 #include "wpimath/MathShared.h"
@@ -22,7 +24,9 @@ namespace frc {
  * velocity components whereas forward kinematics converts left and right
  * component velocities into a linear and angular chassis speed.
  */
-class WPILIB_DLLEXPORT DifferentialDriveKinematics {
+class WPILIB_DLLEXPORT DifferentialDriveKinematics
+    : public Kinematics<DifferentialDriveWheelSpeeds,
+                        DifferentialDriveWheelPositions> {
  public:
   /**
    * Constructs a differential drive kinematics object.
@@ -46,7 +50,7 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics {
    * @return The chassis speed.
    */
   constexpr ChassisSpeeds ToChassisSpeeds(
-      const DifferentialDriveWheelSpeeds& wheelSpeeds) const {
+      const DifferentialDriveWheelSpeeds& wheelSpeeds) const override {
     return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps,
             (wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad};
   }
@@ -60,7 +64,7 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics {
    * @return The left and right velocities.
    */
   constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds(
-      const ChassisSpeeds& chassisSpeeds) const {
+      const ChassisSpeeds& chassisSpeeds) const override {
     return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad,
             chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad};
   }
@@ -79,6 +83,11 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics {
             (rightDistance - leftDistance) / trackWidth * 1_rad};
   }
 
+  Twist2d ToTwist2d(
+      const DifferentialDriveWheelPositions& wheelDeltas) const override {
+    return ToTwist2d(wheelDeltas.left, wheelDeltas.right);
+  }
+
   units::meter_t trackWidth;
 };
 }  // namespace frc

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

@@ -7,6 +7,10 @@
 #include <wpi/SymbolExports.h>
 
 #include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/DifferentialDriveWheelPositions.h"
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "frc/kinematics/Odometry.h"
 #include "units/length.h"
 
 namespace frc {
@@ -22,7 +26,9 @@ namespace frc {
  * It is important that you reset your encoders to zero before using this class.
  * Any subsequent pose resets also require the encoders to be reset to zero.
  */
-class WPILIB_DLLEXPORT DifferentialDriveOdometry {
+class WPILIB_DLLEXPORT DifferentialDriveOdometry
+    : public Odometry<DifferentialDriveWheelSpeeds,
+                      DifferentialDriveWheelPositions> {
  public:
   /**
    * Constructs a DifferentialDriveOdometry object.
@@ -56,20 +62,13 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry {
    */
   void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance,
                      units::meter_t rightDistance, const Pose2d& pose) {
-    m_pose = pose;
-    m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
-
-    m_prevLeftDistance = leftDistance;
-    m_prevRightDistance = rightDistance;
+    Odometry<DifferentialDriveWheelSpeeds,
+             DifferentialDriveWheelPositions>::ResetPosition(gyroAngle,
+                                                             {leftDistance,
+                                                              rightDistance},
+                                                             pose);
   }
 
-  /**
-   * Returns the position of the robot on the field.
-   * @return The pose of the robot.
-   */
-  const Pose2d& GetPose() const { return m_pose; }
-
   /**
    * Updates the robot position on the field using distance measurements from
    * encoders. This method is more numerically accurate than using velocities to
@@ -82,15 +81,14 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry {
    * @return The new pose of the robot.
    */
   const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance,
-                       units::meter_t rightDistance);
+                       units::meter_t rightDistance) {
+    return Odometry<DifferentialDriveWheelSpeeds,
+                    DifferentialDriveWheelPositions>::Update(gyroAngle,
+                                                             {leftDistance,
+                                                              rightDistance});
+  }
 
  private:
-  Pose2d m_pose;
-
-  Rotation2d m_gyroOffset;
-  Rotation2d m_previousAngle;
-
-  units::meter_t m_prevLeftDistance = 0_m;
-  units::meter_t m_prevRightDistance = 0_m;
+  DifferentialDriveKinematics m_kinematicsImpl{units::meter_t{1}};
 };
 }  // namespace frc

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

@@ -0,0 +1,56 @@
+// 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 "units/length.h"
+
+namespace frc {
+/**
+ * Represents the wheel positions for a differential drive drivetrain.
+ */
+struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions {
+  /**
+   * Distance driven by the left side.
+   */
+  units::meter_t left = 0_m;
+
+  /**
+   * Distance driven by the right side.
+   */
+  units::meter_t right = 0_m;
+
+  /**
+   * Checks equality between this DifferentialDriveWheelPositions and another
+   * object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const DifferentialDriveWheelPositions& other) const = default;
+
+  /**
+   * Checks inequality between this DifferentialDriveWheelPositions and another
+   * object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  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),
+            wpi::Lerp(right, endValue.right, t)};
+  }
+};
+}  // namespace frc

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

@@ -0,0 +1,60 @@
+// 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/SymbolExports.h>
+
+#include "frc/geometry/Twist2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+
+namespace frc {
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
+ * into individual wheel speeds. Robot code should not use this directly-
+ * Instead, use the particular type for your drivetrain (e.g.,
+ * DifferentialDriveKinematics).
+ *
+ * Inverse kinematics converts a desired chassis speed into wheel speeds whereas
+ * forward kinematics converts wheel speeds into chassis speed.
+ */
+template <typename WheelSpeeds, typename WheelPositions>
+class WPILIB_DLLEXPORT Kinematics {
+ public:
+  /**
+   * Performs forward kinematics to return the resulting chassis speed from the
+   * wheel speeds. This method is often used for odometry -- determining the
+   * robot's position on the field using data from the real-world speed of each
+   * wheel on the robot.
+   *
+   * @param wheelSpeeds The speeds of the wheels.
+   * @return The chassis speed.
+   */
+  virtual ChassisSpeeds ToChassisSpeeds(
+      const WheelSpeeds& wheelSpeeds) const = 0;
+
+  /**
+   * Performs inverse kinematics to return the wheel speeds from a desired
+   * chassis velocity. This method is often used to convert joystick values into
+   * wheel speeds.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @return The wheel speeds.
+   */
+  virtual WheelSpeeds ToWheelSpeeds(
+      const ChassisSpeeds& chassisSpeeds) const = 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 in the robot's movement.
+   */
+  virtual Twist2d ToTwist2d(const WheelPositions& wheelDeltas) const = 0;
+};
+}  // namespace frc

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

@@ -11,6 +11,7 @@
 #include "frc/geometry/Translation2d.h"
 #include "frc/geometry/Twist2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/Kinematics.h"
 #include "frc/kinematics/MecanumDriveWheelPositions.h"
 #include "frc/kinematics/MecanumDriveWheelSpeeds.h"
 #include "wpimath/MathShared.h"
@@ -39,7 +40,8 @@ namespace frc {
  * Forward kinematics is also used for odometry -- determining the position of
  * the robot on the field using encoders and a gyro.
  */
-class WPILIB_DLLEXPORT MecanumDriveKinematics {
+class WPILIB_DLLEXPORT MecanumDriveKinematics
+    : public Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
  public:
   /**
    * Constructs a mecanum drive kinematics object.
@@ -101,7 +103,12 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics {
    */
   MecanumDriveWheelSpeeds ToWheelSpeeds(
       const ChassisSpeeds& chassisSpeeds,
-      const Translation2d& centerOfRotation = Translation2d{}) const;
+      const Translation2d& centerOfRotation) const;
+
+  MecanumDriveWheelSpeeds ToWheelSpeeds(
+      const ChassisSpeeds& chassisSpeeds) const override {
+    return ToWheelSpeeds(chassisSpeeds, {});
+  }
 
   /**
    * Performs forward kinematics to return the resulting chassis state from the
@@ -114,7 +121,7 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics {
    * @return The resulting chassis speed.
    */
   ChassisSpeeds ToChassisSpeeds(
-      const MecanumDriveWheelSpeeds& wheelSpeeds) const;
+      const MecanumDriveWheelSpeeds& wheelSpeeds) const override;
 
   /**
    * Performs forward kinematics to return the resulting Twist2d from the
@@ -126,7 +133,8 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics {
    *
    * @return The resulting chassis speed.
    */
-  Twist2d ToTwist2d(const MecanumDriveWheelPositions& wheelDeltas) const;
+  Twist2d ToTwist2d(
+      const MecanumDriveWheelPositions& wheelDeltas) const override;
 
  private:
   mutable Matrixd<4, 3> m_inverseKinematics;

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

@@ -9,7 +9,9 @@
 
 #include "frc/geometry/Pose2d.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveWheelPositions.h"
 #include "frc/kinematics/MecanumDriveWheelSpeeds.h"
+#include "frc/kinematics/Odometry.h"
 #include "units/time.h"
 
 namespace frc {
@@ -23,7 +25,8 @@ namespace frc {
  * path following. Furthermore, odometry can be used for latency compensation
  * when using computer-vision systems.
  */
-class WPILIB_DLLEXPORT MecanumDriveOdometry {
+class WPILIB_DLLEXPORT MecanumDriveOdometry
+    : public Odometry<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
  public:
   /**
    * Constructs a MecanumDriveOdometry object.
@@ -38,52 +41,8 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
       const MecanumDriveWheelPositions& wheelPositions,
       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 wheelPositions The current distances measured by each wheel.
-   * @param pose The position on the field that your robot is at.
-   */
-  void ResetPosition(const Rotation2d& gyroAngle,
-                     const MecanumDriveWheelPositions& wheelPositions,
-                     const Pose2d& pose) {
-    m_pose = pose;
-    m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
-    m_previousWheelPositions = wheelPositions;
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   * @return The pose of the robot.
-   */
-  const Pose2d& GetPose() const { return m_pose; }
-
-  /**
-   * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose 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 gyroAngle The angle reported by the gyroscope.
-   * @param wheelPositions The current distances measured by each wheel.
-   *
-   * @return The new pose of the robot.
-   */
-  const Pose2d& Update(const Rotation2d& gyroAngle,
-                       const MecanumDriveWheelPositions& wheelPositions);
-
  private:
-  MecanumDriveKinematics m_kinematics;
-  Pose2d m_pose;
-
-  MecanumDriveWheelPositions m_previousWheelPositions;
-  Rotation2d m_previousAngle;
-  Rotation2d m_gyroOffset;
+  MecanumDriveKinematics m_kinematicsImpl;
 };
 
 }  // namespace frc

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

@@ -4,13 +4,14 @@
 
 #pragma once
 
+#include <wpi/MathExtras.h>
 #include <wpi/SymbolExports.h>
 
 #include "units/length.h"
 
 namespace frc {
 /**
- * Represents the wheel speeds for a mecanum drive drivetrain.
+ * Represents the wheel positions for a mecanum drive drivetrain.
  */
 struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
   /**
@@ -49,5 +50,19 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
    * @return Whether the two objects are not equal.
    */
   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),
+            wpi::Lerp(frontRight, endValue.frontRight, t),
+            wpi::Lerp(rearLeft, endValue.rearLeft, t),
+            wpi::Lerp(rearRight, endValue.rearRight, t)};
+  }
 };
 }  // namespace frc

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

@@ -0,0 +1,88 @@
+// 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/SymbolExports.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/Kinematics.h"
+#include "frc/kinematics/WheelPositions.h"
+
+namespace frc {
+/**
+ * Class for odometry. Robot code should not use this directly- Instead, use the
+ * particular type for your drivetrain (e.g., DifferentialDriveOdometry).
+ * Odometry allows you to track the robot's position on the field over a course
+ * of a match using readings from your wheel encoders.
+ *
+ * Teams can use odometry during the autonomous period for complex tasks like
+ * path following. Furthermore, odometry can be used for latency compensation
+ * when using computer-vision systems.
+ */
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+class WPILIB_DLLEXPORT Odometry {
+ public:
+  /**
+   * Constructs an Odometry object.
+   *
+   * @param kinematics The kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current distances measured by each wheel.
+   * @param initialPose The starting position of the robot on the field.
+   */
+  explicit Odometry(const Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+                    const Rotation2d& gyroAngle,
+                    const WheelPositions& wheelPositions,
+                    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 wheelPositions The current distances measured by each wheel.
+   * @param pose The position on the field that your robot is at.
+   */
+  void ResetPosition(const Rotation2d& gyroAngle,
+                     const WheelPositions& wheelPositions, const Pose2d& pose) {
+    m_pose = pose;
+    m_previousAngle = pose.Rotation();
+    m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    m_previousWheelPositions = wheelPositions;
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   * @return The pose of the robot.
+   */
+  const Pose2d& GetPose() const { return m_pose; }
+
+  /**
+   * Updates the robot's position on the field using forward kinematics and
+   * integration of the pose 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 gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current distances measured by each wheel.
+   *
+   * @return The new pose of the robot.
+   */
+  const Pose2d& Update(const Rotation2d& gyroAngle,
+                       const WheelPositions& wheelPositions);
+
+ private:
+  const Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
+  Pose2d m_pose;
+
+  WheelPositions m_previousWheelPositions;
+  Rotation2d m_previousAngle;
+  Rotation2d m_gyroOffset;
+};
+}  // namespace frc
+
+#include "Odometry.inc"

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

@@ -0,0 +1,40 @@
+// 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 "frc/kinematics/Odometry.h"
+
+namespace frc {
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+Odometry<WheelSpeeds, WheelPositions>::Odometry(
+    const Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+    const Rotation2d& gyroAngle, const WheelPositions& wheelPositions,
+    const Pose2d& initialPose)
+    : m_kinematics(kinematics),
+      m_pose(initialPose),
+      m_previousWheelPositions(wheelPositions) {
+  m_previousAngle = m_pose.Rotation();
+  m_gyroOffset = m_pose.Rotation() - gyroAngle;
+}
+
+template <typename WheelSpeeds, WheelPositions WheelPositions>
+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);
+  twist.dtheta = (angle - m_previousAngle).Radians();
+
+  auto newPose = m_pose.Exp(twist);
+
+  m_previousAngle = angle;
+  m_previousWheelPositions = wheelPositions;
+  m_pose = {newPose.Translation(), angle};
+
+  return m_pose;
+}
+}  // namespace frc

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

@@ -16,12 +16,18 @@
 #include "frc/geometry/Translation2d.h"
 #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"
 #include "wpimath/MathShared.h"
 
 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).
@@ -45,7 +51,9 @@ namespace frc {
  * the robot on the field using encoders and a gyro.
  */
 template <size_t NumModules>
-class SwerveDriveKinematics {
+class SwerveDriveKinematics
+    : public Kinematics<SwerveDriveWheelSpeeds<NumModules>,
+                        SwerveDriveWheelPositions<NumModules>> {
  public:
   /**
    * Constructs a swerve drive kinematics object. This takes in a variable
@@ -130,6 +138,11 @@ class SwerveDriveKinematics {
       const ChassisSpeeds& chassisSpeeds,
       const Translation2d& centerOfRotation = Translation2d{}) const;
 
+  SwerveDriveWheelSpeeds<NumModules> ToWheelSpeeds(
+      const ChassisSpeeds& chassisSpeeds) const override {
+    return ToSwerveModuleStates(chassisSpeeds);
+  }
+
   /**
    * Performs forward kinematics to return the resulting chassis state from the
    * given module states. This method is often used for odometry -- determining
@@ -162,8 +175,8 @@ class SwerveDriveKinematics {
    *
    * @return The resulting chassis speed.
    */
-  ChassisSpeeds ToChassisSpeeds(
-      const wpi::array<SwerveModuleState, NumModules>& moduleStates) const;
+  ChassisSpeeds ToChassisSpeeds(const wpi::array<SwerveModuleState, NumModules>&
+                                    moduleStates) const override;
 
   /**
    * Performs forward kinematics to return the resulting Twist2d from the
@@ -201,6 +214,11 @@ class SwerveDriveKinematics {
   Twist2d ToTwist2d(
       wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const;
 
+  Twist2d ToTwist2d(
+      const SwerveDriveWheelPositions<NumModules>& wheelDeltas) const override {
+    return ToTwist2d(wheelDeltas.positions);
+  }
+
   /**
    * Renormalizes the wheel speeds if any individual speed is above the
    * specified maximum.

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

@@ -11,8 +11,11 @@
 #include <wpi/SymbolExports.h>
 #include <wpi/timestamp.h>
 
+#include "Odometry.h"
 #include "SwerveDriveKinematics.h"
+#include "SwerveDriveWheelPositions.h"
 #include "SwerveModulePosition.h"
+#include "SwerveModuleState.h"
 #include "frc/geometry/Pose2d.h"
 #include "units/time.h"
 
@@ -28,7 +31,9 @@ namespace frc {
  * when using computer-vision systems.
  */
 template <size_t NumModules>
-class SwerveDriveOdometry {
+class SwerveDriveOdometry
+    : public Odometry<SwerveDriveWheelSpeeds<NumModules>,
+                      SwerveDriveWheelPositions<NumModules>> {
  public:
   /**
    * Constructs a SwerveDriveOdometry object.
@@ -56,13 +61,13 @@ class SwerveDriveOdometry {
   void ResetPosition(
       const Rotation2d& gyroAngle,
       const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
-      const Pose2d& pose);
-
-  /**
-   * Returns the position of the robot on the field.
-   * @return The pose of the robot.
-   */
-  const Pose2d& GetPose() const { return m_pose; }
+      const Pose2d& pose) {
+    Odometry<
+        SwerveDriveWheelSpeeds<NumModules>,
+        SwerveDriveWheelPositions<NumModules>>::ResetPosition(gyroAngle,
+                                                              {modulePositions},
+                                                              pose);
+  }
 
   /**
    * Updates the robot's position on the field using forward kinematics and
@@ -79,17 +84,15 @@ class SwerveDriveOdometry {
    */
   const Pose2d& Update(
       const Rotation2d& gyroAngle,
-      const wpi::array<SwerveModulePosition, NumModules>& modulePositions);
+      const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
+    return Odometry<
+        SwerveDriveWheelSpeeds<NumModules>,
+        SwerveDriveWheelPositions<NumModules>>::Update(gyroAngle,
+                                                       {modulePositions});
+  }
 
  private:
-  SwerveDriveKinematics<NumModules> m_kinematics;
-  Pose2d m_pose;
-
-  Rotation2d m_previousAngle;
-  Rotation2d m_gyroOffset;
-
-  wpi::array<SwerveModulePosition, NumModules> m_previousModulePositions{
-      wpi::empty_array};
+  SwerveDriveKinematics<NumModules> m_kinematicsImpl;
 };
 
 extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)

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

@@ -13,58 +13,11 @@ SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
     SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
     const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
     const Pose2d& initialPose)
-    : m_kinematics(kinematics), m_pose(initialPose) {
-  m_previousAngle = m_pose.Rotation();
-  m_gyroOffset = m_pose.Rotation() - gyroAngle;
-
-  for (size_t i = 0; i < NumModules; i++) {
-    m_previousModulePositions[i] = {modulePositions[i].distance,
-                                    modulePositions[i].angle};
-  }
-
+    : Odometry<SwerveDriveWheelSpeeds<NumModules>,
+               SwerveDriveWheelPositions<NumModules>>(
+          m_kinematicsImpl, gyroAngle, {modulePositions}, initialPose),
+      m_kinematicsImpl(kinematics) {
   wpi::math::MathSharedStore::ReportUsage(
       wpi::math::MathUsageId::kOdometry_SwerveDrive, 1);
 }
-
-template <size_t NumModules>
-void SwerveDriveOdometry<NumModules>::ResetPosition(
-    const Rotation2d& gyroAngle,
-    const wpi::array<SwerveModulePosition, NumModules>& modulePositions,
-    const Pose2d& pose) {
-  m_pose = pose;
-  m_previousAngle = pose.Rotation();
-  m_gyroOffset = m_pose.Rotation() - gyroAngle;
-
-  for (size_t i = 0; i < NumModules; i++) {
-    m_previousModulePositions[i].distance = modulePositions[i].distance;
-  }
-}
-
-template <size_t NumModules>
-const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
-    const Rotation2d& gyroAngle,
-    const wpi::array<SwerveModulePosition, NumModules>& modulePositions) {
-  auto moduleDeltas =
-      wpi::array<SwerveModulePosition, NumModules>(wpi::empty_array);
-  for (size_t index = 0; index < NumModules; index++) {
-    auto lastPosition = m_previousModulePositions[index];
-    auto currentPosition = modulePositions[index];
-    moduleDeltas[index] = {currentPosition.distance - lastPosition.distance,
-                           currentPosition.angle};
-
-    m_previousModulePositions[index].distance = modulePositions[index].distance;
-  }
-
-  auto angle = gyroAngle + m_gyroOffset;
-
-  auto twist = m_kinematics.ToTwist2d(moduleDeltas);
-  twist.dtheta = (angle - m_previousAngle).Radians();
-
-  auto newPose = m_pose.Exp(twist);
-
-  m_previousAngle = angle;
-  m_pose = {newPose.Translation(), angle};
-
-  return m_pose;
-}
 }  // namespace frc

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

@@ -0,0 +1,61 @@
+// 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> 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 =
+        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

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

@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <wpi/MathExtras.h>
 #include <wpi/SymbolExports.h>
 
 #include "frc/geometry/Rotation2d.h"
@@ -33,5 +34,15 @@ struct WPILIB_DLLEXPORT SwerveModulePosition {
    * @return Whether the two objects are equal.
    */
   bool operator==(const SwerveModulePosition& other) const;
+
+  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),
+            wpi::Lerp(angle, endValue.angle, t)};
+  }
 };
 }  // namespace frc

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

@@ -0,0 +1,16 @@
+// 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 <concepts>
+
+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