[wpimath] Add ElevatorFeedforward.calculate(currentV, nextV) overload (#5715)

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

wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java

@@ -4,6 +4,10 @@
 
 package edu.wpi.first.math.controller;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+
 /**
  * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting
  * against the force of gravity).
@@ -53,6 +57,48 @@ public class ElevatorFeedforward {
     return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration;
   }
 
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * <p>Note this method is inaccurate when the velocity crosses 0.
+   *
+   * @param currentVelocity The current velocity setpoint.
+   * @param nextVelocity The next velocity setpoint.
+   * @param dtSeconds Time between velocity setpoints in seconds.
+   * @return The computed feedforward.
+   */
+  public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) {
+    // Discretize the affine model.
+    //
+    //   dx/dt = Ax + Bu + c
+    //   dx/dt = Ax + B(u + B⁺c)
+    //   xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
+    //   xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
+    //   xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ
+    //
+    // Solve for uₖ.
+    //
+    //   B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
+    //
+    // For an elevator with the model
+    // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
+    // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B
+    // assuming sgn(x) is a constant for the duration of the step.
+    //
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x)))
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x))
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x)
+    var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
+    var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
+
+    var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity);
+    var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity);
+
+    return kg + ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
+  }
+
   /**
    * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
    * zero).

wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h

@@ -6,6 +6,9 @@
 
 #include <wpi/MathExtras.h>
 
+#include "frc/EigenCore.h"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/system/plant/LinearSystemId.h"
 #include "units/length.h"
 #include "units/time.h"
 #include "units/voltage.h"
@@ -54,6 +57,50 @@ class ElevatorFeedforward {
     return kS * wpi::sgn(velocity) + kG + kV * velocity + kA * acceleration;
   }
 
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param currentVelocity The current velocity setpoint, in distance per
+   *                        second.
+   * @param nextVelocity    The next velocity setpoint, in distance per second.
+   * @param dt              Time between velocity setpoints in seconds.
+   * @return The computed feedforward, in volts.
+   */
+  units::volt_t Calculate(units::unit_t<Velocity> currentVelocity,
+                          units::unit_t<Velocity> nextVelocity,
+                          units::second_t dt) const {
+    // Discretize the affine model.
+    //
+    //   dx/dt = Ax + Bu + c
+    //   dx/dt = Ax + B(u + B⁺c)
+    //   xₖ₊₁ = eᴬᵀxₖ + A⁻¹(eᴬᵀ - I)B(uₖ + B⁺cₖ)
+    //   xₖ₊₁ = A_d xₖ + B_d (uₖ + B⁺cₖ)
+    //   xₖ₊₁ = A_d xₖ + B_duₖ + B_d B⁺cₖ
+    //
+    // Solve for uₖ.
+    //
+    //   B_duₖ = xₖ₊₁ − A_d xₖ − B_d B⁺cₖ
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ − B_d B⁺cₖ)
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − B⁺cₖ
+    //
+    // For an elevator with the model
+    // dx/dt = -Kv/Ka x + 1/Ka u - Kg/Ka - Ks/Ka sgn(x),
+    // A = -Kv/Ka, B = 1/Ka, and c = -(Kg/Ka + Ks/Ka sgn(x)). Substitute in B
+    // assuming sgn(x) is a constant for the duration of the step.
+    //
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) − Ka(-(Kg/Ka + Ks/Ka sgn(x)))
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Ka(Kg/Ka + Ks/Ka sgn(x))
+    //   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ) + Kg + Ks sgn(x)
+    auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
+    LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
+
+    Vectord<1> r{currentVelocity.value()};
+    Vectord<1> nextR{nextVelocity.value()};
+
+    return kG + kS * wpi::sgn(currentVelocity.value()) +
+           units::volt_t{feedforward.Calculate(r, nextR)(0)};
+  }
+
   // Rearranging the main equation from the calculate() method yields the
   // formulas for the methods below:
 

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

@@ -6,6 +6,10 @@ 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.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
 import org.junit.jupiter.api.Test;
 
 class ElevatorFeedforwardTest {
@@ -22,6 +26,18 @@ class ElevatorFeedforwardTest {
     assertEquals(4.5, m_elevatorFF.calculate(2), 0.002);
     assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002);
     assertEquals(-0.5, m_elevatorFF.calculate(-2, 1), 0.002);
+
+    var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-kv / ka);
+    var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / ka);
+    final double dt = 0.02;
+    var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
+
+    var r = VecBuilder.fill(2.0);
+    var nextR = VecBuilder.fill(3.0);
+    assertEquals(
+        plantInversion.calculate(r, nextR).get(0, 0) + ks + kg,
+        m_elevatorFF.calculate(2.0, 3.0, dt),
+        0.002);
   }
 
   @Test

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

@@ -6,7 +6,9 @@
 
 #include <gtest/gtest.h>
 
+#include "frc/EigenCore.h"
 #include "frc/controller/ElevatorFeedforward.h"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
 #include "units/acceleration.h"
 #include "units/length.h"
 #include "units/time.h"
@@ -18,12 +20,23 @@ static constexpr auto Kg = 1_V;
 
 TEST(ElevatorFeedforwardTest, Calculate) {
   frc::ElevatorFeedforward elevatorFF{Ks, Kg, Kv, Ka};
+
   EXPECT_NEAR(elevatorFF.Calculate(0_m / 1_s).value(), Kg.value(), 0.002);
   EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s).value(), 4.5, 0.002);
   EXPECT_NEAR(elevatorFF.Calculate(2_m / 1_s, 1_m / 1_s / 1_s).value(), 6.5,
               0.002);
   EXPECT_NEAR(elevatorFF.Calculate(-2_m / 1_s, 1_m / 1_s / 1_s).value(), -0.5,
               0.002);
+
+  frc::Matrixd<1, 1> A{-Kv.value() / Ka.value()};
+  frc::Matrixd<1, 1> B{1.0 / Ka.value()};
+  constexpr units::second_t dt = 20_ms;
+  frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
+
+  frc::Vectord<1> r{2.0};
+  frc::Vectord<1> nextR{3.0};
+  EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks.value() + Kg.value(),
+              elevatorFF.Calculate(2_mps, 3_mps, dt).value(), 0.002);
 }
 
 TEST(ElevatorFeedforwardTest, AchievableVelocity) {