[wpimath] Add DifferentialDriveFeedforward classes which wrap LinearPlantInversionFeedforward (#4598)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveFeedforwardTest.java

@@ -0,0 +1,85 @@
+// 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 edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveFeedforwardTest {
+  private static final double kVLinear = 1.0;
+  private static final double kALinear = 1.0;
+  private static final double kVAngular = 1.0;
+  private static final double kAAngular = 1.0;
+  private static final double trackwidth = 1.0;
+  private static final double dtSeconds = 0.02;
+
+  @Test
+  void testCalculateWithTrackwidth() {
+    DifferentialDriveFeedforward differentialDriveFeedforward =
+        new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
+    LinearSystem<N2, N2, N2> plant =
+        LinearSystemId.identifyDrivetrainSystem(
+            kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
+    for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) {
+      for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) {
+        for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) {
+          for (int nextRightVelocity = -4; nextRightVelocity <= 4; nextRightVelocity += 2) {
+            DifferentialDriveWheelVoltages u =
+                differentialDriveFeedforward.calculate(
+                    currentLeftVelocity,
+                    nextLeftVelocity,
+                    currentRightVelocity,
+                    nextRightVelocity,
+                    dtSeconds);
+            Matrix<N2, N1> nextX =
+                plant.calculateX(
+                    VecBuilder.fill(currentLeftVelocity, currentRightVelocity),
+                    VecBuilder.fill(u.left, u.right),
+                    dtSeconds);
+            assertEquals(nextX.get(0, 0), nextLeftVelocity, 1e-6);
+            assertEquals(nextX.get(1, 0), nextRightVelocity, 1e-6);
+          }
+        }
+      }
+    }
+  }
+
+  @Test
+  void testCalculateWithoutTrackwidth() {
+    DifferentialDriveFeedforward differentialDriveFeedforward =
+        new DifferentialDriveFeedforward(kVLinear, kALinear, kVAngular, kAAngular);
+    LinearSystem<N2, N2, N2> plant =
+        LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular);
+    for (int currentLeftVelocity = -4; currentLeftVelocity <= 4; currentLeftVelocity += 2) {
+      for (int currentRightVelocity = -4; currentRightVelocity <= 4; currentRightVelocity += 2) {
+        for (int nextLeftVelocity = -4; nextLeftVelocity <= 4; nextLeftVelocity += 2) {
+          for (int nextRightVelocity = -4; nextRightVelocity <= 4; nextRightVelocity += 2) {
+            DifferentialDriveWheelVoltages u =
+                differentialDriveFeedforward.calculate(
+                    currentLeftVelocity,
+                    nextLeftVelocity,
+                    currentRightVelocity,
+                    nextRightVelocity,
+                    dtSeconds);
+            Matrix<N2, N1> nextX =
+                plant.calculateX(
+                    VecBuilder.fill(currentLeftVelocity, currentRightVelocity),
+                    VecBuilder.fill(u.left, u.right),
+                    dtSeconds);
+            assertEquals(nextX.get(0, 0), nextLeftVelocity, 1e-6);
+            assertEquals(nextX.get(1, 0), nextRightVelocity, 1e-6);
+          }
+        }
+      }
+    }
+  }
+}

wpimath/src/test/native/cpp/controller/DifferentialDriveFeedforwardTest.cpp

@@ -0,0 +1,84 @@
+// 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.
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "frc/EigenCore.h"
+#include "frc/controller/DifferentialDriveFeedforward.h"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/system/plant/LinearSystemId.h"
+#include "units/acceleration.h"
+#include "units/length.h"
+#include "units/time.h"
+
+TEST(DifferentialDriveFeedforwardTest, CalculateWithTrackwidth) {
+  constexpr auto kVLinear = 1_V / 1_mps;
+  constexpr auto kALinear = 1_V / 1_mps_sq;
+  constexpr auto kVAngular = 1_V / 1_rad_per_s;
+  constexpr auto kAAngular = 1_V / 1_rad_per_s_sq;
+  constexpr auto trackwidth = 1_m;
+  constexpr auto dt = 20_ms;
+
+  frc::DifferentialDriveFeedforward differentialDriveFeedforward{
+      kVLinear, kALinear, kVAngular, kAAngular, trackwidth};
+  frc::LinearSystem<2, 2, 2> plant =
+      frc::LinearSystemId::IdentifyDrivetrainSystem(
+          kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
+  for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
+       currentLeftVelocity += 2_mps) {
+    for (auto currentRightVelocity = -4_mps; currentRightVelocity <= 4_mps;
+         currentRightVelocity += 2_mps) {
+      for (auto nextLeftVelocity = -4_mps; nextLeftVelocity <= 4_mps;
+           nextLeftVelocity += 2_mps) {
+        for (auto nextRightVelocity = -4_mps; nextRightVelocity <= 4_mps;
+             nextRightVelocity += 2_mps) {
+          auto [left, right] = differentialDriveFeedforward.Calculate(
+              currentLeftVelocity, nextLeftVelocity, currentRightVelocity,
+              nextRightVelocity, dt);
+          frc::Matrixd<2, 1> nextX = plant.CalculateX(
+              frc::Vectord<2>{currentLeftVelocity, currentRightVelocity},
+              frc::Vectord<2>{left, right}, dt);
+          EXPECT_NEAR(nextX(0), nextLeftVelocity.value(), 1e-6);
+          EXPECT_NEAR(nextX(1), nextRightVelocity.value(), 1e-6);
+        }
+      }
+    }
+  }
+}
+
+TEST(DifferentialDriveFeedforwardTest, CalculateWithoutTrackwidth) {
+  constexpr auto kVLinear = 1_V / 1_mps;
+  constexpr auto kALinear = 1_V / 1_mps_sq;
+  constexpr auto kVAngular = 1_V / 1_mps;
+  constexpr auto kAAngular = 1_V / 1_mps_sq;
+  constexpr auto dt = 20_ms;
+
+  frc::DifferentialDriveFeedforward differentialDriveFeedforward{
+      kVLinear, kALinear, kVAngular, kAAngular};
+  frc::LinearSystem<2, 2, 2> plant =
+      frc::LinearSystemId::IdentifyDrivetrainSystem(kVLinear, kALinear,
+                                                    kVAngular, kAAngular);
+  for (auto currentLeftVelocity = -4_mps; currentLeftVelocity <= 4_mps;
+       currentLeftVelocity += 2_mps) {
+    for (auto currentRightVelocity = -4_mps; currentRightVelocity <= 4_mps;
+         currentRightVelocity += 2_mps) {
+      for (auto nextLeftVelocity = -4_mps; nextLeftVelocity <= 4_mps;
+           nextLeftVelocity += 2_mps) {
+        for (auto nextRightVelocity = -4_mps; nextRightVelocity <= 4_mps;
+             nextRightVelocity += 2_mps) {
+          auto [left, right] = differentialDriveFeedforward.Calculate(
+              currentLeftVelocity, nextLeftVelocity, currentRightVelocity,
+              nextRightVelocity, dt);
+          frc::Matrixd<2, 1> nextX = plant.CalculateX(
+              frc::Vectord<2>{currentLeftVelocity, currentRightVelocity},
+              frc::Vectord<2>{left, right}, dt);
+          EXPECT_NEAR(nextX(0), nextLeftVelocity.value(), 1e-6);
+          EXPECT_NEAR(nextX(1), nextRightVelocity.value(), 1e-6);
+        }
+      }
+    }
+  }
+}