[wpilib] Add function to adjust LQR controller gain for pure time delay (#2878)

There were three options for where to put this function:

1. A free function in LinearQuadraticRegulator.h. Returning a K matrix
   means the user can't use the LinearQuadraticRegulator in a loop
   anymore.
2. A default argument added to ctors in LinearQuadraticRegulator for a
   time delay (default of 0). This has the smallest API footprint from
   the user perspective, but it bloats the already substantial
   constructor overload set of LinearQuadraticRegulator.
3. A member function in LinearQuadraticRegulator that modifies the
   internal K. This would still have to take in a LinearSystem or (A, B)
   pair because the ctor doesn't store it. Storing it internally feels
   like paying for what we don't use most of the time.

I went with option 3.

I verified the tests's expected values in Python with
scipy.linalg.fractional_matrix_power().

Closes #2877.

wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java

@@ -69,11 +69,21 @@ public final class WPIMathJNI {
    * Computes the matrix exp.
    *
    * @param src  Array of elements of the matrix to be exponentiated.
-   * @param rows how many rows there are.
+   * @param rows How many rows there are.
    * @param dst  Array where the result will be stored.
    */
   public static native void exp(double[] src, int rows, double[] dst);
 
+  /**
+   * Computes the matrix pow.
+   *
+   * @param src      Array of elements of the matrix to be raised to a power.
+   * @param rows     How many rows there are.
+   * @param exponent The exponent.
+   * @param dst      Array where the result will be stored.
+   */
+  public static native void pow(double[] src, int rows, double exponent, double[] dst);
+
   /**
    * Returns true if (A, B) is a stabilizable pair.
    *

wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java

@@ -211,4 +211,30 @@ public class LinearQuadraticRegulator<States extends Num, Inputs extends Num,
     m_r = nextR;
     return calculate(x);
   }
+
+  /**
+   * Adjusts LQR controller gain to compensate for a pure time delay in the
+   * input.
+   *
+   * <p>Linear-Quadratic regulator controller gains tend to be aggressive. If
+   * sensor measurements are time-delayed too long, the LQR may be unstable.
+   * However, if we know the amount of delay, we can compute the control based
+   * on where the system will be after the time delay.
+   *
+   * <p>See https://file.tavsys.net/control/controls-engineering-in-frc.pdf
+   * appendix C.4 for a derivation.
+   *
+   * @param plant             The plant being controlled.
+   * @param dtSeconds         Discretization timestep in seconds.
+   * @param inputDelaySeconds Input time delay in seconds.
+   */
+  public void latencyCompensate(
+      LinearSystem<States, Inputs, Outputs> plant, double dtSeconds,
+      double inputDelaySeconds) {
+    var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dtSeconds);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    m_K = m_K.times((discA.minus(discB.times(m_K))).pow(inputDelaySeconds / dtSeconds));
+  }
 }

wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java

@@ -368,6 +368,25 @@ public class Matrix<R extends Num, C extends Num> {
     return toReturn;
   }
 
