From b31fd17d050fe5e0115512fe8d3d7269625a9498 Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Thu, 30 Jan 2025 12:33:39 -0800
Subject: [PATCH] =?UTF-8?q?[wpimath]=20Fix=20infinite=20loop=20in=20ArmFee?=
 =?UTF-8?q?dforward::Calculate(x=E2=82=96,=20v=E2=82=96,=20v=E2=82=96?=
 =?UTF-8?q?=E2=82=8A=E2=82=81)=20(#7745)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Small values of kₐ make the iterative solver ill-conditioned. This
change reverts to the constant-acceleration feedforward in that case. It
gives _very_ bad results (hence why we added the iterative solver in the
first place), but it's better than hanging.

```
TEST(ArmFeedforwardTest, CalculateIllConditioned) {
  constexpr auto Ks = 0.5_V;
  constexpr auto Kv = 20_V / 1_rad_per_s;
  constexpr auto Ka = 1e-2_V / 1_rad_per_s_sq;
  constexpr auto Kg = 0_V;
  frc::ArmFeedforward armFF{Ks, Kg, Kv, Ka};

  // Calculate(currentAngle, currentVelocity, nextAngle, dt)
  CalculateAndSimulate(armFF, 0_rad, 0_rad_per_s, 2_rad_per_s, 20_ms);
}
```
This produces 1 V and doesn't accelerate the system at all. Using
nextVelocity instead of currentVelocity in the feedforward outputs 41 V
and still only accelerates to 0.4 rad/s of the requested 2 rad/s.

I picked the kₐ cutoff by increasing kₐ until the iterative solver
started converging.

Fixes #7743.
---
 wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp
index 7c9c44609d..df1f89d984 100644
--- a/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp
+++ b/wpimath/src/main/native/cpp/controller/ArmFeedforward.cpp
@@ -20,6 +20,13 @@ units::volt_t ArmFeedforward::Calculate(
     units::unit_t<Velocity> nextVelocity) const {
   using VarMat = sleipnir::VariableMatrix;
 
+  // Small kₐ values make the solver ill-conditioned
+  if (kA < units::unit_t<ka_unit>{1e-1}) {
+    auto acceleration = (nextVelocity - currentVelocity) / m_dt;
+    return kS * wpi::sgn(currentVelocity.value()) + kV * currentVelocity +
+           kA * acceleration + kG * units::math::cos(currentAngle);
+  }
+
   // Arm dynamics
   Matrixd<2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
   Matrixd<2, 1> B{{0.0}, {1.0 / kA.value()}};