[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/java/org/wpilib/math/estimator/PoseEstimator.java

@@ -66,7 +66,7 @@ public class PoseEstimator<T> {
    */
   @SuppressWarnings("PMD.UnusedFormalParameter")
   public PoseEstimator(
-      Kinematics<?, T> kinematics,
+      Kinematics<T, ?, ?> kinematics,
       Odometry<T> odometry,
       Matrix<N3, N1> stateStdDevs,
       Matrix<N3, N1> visionMeasurementStdDevs) {

wpimath/src/main/java/org/wpilib/math/estimator/PoseEstimator3d.java

@@ -74,7 +74,7 @@ public class PoseEstimator3d<T> {
    */
   @SuppressWarnings("PMD.UnusedFormalParameter")
   public PoseEstimator3d(
-      Kinematics<?, T> kinematics,
+      Kinematics<T, ?, ?> kinematics,
       Odometry3d<T> odometry,
       Matrix<N4, N1> stateStdDevs,
       Matrix<N4, N1> visionMeasurementStdDevs) {

wpimath/src/main/java/org/wpilib/math/kinematics/ChassisAccelerations.java

@@ -0,0 +1,200 @@
+// 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.
+
+package org.wpilib.math.kinematics;
+
+import static org.wpilib.units.Units.MetersPerSecondPerSecond;
+import static org.wpilib.units.Units.RadiansPerSecondPerSecond;
+
+import java.util.Objects;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.geometry.Translation2d;
+import org.wpilib.math.interpolation.Interpolatable;
+import org.wpilib.math.kinematics.proto.ChassisAccelerationsProto;
+import org.wpilib.math.kinematics.struct.ChassisAccelerationsStruct;
+import org.wpilib.math.util.MathUtil;
+import org.wpilib.units.measure.AngularAcceleration;
+import org.wpilib.units.measure.LinearAcceleration;
+import org.wpilib.util.protobuf.ProtobufSerializable;
+import org.wpilib.util.struct.StructSerializable;
+
+/**
+ * Represents the acceleration of a robot chassis.
+ *
+ * <p>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.
+ */
+public class ChassisAccelerations
+    implements ProtobufSerializable, StructSerializable, Interpolatable<ChassisAccelerations> {
+  /** Acceleration along the x-axis in meters per second squared. (Fwd is +) */
+  public double ax;
+
+  /** Acceleration along the y-axis in meters per second squared. (Left is +) */
+  public double ay;
+
+  /** Angular acceleration of the robot frame in radians per second squared. (CCW is +) */
+  public double alpha;
+
+  /** ChassisAccelerations struct for serialization. */
+  public static final ChassisAccelerationsStruct struct = new ChassisAccelerationsStruct();
+
+  /** ChassisAccelerations proto for serialization. */
+  public static final ChassisAccelerationsProto proto = new ChassisAccelerationsProto();
+
+  /** Constructs a ChassisAccelerations with zeros for ax, ay, and omega. */
+  public ChassisAccelerations() {}
+
+  /**
+   * Constructs a ChassisAccelerations object.
+   *
+   * @param ax Forward acceleration in meters per second squared.
+   * @param ay Sideways acceleration in meters per second squared.
+   * @param alpha Angular acceleration in radians per second squared.
+   */
+  public ChassisAccelerations(double ax, double ay, double alpha) {
+    this.ax = ax;
+    this.ay = ay;
+    this.alpha = alpha;
+  }
+
+  /**
+   * Constructs a ChassisAccelerations object.
+   *
+   * @param ax Forward acceleration.
+   * @param ay Sideways acceleration.
+   * @param alpha Angular acceleration.
+   */
+  public ChassisAccelerations(
+      LinearAcceleration ax, LinearAcceleration ay, AngularAcceleration alpha) {
+    this(
+        ax.in(MetersPerSecondPerSecond),
+        ay.in(MetersPerSecondPerSecond),
+        alpha.in(RadiansPerSecondPerSecond));
+  }
+
+  /**
+   * 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.
+   */
+  public ChassisAccelerations toRobotRelative(Rotation2d robotAngle) {
+    // CW rotation into chassis frame
+    var rotated = new Translation2d(ax, ay).rotateBy(robotAngle.unaryMinus());
+    return new ChassisAccelerations(rotated.getX(), rotated.getY(), 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.
+   */
+  public ChassisAccelerations toFieldRelative(Rotation2d robotAngle) {
+    // CCW rotation out of chassis frame
+    var rotated = new Translation2d(ax, ay).rotateBy(robotAngle);
+    return new ChassisAccelerations(rotated.getX(), rotated.getY(), alpha);
+  }
+
+  /**
+   * Adds two ChassisAccelerations and returns the sum.
+   *
+   * <p>For example, ChassisAccelerations{1.0, 0.5, 0.75} + ChassisAccelerations{2.0, 1.5, 0.25} =
+   * ChassisAccelerations{3.0, 2.0, 1.0}
+   *
+   * @param other The ChassisAccelerations to add.
+   * @return The sum of the ChassisAccelerations.
+   */
+  public ChassisAccelerations plus(ChassisAccelerations other) {
+    return new ChassisAccelerations(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.
+   */
+  public ChassisAccelerations minus(ChassisAccelerations other) {
+    return new ChassisAccelerations(ax - other.ax, ay - other.ay, alpha - other.alpha);
+  }
+
+  /**
+   * Returns the inverse of the current ChassisAccelerations. This is equivalent to negating all
+   * components of the ChassisAccelerations.
+   *
+   * @return The inverse of the current ChassisAccelerations.
+   */
+  public ChassisAccelerations unaryMinus() {
+    return new ChassisAccelerations(-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, 1.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled ChassisAccelerations.
+   */
+  public ChassisAccelerations times(double scalar) {
+    return new ChassisAccelerations(ax * scalar, ay * scalar, alpha * scalar);
+  }
+
+  /**
+   * 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.
+   */
+  public ChassisAccelerations div(double scalar) {
+    return new ChassisAccelerations(ax / scalar, ay / scalar, alpha / scalar);
+  }
+
+  @Override
+  public ChassisAccelerations interpolate(ChassisAccelerations endValue, double t) {
+    if (t <= 0) {
+      return this;
+    } else if (t >= 1) {
+      return endValue;
+    } else {
+      return new ChassisAccelerations(
+          MathUtil.lerp(this.ax, endValue.ax, t),
+          MathUtil.lerp(this.ay, endValue.ay, t),
+          MathUtil.lerp(this.alpha, endValue.alpha, t));
+    }
+  }
+
+  @Override
+  public final int hashCode() {
+    return Objects.hash(ax, ay, alpha);
+  }
+
+  @Override
+  public boolean equals(Object o) {
+    return o == this
+        || o instanceof ChassisAccelerations c && ax == c.ax && ay == c.ay && alpha == c.alpha;
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "ChassisAccelerations(Ax: %.2f m/s², Ay: %.2f m/s², Alpha: %.2f rad/s²)", ax, ay, alpha);
+  }
+}

wpimath/src/main/java/org/wpilib/math/kinematics/ChassisSpeeds.java

@@ -12,8 +12,10 @@ import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.geometry.Transform2d;
 import org.wpilib.math.geometry.Translation2d;
 import org.wpilib.math.geometry.Twist2d;
+import org.wpilib.math.interpolation.Interpolatable;
 import org.wpilib.math.kinematics.proto.ChassisSpeedsProto;
 import org.wpilib.math.kinematics.struct.ChassisSpeedsStruct;
+import org.wpilib.math.util.MathUtil;
 import org.wpilib.units.measure.AngularVelocity;
 import org.wpilib.units.measure.LinearVelocity;
 import org.wpilib.util.protobuf.ProtobufSerializable;
@@ -28,7 +30,8 @@ import org.wpilib.util.struct.StructSerializable;
  * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum
  * will often have all three components.
  */
-public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
+public class ChassisSpeeds
+    implements ProtobufSerializable, StructSerializable, Interpolatable<ChassisSpeeds> {
   /** Velocity along the x-axis in meters per second. (Fwd is +) */
   public double vx;
 
@@ -199,6 +202,20 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable {
     return new ChassisSpeeds(vx / scalar, vy / scalar, omega / scalar);
   }
 
+  @Override
+  public ChassisSpeeds interpolate(ChassisSpeeds endValue, double t) {
+    if (t <= 0) {
+      return this;
+    } else if (t >= 1) {
+      return endValue;
+    } else {
+      return new ChassisSpeeds(
+          MathUtil.lerp(this.vx, endValue.vx, t),
+          MathUtil.lerp(this.vy, endValue.vy, t),
+          MathUtil.lerp(this.omega, endValue.omega, t));
+    }
+  }
+
   @Override
   public final int hashCode() {
     return Objects.hash(vx, vy, omega);

wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveKinematics.java

@@ -23,7 +23,10 @@ import org.wpilib.util.struct.StructSerializable;
  * chassis speed.
  */
 public class DifferentialDriveKinematics
-    implements Kinematics<DifferentialDriveWheelSpeeds, DifferentialDriveWheelPositions>,
+    implements Kinematics<
+            DifferentialDriveWheelPositions,
+            DifferentialDriveWheelSpeeds,
+            DifferentialDriveWheelAccelerations>,
         ProtobufSerializable,
         StructSerializable {
   /** Differential drive trackwidth in meters. */
@@ -88,6 +91,23 @@ public class DifferentialDriveKinematics
         chassisSpeeds.vx + trackwidth / 2 * chassisSpeeds.omega);
   }
 
+  @Override
+  public ChassisAccelerations toChassisAccelerations(
+      DifferentialDriveWheelAccelerations wheelAccelerations) {
+    return new ChassisAccelerations(
+        (wheelAccelerations.left + wheelAccelerations.right) / 2,
+        0.0,
+        (wheelAccelerations.right - wheelAccelerations.left) / trackwidth);
+  }
+
+  @Override
+  public DifferentialDriveWheelAccelerations toWheelAccelerations(
+      ChassisAccelerations chassisAccelerations) {
+    return new DifferentialDriveWheelAccelerations(
+        chassisAccelerations.ax - trackwidth / 2 * chassisAccelerations.alpha,
+        chassisAccelerations.ax + trackwidth / 2 * chassisAccelerations.alpha);
+  }
+
   @Override
   public Twist2d toTwist2d(
       DifferentialDriveWheelPositions start, DifferentialDriveWheelPositions end) {

wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelAccelerations.java

@@ -0,0 +1,148 @@
+// 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.
+
+package org.wpilib.math.kinematics;
+
+import static org.wpilib.units.Units.MetersPerSecondPerSecond;
+
+import org.wpilib.math.interpolation.Interpolatable;
+import org.wpilib.math.kinematics.proto.DifferentialDriveWheelAccelerationsProto;
+import org.wpilib.math.kinematics.struct.DifferentialDriveWheelAccelerationsStruct;
+import org.wpilib.units.measure.LinearAcceleration;
+import org.wpilib.util.protobuf.ProtobufSerializable;
+import org.wpilib.util.struct.StructSerializable;
+
+/** Represents the wheel accelerations for a differential drive drivetrain. */
+public class DifferentialDriveWheelAccelerations
+    implements Interpolatable<DifferentialDriveWheelAccelerations>,
+        ProtobufSerializable,
+        StructSerializable {
+  /** Acceleration of the left side of the robot in meters per second squared. */
+  public double left;
+
+  /** Acceleration of the right side of the robot in meters per second squared. */
+  public double right;
+
+  /** DifferentialDriveWheelAccelerations protobuf for serialization. */
+  public static final DifferentialDriveWheelAccelerationsProto proto =
+      new DifferentialDriveWheelAccelerationsProto();
+
+  /** DifferentialDriveWheelAccelerations struct for serialization. */
+  public static final DifferentialDriveWheelAccelerationsStruct struct =
+      new DifferentialDriveWheelAccelerationsStruct();
+
+  /**
+   * Constructs a DifferentialDriveWheelAccelerations with zeros for left and right accelerations.
+   */
+  public DifferentialDriveWheelAccelerations() {}
+
+  /**
+   * Constructs a DifferentialDriveWheelAccelerations.
+   *
+   * @param left The left acceleration in meters per second squared.
+   * @param right The right acceleration in meters per second squared.
+   */
+  public DifferentialDriveWheelAccelerations(double left, double right) {
+    this.left = left;
+    this.right = right;
+  }
+
+  /**
+   * Constructs a DifferentialDriveWheelAccelerations.
+   *
+   * @param left The left acceleration in meters per second squared.
+   * @param right The right acceleration in meters per second squared.
+   */
+  public DifferentialDriveWheelAccelerations(LinearAcceleration left, LinearAcceleration right) {
+    this(left.in(MetersPerSecondPerSecond), right.in(MetersPerSecondPerSecond));
+  }
+
+  /**
+   * 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.
+   */
+  public DifferentialDriveWheelAccelerations plus(DifferentialDriveWheelAccelerations other) {
+    return new DifferentialDriveWheelAccelerations(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.
+   */
+  public DifferentialDriveWheelAccelerations minus(DifferentialDriveWheelAccelerations other) {
+    return new DifferentialDriveWheelAccelerations(left - other.left, right - other.right);
+  }
+
+  /**
+   * Returns the inverse of the current DifferentialDriveWheelAccelerations. This is equivalent to
+   * negating all components of the DifferentialDriveWheelAccelerations.
+   *
+   * @return The inverse of the current DifferentialDriveWheelAccelerations.
+   */
+  public DifferentialDriveWheelAccelerations unaryMinus() {
+    return new DifferentialDriveWheelAccelerations(-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.
+   */
+  public DifferentialDriveWheelAccelerations times(double scalar) {
+    return new DifferentialDriveWheelAccelerations(left * scalar, right * scalar);
+  }
+
+  /**
+   * 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.
+   */
+  public DifferentialDriveWheelAccelerations div(double scalar) {
+    return new DifferentialDriveWheelAccelerations(left / scalar, right / scalar);
+  }
+
+  /**
+   * Returns the linear interpolation of this DifferentialDriveWheelAccelerations and another.
+   *
+   * @param endValue The end value for the interpolation.
+   * @param t How far between the two values to interpolate. This is clamped to [0, 1].
+   * @return The interpolated value.
+   */
+  @Override
+  public DifferentialDriveWheelAccelerations interpolate(
+      DifferentialDriveWheelAccelerations endValue, double t) {
+    // Clamp t to [0, 1]
+    t = Math.max(0.0, Math.min(1.0, t));
+
+    return new DifferentialDriveWheelAccelerations(
+        left + t * (endValue.left - left), right + t * (endValue.right - right));
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "DifferentialDriveWheelAccelerations(Left: %.2f m/s², Right: %.2f m/s²)", left, right);
+  }
+}

wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelSpeeds.java

@@ -6,6 +6,7 @@ package org.wpilib.math.kinematics;
 
 import static org.wpilib.units.Units.MetersPerSecond;
 
+import org.wpilib.math.interpolation.Interpolatable;
 import org.wpilib.math.kinematics.proto.DifferentialDriveWheelSpeedsProto;
 import org.wpilib.math.kinematics.struct.DifferentialDriveWheelSpeedsStruct;
 import org.wpilib.units.measure.LinearVelocity;
@@ -13,7 +14,10 @@ import org.wpilib.util.protobuf.ProtobufSerializable;
 import org.wpilib.util.struct.StructSerializable;
 
 /** Represents the wheel speeds for a differential drive drivetrain. */
-public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, StructSerializable {
+public class DifferentialDriveWheelSpeeds
+    implements Interpolatable<DifferentialDriveWheelSpeeds>,
+        ProtobufSerializable,
+        StructSerializable {
   /** Speed of the left side of the robot in meters per second. */
   public double left;
 
@@ -150,6 +154,22 @@ public class DifferentialDriveWheelSpeeds implements ProtobufSerializable, Struc
     return new DifferentialDriveWheelSpeeds(left / scalar, right / scalar);
   }
 
+  /**
+   * Returns the linear interpolation of this DifferentialDriveWheelSpeeds and another.
+   *
+   * @param endValue The end value for the interpolation.
+   * @param t How far between the two values to interpolate. This is clamped to [0, 1].
+   * @return The interpolated value.
+   */
+  @Override
+  public DifferentialDriveWheelSpeeds interpolate(DifferentialDriveWheelSpeeds endValue, double t) {
+    // Clamp t to [0, 1]
+    t = Math.max(0.0, Math.min(1.0, t));
+
+    return new DifferentialDriveWheelSpeeds(
+        left + t * (endValue.left - left), right + t * (endValue.right - right));
+  }
+
   @Override
   public String toString() {
     return String.format(

wpimath/src/main/java/org/wpilib/math/kinematics/Kinematics.java

@@ -8,14 +8,15 @@ import org.wpilib.math.geometry.Twist2d;
 import org.wpilib.math.interpolation.Interpolator;
 
 /**
- * Helper class that converts a chassis velocity (dx and dtheta components) into wheel speeds. Robot
- * code should not use this directly- Instead, use the particular type for your drivetrain (e.g.,
- * {@link DifferentialDriveKinematics}).
+ * Helper class that converts a chassis velocity (dx and dtheta components) into wheel speeds and
+ * chassis accelerations into wheel accelerations. Robot code should not use this directly- Instead,
+ * use the particular type for your drivetrain (e.g., {@link DifferentialDriveKinematics}).
  *
- * @param <S> The type of the wheel speeds.
  * @param <P> The type of the wheel positions.
+ * @param <S> The type of the wheel speeds.
+ * @param <A> The type of the wheel accelerations.
  */
-public interface Kinematics<S, P> extends Interpolator<P> {
+public interface Kinematics<P, S, A> extends Interpolator<P> {
   /**
    * 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
@@ -35,6 +36,27 @@ public interface Kinematics<S, P> extends Interpolator<P> {
    */
   S toWheelSpeeds(ChassisSpeeds chassisSpeeds);
 
+  /**
+   * 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.
+   */
+  ChassisAccelerations toChassisAccelerations(A wheelAccelerations);
+
+  /**
+   * 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.
+   */
+  A toWheelAccelerations(ChassisAccelerations chassisAccelerations);
+
   /**
    * Performs forward kinematics to return the resulting Twist2d from the given change in wheel
    * positions. This method is often used for odometry -- determining the robot's position on the

wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveKinematics.java

@@ -34,7 +34,8 @@ import org.wpilib.util.struct.StructSerializable;
  * field using encoders and a gyro.
  */
 public class MecanumDriveKinematics
-    implements Kinematics<MecanumDriveWheelSpeeds, MecanumDriveWheelPositions>,
+    implements Kinematics<
+            MecanumDriveWheelPositions, MecanumDriveWheelSpeeds, MecanumDriveWheelAccelerations>,
         ProtobufSerializable,
         StructSerializable {
   private final SimpleMatrix m_inverseKinematics;
@@ -162,6 +163,88 @@ public class MecanumDriveKinematics
         chassisSpeedsVector.get(2, 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.
+   *
+   * <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 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 The wheel accelerations.
+   */
+  public MecanumDriveWheelAccelerations toWheelAccelerations(
+      ChassisAccelerations chassisAccelerations, Translation2d centerOfRotation) {
+    // We have a new center of rotation. We need to compute the matrix again.
+    if (!centerOfRotation.equals(m_prevCoR)) {
+      var fl = m_frontLeftWheel.minus(centerOfRotation);
+      var fr = m_frontRightWheel.minus(centerOfRotation);
+      var rl = m_rearLeftWheel.minus(centerOfRotation);
+      var rr = m_rearRightWheel.minus(centerOfRotation);
+
+      setInverseKinematics(fl, fr, rl, rr);
+      m_prevCoR = centerOfRotation;
+    }
+
+    var chassisAccelerationsVector = new SimpleMatrix(3, 1);
+    chassisAccelerationsVector.setColumn(
+        0, 0, chassisAccelerations.ax, chassisAccelerations.ay, chassisAccelerations.alpha);
+
+    var wheelsVector = m_inverseKinematics.mult(chassisAccelerationsVector);
+    return new MecanumDriveWheelAccelerations(
+        wheelsVector.get(0, 0),
+        wheelsVector.get(1, 0),
+        wheelsVector.get(2, 0),
+        wheelsVector.get(3, 0));
+  }
+
+  /**
+   * Performs inverse kinematics. See {@link #toWheelAccelerations(ChassisAccelerations,
+   * Translation2d)} for more information.
+   *
+   * @param chassisAccelerations The desired chassis accelerations.
+   * @return The wheel accelerations.
+   */
+  @Override
+  public MecanumDriveWheelAccelerations toWheelAccelerations(
+      ChassisAccelerations chassisAccelerations) {
+    return toWheelAccelerations(chassisAccelerations, Translation2d.kZero);
+  }
+
+  /**
+   * Performs forward kinematics to return the resulting chassis accelerations from the given 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 current mecanum drive wheel accelerations.
+   * @return The resulting chassis accelerations.
+   */
+  @Override
+  public ChassisAccelerations toChassisAccelerations(
+      MecanumDriveWheelAccelerations wheelAccelerations) {
+    var wheelAccelerationsVector = new SimpleMatrix(4, 1);
+    wheelAccelerationsVector.setColumn(
+        0,
+        0,
+        wheelAccelerations.frontLeft,
+        wheelAccelerations.frontRight,
+        wheelAccelerations.rearLeft,
+        wheelAccelerations.rearRight);
+    var chassisAccelerationsVector = m_forwardKinematics.mult(wheelAccelerationsVector);
+
+    return new ChassisAccelerations(
+        chassisAccelerationsVector.get(0, 0),
+        chassisAccelerationsVector.get(1, 0),
+        chassisAccelerationsVector.get(2, 0));
+  }
+
   @Override
   public Twist2d toTwist2d(MecanumDriveWheelPositions start, MecanumDriveWheelPositions end) {
     var wheelDeltasVector = new SimpleMatrix(4, 1);

wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelAccelerations.java

@@ -0,0 +1,184 @@
+// 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.
+
+package org.wpilib.math.kinematics;
+
+import static org.wpilib.units.Units.MetersPerSecondPerSecond;
+
+import org.wpilib.math.interpolation.Interpolatable;
+import org.wpilib.math.kinematics.proto.MecanumDriveWheelAccelerationsProto;
+import org.wpilib.math.kinematics.struct.MecanumDriveWheelAccelerationsStruct;
+import org.wpilib.units.measure.LinearAcceleration;
+import org.wpilib.util.protobuf.ProtobufSerializable;
+import org.wpilib.util.struct.StructSerializable;
+
+/** Represents the wheel accelerations for a mecanum drive drivetrain. */
+public class MecanumDriveWheelAccelerations
+    implements Interpolatable<MecanumDriveWheelAccelerations>,
+        ProtobufSerializable,
+        StructSerializable {
+  /** Acceleration of the front left wheel in meters per second squared. */
+  public double frontLeft;
+
+  /** Acceleration of the front right wheel in meters per second squared. */
+  public double frontRight;
+
+  /** Acceleration of the rear left wheel in meters per second squared. */
+  public double rearLeft;
+
+  /** Acceleration of the rear right wheel in meters per second squared. */
+  public double rearRight;
+
+  /** MecanumDriveWheelAccelerations protobuf for serialization. */
+  public static final MecanumDriveWheelAccelerationsProto proto =
+      new MecanumDriveWheelAccelerationsProto();
+
+  /** MecanumDriveWheelAccelerations struct for serialization. */
+  public static final MecanumDriveWheelAccelerationsStruct struct =
+      new MecanumDriveWheelAccelerationsStruct();
+
+  /** Constructs a MecanumDriveWheelAccelerations with zeros for all member fields. */
+  public MecanumDriveWheelAccelerations() {}
+
+  /**
+   * Constructs a MecanumDriveWheelAccelerations.
+   *
+   * @param frontLeft Acceleration of the front left wheel in meters per second squared.
+   * @param frontRight Acceleration of the front right wheel in meters per second squared.
+   * @param rearLeft Acceleration of the rear left wheel in meters per second squared.
+   * @param rearRight Acceleration of the rear right wheel in meters per second squared.
+   */
+  public MecanumDriveWheelAccelerations(
+      double frontLeft, double frontRight, double rearLeft, double rearRight) {
+    this.frontLeft = frontLeft;
+    this.frontRight = frontRight;
+    this.rearLeft = rearLeft;
+    this.rearRight = rearRight;
+  }
+
+  /**
+   * Constructs a MecanumDriveWheelAccelerations.
+   *
+   * @param frontLeft Acceleration of the front left wheel in meters per second squared.
+   * @param frontRight Acceleration of the front right wheel in meters per second squared.
+   * @param rearLeft Acceleration of the rear left wheel in meters per second squared.
+   * @param rearRight Acceleration of the rear right wheel in meters per second squared.
+   */
+  public MecanumDriveWheelAccelerations(
+      LinearAcceleration frontLeft,
+      LinearAcceleration frontRight,
+      LinearAcceleration rearLeft,
+      LinearAcceleration rearRight) {
+    this(
+        frontLeft.in(MetersPerSecondPerSecond),
+        frontRight.in(MetersPerSecondPerSecond),
+        rearLeft.in(MetersPerSecondPerSecond),
+        rearRight.in(MetersPerSecondPerSecond));
+  }
+
+  /**
+   * 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.
+   */
+  public MecanumDriveWheelAccelerations plus(MecanumDriveWheelAccelerations other) {
+    return new MecanumDriveWheelAccelerations(
+        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.
+   */
+  public MecanumDriveWheelAccelerations minus(MecanumDriveWheelAccelerations other) {
+    return new MecanumDriveWheelAccelerations(
+        frontLeft - other.frontLeft,
+        frontRight - other.frontRight,
+        rearLeft - other.rearLeft,
+        rearRight - other.rearRight);
+  }
+
+  /**
+   * Returns the inverse of the current MecanumDriveWheelAccelerations. This is equivalent to
+   * negating all components of the MecanumDriveWheelAccelerations.
+   *
+   * @return The inverse of the current MecanumDriveWheelAccelerations.
+   */
+  public MecanumDriveWheelAccelerations unaryMinus() {
+    return new MecanumDriveWheelAccelerations(-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.
+   */
+  public MecanumDriveWheelAccelerations times(double scalar) {
+    return new MecanumDriveWheelAccelerations(
+        frontLeft * scalar, frontRight * scalar, rearLeft * scalar, rearRight * scalar);
+  }
+
+  /**
+   * 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.
+   */
+  public MecanumDriveWheelAccelerations div(double scalar) {
+    return new MecanumDriveWheelAccelerations(
+        frontLeft / scalar, frontRight / scalar, rearLeft / scalar, rearRight / scalar);
+  }
+
+  /**
+   * Returns the linear interpolation of this MecanumDriveWheelAccelerations and another.
+   *
+   * @param endValue The end value for the interpolation.
+   * @param t How far between the two values to interpolate. This is clamped to [0, 1].
+   * @return The interpolated value.
+   */
+  @Override
+  public MecanumDriveWheelAccelerations interpolate(
+      MecanumDriveWheelAccelerations endValue, double t) {
+    // Clamp t to [0, 1]
+    t = Math.max(0.0, Math.min(1.0, t));
+
+    return new MecanumDriveWheelAccelerations(
+        frontLeft + t * (endValue.frontLeft - frontLeft),
+        frontRight + t * (endValue.frontRight - frontRight),
+        rearLeft + t * (endValue.rearLeft - rearLeft),
+        rearRight + t * (endValue.rearRight - rearRight));
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "MecanumDriveWheelAccelerations(Front Left: %.2f m/s², Front Right: %.2f m/s², "
+            + "Rear Left: %.2f m/s², Rear Right: %.2f m/s²)",
+        frontLeft, frontRight, rearLeft, rearRight);
+  }
+}

wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelSpeeds.java

@@ -6,6 +6,7 @@ package org.wpilib.math.kinematics;
 
 import static org.wpilib.units.Units.MetersPerSecond;
 
+import org.wpilib.math.interpolation.Interpolatable;
 import org.wpilib.math.kinematics.proto.MecanumDriveWheelSpeedsProto;
 import org.wpilib.math.kinematics.struct.MecanumDriveWheelSpeedsStruct;
 import org.wpilib.units.measure.LinearVelocity;
@@ -13,7 +14,8 @@ import org.wpilib.util.protobuf.ProtobufSerializable;
 import org.wpilib.util.struct.StructSerializable;
 
 /** Represents the wheel speeds for a mecanum drive drivetrain. */
-public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSerializable {
+public class MecanumDriveWheelSpeeds
+    implements Interpolatable<MecanumDriveWheelSpeeds>, ProtobufSerializable, StructSerializable {
   /** Speed of the front left wheel in meters per second. */
   public double frontLeft;
 
@@ -186,6 +188,25 @@ public class MecanumDriveWheelSpeeds implements ProtobufSerializable, StructSeri
         frontLeft / scalar, frontRight / scalar, rearLeft / scalar, rearRight / scalar);
   }
 
+  /**
+   * Returns the linear interpolation of this MecanumDriveWheelSpeeds and another.
+   *
+   * @param endValue The end value for the interpolation.
+   * @param t How far between the two values to interpolate. This is clamped to [0, 1].
+   * @return The interpolated value.
+   */
+  @Override
+  public MecanumDriveWheelSpeeds interpolate(MecanumDriveWheelSpeeds endValue, double t) {
+    // Clamp t to [0, 1]
+    t = Math.max(0.0, Math.min(1.0, t));
+
+    return new MecanumDriveWheelSpeeds(
+        frontLeft + t * (endValue.frontLeft - frontLeft),
+        frontRight + t * (endValue.frontRight - frontRight),
+        rearLeft + t * (endValue.rearLeft - rearLeft),
+        rearRight + t * (endValue.rearRight - rearRight));
+  }
+
   @Override
   public String toString() {
     return String.format(

wpimath/src/main/java/org/wpilib/math/kinematics/Odometry.java

@@ -20,7 +20,7 @@ import org.wpilib.math.geometry.Translation2d;
  * @param <T> Wheel positions type.
  */
 public class Odometry<T> {
-  private final Kinematics<?, T> m_kinematics;
+  private final Kinematics<T, ?, ?> m_kinematics;
   private Pose2d m_pose;
 
   private Rotation2d m_gyroOffset;
@@ -36,7 +36,7 @@ public class Odometry<T> {
    * @param initialPose The starting position of the robot on the field.
    */
   public Odometry(
-      Kinematics<?, T> kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPose) {
+      Kinematics<T, ?, ?> kinematics, Rotation2d gyroAngle, T wheelPositions, Pose2d initialPose) {
     m_kinematics = kinematics;
     m_pose = initialPose;
     m_gyroOffset = m_pose.getRotation().minus(gyroAngle);

wpimath/src/main/java/org/wpilib/math/kinematics/Odometry3d.java

@@ -29,7 +29,7 @@ import org.wpilib.math.geometry.Twist3d;
  * @param <T> Wheel positions type.
  */
 public class Odometry3d<T> {
-  private final Kinematics<?, T> m_kinematics;
+  private final Kinematics<T, ?, ?> m_kinematics;
   private Pose3d m_pose;
 
   private Rotation3d m_gyroOffset;
@@ -45,7 +45,7 @@ public class Odometry3d<T> {
    * @param initialPose The starting position of the robot on the field.
    */
   public Odometry3d(
-      Kinematics<?, T> kinematics, Rotation3d gyroAngle, T wheelPositions, Pose3d initialPose) {
+      Kinematics<T, ?, ?> kinematics, Rotation3d gyroAngle, T wheelPositions, Pose3d initialPose) {
     m_kinematics = kinematics;
     m_pose = initialPose;
     m_gyroOffset = m_pose.getRotation().minus(gyroAngle);

wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java

@@ -43,11 +43,13 @@ import org.wpilib.util.struct.StructSerializable;
  */
 @SuppressWarnings("overrides")
 public class SwerveDriveKinematics
-    implements Kinematics<SwerveModuleState[], SwerveModulePosition[]>,
+    implements Kinematics<SwerveModulePosition[], SwerveModuleState[], SwerveModuleAcceleration[]>,
         ProtobufSerializable,
         StructSerializable {
-  private final SimpleMatrix m_inverseKinematics;
-  private final SimpleMatrix m_forwardKinematics;
+  private final SimpleMatrix m_firstOrderInverseKinematics;
+  private final SimpleMatrix m_firstOrderForwardKinematics;
+  private final SimpleMatrix m_secondOrderInverseKinematics;
+  private final SimpleMatrix m_secondOrderForwardKinematics;
 
   private final int m_numModules;
   private final Translation2d[] m_modules;
@@ -72,13 +74,13 @@ public class SwerveDriveKinematics
     m_modules = Arrays.copyOf(moduleTranslations, m_numModules);
     m_moduleHeadings = new Rotation2d[m_numModules];
     Arrays.fill(m_moduleHeadings, Rotation2d.kZero);
-    m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
+    m_firstOrderInverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
+    m_secondOrderInverseKinematics = new SimpleMatrix(m_numModules * 2, 4);
 
-    for (int i = 0; i < m_numModules; i++) {
-      m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY());
-      m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX());
-    }
-    m_forwardKinematics = m_inverseKinematics.pseudoInverse();
+    setInverseKinematics(Translation2d.kZero);
+
+    m_firstOrderForwardKinematics = m_firstOrderInverseKinematics.pseudoInverse();
+    m_secondOrderForwardKinematics = m_secondOrderInverseKinematics.pseudoInverse();
 
     MathSharedStore.reportUsage("SwerveDriveKinematics", "");
   }
@@ -132,19 +134,13 @@ public class SwerveDriveKinematics
     }
 
     if (!centerOfRotation.equals(m_prevCoR)) {
-      for (int i = 0; i < m_numModules; i++) {
-        m_inverseKinematics.setRow(
-            i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY() + centerOfRotation.getY());
-        m_inverseKinematics.setRow(
-            i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX() - centerOfRotation.getX());
-      }
-      m_prevCoR = centerOfRotation;
+      setInverseKinematics(centerOfRotation);
     }
 
     var chassisSpeedsVector = new SimpleMatrix(3, 1);
     chassisSpeedsVector.setColumn(0, 0, chassisSpeeds.vx, chassisSpeeds.vy, chassisSpeeds.omega);
 
-    var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
+    var moduleStatesMatrix = m_firstOrderInverseKinematics.mult(chassisSpeedsVector);
 
     for (int i = 0; i < m_numModules; i++) {
       double x = moduleStatesMatrix.get(i * 2, 0);
@@ -201,7 +197,7 @@ public class SwerveDriveKinematics
       moduleStatesMatrix.set(i * 2 + 1, module.speed * module.angle.getSin());
     }
 
-    var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix);
+    var chassisSpeedsVector = m_firstOrderForwardKinematics.mult(moduleStatesMatrix);
     return new ChassisSpeeds(
         chassisSpeedsVector.get(0, 0),
         chassisSpeedsVector.get(1, 0),
@@ -229,10 +225,10 @@ public class SwerveDriveKinematics
     for (int i = 0; i < m_numModules; i++) {
       var module = moduleDeltas[i];
       moduleDeltaMatrix.set(i * 2, 0, module.distance * module.angle.getCos());
-      moduleDeltaMatrix.set(i * 2 + 1, module.distance * module.angle.getSin());
+      moduleDeltaMatrix.set(i * 2 + 1, 0, module.distance * module.angle.getSin());
     }
 
-    var chassisDeltaVector = m_forwardKinematics.mult(moduleDeltaMatrix);
+    var chassisDeltaVector = m_firstOrderForwardKinematics.mult(moduleDeltaMatrix);
     return new Twist2d(
         chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0));
   }
@@ -441,4 +437,155 @@ public class SwerveDriveKinematics
   public static final SwerveDriveKinematicsStruct getStruct(int numModules) {
     return new SwerveDriveKinematicsStruct(numModules);
   }
+
+  /**
+   * 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.
+   */
+  public SwerveModuleAcceleration[] toSwerveModuleAccelerations(
+      ChassisAccelerations chassisAccelerations,
+      double angularVelocity,
+      Translation2d centerOfRotation) {
+    // 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
+
+    var moduleAccelerations = new SwerveModuleAcceleration[m_numModules];
+
+    if (chassisAccelerations.ax == 0.0
+        && chassisAccelerations.ay == 0.0
+        && chassisAccelerations.alpha == 0.0) {
+      for (int i = 0; i < m_numModules; i++) {
+        moduleAccelerations[i] = new SwerveModuleAcceleration(0.0, Rotation2d.kZero);
+      }
+      return moduleAccelerations;
+    }
+
+    if (!centerOfRotation.equals(m_prevCoR)) {
+      setInverseKinematics(centerOfRotation);
+    }
+
+    var chassisAccelerationsVector = new SimpleMatrix(4, 1);
+    chassisAccelerationsVector.setColumn(
+        0,
+        0,
+        chassisAccelerations.ax,
+        chassisAccelerations.ay,
+        angularVelocity * angularVelocity,
+        chassisAccelerations.alpha);
+
+    var moduleAccelerationsMatrix = m_secondOrderInverseKinematics.mult(chassisAccelerationsVector);
+
+    for (int i = 0; i < m_numModules; i++) {
+      double x = moduleAccelerationsMatrix.get(i * 2, 0);
+      double y = moduleAccelerationsMatrix.get(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
+      double linearAcceleration = Math.hypot(x, y);
+
+      if (linearAcceleration <= 1e-6) {
+        moduleAccelerations[i] = new SwerveModuleAcceleration(linearAcceleration, Rotation2d.kZero);
+      } else {
+        moduleAccelerations[i] =
+            new SwerveModuleAcceleration(linearAcceleration, new Rotation2d(x, y));
+      }
+    }
+
+    return moduleAccelerations;
+  }
+
+  /**
+   * Performs inverse kinematics. See {@link #toSwerveModuleAccelerations(ChassisAccelerations,
+   * double, Translation2d)} toSwerveModuleAccelerations for more information.
+   *
+   * @param chassisAccelerations The desired chassis accelerations.
+   * @param angularVelocity The desired robot angular velocity.
+   * @return An array containing the module accelerations.
+   */
+  public SwerveModuleAcceleration[] toSwerveModuleAccelerations(
+      ChassisAccelerations chassisAccelerations, double angularVelocity) {
+    return toSwerveModuleAccelerations(chassisAccelerations, angularVelocity, Translation2d.kZero);
+  }
+
+  @Override
+  public SwerveModuleAcceleration[] toWheelAccelerations(
+      ChassisAccelerations chassisAccelerations) {
+    return toSwerveModuleAccelerations(chassisAccelerations, 0.0);
+  }
+
+  /**
+   * 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.
+   */
+  @Override
+  public ChassisAccelerations toChassisAccelerations(
+      SwerveModuleAcceleration... moduleAccelerations) {
+    // 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
+
+    if (moduleAccelerations.length != m_numModules) {
+      throw new IllegalArgumentException(
+          "Number of modules is not consistent with number of module locations provided in "
+              + "constructor");
+    }
+    var moduleAccelerationsMatrix = new SimpleMatrix(m_numModules * 2, 1);
+
+    for (int i = 0; i < m_numModules; i++) {
+      var module = moduleAccelerations[i];
+
+      moduleAccelerationsMatrix.set(i * 2 + 0, 0, module.acceleration * module.angle.getCos());
+      moduleAccelerationsMatrix.set(i * 2 + 1, 0, module.acceleration * module.angle.getSin());
+    }
+
+    var chassisAccelerationsVector = m_secondOrderForwardKinematics.mult(moduleAccelerationsMatrix);
+
+    // the second order kinematics equation for swerve drive yields a state vector [aₓ, a_y, ω², α]
+    return new ChassisAccelerations(
+        chassisAccelerationsVector.get(0, 0),
+        chassisAccelerationsVector.get(1, 0),
+        chassisAccelerationsVector.get(3, 0));
+  }
+
+  /**
+   * 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
+   */
+  private void setInverseKinematics(Translation2d centerOfRotation) {
+    for (int i = 0; i < m_numModules; i++) {
+      var rx = m_modules[i].getX() - centerOfRotation.getX();
+      var ry = m_modules[i].getY() - centerOfRotation.getY();
+
+      m_firstOrderInverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -ry);
+      m_firstOrderInverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, rx);
+
+      m_secondOrderInverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -rx, -ry);
+      m_secondOrderInverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, -ry, +rx);
+    }
+    m_prevCoR = centerOfRotation;
+  }
 }

wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleAcceleration.java

@@ -0,0 +1,106 @@
+// 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.
+
+package org.wpilib.math.kinematics;
+
+import static org.wpilib.units.Units.MetersPerSecondPerSecond;
+
+import java.util.Objects;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.interpolation.Interpolatable;
+import org.wpilib.math.kinematics.proto.SwerveModuleAccelerationProto;
+import org.wpilib.math.kinematics.struct.SwerveModuleAccelerationStruct;
+import org.wpilib.units.measure.LinearAcceleration;
+import org.wpilib.util.protobuf.ProtobufSerializable;
+import org.wpilib.util.struct.StructSerializable;
+
+/** Represents the accelerations of one swerve module. */
+public class SwerveModuleAcceleration
+    implements Interpolatable<SwerveModuleAcceleration>,
+        Comparable<SwerveModuleAcceleration>,
+        ProtobufSerializable,
+        StructSerializable {
+  /** Acceleration of the wheel of the module in meters per second squared. */
+  public double acceleration;
+
+  /** Angle of the acceleration vector. */
+  public Rotation2d angle = new Rotation2d();
+
+  /** SwerveModuleAccelerations protobuf for serialization. */
+  public static final SwerveModuleAccelerationProto proto = new SwerveModuleAccelerationProto();
+
+  /** SwerveModuleAccelerations struct for serialization. */
+  public static final SwerveModuleAccelerationStruct struct = new SwerveModuleAccelerationStruct();
+
+  /** Constructs a SwerveModuleAccelerations with zeros for acceleration and angle. */
+  public SwerveModuleAcceleration() {}
+
+  /**
+   * Constructs a SwerveModuleAccelerations.
+   *
+   * @param acceleration The acceleration of the wheel of the module in meters per second squared.
+   * @param angle The angle of the acceleration vector.
+   */
+  public SwerveModuleAcceleration(double acceleration, Rotation2d angle) {
+    this.acceleration = acceleration;
+    this.angle = angle;
+  }
+
+  /**
+   * Constructs a SwerveModuleAccelerations.
+   *
+   * @param acceleration The acceleration of the wheel of the module.
+   * @param angle The angle of the acceleration vector.
+   */
+  public SwerveModuleAcceleration(LinearAcceleration acceleration, Rotation2d angle) {
+    this(acceleration.in(MetersPerSecondPerSecond), angle);
+  }
+
+  /**
+   * Returns the linear interpolation of this SwerveModuleAccelerations and another.
+   *
+   * @param endValue The end value for the interpolation.
+   * @param t How far between the two values to interpolate. This is clamped to [0, 1].
+   * @return The interpolated value.
+   */
+  @Override
+  public SwerveModuleAcceleration interpolate(SwerveModuleAcceleration endValue, double t) {
+    // Clamp t to [0, 1]
+    t = Math.max(0.0, Math.min(1.0, t));
+
+    return new SwerveModuleAcceleration(
+        acceleration + t * (endValue.acceleration - acceleration),
+        angle.interpolate(endValue.angle, t));
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    return obj instanceof SwerveModuleAcceleration other
+        && Math.abs(other.acceleration - acceleration) < 1E-9
+        && angle.equals(other.angle);
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(acceleration, angle);
+  }
+
+  /**
+   * Compares two swerve module accelerations. One swerve module is "greater" than the other if its
+   * acceleration magnitude is higher than the other.
+   *
+   * @param other The other swerve module.
+   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
+   */
+  @Override
+  public int compareTo(SwerveModuleAcceleration other) {
+    return Double.compare(this.acceleration, other.acceleration);
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "SwerveModuleAccelerations(Acceleration: %.2f m/s², Angle: %s)", acceleration, angle);
+  }
+}

wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleState.java

@@ -8,6 +8,7 @@ import static org.wpilib.units.Units.MetersPerSecond;
 
 import java.util.Objects;
 import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.interpolation.Interpolatable;
 import org.wpilib.math.kinematics.proto.SwerveModuleStateProto;
 import org.wpilib.math.kinematics.struct.SwerveModuleStateStruct;
 import org.wpilib.units.measure.LinearVelocity;
@@ -16,7 +17,10 @@ import org.wpilib.util.struct.StructSerializable;
 
 /** Represents the state of one swerve module. */
 public class SwerveModuleState
-    implements Comparable<SwerveModuleState>, ProtobufSerializable, StructSerializable {
+    implements Interpolatable<SwerveModuleState>,
+        Comparable<SwerveModuleState>,
+        ProtobufSerializable,
+        StructSerializable {
   /** Speed of the wheel of the module in meters per second. */
   public double speed;
 
@@ -119,6 +123,28 @@ public class SwerveModuleState
     }
   }
 
+  /**
+   * Returns the linear interpolation of this SwerveModuleState and another. The angle is
+   * interpolated using the shortest path between the two angles.
+   *
+   * @param endValue The end value for the interpolation.
+   * @param t How far between the two values to interpolate. This is clamped to [0, 1].
+   * @return The interpolated value.
+   */
+  @Override
+  public SwerveModuleState interpolate(SwerveModuleState endValue, double t) {
+    // Clamp t to [0, 1]
+    t = Math.max(0.0, Math.min(1.0, t));
+
+    // Interpolate speed linearly
+    double interpolatedSpeed = speed + t * (endValue.speed - speed);
+
+    // Interpolate angle using the shortest path
+    Rotation2d interpolatedAngle = angle.interpolate(endValue.angle, t);
+
+    return new SwerveModuleState(interpolatedSpeed, interpolatedAngle);
+  }
+
   /**
    * Scales speed by cosine of angle error. This scales down movement perpendicular to the desired
    * direction of travel that can occur when modules change directions. This results in smoother

wpimath/src/main/java/org/wpilib/math/kinematics/proto/ChassisAccelerationsProto.java

@@ -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.
+
+package org.wpilib.math.kinematics.proto;
+
+import org.wpilib.math.kinematics.ChassisAccelerations;
+import org.wpilib.math.proto.Kinematics.ProtobufChassisAccelerations;
+import org.wpilib.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class ChassisAccelerationsProto
+    implements Protobuf<ChassisAccelerations, ProtobufChassisAccelerations> {
+  @Override
+  public Class<ChassisAccelerations> getTypeClass() {
+    return ChassisAccelerations.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufChassisAccelerations.getDescriptor();
+  }
+
+  @Override
+  public ProtobufChassisAccelerations createMessage() {
+    return ProtobufChassisAccelerations.newInstance();
+  }
+
+  @Override
+  public ChassisAccelerations unpack(ProtobufChassisAccelerations msg) {
+    return new ChassisAccelerations(msg.getAx(), msg.getAy(), msg.getAlpha());
+  }
+
+  @Override
+  public void pack(ProtobufChassisAccelerations msg, ChassisAccelerations value) {
+    msg.setAx(value.ax);
+    msg.setAy(value.ay);
+    msg.setAlpha(value.alpha);
+  }
+}

wpimath/src/main/java/org/wpilib/math/kinematics/proto/DifferentialDriveWheelAccelerationsProto.java

@@ -0,0 +1,42 @@
+// 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.
+
+package org.wpilib.math.kinematics.proto;
+
+import org.wpilib.math.kinematics.DifferentialDriveWheelAccelerations;
+import org.wpilib.math.proto.Kinematics.ProtobufDifferentialDriveWheelAccelerations;
+import org.wpilib.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class DifferentialDriveWheelAccelerationsProto
+    implements Protobuf<
+        DifferentialDriveWheelAccelerations, ProtobufDifferentialDriveWheelAccelerations> {
+  @Override
+  public Class<DifferentialDriveWheelAccelerations> getTypeClass() {
+    return DifferentialDriveWheelAccelerations.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufDifferentialDriveWheelAccelerations.getDescriptor();
+  }
+
+  @Override
+  public ProtobufDifferentialDriveWheelAccelerations createMessage() {
+    return ProtobufDifferentialDriveWheelAccelerations.newInstance();
+  }
+
+  @Override
+  public DifferentialDriveWheelAccelerations unpack(
+      ProtobufDifferentialDriveWheelAccelerations msg) {
+    return new DifferentialDriveWheelAccelerations(msg.getLeft(), msg.getRight());
+  }
+
+  @Override
+  public void pack(
+      ProtobufDifferentialDriveWheelAccelerations msg, DifferentialDriveWheelAccelerations value) {
+    msg.setLeft(value.left);
+    msg.setRight(value.right);
+  }
+}

wpimath/src/main/java/org/wpilib/math/kinematics/proto/MecanumDriveWheelAccelerationsProto.java

@@ -0,0 +1,43 @@
+// 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.
+
+package org.wpilib.math.kinematics.proto;
+
+import org.wpilib.math.kinematics.MecanumDriveWheelAccelerations;
+import org.wpilib.math.proto.Kinematics.ProtobufMecanumDriveWheelAccelerations;
+import org.wpilib.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class MecanumDriveWheelAccelerationsProto
+    implements Protobuf<MecanumDriveWheelAccelerations, ProtobufMecanumDriveWheelAccelerations> {
+  @Override
+  public Class<MecanumDriveWheelAccelerations> getTypeClass() {
+    return MecanumDriveWheelAccelerations.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufMecanumDriveWheelAccelerations.getDescriptor();
+  }
+
+  @Override
+  public ProtobufMecanumDriveWheelAccelerations createMessage() {
+    return ProtobufMecanumDriveWheelAccelerations.newInstance();
+  }
+
+  @Override
+  public MecanumDriveWheelAccelerations unpack(ProtobufMecanumDriveWheelAccelerations msg) {
+    return new MecanumDriveWheelAccelerations(
+        msg.getFrontLeft(), msg.getFrontRight(), msg.getRearLeft(), msg.getRearRight());
+  }
+
+  @Override
+  public void pack(
+      ProtobufMecanumDriveWheelAccelerations msg, MecanumDriveWheelAccelerations value) {
+    msg.setFrontLeft(value.frontLeft);
+    msg.setFrontRight(value.frontRight);
+    msg.setRearLeft(value.rearLeft);
+    msg.setRearRight(value.rearRight);
+  }
+}

wpimath/src/main/java/org/wpilib/math/kinematics/proto/SwerveModuleAccelerationProto.java

@@ -0,0 +1,41 @@
+// 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.
+
+package org.wpilib.math.kinematics.proto;
+
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.kinematics.SwerveModuleAcceleration;
+import org.wpilib.math.proto.Kinematics.ProtobufSwerveModuleAcceleration;
+import org.wpilib.util.protobuf.Protobuf;
+import us.hebi.quickbuf.Descriptors.Descriptor;
+
+public class SwerveModuleAccelerationProto
+    implements Protobuf<SwerveModuleAcceleration, ProtobufSwerveModuleAcceleration> {
+  @Override
+  public Class<SwerveModuleAcceleration> getTypeClass() {
+    return SwerveModuleAcceleration.class;
+  }
+
+  @Override
+  public Descriptor getDescriptor() {
+    return ProtobufSwerveModuleAcceleration.getDescriptor();
+  }
+
+  @Override
+  public ProtobufSwerveModuleAcceleration createMessage() {
+    return ProtobufSwerveModuleAcceleration.newInstance();
+  }
+
+  @Override
+  public SwerveModuleAcceleration unpack(ProtobufSwerveModuleAcceleration msg) {
+    return new SwerveModuleAcceleration(
+        msg.getAcceleration(), Rotation2d.proto.unpack(msg.getAngle()));
+  }
+
+  @Override
+  public void pack(ProtobufSwerveModuleAcceleration msg, SwerveModuleAcceleration value) {
+    msg.setAcceleration(value.acceleration);
+    Rotation2d.proto.pack(msg.getMutableAngle(), value.angle);
+  }
+}

wpimath/src/main/java/org/wpilib/math/kinematics/struct/ChassisAccelerationsStruct.java

@@ -0,0 +1,46 @@
+// 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.
+
+package org.wpilib.math.kinematics.struct;
+
+import java.nio.ByteBuffer;
+import org.wpilib.math.kinematics.ChassisAccelerations;
+import org.wpilib.util.struct.Struct;
+
+public class ChassisAccelerationsStruct implements Struct<ChassisAccelerations> {
+  @Override
+  public Class<ChassisAccelerations> getTypeClass() {
+    return ChassisAccelerations.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "ChassisAccelerations";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 3;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double ax;double ay;double alpha";
+  }
+
+  @Override
+  public ChassisAccelerations unpack(ByteBuffer bb) {
+    double ax = bb.getDouble();
+    double ay = bb.getDouble();
+    double alpha = bb.getDouble();
+    return new ChassisAccelerations(ax, ay, alpha);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, ChassisAccelerations value) {
+    bb.putDouble(value.ax);
+    bb.putDouble(value.ay);
+    bb.putDouble(value.alpha);
+  }
+}

wpimath/src/main/java/org/wpilib/math/kinematics/struct/DifferentialDriveWheelAccelerationsStruct.java

@@ -0,0 +1,45 @@
+// 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.
+
+package org.wpilib.math.kinematics.struct;
+
+import java.nio.ByteBuffer;
+import org.wpilib.math.kinematics.DifferentialDriveWheelAccelerations;
+import org.wpilib.util.struct.Struct;
+
+public class DifferentialDriveWheelAccelerationsStruct
+    implements Struct<DifferentialDriveWheelAccelerations> {
+  @Override
+  public Class<DifferentialDriveWheelAccelerations> getTypeClass() {
+    return DifferentialDriveWheelAccelerations.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "DifferentialDriveWheelAccelerations";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 2;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double left;double right";
+  }
+
+  @Override
+  public DifferentialDriveWheelAccelerations unpack(ByteBuffer bb) {
+    double left = bb.getDouble();
+    double right = bb.getDouble();
+    return new DifferentialDriveWheelAccelerations(left, right);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, DifferentialDriveWheelAccelerations value) {
+    bb.putDouble(value.left);
+    bb.putDouble(value.right);
+  }
+}

wpimath/src/main/java/org/wpilib/math/kinematics/struct/MecanumDriveWheelAccelerationsStruct.java

@@ -0,0 +1,49 @@
+// 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.
+
+package org.wpilib.math.kinematics.struct;
+
+import java.nio.ByteBuffer;
+import org.wpilib.math.kinematics.MecanumDriveWheelAccelerations;
+import org.wpilib.util.struct.Struct;
+
+public class MecanumDriveWheelAccelerationsStruct
+    implements Struct<MecanumDriveWheelAccelerations> {
+  @Override
+  public Class<MecanumDriveWheelAccelerations> getTypeClass() {
+    return MecanumDriveWheelAccelerations.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "MecanumDriveWheelAccelerations";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble * 4;
+  }
+
+  @Override
+  public String getSchema() {
+    return "double front_left;double front_right;double rear_left;double rear_right";
+  }
+
+  @Override
+  public MecanumDriveWheelAccelerations unpack(ByteBuffer bb) {
+    double frontLeft = bb.getDouble();
+    double frontRight = bb.getDouble();
+    double rearLeft = bb.getDouble();
+    double rearRight = bb.getDouble();
+    return new MecanumDriveWheelAccelerations(frontLeft, frontRight, rearLeft, rearRight);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, MecanumDriveWheelAccelerations value) {
+    bb.putDouble(value.frontLeft);
+    bb.putDouble(value.frontRight);
+    bb.putDouble(value.rearLeft);
+    bb.putDouble(value.rearRight);
+  }
+}

wpimath/src/main/java/org/wpilib/math/kinematics/struct/SwerveModuleAccelerationStruct.java

@@ -0,0 +1,50 @@
+// 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.
+
+package org.wpilib.math.kinematics.struct;
+
+import java.nio.ByteBuffer;
+import org.wpilib.math.geometry.Rotation2d;
+import org.wpilib.math.kinematics.SwerveModuleAcceleration;
+import org.wpilib.util.struct.Struct;
+
+public class SwerveModuleAccelerationStruct implements Struct<SwerveModuleAcceleration> {
+  @Override
+  public Class<SwerveModuleAcceleration> getTypeClass() {
+    return SwerveModuleAcceleration.class;
+  }
+
+  @Override
+  public String getTypeName() {
+    return "SwerveModuleAccelerations";
+  }
+
+  @Override
+  public int getSize() {
+    return kSizeDouble + Rotation2d.struct.getSize();
+  }
+
+  @Override
+  public String getSchema() {
+    return "double acceleration;Rotation2d angle";
+  }
+
+  @Override
+  public Struct<?>[] getNested() {
+    return new Struct<?>[] {Rotation2d.struct};
+  }
+
+  @Override
+  public SwerveModuleAcceleration unpack(ByteBuffer bb) {
+    double acceleration = bb.getDouble();
+    Rotation2d angle = Rotation2d.struct.unpack(bb);
+    return new SwerveModuleAcceleration(acceleration, angle);
+  }
+
+  @Override
+  public void pack(ByteBuffer bb, SwerveModuleAcceleration value) {
+    bb.putDouble(value.acceleration);
+    Rotation2d.struct.pack(bb, value.angle);
+  }
+}