From 6259f746bd9b01aaaf50bc61b2e4342577808934 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Fri, 14 Feb 2025 21:11:36 -0800 Subject: [PATCH] =?UTF-8?q?[wpimath]=20Fix=20feedforward=20returning=20NaN?= =?UTF-8?q?=20when=20k=E1=B5=A5=20=3D=200?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- wpimath/algorithms.md | 34 +++++++++++++++++-- .../math/controller/ElevatorFeedforward.java | 2 +- .../controller/SimpleMotorFeedforward.java | 2 +- .../frc/controller/ElevatorFeedforward.h | 2 +- .../frc/controller/SimpleMotorFeedforward.h | 2 +- 5 files changed, 36 insertions(+), 6 deletions(-) diff --git a/wpimath/algorithms.md b/wpimath/algorithms.md index a4138948872..cfc48dac7d8 100644 --- 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]T + [0 0] [A_d B_d] +e = [ 0 I ] + + [0 B]T + [0 0] [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]T + [0 0] [A_d B_d] +e = [ 0 I ] + + [0 B]T + [0 0] [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 index e33de90b154..c9cdf13f769 100644 --- 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 index 7c04f4e4ec4..0fd5154935e 100644 --- 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 index 93575276c50..fe84ffc59cb 100644 --- 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 index eb48be1c31e..17ec62f1691 100644 --- 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 *