[wpimath] Position Delta Odometry for Swerve (#4493)

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

@@ -13,7 +13,9 @@
 #include "frc/EigenCore.h"
 #include "frc/geometry/Rotation2d.h"
 #include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Twist2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/SwerveModulePosition.h"
 #include "frc/kinematics/SwerveModuleState.h"
 #include "units/velocity.h"
 #include "wpimath/MathShared.h"
@@ -162,6 +164,38 @@ class SwerveDriveKinematics {
   ChassisSpeeds ToChassisSpeeds(
       wpi::array<SwerveModuleState, NumModules> moduleStates) const;
 
+  /**
+   * Performs forward kinematics to return the resulting Twist2d from the
+   * given module position deltas. This method is often used for odometry --
+   * determining the robot's position on the field using data from the
+   * real-world position delta and angle of each module on the robot.
+   *
+   * @param wheelDeltas The latest change in position of the modules (as a
+   * SwerveModulePosition type) as measured from respective encoders and gyros.
+   * The order of the swerve module states should be same as passed into the
+   * constructor of this class.
+   *
+   * @return The resulting Twist2d.
+   */
+  template <typename... ModuleDeltas>
+  Twist2d ToTwist2d(ModuleDeltas&&... wheelDeltas) const;
+
+  /**
+   * Performs forward kinematics to return the resulting Twist2d from the
+   * given module position deltas. This method is often used for odometry --
+   * determining the robot's position on the field using data from the
+   * real-world position delta and angle of each module on the robot.
+   *
+   * @param wheelDeltas The latest change in position of the modules (as a
+   * SwerveModulePosition type) as measured from respective encoders and gyros.
+   * The order of the swerve module states should be same as passed into the
+   * constructor of this class.
+   *
+   * @return The resulting Twist2d.
+   */
+  Twist2d ToTwist2d(
+      wpi::array<SwerveModulePosition, NumModules> wheelDeltas) const;
+
   /**
    * Renormalizes the wheel speeds if any individual speed is above the
    * specified maximum.

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

@@ -95,6 +95,39 @@ ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
           units::radians_per_second_t{chassisSpeedsVector(2)}};
 }
 
+template <size_t NumModules>
+template <typename... ModuleDeltas>
+Twist2d SwerveDriveKinematics<NumModules>::ToTwist2d(
+    ModuleDeltas&&... wheelDeltas) const {
+  static_assert(sizeof...(wheelDeltas) == NumModules,
+                "Number of modules is not consistent with number of wheel "
+                "locations provided in constructor.");
+
+  wpi::array<SwerveModulePosition, NumModules> moduleDeltas{wheelDeltas...};
+
+  return this->ToTwist2d(moduleDeltas);
+}
+
+template <size_t NumModules>
+Twist2d SwerveDriveKinematics<NumModules>::ToTwist2d(
+    wpi::array<SwerveModulePosition, NumModules> moduleDeltas) const {
+  Matrixd<NumModules * 2, 1> moduleDeltaMatrix;
+
+  for (size_t i = 0; i < NumModules; ++i) {
+    SwerveModulePosition module = moduleDeltas[i];
+    moduleDeltaMatrix(i * 2, 0) = module.distance.value() * module.angle.Cos();
+    moduleDeltaMatrix(i * 2 + 1, 0) =
+        module.distance.value() * module.angle.Sin();
+  }
+
+  Eigen::Vector3d chassisDeltaVector =
+      m_forwardKinematics.solve(moduleDeltaMatrix);
+
+  return {units::meter_t{chassisDeltaVector(0)},
+          units::meter_t{chassisDeltaVector(1)},
+          units::radian_t{chassisDeltaVector(2)}};
+}
+
 template <size_t NumModules>
 void SwerveDriveKinematics<NumModules>::DesaturateWheelSpeeds(
     wpi::array<SwerveModuleState, NumModules>* moduleStates,

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

@@ -12,7 +12,7 @@
 #include <wpi/timestamp.h>
 
 #include "SwerveDriveKinematics.h"
-#include "SwerveModuleState.h"
+#include "SwerveModulePosition.h"
 #include "frc/geometry/Pose2d.h"
 #include "units/time.h"
 
@@ -35,11 +35,13 @@ class SwerveDriveOdometry {
    *
    * @param kinematics The swerve drive kinematics for your drivetrain.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param modulePositions The wheel positions reported by each module.
    * @param initialPose The starting position of the robot on the field.
    */
-  SwerveDriveOdometry(SwerveDriveKinematics<NumModules> kinematics,
-                      const Rotation2d& gyroAngle,
-                      const Pose2d& initialPose = Pose2d{});
+  SwerveDriveOdometry(
+      SwerveDriveKinematics<NumModules> kinematics, const Rotation2d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules> modulePositions,
+      const Pose2d& initialPose = Pose2d{});
 
   /**
    * Resets the robot's position on the field.
@@ -49,12 +51,25 @@ class SwerveDriveOdometry {
    *
    * @param pose The position on the field that your robot is at.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The wheel positions reported by each module.
    */
-  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
-    m_pose = pose;
-    m_previousAngle = pose.Rotation();
-    m_gyroOffset = m_pose.Rotation() - gyroAngle;
-  }
+  template <typename... ModulePositions>
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle,
+                     ModulePositions&&... wheelPositions);
+
+  /**
+   * 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 pose The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param modulePositions The wheel positions reported by each module.
+   */
+  void ResetPosition(
+      const Pose2d& pose, const Rotation2d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules> modulePositions);
 
   /**
    * Returns the position of the robot on the field.
@@ -70,47 +85,44 @@ class SwerveDriveOdometry {
    * also takes in an angle parameter which is used instead of the
    * angular rate that is calculated from forward kinematics.
    *
-   * @param currentTime The current time.
    * @param gyroAngle The angle reported by the gyroscope.
-   * @param moduleStates The current state of all swerve modules. Please provide
-   *                     the states in the same order in which you instantiated
-   *                     your SwerveDriveKinematics.
+   * @param wheelPositions 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.
    */
