diff --git a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java index 30984ac3b1..ca3f4dca7f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java +++ b/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. * diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java index 195ba4f3bc..d7cf1ba8f6 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java @@ -211,4 +211,30 @@ public class LinearQuadraticRegulatorLinear-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 dtSeconds Discretization timestep in seconds. + * @param inputDelaySeconds Input time delay in seconds. + */ + public void latencyCompensate( + LinearSystem 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)); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java index a87b98ada8..fd4d639547 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java +++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java @@ -368,6 +368,25 @@ public class Matrix { 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 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 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. * diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index 26d57e2ab0..f350baf60e 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/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> + Amat{arrayBody, rows, rows}; + Eigen::Matrix 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 diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h index f448957803..7224682b01 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h +++ b/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 + void LatencyCompensate(const LinearSystem& plant, + units::second_t dt, units::second_t inputDelay) { + Eigen::Matrix discA; + Eigen::Matrix discB; + DiscretizeAB(plant.A(), plant.B(), dt, &discA, &discB); + m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt); + } + private: // Current reference Eigen::Matrix 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. diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulatorTest.java index e047198397..c1cb2aeebf 100644 --- a/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulatorTest.java +++ b/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); + } } diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp index eecaf34c2c..949d99a047 100644 --- a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp +++ b/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