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 + #include #include #include @@ -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.