[wpimath] Fix feedforward returning NaN when kᵥ = 0 (#7790)

wpilibsuite · Feb 26, 2025 · 822457d · 822457d
1 parent 75321f1
commit 822457d
Show file tree

Hide file tree

Showing 5 changed files with 40 additions and 10 deletions.
diff --git a/wpimath/algorithms.md b/wpimath/algorithms.md
@@ -92,7 +92,7 @@ Substitute these into the feedforward equation.
   uₖ = kₛ sgn(x) + kᵥxₖ₊₁
 ```
 
-Simplify the model when ka ≠ 0
+Simplify the model when kₐ ≠ 0
 
 ```
   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ)
@@ -107,6 +107,21 @@ where
   B_d = A⁻¹(eᴬᵀ - I)B
 ```
 
+When kᵥ = 0, A = 0 and B_d has a singularity. We can eliminate the singularity using the matrix exponential discretization method.
+
+```
+ [A  B]
+ [0  0]T   [A_d  B_d]
+e        = [ 0    I ]
+
+ [0  B]
+ [0  0]T   [1  BT]
+e        = [0   1]
+
+A_d = 1
+B_d = BT
+```
+
 ## Elevator feedforward
 
 ### Derivation
@@ -199,7 +214,7 @@ Substitute these into the feedforward equation.
   uₖ = kₛ sgn(x) + kg + kᵥxₖ₊₁
 ```
 
-Simplify the model when ka ≠ 0
+Simplify the model when kₐ ≠ 0
 
 ```
   uₖ = B_d⁺(xₖ₊₁ − A_d xₖ)
@@ -214,6 +229,21 @@ where
   B_d = A⁻¹(eᴬᵀ - I)B
 ```
 
+When kᵥ = 0, A = 0 and B_d has a singularity. We can eliminate the singularity using the matrix exponential discretization method.
+
+```
+ [A  B]
+ [0  0]T   [A_d  B_d]
+e        = [ 0    I ]
+
+ [0  B]
+ [0  0]T   [1  BT]
+e        = [0   1]
+
+A_d = 1
+B_d = BT
+```
+
 ## Closed form Kalman gain for continuous Kalman filter with A = 0 and C = I
 
 ### Derivation

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
@@ -202,13 +202,13 @@ public double calculate(double velocity) {
    */
   public double calculateWithVelocities(double currentVelocity, double nextVelocity) {
     // See wpimath/algorithms.md#Elevator_feedforward for derivation
-    if (ka == 0.0) {
+    if (ka < 1e-9) {
       return ks * Math.signum(nextVelocity) + kg + kv * nextVelocity;
     } else {
       double A = -kv / ka;
       double B = 1.0 / ka;
       double A_d = Math.exp(A * m_dt);
-      double B_d = 1.0 / A * (A_d - 1.0) * B;
+      double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B;
       return kg
           + ks * Math.signum(currentVelocity)
           + 1.0 / B_d * (nextVelocity - A_d * currentVelocity);

diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
@@ -186,13 +186,13 @@ public double calculate(double velocity) {
    */
   public double calculateWithVelocities(double currentVelocity, double nextVelocity) {
     // See wpimath/algorithms.md#Simple_motor_feedforward for derivation
-    if (ka == 0.0) {
+    if (ka < 1e-9) {
       return ks * Math.signum(nextVelocity) + kv * nextVelocity;
     } else {
       double A = -kv / ka;
       double B = 1.0 / ka;
       double A_d = Math.exp(A * m_dt);
-      double B_d = 1.0 / A * (A_d - 1.0) * B;
+      double B_d = A > -1e-9 ? B * m_dt : 1.0 / A * (A_d - 1.0) * B;
       return ks * Math.signum(currentVelocity) + 1.0 / B_d * (nextVelocity - A_d * currentVelocity);
     }
   }

diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
@@ -133,13 +133,13 @@ class ElevatorFeedforward {
       units::unit_t<Velocity> currentVelocity,
       units::unit_t<Velocity> nextVelocity) const {
     // See wpimath/algorithms.md#Elevator_feedforward for derivation
-    if (kA == decltype(kA)(0)) {
+    if (kA < decltype(kA)(1e-9)) {
       return kS * wpi::sgn(nextVelocity) + kG + kV * nextVelocity;
     } else {
       double A = -kV.value() / kA.value();
       double B = 1.0 / kA.value();
       double A_d = gcem::exp(A * m_dt.value());
-      double B_d = 1.0 / A * (A_d - 1.0) * B;
+      double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B;
       return kG + kS * wpi::sgn(currentVelocity) +
              units::volt_t{
                  1.0 / B_d *

diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
@@ -109,13 +109,13 @@ class SimpleMotorFeedforward {
       units::unit_t<Velocity> currentVelocity,
       units::unit_t<Velocity> nextVelocity) const {
     // See wpimath/algorithms.md#Simple_motor_feedforward for derivation
-    if (kA == decltype(kA)(0)) {
+    if (kA < decltype(kA)(1e-9)) {
       return kS * wpi::sgn(nextVelocity) + kV * nextVelocity;
     } else {
       double A = -kV.value() / kA.value();
       double B = 1.0 / kA.value();
       double A_d = gcem::exp(A * m_dt.value());
-      double B_d = 1.0 / A * (A_d - 1.0) * B;
+      double B_d = A > -1e-9 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B;
       return kS * wpi::sgn(currentVelocity) +
              units::volt_t{
                  1.0 / B_d *