[wpimath] Check LTV controller max velocity precondition (wpilibsuite…

…#5142)
Starlight220 · Mar 2, 2023 · 8279892 · 8279892
1 parent a74f173
commit 8279892
Show file tree

Hide file tree

Showing 6 changed files with 27 additions and 0 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
@@ -61,6 +61,7 @@ private enum State {
    * @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 &lt;= 0.
    */
   public LTVDifferentialDriveController(
       LinearSystem<N2, N2, N2> plant,
@@ -127,6 +128,11 @@ public LTVDifferentialDriveController(
             .times(-1.0)
             .get(0, 0);
 
+    if (maxV <= 0.0) {
+      throw new IllegalArgumentException(
+          "Max velocity of plant with 12 V input must be greater than zero.");
+    }
+
     for (double velocity = -maxV; velocity < maxV; velocity += 0.01) {
       // The DARE is ill-conditioned if the velocity is close to zero, so don't
       // let the system stop.

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
@@ -66,6 +66,7 @@ public LTVUnicycleController(double dt) {
    * @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 &lt;= 0.
    */
   public LTVUnicycleController(double dt, double maxVelocity) {
     this(VecBuilder.fill(0.0625, 0.125, 2.0), VecBuilder.fill(1.0, 2.0), dt, maxVelocity);
@@ -90,9 +91,14 @@ public LTVUnicycleController(Vector<N3> qelems, Vector<N2> relems, double dt) {
    * @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 &lt;= 0.
    */
   public LTVUnicycleController(
       Vector<N3> qelems, Vector<N2> relems, double dt, double maxVelocity) {
+    if (maxVelocity <= 0.0) {
+      throw new IllegalArgumentException("Max velocity must be greater than zero.");
+    }
+
     // The change in global pose for a unicycle is defined by the following
     // three equations.
     //

diff --git a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp
@@ -5,6 +5,7 @@
 #include "frc/controller/LTVDifferentialDriveController.h"
 
 #include <cmath>
+#include <stdexcept>
 
 #include "frc/MathUtil.h"
 #include "frc/StateSpaceUtil.h"
@@ -65,6 +66,11 @@ LTVDifferentialDriveController::LTVDifferentialDriveController(
   units::meters_per_second_t maxV{
       -plant.A().householderQr().solve(plant.B() * Vectord<2>{12.0, 12.0})(0)};
 
+  if (maxV <= 0_mps) {
+    throw std::domain_error(
+        "Max velocity of plant with 12 V input must be greater than zero.");
+  }
+
   for (auto velocity = -maxV; velocity < maxV; velocity += 0.01_mps) {
     // The DARE is ill-conditioned if the velocity is close to zero, so don't
     // let the system stop.

diff --git a/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp b/wpimath/src/main/native/cpp/controller/LTVUnicycleController.cpp
@@ -4,6 +4,8 @@
 
 #include "frc/controller/LTVUnicycleController.h"
 
+#include <stdexcept>
+
 #include "frc/StateSpaceUtil.h"
 #include "frc/controller/LinearQuadraticRegulator.h"
 #include "units/math.h"
@@ -37,6 +39,10 @@ LTVUnicycleController::LTVUnicycleController(
 LTVUnicycleController::LTVUnicycleController(
     const wpi::array<double, 3>& Qelems, const wpi::array<double, 2>& 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.");
+  }
+
   // The change in global pose for a unicycle is defined by the following three
   // equations.
   //

diff --git a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h
@@ -41,6 +41,7 @@ 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.
    */
   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
@@ -36,6 +36,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 &lt;= 0.
    */
   explicit LTVUnicycleController(
       units::second_t dt, units::meters_per_second_t maxVelocity = 9_mps);
@@ -48,6 +49,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 &lt;= 0.
    */
   LTVUnicycleController(const wpi::array<double, 3>& Qelems,
                         const wpi::array<double, 2>& Relems, units::second_t dt,