From 967dd14f482820f7808b99c98f7568868887c85b Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sat, 9 Mar 2024 15:32:13 -0800
Subject: [PATCH] [sysid] Fix crash on negative feedforward gains

LinearSystemId's linear system factories throw on negative feedforward
gains, but SysId can compute the feedback gains just fine in that case.
Now we construct the system manually instead.

Fixes #6423.
---
 .../main/native/cpp/analysis/FeedbackAnalysis.cpp    | 12 +++++++-----
 1 file changed, 7 insertions(+), 5 deletions(-)

diff --git a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
index 28096255e55..0a4f53a65c2 100644
--- a/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
+++ b/sysid/src/main/native/cpp/analysis/FeedbackAnalysis.cpp
@@ -8,7 +8,6 @@
 
 #include <frc/controller/LinearQuadraticRegulator.h>
 #include <frc/system/LinearSystem.h>
-#include <frc/system/plant/LinearSystemId.h>
 #include <units/acceleration.h>
 #include <units/velocity.h>
 #include <units/voltage.h>
@@ -32,8 +31,10 @@ FeedbackGains sysid::CalculatePositionFeedbackGains(
   // instabilities in the LQR.
   if (Ka > 1E-7) {
     // Create a position system from our feedforward gains.
-    auto system = frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
-        Kv_t(Kv), Ka_t(Ka));
+    frc::LinearSystem<2, 1, 1> system{
+        frc::Matrixd<2, 2>{{0.0, 1.0}, {0.0, -Kv / Ka}},
+        frc::Matrixd<2, 1>{0.0, 1.0 / Ka}, frc::Matrixd<1, 2>{1.0, 0.0},
+        frc::Matrixd<1, 1>{0.0}};
     // Create an LQR with 2 states to control -- position and velocity.
     frc::LinearQuadraticRegulator<2, 1> controller{
         system, {params.qp, params.qv}, {params.r}, preset.period};
@@ -74,8 +75,9 @@ FeedbackGains sysid::CalculateVelocityFeedbackGains(
   }
 
   // Create a velocity system from our feedforward gains.
-  auto system = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
-      Kv_t(Kv), Ka_t(Ka));
+  frc::LinearSystem<1, 1, 1> system{
+      frc::Matrixd<1, 1>{-Kv / Ka}, frc::Matrixd<1, 1>{1.0 / Ka},
+      frc::Matrixd<1, 1>{1.0}, frc::Matrixd<1, 1>{0.0}};
   // Create an LQR controller with 1 state -- velocity.
   frc::LinearQuadraticRegulator<1, 1> controller{
       system, {params.qv}, {params.r}, preset.period};