[wpimath] Position Delta Odometry for Mecanum (#4514)

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

@@ -9,7 +9,9 @@
 #include "Eigen/QR"
 #include "frc/EigenCore.h"
 #include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Twist2d.h"
 #include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/kinematics/MecanumDriveWheelPositions.h"
 #include "frc/kinematics/MecanumDriveWheelSpeeds.h"
 #include "wpimath/MathShared.h"
 
@@ -114,6 +116,18 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics {
   ChassisSpeeds ToChassisSpeeds(
       const MecanumDriveWheelSpeeds& wheelSpeeds) const;
 
+  /**
+   * Performs forward kinematics to return the resulting Twist2d from the
+   * given wheel position deltas. This method is often used for odometry --
+   * determining the robot's position on the field using data from the
+   * distance driven by each wheel on the robot.
+   *
+   * @param wheelDeltas The change in distance driven by each wheel.
+   *
+   * @return The resulting chassis speed.
+   */
+  Twist2d ToTwist2d(const MecanumDriveWheelPositions& wheelDeltas) const;
+
  private:
   mutable Matrixd<4, 3> m_inverseKinematics;
   Eigen::HouseholderQR<Matrixd<4, 3>> m_forwardKinematics;

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

@@ -30,10 +30,12 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
    *
    * @param kinematics The mecanum drive 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 MecanumDriveOdometry(MecanumDriveKinematics kinematics,
                                 const Rotation2d& gyroAngle,
+                                const MecanumDriveWheelPositions wheelPositions,
                                 const Pose2d& initialPose = Pose2d{});
 
   /**
@@ -44,11 +46,14 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
    *
    * @param pose The position on the field that your robot is at.
    * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelPositions The current distances measured by each wheel.
    */
-  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle) {
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle,
+                     const MecanumDriveWheelPositions wheelPositions) {
     m_pose = pose;
     m_previousAngle = pose.Rotation();
     m_gyroOffset = m_pose.Rotation() - gyroAngle;
+    m_previousWheelPositions = wheelPositions;
   }
 
   /**
@@ -59,45 +64,23 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry {
 
   /**
    * Updates the robot's position on the field using forward kinematics and
-   * 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 currentTime The current time.
-   * @param gyroAngle The angle reported by the gyroscope.
-   * @param wheelSpeeds The current wheel speeds.
-   *
-   * @return The new pose of the robot.
-   */
-  const Pose2d& UpdateWithTime(units::second_t currentTime,
-                               const Rotation2d& gyroAngle,
-                               MecanumDriveWheelSpeeds wheelSpeeds);
-
-  /**
-   * 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
-   * angular rate that is calculated from forward kinematics.
+   * 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 wheelSpeeds The current wheel speeds.
+   * @param wheelPositions The current distances measured by each wheel.
    *
    * @return The new pose of the robot.
    */
   const Pose2d& Update(const Rotation2d& gyroAngle,
-                       MecanumDriveWheelSpeeds wheelSpeeds) {
-    return UpdateWithTime(wpi::Now() * 1.0e-6_s, gyroAngle, wheelSpeeds);
-  }
+                       const MecanumDriveWheelPositions wheelPositions);
 
  private:
   MecanumDriveKinematics m_kinematics;
   Pose2d m_pose;
 
-  units::second_t m_previousTime = -1_s;
+  MecanumDriveWheelPositions m_previousWheelPositions;
   Rotation2d m_previousAngle;
   Rotation2d m_gyroOffset;
 };

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

@@ -0,0 +1,36 @@
+// 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 "units/length.h"
+
+namespace frc {
+/**
+ * Represents the wheel speeds for a mecanum drive drivetrain.
+ */
+struct WPILIB_DLLEXPORT MecanumDriveWheelPositions {
+  /**
+   * Distance driven by the front-left wheel.
+   */
+  units::meter_t frontLeft = 0_m;
+
+  /**
+   * Distance driven by the front-right wheel.
+   */
+  units::meter_t frontRight = 0_m;
+
+  /**
+   * Distance driven by the rear-left wheel.
+   */
+  units::meter_t rearLeft = 0_m;
+
+  /**
+   * Distance driven by the rear-right wheel.
+   */
+  units::meter_t rearRight = 0_m;
+};
+}  // namespace frc