[wpimath] Catch incorrect parameters to state-space models earlier (#3680)

This allows giving more descriptive exceptions than if they are thrown
later in KalmanFilter, for example.

Fixes #3678.

wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java

@@ -33,6 +33,7 @@ public class LinearSystem<States extends Num, Inputs extends Num, Outputs extend
    * @param b The input matrix B.
    * @param c The output matrix C.
    * @param d The feedthrough matrix D.
+   * @throws IllegalArgumentException if any matrix element isn't finite.
    */
   @SuppressWarnings("ParameterName")
   public LinearSystem(
@@ -40,6 +41,39 @@ public class LinearSystem<States extends Num, Inputs extends Num, Outputs extend
       Matrix<States, Inputs> b,
       Matrix<Outputs, States> c,
       Matrix<Outputs, Inputs> d) {
+    for (int row = 0; row < a.getNumRows(); ++row) {
+      for (int col = 0; col < a.getNumCols(); ++col) {
+        if (!Double.isFinite(a.get(row, col))) {
+          throw new IllegalArgumentException(
+              "Elements of A aren't finite. This is usually due to model implementation errors.");
+        }
+      }
+    }
+    for (int row = 0; row < b.getNumRows(); ++row) {
+      for (int col = 0; col < b.getNumCols(); ++col) {
+        if (!Double.isFinite(b.get(row, col))) {
+          throw new IllegalArgumentException(
+              "Elements of B aren't finite. This is usually due to model implementation errors.");
+        }
+      }
+    }
+    for (int row = 0; row < c.getNumRows(); ++row) {
+      for (int col = 0; col < c.getNumCols(); ++col) {
+        if (!Double.isFinite(c.get(row, col))) {
+          throw new IllegalArgumentException(
+              "Elements of C aren't finite. This is usually due to model implementation errors.");
+        }
+      }
+    }
+    for (int row = 0; row < d.getNumRows(); ++row) {
+      for (int col = 0; col < d.getNumCols(); ++col) {
+        if (!Double.isFinite(d.get(row, col))) {
+          throw new IllegalArgumentException(
+              "Elements of D aren't finite. This is usually due to model implementation errors.");
+        }
+      }
+    }
+
     this.m_A = a;
     this.m_B = b;
     this.m_C = c;

wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java

@@ -25,10 +25,21 @@ public final class LinearSystemId {
    * @param radiusMeters The radius of thd driving drum of the elevator, in meters.
    * @param G The reduction between motor and drum, as a ratio of output to input.
    * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if massKg <= 0, radiusMeters <= 0, or G <= 0.
    */
   @SuppressWarnings("ParameterName")
   public static LinearSystem<N2, N1, N1> createElevatorSystem(
       DCMotor motor, double massKg, double radiusMeters, double G) {
+    if (massKg <= 0.0) {
+      throw new IllegalArgumentException("massKg must be greater than zero.");
+    }
+    if (radiusMeters <= 0.0) {
+      throw new IllegalArgumentException("radiusMeters must be greater than zero.");
+    }
+    if (G <= 0) {
+      throw new IllegalArgumentException("G must be greater than zero.");
+    }
+
     return new LinearSystem<>(
         Matrix.mat(Nat.N2(), Nat.N2())
             .fill(
@@ -55,10 +66,18 @@ public final class LinearSystemId {
    * @param jKgMetersSquared The moment of inertia J of the flywheel.
    * @param G The reduction between motor and drum, as a ratio of output to input.
    * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if jKgMetersSquared &lt;= 0 or G &lt;= 0.
    */
   @SuppressWarnings("ParameterName")
   public static LinearSystem<N1, N1, N1> createFlywheelSystem(
       DCMotor motor, double jKgMetersSquared, double G) {
+    if (jKgMetersSquared <= 0.0) {
+      throw new IllegalArgumentException("J must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw new IllegalArgumentException("G must be greater than zero.");
+    }
+
     return new LinearSystem<>(
         VecBuilder.fill(
             -G
@@ -81,6 +100,7 @@ public final class LinearSystemId {
    * @param JKgMetersSquared the moment of inertia of the robot.
    * @param G the gearing reduction as output over input.
    * @return A LinearSystem representing a differential drivetrain.
+   * @throws IllegalArgumentException if m &lt;= 0, r &lt;= 0, rb &lt;= 0, J &lt;= 0, or G &lt;= 0.
    */
   @SuppressWarnings({"LocalVariableName", "ParameterName"})
   public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(
@@ -90,6 +110,22 @@ public final class LinearSystemId {
       double rbMeters,
       double JKgMetersSquared,
       double G) {
+    if (massKg <= 0.0) {
+      throw new IllegalArgumentException("massKg must be greater than zero.");
+    }
+    if (rMeters <= 0.0) {
+      throw new IllegalArgumentException("rMeters must be greater than zero.");
+    }
+    if (rbMeters <= 0.0) {
+      throw new IllegalArgumentException("rbMeters must be greater than zero.");
+    }
+    if (JKgMetersSquared <= 0.0) {
+      throw new IllegalArgumentException("JKgMetersSquared must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw new IllegalArgumentException("G must be greater than zero.");
+    }
+
     var C1 =
         -(G * G) * motor.KtNMPerAmp / (motor.KvRadPerSecPerVolt * motor.rOhms * rMeters * rMeters);
     var C2 = G * motor.KtNMPerAmp / (motor.rOhms * rMeters);
@@ -117,6 +153,13 @@ public final class LinearSystemId {
   @SuppressWarnings("ParameterName")
   public static LinearSystem<N2, N1, N1> createSingleJointedArmSystem(
       DCMotor motor, double jKgSquaredMeters, double G) {
+    if (jKgSquaredMeters <= 0.0) {
+      throw new IllegalArgumentException("jKgSquaredMeters must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw new IllegalArgumentException("G must be greater than zero.");
+    }
+
     return new LinearSystem<>(
         Matrix.mat(Nat.N2(), Nat.N2())
             .fill(
@@ -142,10 +185,18 @@ public final class LinearSystemId {
    * @param kV The velocity gain, in volts per (units per second)
    * @param kA The acceleration gain, in volts per (units per second squared)
    * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if kV &lt;= 0 or kA &lt;= 0.
    * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
   @SuppressWarnings("ParameterName")
   public static LinearSystem<N1, N1, N1> identifyVelocitySystem(double kV, double kA) {
+    if (kV <= 0.0) {
+      throw new IllegalArgumentException("Kv must be greater than zero.");
+    }
+    if (kA <= 0.0) {
+      throw new IllegalArgumentException("Ka must be greater than zero.");
+    }
+
     return new LinearSystem<>(
         VecBuilder.fill(-kV / kA),
         VecBuilder.fill(1.0 / kA),
@@ -164,10 +215,18 @@ public final class LinearSystemId {
    * @param kV The velocity gain, in volts per (units per second)
    * @param kA The acceleration gain, in volts per (units per second squared)
    * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if kV &lt;= 0 or kA &lt;= 0.
    * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
   @SuppressWarnings("ParameterName")
   public static LinearSystem<N2, N1, N1> identifyPositionSystem(double kV, double kA) {
+    if (kV <= 0.0) {
+      throw new IllegalArgumentException("Kv must be greater than zero.");
+    }
+    if (kA <= 0.0) {
+      throw new IllegalArgumentException("Ka must be greater than zero.");
+    }
+
     return new LinearSystem<>(
         Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -kV / kA),
         VecBuilder.fill(0.0, 1.0 / kA),
@@ -187,11 +246,26 @@ public final class LinearSystemId {
    * @param kVAngular The angular velocity gain, volts per (meter per second).
    * @param kAAngular The angular acceleration gain, volts per (meter per second squared).
    * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0, or
+   *     kAAngular &lt;= 0.
    * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
   @SuppressWarnings("ParameterName")
   public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
       double kVLinear, double kALinear, double kVAngular, double kAAngular) {
+    if (kVLinear <= 0.0) {
+      throw new IllegalArgumentException("Kv,linear must be greater than zero.");
+    }
+    if (kALinear <= 0.0) {
+      throw new IllegalArgumentException("Ka,linear must be greater than zero.");
+    }
+    if (kVAngular <= 0.0) {
+      throw new IllegalArgumentException("Kv,angular must be greater than zero.");
+    }
+    if (kAAngular <= 0.0) {
+      throw new IllegalArgumentException("Ka,angular must be greater than zero.");
+    }
+
     final double A1 = 0.5 * -(kVLinear / kALinear + kVAngular / kAAngular);
     final double A2 = 0.5 * -(kVLinear / kALinear - kVAngular / kAAngular);
     final double B1 = 0.5 * (1.0 / kALinear + 1.0 / kAAngular);
@@ -217,11 +291,29 @@ public final class LinearSystemId {
    * @param kAAngular The angular acceleration gain, volts per (radians per second squared).
    * @param trackwidth The width of the drivetrain in meters.
    * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0,
+   *     kAAngular &lt;= 0, or trackwidth &lt;= 0.
    * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
    */
   @SuppressWarnings("ParameterName")
   public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
       double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
+    if (kVLinear <= 0.0) {
+      throw new IllegalArgumentException("Kv,linear must be greater than zero.");
+    }
+    if (kALinear <= 0.0) {
+      throw new IllegalArgumentException("Ka,linear must be greater than zero.");
+    }
+    if (kVAngular <= 0.0) {
+      throw new IllegalArgumentException("Kv,angular must be greater than zero.");
+    }
+    if (kAAngular <= 0.0) {
+      throw new IllegalArgumentException("Ka,angular must be greater than zero.");
+    }
+    if (trackwidth <= 0.0) {
+      throw new IllegalArgumentException("trackwidth must be greater than zero.");
+    }
+
     // We want to find a factor to include in Kv,angular that will convert
     // `u = Kv,angular omega` to `u = Kv,angular v`.
     //

wpimath/src/main/native/include/frc/system/LinearSystem.h

@@ -6,6 +6,7 @@
 
 #include <algorithm>
 #include <functional>
+#include <stdexcept>
 
 #include "Eigen/Core"
 #include "frc/StateSpaceUtil.h"
@@ -32,11 +33,33 @@ class LinearSystem {
    * @param B    Input matrix.
    * @param C    Output matrix.
    * @param D    Feedthrough matrix.
+   * @throws std::domain_error if any matrix element isn't finite.
    */
   LinearSystem(const Eigen::Matrix<double, States, States>& A,
                const Eigen::Matrix<double, States, Inputs>& B,
                const Eigen::Matrix<double, Outputs, States>& C,
                const Eigen::Matrix<double, Outputs, Inputs>& D) {
+    if (!A.allFinite()) {
+      throw std::domain_error(
+          "Elements of A aren't finite. This is usually due to model "
+          "implementation errors.");
+    }
+    if (!B.allFinite()) {
+      throw std::domain_error(
+          "Elements of B aren't finite. This is usually due to model "
+          "implementation errors.");
+    }
+    if (!C.allFinite()) {
+      throw std::domain_error(
+          "Elements of C aren't finite. This is usually due to model "
+          "implementation errors.");
+    }
+    if (!D.allFinite()) {
+      throw std::domain_error(
+          "Elements of D aren't finite. This is usually due to model "
+          "implementation errors.");
+    }
+
     m_A = A;
     m_B = B;
     m_C = C;

wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h

@@ -4,6 +4,8 @@
 
 #pragma once
 
+#include <stdexcept>
+
 #include <wpi/SymbolExports.h>
 
 #include "frc/system/LinearSystem.h"
@@ -39,10 +41,21 @@ class WPILIB_DLLEXPORT LinearSystemId {
    * @param m Carriage mass.
    * @param r Pulley radius.
    * @param G Gear ratio from motor to carriage.
+   * @throws std::domain_error if m <= 0, r <= 0, or G <= 0.
    */
   static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor,
                                               units::kilogram_t m,
                                               units::meter_t r, double G) {
+    if (m <= 0_kg) {
+      throw std::domain_error("m must be greater than zero.");
+    }
+    if (r <= 0_m) {
+      throw std::domain_error("r must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw std::domain_error("G must be greater than zero.");
+    }
+
     Eigen::Matrix<double, 2, 2> A{
         {0.0, 1.0},
         {0.0, (-std::pow(G, 2) * motor.Kt /
@@ -66,9 +79,17 @@ class WPILIB_DLLEXPORT LinearSystemId {
    * @param motor Instance of DCMotor.
    * @param J Moment of inertia.
    * @param G Gear ratio from motor to carriage.
+   * @throws std::domain_error if J <= 0 or G <= 0.
    */
   static LinearSystem<2, 1, 1> SingleJointedArmSystem(
       DCMotor motor, units::kilogram_square_meter_t J, double G) {
+    if (J <= 0_kg_sq_m) {
+      throw std::domain_error("J must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw std::domain_error("G must be greater than zero.");
+    }
+
     Eigen::Matrix<double, 2, 2> A{
         {0.0, 1.0},
         {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
@@ -98,6 +119,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
    *
    * @param kV The velocity gain, in volt seconds per distance.
    * @param kA The acceleration gain, in volt seconds^2 per distance.
+   * @throws std::domain_error if kV <= 0 or kA <= 0.
    */
   template <typename Distance, typename = std::enable_if_t<
                                    std::is_same_v<units::meter, Distance> ||
@@ -105,6 +127,13 @@ class WPILIB_DLLEXPORT LinearSystemId {
   static LinearSystem<1, 1, 1> IdentifyVelocitySystem(
       decltype(1_V / Velocity_t<Distance>(1)) kV,
       decltype(1_V / Acceleration_t<Distance>(1)) kA) {
+    if (kV <= decltype(kV){0}) {
+      throw std::domain_error("Kv must be greater than zero.");
+    }
+    if (kA <= decltype(kA){0}) {
+      throw std::domain_error("Ka must be greater than zero.");
+    }
+
     Eigen::Matrix<double, 1, 1> A{-kV.value() / kA.value()};
     Eigen::Matrix<double, 1, 1> B{1.0 / kA.value()};
     Eigen::Matrix<double, 1, 1> C{1.0};
@@ -132,6 +161,7 @@ class WPILIB_DLLEXPORT LinearSystemId {
    *
    * @param kV The velocity gain, in volt seconds per distance.
    * @param kA The acceleration gain, in volt seconds^2 per distance.
+   * @throws std::domain_error if kV <= 0 or kA <= 0.
    */
   template <typename Distance, typename = std::enable_if_t<
                                    std::is_same_v<units::meter, Distance> ||
@@ -139,6 +169,13 @@ class WPILIB_DLLEXPORT LinearSystemId {
   static LinearSystem<2, 1, 1> IdentifyPositionSystem(
       decltype(1_V / Velocity_t<Distance>(1)) kV,
       decltype(1_V / Acceleration_t<Distance>(1)) kA) {
+    if (kV <= decltype(kV){0}) {
+      throw std::domain_error("Kv must be greater than zero.");
+    }
+    if (kA <= decltype(kA){0}) {
+      throw std::domain_error("Ka must be greater than zero.");
+    }
+
     Eigen::Matrix<double, 2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
     Eigen::Matrix<double, 2, 1> B{0.0, 1.0 / kA.value()};
     Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
@@ -161,10 +198,25 @@ class WPILIB_DLLEXPORT LinearSystemId {
    * @param kVangular The angular velocity gain in volts per (meter per second).
    * @param kAangular The angular acceleration gain in volts per (meter per
    *                  second squared).
+   * @throws domain_error if kVlinear <= 0, kAlinear <= 0, kVangular <= 0,
+   *         or kAangular <= 0.
    */
   static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
       decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear,
       decltype(1_V / 1_mps) kVangular, decltype(1_V / 1_mps_sq) kAangular) {
+    if (kVlinear <= decltype(kVlinear){0}) {
+      throw std::domain_error("Kv,linear must be greater than zero.");
+    }
+    if (kAlinear <= decltype(kAlinear){0}) {
+      throw std::domain_error("Ka,linear must be greater than zero.");
+    }
+    if (kVangular <= decltype(kVangular){0}) {
+      throw std::domain_error("Kv,angular must be greater than zero.");
+    }
+    if (kAangular <= decltype(kAangular){0}) {
+      throw std::domain_error("Ka,angular must be greater than zero.");
+    }
+
     double A1 = -(kVlinear.value() / kAlinear.value() +
                   kVangular.value() / kAangular.value());
     double A2 = -(kVlinear.value() / kAlinear.value() -
@@ -198,11 +250,29 @@ class WPILIB_DLLEXPORT LinearSystemId {
    * @param kAangular  The angular acceleration gain in volts per (radian per
    *                   second squared).
    * @param trackwidth The width of the drivetrain.
+   * @throws domain_error if kVlinear <= 0, kAlinear <= 0, kVangular <= 0,
+   *         kAangular <= 0, or trackwidth <= 0.
    */
   static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
       decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear,
       decltype(1_V / 1_rad_per_s) kVangular,
       decltype(1_V / 1_rad_per_s_sq) kAangular, units::meter_t trackwidth) {
+    if (kVlinear <= decltype(kVlinear){0}) {
+      throw std::domain_error("Kv,linear must be greater than zero.");
+    }
+    if (kAlinear <= decltype(kAlinear){0}) {
+      throw std::domain_error("Ka,linear must be greater than zero.");
+    }
+    if (kVangular <= decltype(kVangular){0}) {
+      throw std::domain_error("Kv,angular must be greater than zero.");
+    }
+    if (kAangular <= decltype(kAangular){0}) {
+      throw std::domain_error("Ka,angular must be greater than zero.");
+    }
+    if (trackwidth <= 0_m) {
+      throw std::domain_error("r must be greater than zero.");
+    }
+
     // We want to find a factor to include in Kv,angular that will convert
     // `u = Kv,angular omega` to `u = Kv,angular v`.
     //
@@ -229,10 +299,18 @@ class WPILIB_DLLEXPORT LinearSystemId {
    * @param motor Instance of DCMotor.
    * @param J Moment of inertia.
    * @param G Gear ratio from motor to carriage.
+   * @throws std::domain_error if J <= 0 or G <= 0.
    */
   static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor,
                                               units::kilogram_square_meter_t J,
                                               double G) {
+    if (J <= 0_kg_sq_m) {
+      throw std::domain_error("J must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw std::domain_error("G must be greater than zero.");
+    }
+
     Eigen::Matrix<double, 1, 1> A{
         (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
     Eigen::Matrix<double, 1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
@@ -253,12 +331,30 @@ class WPILIB_DLLEXPORT LinearSystemId {
    * @param m Drivetrain mass.
    * @param r Wheel radius.
    * @param rb Robot radius.
-   * @param G Gear ratio from motor to wheel.
    * @param J Moment of inertia.
+   * @param G Gear ratio from motor to wheel.
+   * @throws std::domain_error if m <= 0, r <= 0, rb <= 0, J <= 0, or
+   *         G <= 0.
    */
   static LinearSystem<2, 2, 2> DrivetrainVelocitySystem(
       const DCMotor& motor, units::kilogram_t m, units::meter_t r,
       units::meter_t rb, units::kilogram_square_meter_t J, double G) {
+    if (m <= 0_kg) {
+      throw std::domain_error("m must be greater than zero.");
+    }
+    if (r <= 0_m) {
+      throw std::domain_error("r must be greater than zero.");
+    }
+    if (rb <= 0_m) {
+      throw std::domain_error("rb must be greater than zero.");
+    }
+    if (J <= 0_kg_sq_m) {
+      throw std::domain_error("J must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw std::domain_error("G must be greater than zero.");
+    }
+
     auto C1 = -std::pow(G, 2) * motor.Kt /
               (motor.Kv * motor.R * units::math::pow<2>(r));
     auto C2 = G * motor.Kt / (motor.R * r);