From f0f54953265d48f957433be7dee1c1834557e0cb Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sat, 9 Mar 2024 09:54:37 -0800
Subject: [PATCH] [sysid] Fix arm characterization crash (#6422)

Fixes #6421.
---
 .../src/main/native/cpp/analysis/FeedbackAnalysis.cpp  | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
index a4e6d2e3b09..28096255e55 100644
--- a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
+++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
@@ -4,6 +4,8 @@
 
 #include "sysid/analysis/FeedbackAnalysis.h"
 
+#include <cmath>
+
 #include <frc/controller/LinearQuadraticRegulator.h>
 #include <frc/system/LinearSystem.h>
 #include <frc/system/plant/LinearSystemId.h>
@@ -21,6 +23,10 @@ using Ka_t = decltype(1_V / 1_mps_sq);
 FeedbackGains sysid::CalculatePositionFeedbackGains(
     const FeedbackControllerPreset& preset, const LQRParameters& params,
     double Kv, double Ka) {
+  if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
+    return {0.0, 0.0};
+  }
+
   // If acceleration requires no effort, velocity becomes an input for position
   // control. We choose an appropriate model in this case to avoid numerical
   // instabilities in the LQR.
@@ -56,6 +62,10 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
 FeedbackGains sysid::CalculateVelocityFeedbackGains(
     const FeedbackControllerPreset& preset, const LQRParameters& params,
     double Kv, double Ka, double encFactor) {
+  if (!std::isfinite(Kv) || !std::isfinite(Ka)) {
+    return {0.0, 0.0};
+  }
+
   // If acceleration for velocity control requires no effort, the feedback
   // control gains approach zero. We special-case it here because numerical
   // instabilities arise in LQR otherwise.