From 21439b606cea649182d240ab605ff5b64579bb04 Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Wed, 2 Aug 2023 23:37:49 -0700 Subject: [PATCH] [wpimath] Disallow LTV controller max velocities above 15 m/s (#5495) 15 m/s is about 50 ft/s, which is way above what FRC robots should be able to achieve. This limit lets us catch user errors from bad unit conversions immediately instead of the LUT generation in the LTV controllers hanging for a really long time. Fixes #5027. --- .../math/controller/LTVDifferentialDriveController.java | 9 +++++++-- .../wpi/first/math/controller/LTVUnicycleController.java | 7 +++++-- .../cpp/controller/LTVDifferentialDriveController.cpp | 6 +++++- .../main/native/cpp/controller/LTVUnicycleController.cpp | 5 ++++- .../frc/controller/LTVDifferentialDriveController.h | 3 ++- .../include/frc/controller/LTVUnicycleController.h | 2 +- 6 files changed, 24 insertions(+), 8 deletions(-) diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java index 866d062388..c30b200792 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java @@ -65,7 +65,8 @@ public class LTVDifferentialDriveController { * @param qelems The maximum desired error tolerance for each state. * @param relems The maximum desired control effort for each input. * @param dt Discretization timestep in seconds. - * @throws IllegalArgumentException if max velocity of plant with 12 V input <= 0. + * @throws IllegalArgumentException if max velocity of plant with 12 V input <= 0 m/s or >= + * 15 m/s. */ public LTVDifferentialDriveController( LinearSystem plant, @@ -134,7 +135,11 @@ public class LTVDifferentialDriveController { if (maxV <= 0.0) { throw new IllegalArgumentException( - "Max velocity of plant with 12 V input must be greater than zero."); + "Max velocity of plant with 12 V input must be greater than 0 m/s."); + } + if (maxV >= 15.0) { + throw new IllegalArgumentException( + "Max velocity of plant with 12 V input must be less than 15 m/s."); } for (double velocity = -maxV; velocity < maxV; velocity += 0.01) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java index b0a18df6f1..d2ac72b7c3 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVUnicycleController.java @@ -99,12 +99,15 @@ public class LTVUnicycleController { * @param dt Discretization timestep in seconds. * @param maxVelocity The maximum velocity in meters per second for the controller gain lookup * table. The default is 9 m/s. - * @throws IllegalArgumentException if maxVelocity <= 0. + * @throws IllegalArgumentException if maxVelocity <= 0 m/s or >= 15 m/s. */ public LTVUnicycleController( Vector qelems, Vector relems, double dt, double maxVelocity) { if (maxVelocity <= 0.0) { - throw new IllegalArgumentException("Max velocity must be greater than zero."); + throw new IllegalArgumentException("Max velocity must be greater than 0 m/s."); + } + if (maxVelocity >= 15.0) { + throw new IllegalArgumentException("Max velocity must be less than 15 m/s."); } // The change in global pose for a unicycle is defined by the following diff --git a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp index 7a3a78d4d0..7d32bfdebd 100644 --- a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp +++ b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp @@ -68,7 +68,11 @@ LTVDifferentialDriveController::LTVDifferentialDriveController( if (maxV <= 0_mps) { throw std::domain_error( - "Max velocity of plant with 12 V input must be greater than zero."); + "Max velocity of plant with 12 V input must be greater than 0 m/s."); + } + if (maxV >= 15_mps) { + throw std::domain_error( + "Max velocity of plant with 12 V input must be less than 15 m/s."); } for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) { diff --git a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp index 4f12ac6ac9..75188cb7a8 100644 --- a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp +++ b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp @@ -40,7 +40,10 @@ LTVUnicycleController::LTVUnicycleController( const wpi::array& Qelems, const wpi::array& Relems, units::second_t dt, units::meters_per_second_t maxVelocity) { if (maxVelocity <= 0_mps) { - throw std::domain_error("Max velocity must be greater than zero."); + throw std::domain_error("Max velocity must be greater than 0 m/s."); + } + if (maxVelocity >= 15_mps) { + throw std::domain_error("Max velocity must be less than 15 m/s."); } // The change in global pose for a unicycle is defined by the following three diff --git a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h index 7c85350608..3995d3a3a7 100644 --- a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h +++ b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h @@ -45,7 +45,8 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController { * @param Qelems The maximum desired error tolerance for each state. * @param Relems The maximum desired control effort for each input. * @param dt Discretization timestep. - * @throws std::domain_error if max velocity of plant with 12 V input <= 0. + * @throws std::domain_error if max velocity of plant with 12 V input <= 0 m/s + * or >= 15 m/s. */ LTVDifferentialDriveController(const frc::LinearSystem<2, 2, 2>& plant, units::meter_t trackwidth, diff --git a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h index 9ea4606b14..b128dbb7d4 100644 --- a/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h +++ b/wpimath/src/main/native/include/frc/controller/LTVUnicycleController.h @@ -53,7 +53,7 @@ class WPILIB_DLLEXPORT LTVUnicycleController { * @param dt Discretization timestep. * @param maxVelocity The maximum velocity for the controller gain lookup * table. - * @throws std::domain_error if maxVelocity <= 0. + * @throws std::domain_error if maxVelocity <= 0 m/s or >= 15 m/s. */ LTVUnicycleController(const wpi::array& Qelems, const wpi::array& Relems, units::second_t dt,