+  /**
+   * Computes the matrix power using Eigen's solver.
+   * This method only works for square matrices, and will
+   * otherwise throw an {@link MatrixDimensionException}.
+   *
+   * @param exponent The exponent.
+   * @return The exponential of A.
+   */
+  public final Matrix<R, C> pow(double exponent) {
+    if (this.getNumRows() != this.getNumCols()) {
+      throw new MatrixDimensionException("Non-square matrices cannot be raised to a power! "
+            + "This matrix is " + this.getNumRows() + " x " + this.getNumCols());
+    }
+    Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
+    WPIMathJNI.pow(this.m_storage.getDDRM().getData(), this.getNumRows(), exponent,
+          toReturn.m_storage.getDDRM().getData());
+    return toReturn;
+  }
+
   /**
    * Returns the determinant of this matrix.
    *

wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp

@@ -105,6 +105,28 @@ Java_edu_wpi_first_math_WPIMathJNI_exp
   env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data());
 }
 
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    pow
+ * Signature: ([DID[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_pow
+  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdouble exponent,
+   jdoubleArray dst)
+{
+  jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr);
+
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      Amat{arrayBody, rows, rows};
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Apow =
+      Amat.pow(exponent);
+
+  env->ReleaseDoubleArrayElements(src, arrayBody, 0);
+  env->SetDoubleArrayRegion(dst, 0, rows * rows, Apow.data());
+}
+
 /*
  * Class:     edu_wpi_first_math_WPIMathJNI
  * Method:    isStabilizable

wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h

@@ -11,11 +11,16 @@
 
 #include "Eigen/Core"
 #include "Eigen/src/Cholesky/LLT.h"
+#include "Eigen/src/Eigenvalues/ComplexSchur.h"
+#include "Eigen/src/LU/Determinant.h"
+#include "Eigen/src/LU/InverseImpl.h"
 #include "drake/math/discrete_algebraic_riccati_equation.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/system/Discretization.h"
 #include "frc/system/LinearSystem.h"
 #include "units/time.h"
+#include "unsupported/Eigen/src/MatrixFunctions/MatrixPower.h"
+#include "unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h"
 
 namespace frc {
 namespace detail {
@@ -172,6 +177,31 @@ class LinearQuadraticRegulatorImpl {
     return Calculate(x);
   }
 
+  /**
+   * Adjusts LQR controller gain to compensate for a pure time delay in the
+   * input.
+   *
+   * Linear-Quadratic regulator controller gains tend to be aggressive. If
+   * sensor measurements are time-delayed too long, the LQR may be unstable.
+   * However, if we know the amount of delay, we can compute the control based
+   * on where the system will be after the time delay.
+   *
+   * See https://file.tavsys.net/control/controls-engineering-in-frc.pdf
+   * appendix C.4 for a derivation.
+   *
+   * @param plant      The plant being controlled.
+   * @param dt         Discretization timestep.
+   * @param inputDelay Input time delay.
+   */
+  template <int Outputs>
+  void LatencyCompensate(const LinearSystem<States, Inputs, Outputs>& plant,
+                         units::second_t dt, units::second_t inputDelay) {
+    Eigen::Matrix<double, States, States> discA;
+    Eigen::Matrix<double, States, Inputs> discB;
+    DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
+    m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
+  }
+
  private:
   // Current reference
   Eigen::Matrix<double, States, 1> m_r;
@@ -192,7 +222,7 @@ class LinearQuadraticRegulator
   /**
    * Constructs a controller with the given coefficients and plant.
    *
-   * @param system The plant being controlled.
+   * @param plant  The plant being controlled.
    * @param Qelems The maximum desired error tolerance for each state.
    * @param Relems The maximum desired control effort for each input.
    * @param dt     Discretization timestep.

wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulatorTest.java

@@ -113,4 +113,26 @@ public class LinearQuadraticRegulatorTest {
     assertEquals(19.16, k.get(0, 0), 0.1);
     assertEquals(3.32, k.get(0, 1), 0.1);
   }
+
+  @Test
+  public void testLatencyCompensate() {
+    var dt = 0.02;
+
+    var plant = LinearSystemId.createElevatorSystem(
+          DCMotor.getVex775Pro(4),
+          8.0,
+          0.75 * 25.4 / 1000.0,
+          14.67);
+
+    var regulator = new LinearQuadraticRegulator<>(
+          plant,
+          VecBuilder.fill(0.1, 0.2),
+          VecBuilder.fill(12.0),
+          dt);
+
+    regulator.latencyCompensate(plant, dt, 0.01);
+
+    assertEquals(8.97115941, regulator.getK().get(0, 0), 1e-3);
+    assertEquals(0.07904881, regulator.getK().get(0, 1), 1e-3);
+  }
 }

wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp

@@ -85,4 +85,27 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) {
   EXPECT_NEAR(0.69, controller.K(0, 1), 1e-1);
 }
 
+TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {
+  LinearSystem<2, 1, 1> plant = [] {
+    auto motors = DCMotor::Vex775Pro(4);
+
+    // Carriage mass
+    constexpr auto m = 8_kg;
+
+    // Radius of pulley
+    constexpr auto r = 0.75_in;
+
+    // Gear ratio
+    constexpr double G = 14.67;
+
+    return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
+  }();
+  LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 0.02_s};
+
+  controller.LatencyCompensate(plant, 0.02_s, 0.01_s);
+
+  EXPECT_NEAR(8.97115941, controller.K(0, 0), 1e-3);
+  EXPECT_NEAR(0.07904881, controller.K(0, 1), 1e-3);
+}
+
 }  // namespace frc