-  template <typename... ModuleStates>
-  const Pose2d& UpdateWithTime(units::second_t currentTime,
-                               const Rotation2d& gyroAngle,
-                               ModuleStates&&... moduleStates);
+  template <typename... ModulePositions>
+  const Pose2d& Update(const Rotation2d& gyroAngle,
+                       ModulePositions&&... wheelPositions);
 
   /**
    * Updates the robot's position on the field using forward kinematics and
-   * integration of the pose over time. This method automatically calculates
-   * the current time to calculate period (difference between two timestamps).
-   * The period is used to calculate the change in distance from a velocity.
-   * This also takes in an angle parameter which is used instead of the
+   * integration of the pose over time. This method takes in the current time as
+   * a parameter to calculate period (difference between two timestamps). The
+   * period is used to calculate the change in distance from a velocity. 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 moduleStates The current state of all swerve modules. Please provide
-   *                     the states in the same order in which you instantiated
-   *                     your SwerveDriveKinematics.
+   * @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.
    */
-  template <typename... ModuleStates>
-  const Pose2d& Update(const Rotation2d& gyroAngle,
-                       ModuleStates&&... moduleStates) {
-    return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, moduleStates...);
-  }
+  const Pose2d& Update(
+      const Rotation2d& gyroAngle,
+      const wpi::array<SwerveModulePosition, NumModules> modulePositions);
 
  private:
   SwerveDriveKinematics<NumModules> m_kinematics;
   Pose2d m_pose;
 
-  units::second_t m_previousTime = -1_s;
   Rotation2d m_previousAngle;
   Rotation2d m_gyroOffset;
+
+  wpi::array<SwerveModulePosition, NumModules> m_previousModulePositions;
 };
 
 extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT)

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

