[wpimath] Fix feedforward returning NaN when kᵥ = 0

wpilibsuite · Feb 15, 2025 · 41dd46b · 41dd46b
1 parent 1362606
commit 41dd46b
Show file tree

Hide file tree

Showing 5 changed files with 36 additions and 6 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
@@ -208,7 +208,7 @@ public double calculateWithVelocities(double currentVelocity, double nextVelocit
       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 == 0.0 ? 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
@@ -192,7 +192,7 @@ public double calculateWithVelocities(double currentVelocity, double nextVelocit
       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 == 0.0 ? 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
@@ -139,7 +139,7 @@ class ElevatorFeedforward {
       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 == 0.0 ? 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
@@ -115,7 +115,7 @@ class SimpleMotorFeedforward {
       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 == 0.0 ? B * m_dt.value() : 1.0 / A * (A_d - 1.0) * B;
       return kS * wpi::sgn(currentVelocity) +
              units::volt_t{
                  1.0 / B_d *