[wpimath] Add ChassisAccelerations and drivetrain accelerations classes and add forward and inverse kinematics for accelerations to the interface (#8185)

ChassisAccelerations and the drivetrain acceleration types are added in
both Java and C++. `ChassisAccelerations` is basically just
`ChassisSpeeds` but for accelerations!
`DifferentialDriveWheelAccelerations`, `MecanumDriveWheelAccelerations`,
and `SwerveModuleAccelerations` are the acceleration equivalent of the
drivetrain speeds types.

In Java, the `Kinematics` interface now has an additional generic
parameter `A` which represents the accelerations, and
`toChassisAccelerations` and `toWheelAccelerations` methods, which are
implemented the same way as `toChassisSpeeds` and `toWheelSpeeds`.

Protobuf and struct classes were also added for all four classes in Java
and C++.

---------

Signed-off-by: Zach Harel <zach@zharel.me>
Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator.hpp

@@ -30,8 +30,9 @@ namespace wpi::math {
  * never call it, then this class will behave like regular encoder odometry.
  */
 class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator
-    : public PoseEstimator<DifferentialDriveWheelSpeeds,
-                           DifferentialDriveWheelPositions> {
+    : public PoseEstimator<DifferentialDriveWheelPositions,
+                           DifferentialDriveWheelSpeeds,
+                           DifferentialDriveWheelAccelerations> {
  public:
   /**
    * Constructs a DifferentialDrivePoseEstimator with default standard

wpimath/src/main/native/include/wpi/math/estimator/DifferentialDrivePoseEstimator3d.hpp

@@ -34,8 +34,9 @@ namespace wpi::math {
  * never call it, then this class will behave like regular encoder odometry.
  */
 class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d
-    : public PoseEstimator3d<DifferentialDriveWheelSpeeds,
-                             DifferentialDriveWheelPositions> {
+    : public PoseEstimator3d<DifferentialDriveWheelPositions,
+                             DifferentialDriveWheelSpeeds,
+                             DifferentialDriveWheelAccelerations> {
  public:
   /**
    * Constructs a DifferentialDrivePoseEstimator3d with default standard

wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator.hpp

@@ -32,8 +32,8 @@ namespace wpi::math {
  * odometry.
  */
 class WPILIB_DLLEXPORT MecanumDrivePoseEstimator
-    : public PoseEstimator<MecanumDriveWheelSpeeds,
-                           MecanumDriveWheelPositions> {
+    : public PoseEstimator<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
+                           MecanumDriveWheelAccelerations> {
  public:
   /**
    * Constructs a MecanumDrivePoseEstimator with default standard deviations

wpimath/src/main/native/include/wpi/math/estimator/MecanumDrivePoseEstimator3d.hpp

@@ -36,8 +36,9 @@ namespace wpi::math {
  * odometry.
  */
 class WPILIB_DLLEXPORT MecanumDrivePoseEstimator3d
-    : public PoseEstimator3d<MecanumDriveWheelSpeeds,
-                             MecanumDriveWheelPositions> {
+    : public PoseEstimator3d<MecanumDriveWheelPositions,
+                             MecanumDriveWheelSpeeds,
+                             MecanumDriveWheelAccelerations> {
  public:
   /**
    * Constructs a MecanumDrivePoseEstimator3d with default standard deviations

wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator.hpp

@@ -37,10 +37,12 @@ namespace wpi::math {
  * AddVisionMeasurement() can be called as infrequently as you want; if you
  * never call it, then this class will behave like regular encoder odometry.
  *
- * @tparam WheelSpeeds Wheel speeds type.
  * @tparam WheelPositions Wheel positions type.
+ * @tparam WheelSpeeds Wheel speeds type.
+ * @tparam WheelAccelerations Wheel accelerations type.
  */
-template <typename WheelSpeeds, typename WheelPositions>
+template <typename WheelPositions, typename WheelSpeeds,
+          typename WheelAccelerations>
 class WPILIB_DLLEXPORT PoseEstimator {
  public:
   /**
@@ -60,10 +62,11 @@ class WPILIB_DLLEXPORT PoseEstimator {
    *     radians). Increase these numbers to trust the vision pose measurement
    *     less.
    */
-  PoseEstimator(Kinematics<WheelSpeeds, WheelPositions>& kinematics,
-                Odometry<WheelSpeeds, WheelPositions>& odometry,
-                const wpi::util::array<double, 3>& stateStdDevs,
-                const wpi::util::array<double, 3>& visionMeasurementStdDevs)
+  PoseEstimator(
+      Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>& kinematics,
+      Odometry<WheelPositions, WheelSpeeds, WheelAccelerations>& odometry,
+      const wpi::util::array<double, 3>& stateStdDevs,
+      const wpi::util::array<double, 3>& visionMeasurementStdDevs)
       : m_odometry(odometry) {
     for (size_t i = 0; i < 3; ++i) {
       m_q[i] = stateStdDevs[i] * stateStdDevs[i];
@@ -426,7 +429,7 @@ class WPILIB_DLLEXPORT PoseEstimator {
 
   static constexpr wpi::units::second_t kBufferDuration = 1.5_s;
 
-  Odometry<WheelSpeeds, WheelPositions>& m_odometry;
+  Odometry<WheelPositions, WheelSpeeds, WheelAccelerations>& m_odometry;
   wpi::util::array<double, 3> m_q{wpi::util::empty_array};
   Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero();
 

wpimath/src/main/native/include/wpi/math/estimator/PoseEstimator3d.hpp

@@ -43,10 +43,12 @@ namespace wpi::math {
  * AddVisionMeasurement() can be called as infrequently as you want; if you
  * never call it, then this class will behave like regular encoder odometry.
  *
- * @tparam WheelSpeeds Wheel speeds type.
  * @tparam WheelPositions Wheel positions type.
+ * @tparam WheelSpeeds Wheel speeds type.
+ * @tparam WheelAccelerations Wheel accelerations type.
  */
-template <typename WheelSpeeds, typename WheelPositions>
+template <typename WheelPositions, typename WheelSpeeds,
+          typename WheelAccelerations>
 class WPILIB_DLLEXPORT PoseEstimator3d {
  public:
   /**
@@ -66,10 +68,11 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
    * in meters, and angle in radians). Increase these numbers to trust the
    * vision pose measurement less.
    */
-  PoseEstimator3d(Kinematics<WheelSpeeds, WheelPositions>& kinematics,
-                  Odometry3d<WheelSpeeds, WheelPositions>& odometry,
-                  const wpi::util::array<double, 4>& stateStdDevs,
-                  const wpi::util::array<double, 4>& visionMeasurementStdDevs)
+  PoseEstimator3d(
+      Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>& kinematics,
+      Odometry3d<WheelPositions, WheelSpeeds, WheelAccelerations>& odometry,
+      const wpi::util::array<double, 4>& stateStdDevs,
+      const wpi::util::array<double, 4>& visionMeasurementStdDevs)
       : m_odometry(odometry) {
     for (size_t i = 0; i < 4; ++i) {
       m_q[i] = stateStdDevs[i] * stateStdDevs[i];
@@ -441,7 +444,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d {
 
   static constexpr wpi::units::second_t kBufferDuration = 1.5_s;
 
-  Odometry3d<WheelSpeeds, WheelPositions>& m_odometry;
+  Odometry3d<WheelPositions, WheelSpeeds, WheelAccelerations>& m_odometry;
   wpi::util::array<double, 4> m_q{wpi::util::empty_array};
   wpi::math::Matrixd<6, 6> m_visionK = wpi::math::Matrixd<6, 6>::Zero();
 

wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator.hpp

@@ -30,8 +30,10 @@ namespace wpi::math {
  */
 template <size_t NumModules>
 class SwerveDrivePoseEstimator
-    : public PoseEstimator<wpi::util::array<SwerveModuleState, NumModules>,
-                           wpi::util::array<SwerveModulePosition, NumModules>> {
+    : public PoseEstimator<
+          wpi::util::array<SwerveModulePosition, NumModules>,
+          wpi::util::array<SwerveModuleState, NumModules>,
+          wpi::util::array<SwerveModuleAcceleration, NumModules>> {
  public:
   /**
    * Constructs a SwerveDrivePoseEstimator with default standard deviations

wpimath/src/main/native/include/wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp

@@ -35,8 +35,9 @@ namespace wpi::math {
 template <size_t NumModules>
 class SwerveDrivePoseEstimator3d
     : public PoseEstimator3d<
+          wpi::util::array<SwerveModulePosition, NumModules>,
           wpi::util::array<SwerveModuleState, NumModules>,
-          wpi::util::array<SwerveModulePosition, NumModules>> {
+          wpi::util::array<SwerveModuleAcceleration, NumModules>> {
  public:
   /**
    * Constructs a SwerveDrivePoseEstimator3d with default standard deviations

wpimath/src/main/native/include/wpi/math/kinematics/ChassisAccelerations.hpp

@@ -0,0 +1,166 @@
+// 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/math/geometry/Rotation2d.hpp"
+#include "wpi/math/geometry/Translation2d.hpp"
+#include "wpi/math/util/MathUtil.hpp"
+#include "wpi/units/acceleration.hpp"
+#include "wpi/units/angular_acceleration.hpp"
+#include "wpi/util/SymbolExports.hpp"
+
+namespace wpi::math {
+/**
+ * Represents the acceleration of a robot chassis. Although this struct contains
+ * similar members compared to a ChassisSpeeds, they do NOT represent the same
+ * thing. Whereas a ChassisSpeeds object represents a robot's velocity, a
+ * ChassisAccelerations object represents a robot's acceleration.
+ *
+ * A strictly non-holonomic drivetrain, such as a differential drive, should
+ * never have an ay component because it can never move sideways. Holonomic
+ * drivetrains such as swerve and mecanum will often have all three components.
+ */
+struct WPILIB_DLLEXPORT ChassisAccelerations {
+  /**
+   * Acceleration along the x-axis. (Fwd is +)
+   */
+  units::meters_per_second_squared_t ax = 0_mps_sq;
+
+  /**
+   * Acceleration along the y-axis. (Left is +)
+   */
+  units::meters_per_second_squared_t ay = 0_mps_sq;
+
+  /**
+   * Angular acceleration of the robot frame. (CCW is +)
+   */
+  units::radians_per_second_squared_t alpha = 0_rad_per_s_sq;
+
+  /**
+   * Converts this field-relative set of accelerations into a robot-relative
+   * ChassisAccelerations object.
+   *
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The
+   *     robot's angle is considered to be zero when it is facing directly away
+   *     from your alliance station wall. Remember that this should be CCW
+   *     positive.
+   * @return ChassisAccelerations object representing the accelerations in the
+   *     robot's frame of reference.
+   */
+  constexpr ChassisAccelerations ToRobotRelative(
+      const Rotation2d& robotAngle) const {
+    // CW rotation into chassis frame
+    auto rotated =
+        Translation2d{units::meter_t{ax.value()}, units::meter_t{ay.value()}}
+            .RotateBy(-robotAngle);
+    return {units::meters_per_second_squared_t{rotated.X().value()},
+            units::meters_per_second_squared_t{rotated.Y().value()}, alpha};
+  }
+
+  /**
+   * Converts this robot-relative set of accelerations into a field-relative
+   * ChassisAccelerations object.
+   *
+   * @param robotAngle The angle of the robot as measured by a gyroscope. The
+   *     robot's angle is considered to be zero when it is facing directly away
+   *     from your alliance station wall. Remember that this should be CCW
+   *     positive.
+   * @return ChassisAccelerations object representing the accelerations in the
+   *     field's frame of reference.
+   */
+  constexpr ChassisAccelerations ToFieldRelative(
+      const Rotation2d& robotAngle) const {
+    // CCW rotation out of chassis frame
+    auto rotated =
+        Translation2d{units::meter_t{ax.value()}, units::meter_t{ay.value()}}
+            .RotateBy(robotAngle);
+    return {units::meters_per_second_squared_t{rotated.X().value()},
+            units::meters_per_second_squared_t{rotated.Y().value()}, alpha};
+  }
+
+  /**
+   * Adds two ChassisAccelerations and returns the sum.
+   *
+   * <p>For example, ChassisAccelerations{1.0, 0.5, 1.5} +
+   * ChassisAccelerations{2.0, 1.5, 0.5} = ChassisAccelerations{3.0, 2.0, 2.0}
+   *
+   * @param other The ChassisAccelerations to add.
+   *
+   * @return The sum of the ChassisAccelerations.
+   */
+  constexpr ChassisAccelerations operator+(
+      const ChassisAccelerations& other) const {
+    return {ax + other.ax, ay + other.ay, alpha + other.alpha};
+  }
+
+  /**
+   * Subtracts the other ChassisAccelerations from the current
+   * ChassisAccelerations and returns the difference.
+   *
+   * <p>For example, ChassisAccelerations{5.0, 4.0, 2.0} -
+   * ChassisAccelerations{1.0, 2.0, 1.0} = ChassisAccelerations{4.0, 2.0, 1.0}
+   *
+   * @param other The ChassisAccelerations to subtract.
+   *
+   * @return The difference between the two ChassisAccelerations.
+   */
+  constexpr ChassisAccelerations operator-(
+      const ChassisAccelerations& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Returns the inverse of the current ChassisAccelerations.
+   * This is equivalent to negating all components of the ChassisAccelerations.
+   *
+   * @return The inverse of the current ChassisAccelerations.
+   */
+  constexpr ChassisAccelerations operator-() const {
+    return {-ax, -ay, -alpha};
+  }
+
+  /**
+   * Multiplies the ChassisAccelerations by a scalar and returns the new
+   * ChassisAccelerations.
+   *
+   * <p>For example, ChassisAccelerations{2.0, 2.5, 1.0} * 2
+   * = ChassisAccelerations{4.0, 5.0, 2.0}
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled ChassisAccelerations.
+   */
+  constexpr ChassisAccelerations operator*(double scalar) const {
+    return {scalar * ax, scalar * ay, scalar * alpha};
+  }
+
+  /**
+   * Divides the ChassisAccelerations by a scalar and returns the new
+   * ChassisAccelerations.
+   *
+   * <p>For example, ChassisAccelerations{2.0, 2.5, 1.0} / 2
+   * = ChassisAccelerations{1.0, 1.25, 0.5}
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled ChassisAccelerations.
+   */
+  constexpr ChassisAccelerations operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
+
+  /**
+   * Compares the ChassisAccelerations with another ChassisAccelerations.
+   *
+   * @param other The other ChassisAccelerations.
+   *
+   * @return The result of the comparison. Is true if they are the same.
+   */
+  constexpr bool operator==(const ChassisAccelerations& other) const = default;
+};
+}  // namespace wpi::math
+
+#include "wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp"
+#include "wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp"

wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveKinematics.hpp

@@ -7,7 +7,9 @@
 #include <type_traits>
 
 #include "wpi/math/geometry/Twist2d.hpp"
+#include "wpi/math/kinematics/ChassisAccelerations.hpp"
 #include "wpi/math/kinematics/ChassisSpeeds.hpp"
+#include "wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp"
 #include "wpi/math/kinematics/DifferentialDriveWheelPositions.hpp"
 #include "wpi/math/kinematics/DifferentialDriveWheelSpeeds.hpp"
 #include "wpi/math/kinematics/Kinematics.hpp"
@@ -26,8 +28,9 @@ namespace wpi::math {
  * component velocities into a linear and angular chassis speed.
  */
 class WPILIB_DLLEXPORT DifferentialDriveKinematics
-    : public Kinematics<DifferentialDriveWheelSpeeds,
-                        DifferentialDriveWheelPositions> {
+    : public Kinematics<DifferentialDriveWheelPositions,
+                        DifferentialDriveWheelSpeeds,
+                        DifferentialDriveWheelAccelerations> {
  public:
   /**
    * Constructs a differential drive kinematics object.
@@ -98,6 +101,23 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics
     return start.Interpolate(end, t);
   }
 
+  constexpr ChassisAccelerations ToChassisAccelerations(
+      const DifferentialDriveWheelAccelerations& wheelAccelerations)
+      const override {
+    return {(wheelAccelerations.left + wheelAccelerations.right) / 2.0,
+            0_mps_sq,
+            (wheelAccelerations.right - wheelAccelerations.left) / trackwidth *
+                1_rad};
+  }
+
+  constexpr DifferentialDriveWheelAccelerations ToWheelAccelerations(
+      const ChassisAccelerations& chassisAccelerations) const override {
+    return {chassisAccelerations.ax -
+                trackwidth / 2 * chassisAccelerations.alpha / 1_rad,
+            chassisAccelerations.ax +
+                trackwidth / 2 * chassisAccelerations.alpha / 1_rad};
+  }
+
   /// Differential drive trackwidth.
   wpi::units::meter_t trackwidth;
 };

wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry.hpp

@@ -26,8 +26,9 @@ namespace wpi::math {
  * Any subsequent pose resets also require the encoders to be reset to zero.
  */
 class WPILIB_DLLEXPORT DifferentialDriveOdometry
-    : public Odometry<DifferentialDriveWheelSpeeds,
-                      DifferentialDriveWheelPositions> {
+    : public Odometry<DifferentialDriveWheelPositions,
+                      DifferentialDriveWheelSpeeds,
+                      DifferentialDriveWheelAccelerations> {
  public:
   /**
    * Constructs a DifferentialDriveOdometry object.

wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveOdometry3d.hpp

@@ -26,8 +26,9 @@ namespace wpi::math {
  * Any subsequent pose resets also require the encoders to be reset to zero.
  */
 class WPILIB_DLLEXPORT DifferentialDriveOdometry3d
-    : public Odometry3d<DifferentialDriveWheelSpeeds,
-                        DifferentialDriveWheelPositions> {
+    : public Odometry3d<DifferentialDriveWheelPositions,
+                        DifferentialDriveWheelSpeeds,
+                        DifferentialDriveWheelAccelerations> {
  public:
   /**
    * Constructs a DifferentialDriveOdometry3d object.

wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelAccelerations.hpp

@@ -0,0 +1,113 @@
+// 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/units/acceleration.hpp"
+#include "wpi/units/math.hpp"
+#include "wpi/util/SymbolExports.hpp"
+
+namespace wpi::math {
+/**
+ * Represents the wheel accelerations for a differential drive drivetrain.
+ */
+struct WPILIB_DLLEXPORT DifferentialDriveWheelAccelerations {
+  /**
+   * Acceleration of the left side of the robot.
+   */
+  units::meters_per_second_squared_t left = 0_mps_sq;
+
+  /**
+   * Acceleration of the right side of the robot.
+   */
+  units::meters_per_second_squared_t right = 0_mps_sq;
+
+  /**
+   * Adds two DifferentialDriveWheelAccelerations and returns the sum.
+   *
+   * <p>For example, DifferentialDriveWheelAccelerations{1.0, 0.5} +
+   * DifferentialDriveWheelAccelerations{2.0, 1.5} =
+   * DifferentialDriveWheelAccelerations{3.0, 2.0}
+   *
+   * @param other The DifferentialDriveWheelAccelerations to add.
+   *
+   * @return The sum of the DifferentialDriveWheelAccelerations.
+   */
+  constexpr DifferentialDriveWheelAccelerations operator+(
+      const DifferentialDriveWheelAccelerations& other) const {
+    return {left + other.left, right + other.right};
+  }
+
+  /**
+   * Subtracts the other DifferentialDriveWheelAccelerations from the current
+   * DifferentialDriveWheelAccelerations and returns the difference.
+   *
+   * <p>For example, DifferentialDriveWheelAccelerations{5.0, 4.0} -
+   * DifferentialDriveWheelAccelerations{1.0, 2.0} =
+   * DifferentialDriveWheelAccelerations{4.0, 2.0}
+   *
+   * @param other The DifferentialDriveWheelAccelerations to subtract.
+   *
+   * @return The difference between the two DifferentialDriveWheelAccelerations.
+   */
+  constexpr DifferentialDriveWheelAccelerations operator-(
+      const DifferentialDriveWheelAccelerations& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Returns the inverse of the current DifferentialDriveWheelAccelerations.
+   * This is equivalent to negating all components of the
+   * DifferentialDriveWheelAccelerations.
+   *
+   * @return The inverse of the current DifferentialDriveWheelAccelerations.
+   */
+  constexpr DifferentialDriveWheelAccelerations operator-() const {
+    return {-left, -right};
+  }
+
+  /**
+   * Multiplies the DifferentialDriveWheelAccelerations by a scalar and returns
+   * the new DifferentialDriveWheelAccelerations.
+   *
+   * <p>For example, DifferentialDriveWheelAccelerations{2.0, 2.5} * 2
+   * = DifferentialDriveWheelAccelerations{4.0, 5.0}
+   *
+   * @param scalar The scalar to multiply by.
+   *
+   * @return The scaled DifferentialDriveWheelAccelerations.
+   */
+  constexpr DifferentialDriveWheelAccelerations operator*(double scalar) const {
+    return {scalar * left, scalar * right};
+  }
+
+  /**
+   * Divides the DifferentialDriveWheelAccelerations by a scalar and returns the
+   * new DifferentialDriveWheelAccelerations.
+   *
+   * <p>For example, DifferentialDriveWheelAccelerations{2.0, 2.5} / 2
+   * = DifferentialDriveWheelAccelerations{1.0, 1.25}
+   *
+   * @param scalar The scalar to divide by.
+   *
+   * @return The scaled DifferentialDriveWheelAccelerations.
+   */
+  constexpr DifferentialDriveWheelAccelerations operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
+
+  /**
+   * Compares two DifferentialDriveWheelAccelerations objects.
+   *
+   * @param other The other DifferentialDriveWheelAccelerations.
+   *
+   * @return True if the DifferentialDriveWheelAccelerations are equal.
+   */
+  constexpr bool operator==(
+      const DifferentialDriveWheelAccelerations& other) const = default;
+};
+}  // namespace wpi::math
+
+#include "wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp"
+#include "wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp"

wpimath/src/main/native/include/wpi/math/kinematics/Kinematics.hpp

@@ -5,6 +5,7 @@
 #pragma once
 
 #include "wpi/math/geometry/Twist2d.hpp"
+#include "wpi/math/kinematics/ChassisAccelerations.hpp"
 #include "wpi/math/kinematics/ChassisSpeeds.hpp"
 #include "wpi/util/SymbolExports.hpp"
 
@@ -18,7 +19,8 @@ namespace wpi::math {
  * Inverse kinematics converts a desired chassis speed into wheel speeds whereas
  * forward kinematics converts wheel speeds into chassis speed.
  */
-template <typename WheelSpeeds, typename WheelPositions>
+template <typename WheelPositions, typename WheelSpeeds,
+          typename WheelAccelerations>
   requires std::copy_constructible<WheelPositions> &&
            std::assignable_from<WheelPositions&, const WheelPositions&>
 class WPILIB_DLLEXPORT Kinematics {
@@ -74,5 +76,29 @@ class WPILIB_DLLEXPORT Kinematics {
   virtual WheelPositions Interpolate(const WheelPositions& start,
                                      const WheelPositions& end,
                                      double t) const = 0;
+
+  /**
+   * Performs forward kinematics to return the resulting chassis accelerations
+   * from the wheel accelerations. This method is often used for dynamics
+   * calculations -- determining the robot's acceleration on the field using
+   * data from the real-world acceleration of each wheel on the robot.
+   *
+   * @param wheelAccelerations The accelerations of the wheels.
+   * @return The chassis accelerations.
+   */
+  virtual ChassisAccelerations ToChassisAccelerations(
+      const WheelAccelerations& wheelAccelerations) const = 0;
+
+  /**
+   * Performs inverse kinematics to return the wheel accelerations from a
+   * desired chassis acceleration. This method is often used for dynamics
+   * calculations -- converting desired robot accelerations into individual
+   * wheel accelerations.
+   *
+   * @param chassisAccelerations The desired chassis accelerations.
+   * @return The wheel accelerations.
+   */
+  virtual WheelAccelerations ToWheelAccelerations(
+      const ChassisAccelerations& chassisAccelerations) const = 0;
 };
 }  // namespace wpi::math

wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveKinematics.hpp

@@ -8,8 +8,10 @@
 
 #include "wpi/math/geometry/Translation2d.hpp"
 #include "wpi/math/geometry/Twist2d.hpp"
+#include "wpi/math/kinematics/ChassisAccelerations.hpp"
 #include "wpi/math/kinematics/ChassisSpeeds.hpp"
 #include "wpi/math/kinematics/Kinematics.hpp"
+#include "wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp"
 #include "wpi/math/kinematics/MecanumDriveWheelPositions.hpp"
 #include "wpi/math/kinematics/MecanumDriveWheelSpeeds.hpp"
 #include "wpi/math/linalg/EigenCore.hpp"
@@ -41,7 +43,8 @@ namespace wpi::math {
  * the robot on the field using encoders and a gyro.
  */
 class WPILIB_DLLEXPORT MecanumDriveKinematics
-    : public Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
+    : public Kinematics<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
+                        MecanumDriveWheelAccelerations> {
  public:
   /**
    * Constructs a mecanum drive kinematics object.
@@ -171,6 +174,18 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics
     return start.Interpolate(end, t);
   }
 
+  ChassisAccelerations ToChassisAccelerations(
+      const MecanumDriveWheelAccelerations& wheelAccelerations) const override;
+
+  MecanumDriveWheelAccelerations ToWheelAccelerations(
+      const ChassisAccelerations& chassisAccelerations,
+      const Translation2d& centerOfRotation) const;
+
+  MecanumDriveWheelAccelerations ToWheelAccelerations(
+      const ChassisAccelerations& chassisAccelerations) const override {
+    return ToWheelAccelerations(chassisAccelerations, {});
+  }
+
  private:
   mutable Matrixd<4, 3> m_inverseKinematics;
   Eigen::HouseholderQR<Matrixd<4, 3>> m_forwardKinematics;

wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveOdometry.hpp

@@ -25,7 +25,8 @@ namespace wpi::math {
  * when using computer-vision systems.
  */
 class WPILIB_DLLEXPORT MecanumDriveOdometry
-    : public Odometry<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
+    : public Odometry<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
+                      MecanumDriveWheelAccelerations> {
  public:
   /**
    * Constructs a MecanumDriveOdometry object.

wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveOdometry3d.hpp

@@ -25,7 +25,8 @@ namespace wpi::math {
  * when using computer-vision systems.
  */
 class WPILIB_DLLEXPORT MecanumDriveOdometry3d
-    : public Odometry3d<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions> {
+    : public Odometry3d<MecanumDriveWheelPositions, MecanumDriveWheelSpeeds,
+                        MecanumDriveWheelAccelerations> {
  public:
   /**
    * Constructs a MecanumDriveOdometry3d object.

wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelAccelerations.hpp

@@ -0,0 +1,125 @@
+// 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 <algorithm>
+#include <array>
+#include <cmath>
+
+#include "wpi/units/acceleration.hpp"
+#include "wpi/units/math.hpp"
+#include "wpi/util/SymbolExports.hpp"
+
+namespace wpi::math {
+/**
+ * Represents the wheel accelerations for a mecanum drive drivetrain.
+ */
+struct WPILIB_DLLEXPORT MecanumDriveWheelAccelerations {
+  /**
+   * Acceleration of the front-left wheel.
+   */
+  units::meters_per_second_squared_t frontLeft = 0_mps_sq;
+
+  /**
+   * Acceleration of the front-right wheel.
+   */
+  units::meters_per_second_squared_t frontRight = 0_mps_sq;
+
+  /**
+   * Acceleration of the rear-left wheel.
+   */
+  units::meters_per_second_squared_t rearLeft = 0_mps_sq;
+
+  /**
+   * Acceleration of the rear-right wheel.
+   */
+  units::meters_per_second_squared_t rearRight = 0_mps_sq;
+
+  /**
+   * Adds two MecanumDriveWheelAccelerations and returns the sum.
+   *
+   * <p>For example, MecanumDriveWheelAccelerations{1.0, 0.5, 2.0, 1.5} +
+   * MecanumDriveWheelAccelerations{2.0, 1.5, 0.5, 1.0} =
+   * MecanumDriveWheelAccelerations{3.0, 2.0, 2.5, 2.5}
+   *
+   * @param other The MecanumDriveWheelAccelerations to add.
+   * @return The sum of the MecanumDriveWheelAccelerations.
+   */
+  constexpr MecanumDriveWheelAccelerations operator+(
+      const MecanumDriveWheelAccelerations& other) const {
+    return {frontLeft + other.frontLeft, frontRight + other.frontRight,
+            rearLeft + other.rearLeft, rearRight + other.rearRight};
+  }
+
+  /**
+   * Subtracts the other MecanumDriveWheelAccelerations from the current
+   * MecanumDriveWheelAccelerations and returns the difference.
+   *
+   * <p>For example, MecanumDriveWheelAccelerations{5.0, 4.0, 6.0, 2.5} -
+   * MecanumDriveWheelAccelerations{1.0, 2.0, 3.0, 0.5} =
+   * MecanumDriveWheelAccelerations{4.0, 2.0, 3.0, 2.0}
+   *
+   * @param other The MecanumDriveWheelAccelerations to subtract.
+   * @return The difference between the two MecanumDriveWheelAccelerations.
+   */
+  constexpr MecanumDriveWheelAccelerations operator-(
+      const MecanumDriveWheelAccelerations& other) const {
+    return *this + -other;
+  }
+
+  /**
+   * Returns the inverse of the current MecanumDriveWheelAccelerations.
+   * This is equivalent to negating all components of the
+   * MecanumDriveWheelAccelerations.
+   *
+   * @return The inverse of the current MecanumDriveWheelAccelerations.
+   */
+  constexpr MecanumDriveWheelAccelerations operator-() const {
+    return {-frontLeft, -frontRight, -rearLeft, -rearRight};
+  }
+
+  /**
+   * Multiplies the MecanumDriveWheelAccelerations by a scalar and returns the
+   * new MecanumDriveWheelAccelerations.
+   *
+   * <p>For example, MecanumDriveWheelAccelerations{2.0, 2.5, 3.0, 3.5} * 2 =
+   * MecanumDriveWheelAccelerations{4.0, 5.0, 6.0, 7.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled MecanumDriveWheelAccelerations.
+   */
+  constexpr MecanumDriveWheelAccelerations operator*(double scalar) const {
+    return {scalar * frontLeft, scalar * frontRight, scalar * rearLeft,
+            scalar * rearRight};
+  }
+
+  /**
+   * Divides the MecanumDriveWheelAccelerations by a scalar and returns the new
+   * MecanumDriveWheelAccelerations.
+   *
+   * <p>For example, MecanumDriveWheelAccelerations{2.0, 2.5, 1.5, 1.0} / 2 =
+   * MecanumDriveWheelAccelerations{1.0, 1.25, 0.75, 0.5}
+   *
+   * @param scalar The scalar to divide by.
+   * @return The scaled MecanumDriveWheelAccelerations.
+   */
+  constexpr MecanumDriveWheelAccelerations operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
+
+  /**
+   * Compares two MecanumDriveWheelAccelerations objects.
+   *
+   * @param other The other MecanumDriveWheelAccelerations.
+   *
+   * @return True if the MecanumDriveWheelAccelerations are equal.
+   */
+  constexpr bool operator==(const MecanumDriveWheelAccelerations& other) const =
+      default;
+};
+}  // namespace wpi::math
+
+#include "wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp"
+#include "wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp"

wpimath/src/main/native/include/wpi/math/kinematics/Odometry.hpp

@@ -22,10 +22,12 @@ namespace wpi::math {
  * path following. Furthermore, odometry can be used for latency compensation
  * when using computer-vision systems.
  *
- * @tparam WheelSpeeds Wheel speeds type.
  * @tparam WheelPositions Wheel positions type.
+ * @tparam WheelSpeeds Wheel speeds type.
+ * @tparam WheelAccelerations Wheel accelerations type.
  */
-template <typename WheelSpeeds, typename WheelPositions>
+template <typename WheelPositions, typename WheelSpeeds,
+          typename WheelAccelerations>
 class WPILIB_DLLEXPORT Odometry {
  public:
   /**
@@ -36,7 +38,8 @@ class WPILIB_DLLEXPORT Odometry {
    * @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,
+  explicit Odometry(const Kinematics<WheelPositions, WheelSpeeds,
+                                     WheelAccelerations>& kinematics,
                     const Rotation2d& gyroAngle,
                     const WheelPositions& wheelPositions,
                     const Pose2d& initialPose = Pose2d{})
@@ -131,7 +134,8 @@ class WPILIB_DLLEXPORT Odometry {
   }
 
  private:
-  const Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
+  const Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>&
+      m_kinematics;
   Pose2d m_pose;
 
   WheelPositions m_previousWheelPositions;

wpimath/src/main/native/include/wpi/math/kinematics/Odometry3d.hpp

@@ -25,10 +25,12 @@ namespace wpi::math {
  * path following. Furthermore, odometry can be used for latency compensation
  * when using computer-vision systems.
  *
- * @tparam WheelSpeeds Wheel speeds type.
  * @tparam WheelPositions Wheel positions type.
+ * @tparam WheelSpeeds Wheel speeds type.
+ * @tparam WheelAccelerations Wheel accelerations type.
  */
-template <typename WheelSpeeds, typename WheelPositions>
+template <typename WheelPositions, typename WheelSpeeds,
+          typename WheelAccelerations>
 class WPILIB_DLLEXPORT Odometry3d {
  public:
   /**
@@ -39,7 +41,8 @@ class WPILIB_DLLEXPORT Odometry3d {
    * @param wheelPositions The current distances measured by each wheel.
    * @param initialPose The starting position of the robot on the field.
    */
-  explicit Odometry3d(const Kinematics<WheelSpeeds, WheelPositions>& kinematics,
+  explicit Odometry3d(const Kinematics<WheelPositions, WheelSpeeds,
+                                       WheelAccelerations>& kinematics,
                       const Rotation3d& gyroAngle,
                       const WheelPositions& wheelPositions,
                       const Pose3d& initialPose = Pose3d{})
@@ -140,7 +143,8 @@ class WPILIB_DLLEXPORT Odometry3d {
   }
 
  private:
-  const Kinematics<WheelSpeeds, WheelPositions>& m_kinematics;
+  const Kinematics<WheelPositions, WheelSpeeds, WheelAccelerations>&
+      m_kinematics;
   Pose3d m_pose;
 
   WheelPositions m_previousWheelPositions;

wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp

@@ -13,8 +13,10 @@
 #include "wpi/math/geometry/Rotation2d.hpp"
 #include "wpi/math/geometry/Translation2d.hpp"
 #include "wpi/math/geometry/Twist2d.hpp"
+#include "wpi/math/kinematics/ChassisAccelerations.hpp"
 #include "wpi/math/kinematics/ChassisSpeeds.hpp"
 #include "wpi/math/kinematics/Kinematics.hpp"
+#include "wpi/math/kinematics/SwerveModuleAcceleration.hpp"
 #include "wpi/math/kinematics/SwerveModulePosition.hpp"
 #include "wpi/math/kinematics/SwerveModuleState.hpp"
 #include "wpi/math/linalg/EigenCore.hpp"
@@ -50,8 +52,10 @@ namespace wpi::math {
  */
 template <size_t NumModules>
 class SwerveDriveKinematics
-    : public Kinematics<wpi::util::array<SwerveModuleState, NumModules>,
-                        wpi::util::array<SwerveModulePosition, NumModules>> {
+    : public Kinematics<
+          wpi::util::array<SwerveModulePosition, NumModules>,
+          wpi::util::array<SwerveModuleState, NumModules>,
+          wpi::util::array<SwerveModuleAcceleration, NumModules>> {
  public:
   /**
    * Constructs a swerve drive kinematics object. This takes in a variable
@@ -71,13 +75,20 @@ class SwerveDriveKinematics
         m_moduleHeadings(wpi::util::empty_array) {
     for (size_t i = 0; i < NumModules; i++) {
       // clang-format off
-      m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
+      m_firstOrderInverseKinematics.template block<2, 3>(i * 2, 0) <<
         1, 0, (-m_modules[i].Y()).value(),
         0, 1, (+m_modules[i].X()).value();
+
+      m_secondOrderInverseKinematics.template block<2, 4>(i * 2, 0) <<
+        1, 0, (-m_modules[i].X()).value(), (-m_modules[i].Y()).value(),
+        0, 1, (-m_modules[i].Y()).value(), (+m_modules[i].X()).value();
       // clang-format on
     }
 
-    m_forwardKinematics = m_inverseKinematics.householderQr();
+    m_firstOrderForwardKinematics =
+        m_firstOrderInverseKinematics.householderQr();
+    m_secondOrderForwardKinematics =
+        m_secondOrderInverseKinematics.householderQr();
 
     wpi::math::MathSharedStore::ReportUsage("SwerveDriveKinematics", "");
   }
@@ -87,13 +98,20 @@ class SwerveDriveKinematics
       : m_modules{modules}, m_moduleHeadings(wpi::util::empty_array) {
     for (size_t i = 0; i < NumModules; i++) {
       // clang-format off
-      m_inverseKinematics.template block<2, 3>(i * 2, 0) <<
+      m_firstOrderInverseKinematics.template block<2, 3>(i * 2, 0) <<
         1, 0, (-m_modules[i].Y()).value(),
         0, 1, (+m_modules[i].X()).value();
+
+      m_secondOrderInverseKinematics.template block<2, 4>(i * 2, 0) <<
+        1, 0, (-m_modules[i].X()).value(), (-m_modules[i].Y()).value(),
+        0, 1, (-m_modules[i].Y()).value(), (+m_modules[i].X()).value();
       // clang-format on
     }
 
-    m_forwardKinematics = m_inverseKinematics.householderQr();
+    m_firstOrderForwardKinematics =
+        m_firstOrderInverseKinematics.householderQr();
+    m_secondOrderForwardKinematics =
+        m_secondOrderInverseKinematics.householderQr();
 
     wpi::math::MathSharedStore::ReportUsage("Kinematics_SwerveDrive", "");
   }
@@ -171,15 +189,7 @@ class SwerveDriveKinematics
 
     // We have a new center of rotation. We need to compute the matrix again.
     if (centerOfRotation != m_previousCoR) {
-      for (size_t i = 0; i < NumModules; i++) {
-        // clang-format off
-      m_inverseKinematics.template block<2, 3>(i * 2, 0) =
-        Matrixd<2, 3>{
-          {1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).value()},
-          {0, 1, (+m_modules[i].X() - centerOfRotation.X()).value()}};
-        // clang-format on
-      }
-      m_previousCoR = centerOfRotation;
+      SetInverseKinematics(centerOfRotation);
     }
 
     Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(),
@@ -187,7 +197,7 @@ class SwerveDriveKinematics
                                         chassisSpeeds.omega.value()};
 
     Matrixd<NumModules * 2, 1> moduleStateMatrix =
-        m_inverseKinematics * chassisSpeedsVector;
+        m_firstOrderInverseKinematics * chassisSpeedsVector;
 
     for (size_t i = 0; i < NumModules; i++) {
       wpi::units::meters_per_second_t x{moduleStateMatrix(i * 2, 0)};
@@ -254,7 +264,7 @@ class SwerveDriveKinematics
     }
 
     Eigen::Vector3d chassisSpeedsVector =
-        m_forwardKinematics.solve(moduleStateMatrix);
+        m_firstOrderForwardKinematics.solve(moduleStateMatrix);
 
     return {wpi::units::meters_per_second_t{chassisSpeedsVector(0)},
             wpi::units::meters_per_second_t{chassisSpeedsVector(1)},
@@ -307,7 +317,7 @@ class SwerveDriveKinematics
     }
 
     Eigen::Vector3d chassisDeltaVector =
-        m_forwardKinematics.solve(moduleDeltaMatrix);
+        m_firstOrderForwardKinematics.solve(moduleDeltaMatrix);
 
     return {wpi::units::meter_t{chassisDeltaVector(0)},
             wpi::units::meter_t{chassisDeltaVector(1)},
@@ -445,13 +455,188 @@ class SwerveDriveKinematics
     return m_modules;
   }
 
+  /**
+   * Performs inverse kinematics to return the module accelerations from a
+   * desired chassis acceleration. This method is often used for dynamics
+   * calculations -- converting desired robot accelerations into individual
+   * module accelerations.
+   *
+   * <p>This function also supports variable centers of rotation. During normal
+   * operations, the center of rotation is usually the same as the physical
+   * center of the robot; therefore, the argument is defaulted to that use case.
+   * However, if you wish to change the center of rotation for evasive
+   * maneuvers, vision alignment, or for any other use case, you can do so.
+   *
+   * @param chassisAccelerations The desired chassis accelerations.
+   * @param angularVelocity The desired robot angular velocity.
+   * @param centerOfRotation The center of rotation. For example, if you set the
+   * center of rotation at one corner of the robot and provide a chassis
+   * acceleration that only has a dtheta component, the robot will rotate around
+   * that corner.
+   * @return An array containing the module accelerations.
+   */
+  wpi::util::array<SwerveModuleAcceleration, NumModules>
+  ToSwerveModuleAccelerations(
+      const ChassisAccelerations& chassisAccelerations,
+      const units::radians_per_second_t angularVelocity = 0.0_rad_per_s,
+      const Translation2d& centerOfRotation = Translation2d{}) const {
+    // Derivation for second-order kinematics from "Swerve Drive Second Order
+    // Kinematics" by FRC Team 449 - The Blair Robot Project, Rafi Pedersen
+    // https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf
+
+    wpi::util::array<SwerveModuleAcceleration, NumModules> moduleAccelerations(
+        wpi::util::empty_array);
+
+    if (chassisAccelerations.ax == 0.0_mps_sq &&
+        chassisAccelerations.ay == 0.0_mps_sq &&
+        chassisAccelerations.alpha == 0.0_rad_per_s_sq) {
+      for (size_t i = 0; i < NumModules; i++) {
+        moduleAccelerations[i] = {0.0_mps_sq, Rotation2d{0.0_rad}};
+      }
+      return moduleAccelerations;
+    }
+
+    if (centerOfRotation != m_previousCoR) {
+      SetInverseKinematics(centerOfRotation);
+    }
+
+    Eigen::Vector4d chassisAccelerationsVector{
+        chassisAccelerations.ax.value(), chassisAccelerations.ay.value(),
+        angularVelocity.value() * angularVelocity.value(),
+        chassisAccelerations.alpha.value()};
+
+    Matrixd<NumModules * 2, 1> moduleAccelerationsMatrix =
+        m_secondOrderInverseKinematics * chassisAccelerationsVector;
+
+    for (size_t i = 0; i < NumModules; i++) {
+      units::meters_per_second_squared_t x{moduleAccelerationsMatrix(i * 2, 0)};
+      units::meters_per_second_squared_t y{
+          moduleAccelerationsMatrix(i * 2 + 1, 0)};
+
+      // For swerve modules, we need to compute both linear acceleration and
+      // angular acceleration The linear acceleration is the magnitude of the
+      // acceleration vector
+      units::meters_per_second_squared_t linearAcceleration =
+          units::math::hypot(x, y);
+
+      if (linearAcceleration.value() < 1e-6) {
+        moduleAccelerations[i] = {linearAcceleration, {}};
+      } else {
+        moduleAccelerations[i] = {linearAcceleration,
+                                  Rotation2d{x.value(), y.value()}};
+      }
+    }
+
+    return moduleAccelerations;
+  }
+
+  /**
+   * Performs inverse kinematics to return the module accelerations from a
+   * desired chassis acceleration. This method is often used for dynamics
+   * calculations -- converting desired robot accelerations into individual
+   * module accelerations.
+   *
+   * @param chassisAccelerations The desired chassis accelerations.
+   * @return An array containing the module accelerations.
+   */
+  wpi::util::array<SwerveModuleAcceleration, NumModules> ToWheelAccelerations(
+      const ChassisAccelerations& chassisAccelerations) const override {
+    return ToSwerveModuleAccelerations(chassisAccelerations);
+  }
+
+  /**
+   * Performs forward kinematics to return the resulting chassis accelerations
+   * from the given module accelerations. This method is often used for dynamics
+   * calculations -- determining the robot's acceleration on the field using
+   * data from the real-world acceleration of each module on the robot.
+   *
+   * @param moduleAccelerations The accelerations of the modules as measured
+   * from respective encoders and gyros. The order of the swerve module
+   * accelerations should be same as passed into the constructor of this class.
+   * @return The resulting chassis accelerations.
+   */
+  template <
+      std::convertible_to<SwerveModuleAcceleration>... ModuleAccelerations>
+    requires(sizeof...(ModuleAccelerations) == NumModules)
+  ChassisAccelerations ToChassisAccelerations(
+      ModuleAccelerations&&... moduleAccelerations) const {
+    return this->ToChassisAccelerations(
+        wpi::util::array<SwerveModuleAcceleration, NumModules>{
+            moduleAccelerations...});
+  }
+
+  /**
+   * Performs forward kinematics to return the resulting chassis accelerations
+   * from the given module accelerations. This method is often used for dynamics
+   * calculations -- determining the robot's acceleration on the field using
+   * data from the real-world acceleration of each module on the robot.
+   *
+   * @param moduleAccelerations The accelerations of the modules as measured
+   * from respective encoders and gyros. The order of the swerve module
+   * accelerations should be same as passed into the constructor of this class.
+   * @return The resulting chassis accelerations.
+   */
+  ChassisAccelerations ToChassisAccelerations(
+      const wpi::util::array<SwerveModuleAcceleration, NumModules>&
+          moduleAccelerations) const override {
+    // Derivation for second-order kinematics from "Swerve Drive Second Order
+    // Kinematics" by FRC Team 449 - The Blair Robot Project, Rafi Pedersen
+    // https://www.chiefdelphi.com/uploads/short-url/qzj4k2LyBs7rLxAem0YajNIlStH.pdf
+
+    Matrixd<NumModules * 2, 1> moduleAccelerationsMatrix;
+
+    for (size_t i = 0; i < NumModules; i++) {
+      SwerveModuleAcceleration module = moduleAccelerations[i];
+
+      moduleAccelerationsMatrix(i * 2 + 0, 0) =
+          module.acceleration.value() * module.angle.Cos();
+      moduleAccelerationsMatrix(i * 2 + 1, 0) =
+          module.acceleration.value() * module.angle.Sin();
+    }
+
+    Eigen::Vector4d chassisAccelerationsVector =
+        m_secondOrderForwardKinematics.solve(moduleAccelerationsMatrix);
+
+    // the second order kinematics equation for swerve drive yields a state
+    // vector [aₓ, a_y, ω², α]
+    return {units::meters_per_second_squared_t{chassisAccelerationsVector(0)},
+            units::meters_per_second_squared_t{chassisAccelerationsVector(1)},
+            units::radians_per_second_squared_t{chassisAccelerationsVector(3)}};
+  }
+
  private:
   wpi::util::array<Translation2d, NumModules> m_modules;
-  mutable Matrixd<NumModules * 2, 3> m_inverseKinematics;
-  Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>> m_forwardKinematics;
+  mutable Matrixd<NumModules * 2, 3> m_firstOrderInverseKinematics;
+  Eigen::HouseholderQR<Matrixd<NumModules * 2, 3>>
+      m_firstOrderForwardKinematics;
+  mutable Matrixd<NumModules * 2, 4> m_secondOrderInverseKinematics;
+  Eigen::HouseholderQR<Matrixd<NumModules * 2, 4>>
+      m_secondOrderForwardKinematics;
   mutable wpi::util::array<Rotation2d, NumModules> m_moduleHeadings;
 
   mutable Translation2d m_previousCoR;
+
+  /**
+   * Sets both inverse kinematics matrices based on the new center of rotation.
+   * This does not check if the new center of rotation is different from the
+   * previous one, so a check should be included before the call to this
+   * function.
+   *
+   * @param centerOfRotation new center of rotation
+   */
+  void SetInverseKinematics(const Translation2d& centerOfRotation) const {
+    for (size_t i = 0; i < NumModules; i++) {
+      const double rx = m_modules[i].X().value() - centerOfRotation.X().value();
+      const double ry = m_modules[i].Y().value() - centerOfRotation.Y().value();
+
+      m_firstOrderInverseKinematics.row(i * 2 + 0) << 1, 0, -ry;
+      m_firstOrderInverseKinematics.row(i * 2 + 1) << 0, 1, rx;
+
+      m_secondOrderInverseKinematics.row(i * 2 + 0) << 1, 0, -rx, -ry;
+      m_secondOrderInverseKinematics.row(i * 2 + 1) << 0, 1, -ry, +rx;
+    }
+    m_previousCoR = centerOfRotation;
+  }
 };
 
 template <typename ModuleTranslation, typename... ModuleTranslations>

wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry.hpp

@@ -29,8 +29,9 @@ namespace wpi::math {
  */
 template <size_t NumModules>
 class SwerveDriveOdometry
-    : public Odometry<wpi::util::array<SwerveModuleState, NumModules>,
-                      wpi::util::array<SwerveModulePosition, NumModules>> {
+    : public Odometry<wpi::util::array<SwerveModulePosition, NumModules>,
+                      wpi::util::array<SwerveModuleState, NumModules>,
+                      wpi::util::array<SwerveModuleAcceleration, NumModules>> {
  public:
   /**
    * Constructs a SwerveDriveOdometry object.

wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveOdometry3d.hpp

@@ -30,8 +30,10 @@ namespace wpi::math {
  */
 template <size_t NumModules>
 class SwerveDriveOdometry3d
-    : public Odometry3d<wpi::util::array<SwerveModuleState, NumModules>,
-                        wpi::util::array<SwerveModulePosition, NumModules>> {
+    : public Odometry3d<
+          wpi::util::array<SwerveModulePosition, NumModules>,
+          wpi::util::array<SwerveModuleState, NumModules>,
+          wpi::util::array<SwerveModuleAcceleration, NumModules>> {
  public:
   /**
    * Constructs a SwerveDriveOdometry3d object.

wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleAcceleration.hpp

@@ -0,0 +1,112 @@
+// 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/math/geometry/Rotation2d.hpp"
+#include "wpi/units/acceleration.hpp"
+#include "wpi/units/math.hpp"
+#include "wpi/util/SymbolExports.hpp"
+
+namespace wpi::math {
+/**
+ * Represents the accelerations of one swerve module.
+ */
+struct WPILIB_DLLEXPORT SwerveModuleAcceleration {
+  /**
+   * Acceleration of the wheel of the module.
+   */
+  units::meters_per_second_squared_t acceleration = 0_mps_sq;
+
+  /**
+   * Angle of the acceleration vector.
+   */
+  Rotation2d angle;
+
+  /**
+   * Checks equality between this SwerveModuleAccelerations and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  constexpr bool operator==(const SwerveModuleAcceleration& other) const {
+    return units::math::abs(acceleration - other.acceleration) < 1E-9_mps_sq &&
+           angle == other.angle;
+  }
+
+  /**
+   * Adds two SwerveModuleAccelerations and returns the sum.
+   * Note: This performs vector addition of the accelerations.
+   *
+   * @param other The SwerveModuleAccelerations to add.
+   * @return The sum of the SwerveModuleAccelerations.
+   */
+  SwerveModuleAcceleration operator+(
+      const SwerveModuleAcceleration& other) const {
+    // Convert to Cartesian coordinates, add, then convert back
+    auto thisX = acceleration * angle.Cos();
+    auto thisY = acceleration * angle.Sin();
+    auto otherX = other.acceleration * other.angle.Cos();
+    auto otherY = other.acceleration * other.angle.Sin();
+
+    auto sumX = thisX + otherX;
+    auto sumY = thisY + otherY;
+
+    auto resultAcceleration = units::math::hypot(sumX, sumY);
+    auto resultAngle = Rotation2d{sumX.value(), sumY.value()};
+
+    return {resultAcceleration, resultAngle};
+  }
+
+  /**
+   * Subtracts the other SwerveModuleAccelerations from the current
+   * SwerveModuleAccelerations and returns the difference.
+   *
+   * @param other The SwerveModuleAccelerations to subtract.
+   * @return The difference between the two SwerveModuleAccelerations.
+   */
+  SwerveModuleAcceleration operator-(
+      const SwerveModuleAcceleration& other) const {
+    return *this + (-other);
+  }
+
+  /**
+   * Returns the inverse of the current SwerveModuleAccelerations.
+   * This is equivalent to rotating the acceleration by pi (or 180 degrees).
+   *
+   * @return The inverse of the current SwerveModuleAccelerations.
+   */
+  constexpr SwerveModuleAcceleration operator-() const {
+    return {acceleration, angle + Rotation2d{180_deg}};
+  }
+
+  /**
+   * Multiplies the SwerveModuleAccelerations by a scalar and returns the new
+   * SwerveModuleAccelerations.
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled SwerveModuleAccelerations.
+   */
+  constexpr SwerveModuleAcceleration operator*(double scalar) const {
+    if (scalar < 0) {
+      return {-scalar * acceleration, angle + Rotation2d{180_deg}};
+    }
+    return {scalar * acceleration, angle};
+  }
+
+  /**
+   * Divides the SwerveModuleAccelerations by a scalar and returns the new
+   * SwerveModuleAccelerations.
+   *
+   * @param scalar The scalar to divide by.
+   * @return The scaled SwerveModuleAccelerations.
+   */
+  constexpr SwerveModuleAcceleration operator/(double scalar) const {
+    return operator*(1.0 / scalar);
+  }
+};
+}  // namespace wpi::math
+
+#include "wpi/math/kinematics/proto/SwerveModuleAccelerationProto.hpp"
+#include "wpi/math/kinematics/struct/SwerveModuleAccelerationStruct.hpp"

wpimath/src/main/native/include/wpi/math/kinematics/proto/ChassisAccelerationsProto.hpp

@@ -0,0 +1,23 @@
+// 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/math/kinematics/ChassisAccelerations.hpp"
+#include "wpi/util/SymbolExports.hpp"
+#include "wpi/util/protobuf/Protobuf.hpp"
+#include "wpimath/protobuf/kinematics.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::util::Protobuf<wpi::math::ChassisAccelerations> {
+  using MessageStruct = wpi_proto_ProtobufChassisAccelerations;
+  using InputStream =
+      wpi::util::ProtoInputStream<wpi::math::ChassisAccelerations>;
+  using OutputStream =
+      wpi::util::ProtoOutputStream<wpi::math::ChassisAccelerations>;
+  static std::optional<wpi::math::ChassisAccelerations> Unpack(
+      InputStream& stream);
+  static bool Pack(OutputStream& stream,
+                   const wpi::math::ChassisAccelerations& value);
+};

wpimath/src/main/native/include/wpi/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.hpp

@@ -0,0 +1,24 @@
+// 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/math/kinematics/DifferentialDriveWheelAccelerations.hpp"
+#include "wpi/util/SymbolExports.hpp"
+#include "wpi/util/protobuf/Protobuf.hpp"
+#include "wpimath/protobuf/kinematics.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT
+    wpi::util::Protobuf<wpi::math::DifferentialDriveWheelAccelerations> {
+  using MessageStruct = wpi_proto_ProtobufDifferentialDriveWheelAccelerations;
+  using InputStream = wpi::util::ProtoInputStream<
+      wpi::math::DifferentialDriveWheelAccelerations>;
+  using OutputStream = wpi::util::ProtoOutputStream<
+      wpi::math::DifferentialDriveWheelAccelerations>;
+  static std::optional<wpi::math::DifferentialDriveWheelAccelerations> Unpack(
+      InputStream& stream);
+  static bool Pack(OutputStream& stream,
+                   const wpi::math::DifferentialDriveWheelAccelerations& value);
+};

wpimath/src/main/native/include/wpi/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.hpp

@@ -0,0 +1,24 @@
+// 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/math/kinematics/MecanumDriveWheelAccelerations.hpp"
+#include "wpi/util/SymbolExports.hpp"
+#include "wpi/util/protobuf/Protobuf.hpp"
+#include "wpimath/protobuf/kinematics.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT
+    wpi::util::Protobuf<wpi::math::MecanumDriveWheelAccelerations> {
+  using MessageStruct = wpi_proto_ProtobufMecanumDriveWheelAccelerations;
+  using InputStream =
+      wpi::util::ProtoInputStream<wpi::math::MecanumDriveWheelAccelerations>;
+  using OutputStream =
+      wpi::util::ProtoOutputStream<wpi::math::MecanumDriveWheelAccelerations>;
+  static std::optional<wpi::math::MecanumDriveWheelAccelerations> Unpack(
+      InputStream& stream);
+  static bool Pack(OutputStream& stream,
+                   const wpi::math::MecanumDriveWheelAccelerations& value);
+};

wpimath/src/main/native/include/wpi/math/kinematics/proto/SwerveModuleAccelerationProto.hpp

@@ -0,0 +1,24 @@
+// 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/math/kinematics/SwerveModuleAcceleration.hpp"
+#include "wpi/util/SymbolExports.hpp"
+#include "wpi/util/protobuf/Protobuf.hpp"
+#include "wpimath/protobuf/kinematics.npb.h"
+
+template <>
+struct WPILIB_DLLEXPORT
+    wpi::util::Protobuf<wpi::math::SwerveModuleAcceleration> {
+  using MessageStruct = wpi_proto_ProtobufSwerveModuleAcceleration;
+  using InputStream =
+      wpi::util::ProtoInputStream<wpi::math::SwerveModuleAcceleration>;
+  using OutputStream =
+      wpi::util::ProtoOutputStream<wpi::math::SwerveModuleAcceleration>;
+  static std::optional<wpi::math::SwerveModuleAcceleration> Unpack(
+      InputStream& stream);
+  static bool Pack(OutputStream& stream,
+                   const wpi::math::SwerveModuleAcceleration& value);
+};

wpimath/src/main/native/include/wpi/math/kinematics/struct/ChassisAccelerationsStruct.hpp

@@ -0,0 +1,26 @@
+// 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/math/kinematics/ChassisAccelerations.hpp"
+#include "wpi/util/SymbolExports.hpp"
+#include "wpi/util/struct/Struct.hpp"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::ChassisAccelerations> {
+  static constexpr std::string_view GetTypeName() {
+    return "ChassisAccelerations";
+  }
+  static constexpr size_t GetSize() { return 24; }
+  static constexpr std::string_view GetSchema() {
+    return "double ax;double ay;double alpha";
+  }
+
+  static wpi::math::ChassisAccelerations Unpack(std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const wpi::math::ChassisAccelerations& value);
+};
+
+static_assert(wpi::util::StructSerializable<wpi::math::ChassisAccelerations>);

wpimath/src/main/native/include/wpi/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.hpp

@@ -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/math/kinematics/DifferentialDriveWheelAccelerations.hpp"
+#include "wpi/util/SymbolExports.hpp"
+#include "wpi/util/struct/Struct.hpp"
+
+template <>
+struct WPILIB_DLLEXPORT
+    wpi::util::Struct<wpi::math::DifferentialDriveWheelAccelerations> {
+  static constexpr std::string_view GetTypeName() {
+    return "DifferentialDriveWheelAccelerations";
+  }
+  static constexpr size_t GetSize() { return 16; }
+  static constexpr std::string_view GetSchema() {
+    return "double left;double right";
+  }
+
+  static wpi::math::DifferentialDriveWheelAccelerations Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const wpi::math::DifferentialDriveWheelAccelerations& value);
+};
+
+static_assert(wpi::util::StructSerializable<
+              wpi::math::DifferentialDriveWheelAccelerations>);

wpimath/src/main/native/include/wpi/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.hpp

@@ -0,0 +1,30 @@
+// 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/math/kinematics/MecanumDriveWheelAccelerations.hpp"
+#include "wpi/util/SymbolExports.hpp"
+#include "wpi/util/struct/Struct.hpp"
+
+template <>
+struct WPILIB_DLLEXPORT
+    wpi::util::Struct<wpi::math::MecanumDriveWheelAccelerations> {
+  static constexpr std::string_view GetTypeName() {
+    return "MecanumDriveWheelAccelerations";
+  }
+  static constexpr size_t GetSize() { return 32; }
+  static constexpr std::string_view GetSchema() {
+    return "double front_left;double front_right;double rear_left;double "
+           "rear_right";
+  }
+
+  static wpi::math::MecanumDriveWheelAccelerations Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const wpi::math::MecanumDriveWheelAccelerations& value);
+};
+
+static_assert(
+    wpi::util::StructSerializable<wpi::math::MecanumDriveWheelAccelerations>);

wpimath/src/main/native/include/wpi/math/kinematics/struct/SwerveModuleAccelerationStruct.hpp

@@ -0,0 +1,35 @@
+// 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/math/kinematics/SwerveModuleAcceleration.hpp"
+#include "wpi/util/SymbolExports.hpp"
+#include "wpi/util/struct/Struct.hpp"
+
+template <>
+struct WPILIB_DLLEXPORT wpi::util::Struct<wpi::math::SwerveModuleAcceleration> {
+  static constexpr std::string_view GetTypeName() {
+    return "SwerveModuleAccelerations";
+  }
+  static constexpr size_t GetSize() {
+    return 8 + wpi::util::GetStructSize<wpi::math::Rotation2d>();
+  }
+  static constexpr std::string_view GetSchema() {
+    return "double acceleration;Rotation2d angle";
+  }
+
+  static wpi::math::SwerveModuleAcceleration Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(std::span<uint8_t> data,
+                   const wpi::math::SwerveModuleAcceleration& value);
+  static void ForEachNested(
+      std::invocable<std::string_view, std::string_view> auto fn) {
+    wpi::util::ForEachStructSchema<wpi::math::Rotation2d>(fn);
+  }
+};
+
+static_assert(
+    wpi::util::StructSerializable<wpi::math::SwerveModuleAcceleration>);
+static_assert(wpi::util::HasNestedStruct<wpi::math::SwerveModuleAcceleration>);