@@ -11,8 +11,11 @@ namespace frc {
 template <size_t NumModules>
 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_kinematics(kinematics),
+      m_pose(initialPose),
+      m_previousModulePositions(modulePositions) {
   m_previousAngle = m_pose.Rotation();
   m_gyroOffset = m_pose.Rotation() - gyroAngle;
   wpi::math::MathSharedStore::ReportUsage(
@@ -20,24 +23,66 @@ SwerveDriveOdometry<NumModules>::SwerveDriveOdometry(
 }
 
 template <size_t NumModules>
-template <typename... ModuleStates>
-const Pose2d& frc::SwerveDriveOdometry<NumModules>::UpdateWithTime(
-    units::second_t currentTime, const Rotation2d& gyroAngle,
-    ModuleStates&&... moduleStates) {
-  units::second_t deltaTime =
-      (m_previousTime >= 0_s) ? currentTime - m_previousTime : 0_s;
-  m_previousTime = currentTime;
+template <typename... ModulePositions>
+void SwerveDriveOdometry<NumModules>::ResetPosition(
+    const Pose2d& pose, const Rotation2d& gyroAngle,
+    ModulePositions&&... wheelPositions) {
+  static_assert(sizeof...(wheelPositions) == NumModules,
+                "Number of modules is not consistent with number of wheel "
+                "locations provided in constructor.");
+
+  wpi::array<SwerveModulePosition, NumModules> modulePositions{
+      wheelPositions...};
+  this->ResetPosition(pose, gyroAngle, modulePositions);
+}
+
+template <size_t NumModules>
+void SwerveDriveOdometry<NumModules>::ResetPosition(
+    const Pose2d& pose, const Rotation2d& gyroAngle,
+    const wpi::array<SwerveModulePosition, NumModules> modulePositions) {
+  m_pose = pose;
+  m_previousAngle = pose.Rotation();
+  m_gyroOffset = m_pose.Rotation() - gyroAngle;
+  m_previousModulePositions = modulePositions;
+}
+
+template <size_t NumModules>
+template <typename... ModulePositions>
+const Pose2d& frc::SwerveDriveOdometry<NumModules>::Update(
+    const Rotation2d& gyroAngle, ModulePositions&&... wheelPositions) {
+  static_assert(sizeof...(wheelPositions) == NumModules,
+                "Number of modules is not consistent with number of wheel "
+                "locations provided in constructor.");
+
+  wpi::array<SwerveModulePosition, NumModules> modulePositions{
+      wheelPositions...};
+
+  return this->Update(gyroAngle, modulePositions);
+}
+
+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 < modulePositions.size(); index++) {
+    auto lastPosition = m_previousModulePositions[index];
+    auto currentPosition = modulePositions[index];
+    moduleDeltas[index] = {currentPosition.distance - lastPosition.distance,
+                           currentPosition.angle};
+  }
 
   auto angle = gyroAngle + m_gyroOffset;
 
-  auto [dx, dy, dtheta] = m_kinematics.ToChassisSpeeds(moduleStates...);
-  static_cast<void>(dtheta);
+  auto twist = m_kinematics.ToTwist2d(moduleDeltas);
+  twist.dtheta = (angle - m_previousAngle).Radians();
 
-  auto newPose = m_pose.Exp(
-      {dx * deltaTime, dy * deltaTime, (angle - m_previousAngle).Radians()});
+  auto newPose = m_pose.Exp(twist);
 
   m_previousAngle = angle;
   m_pose = {newPose.Translation(), angle};
+  m_previousModulePositions = modulePositions;
 
   return m_pose;
 }

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

@@ -0,0 +1,29 @@
+// 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/Rotation2d.h"
+#include "units/angle.h"
+#include "units/length.h"
+#include "units/math.h"
+
+namespace frc {
+/**
+ * Represents the position of one swerve module.
+ */
+struct WPILIB_DLLEXPORT SwerveModulePosition {
+  /**
+   * Distance the wheel of a module has traveled
+   */
+  units::meter_t distance = 0_m;
+
+  /**
+   * Angle of the module.
+   */
+  Rotation2d angle;
+};
+}  // namespace frc