diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java index bcd6f731c9..e244c5f1c2 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java @@ -118,8 +118,13 @@ public class LinearQuadraticRegulator(new SimpleMatrix(B.getNumRows(), 1)); m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1)); @@ -152,8 +157,13 @@ public class LinearQuadraticRegulator(new SimpleMatrix(B.getNumRows(), 1)); m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1)); diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h index c934e0aa32..44a850178b 100644 --- a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h +++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h @@ -135,7 +135,7 @@ class LinearQuadraticRegulatorImpl { drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N); // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ) - m_K = (B.transpose() * S * B + R) + m_K = (discB.transpose() * S * discB + R) .llt() .solve(discB.transpose() * S * discA + N.transpose()); diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java index 7659f2bd12..93b89c8bbc 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java @@ -6,7 +6,11 @@ package edu.wpi.first.math.controller; import static org.junit.jupiter.api.Assertions.assertEquals; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +import edu.wpi.first.math.Num; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.system.Discretization; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import org.junit.jupiter.api.Test; @@ -27,15 +31,14 @@ class LinearQuadraticRegulatorTest { var rElms = VecBuilder.fill(12.0); var dt = 0.00505; - var controller = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt); + var K = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt).getK(); - var k = controller.getK(); - - assertEquals(522.153, k.get(0, 0), 0.1); - assertEquals(38.2, k.get(0, 1), 0.1); + assertEquals(522.153, K.get(0, 0), 0.1); + assertEquals(38.2, K.get(0, 1), 0.1); } @Test + @SuppressWarnings("LocalVariableName") void testFourMotorElevator() { var dt = 0.020; @@ -43,11 +46,12 @@ class LinearQuadraticRegulatorTest { 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); + var K = + new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt) + .getK(); - assertEquals(10.381, regulator.getK().get(0, 0), 1e-2); - assertEquals(0.6929, regulator.getK().get(0, 1), 1e-2); + assertEquals(10.381, K.get(0, 0), 1e-2); + assertEquals(0.6929, K.get(0, 1), 1e-2); } @Test @@ -65,12 +69,95 @@ class LinearQuadraticRegulatorTest { var rElms = VecBuilder.fill(12.0); var dt = 0.00505; - var controller = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt); + var K = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt).getK(); - var k = controller.getK(); + assertEquals(19.16, K.get(0, 0), 0.1); + assertEquals(3.32, K.get(0, 1), 0.1); + } - assertEquals(19.16, k.get(0, 0), 0.1); - assertEquals(3.32, k.get(0, 1), 0.1); + /** + * Returns feedback control gain for implicit model following. + * + *

This is used to test the QRN overload of LQR. + * + * @param States Number of states. + * @param Inputs Number of inputs. + * @param A State matrix. + * @param B Input matrix. + * @param Q State cost matrix. + * @param R Input cost matrix. + * @param Aref Desired state matrix. + * @param dtSeconds Discretization timestep in seconds. + */ + @SuppressWarnings({"LocalVariableName", "MethodTypeParameterName", "ParameterName"}) + Matrix getImplicitModelFollowingK( + Matrix A, + Matrix B, + Matrix Q, + Matrix R, + Matrix Aref, + double dtSeconds) { + // Discretize real dynamics + var discABPair = Discretization.discretizeAB(A, B, dtSeconds); + var discA = discABPair.getFirst(); + var discB = discABPair.getSecond(); + + // Discretize desired dynamics + var discAref = Discretization.discretizeA(Aref, dtSeconds); + + Matrix Qimf = + (discA.minus(discAref)).transpose().times(Q).times(discA.minus(discAref)); + Matrix Rimf = discB.transpose().times(Q).times(discB).plus(R); + Matrix Nimf = (discA.minus(discAref)).transpose().times(Q).times(discB); + + return new LinearQuadraticRegulator<>(A, B, Qimf, Rimf, Nimf, dtSeconds).getK(); + } + + @Test + @SuppressWarnings("LocalVariableName") + void testMatrixOverloadsWithSingleIntegrator() { + var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0); + var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); + var Q = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); + var R = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); + + // QR overload + var K = new LinearQuadraticRegulator<>(A, B, Q, R, 0.005).getK(); + assertEquals(0.99750312499512261, K.get(0, 0), 1e-10); + assertEquals(0.0, K.get(0, 1), 1e-10); + assertEquals(0.0, K.get(1, 0), 1e-10); + assertEquals(0.99750312499512261, K.get(1, 1), 1e-10); + + // QRN overload + var N = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); + var Kimf = new LinearQuadraticRegulator<>(A, B, Q, R, N, 0.005).getK(); + assertEquals(1.0, Kimf.get(0, 0), 1e-10); + assertEquals(0.0, Kimf.get(0, 1), 1e-10); + assertEquals(0.0, Kimf.get(1, 0), 1e-10); + assertEquals(1.0, Kimf.get(1, 1), 1e-10); + } + + @Test + @SuppressWarnings("LocalVariableName") + void testMatrixOverloadsWithDoubleIntegrator() { + double Kv = 3.02; + double Ka = 0.642; + + var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / Ka); + var B = Matrix.mat(Nat.N2(), Nat.N1()).fill(0, 1.0 / Ka); + var Q = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.2); + var R = Matrix.mat(Nat.N1(), Nat.N1()).fill(0.25); + + // QR overload + var K = new LinearQuadraticRegulator<>(A, B, Q, R, 0.005).getK(); + assertEquals(1.9960017786537287, K.get(0, 0), 1e-10); + assertEquals(0.51182128351092726, K.get(0, 1), 1e-10); + + // QRN overload + var Aref = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -Kv / (Ka * 2.0)); + var Kimf = getImplicitModelFollowingK(A, B, Q, R, Aref, 0.005); + assertEquals(0.0, Kimf.get(0, 0), 1e-10); + assertEquals(-5.367540084534802e-05, Kimf.get(0, 1), 1e-10); } @Test diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp index 8c52cd00a8..7055530d71 100644 --- a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp +++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp @@ -30,11 +30,11 @@ TEST(LinearQuadraticRegulatorTest, ElevatorGains) { return frc::LinearSystemId::ElevatorSystem(motors, m, r, G); }(); - LinearQuadraticRegulator<2, 1> controller{ - plant, {0.02, 0.4}, {12.0}, 0.00505_s}; + Eigen::Matrix K = + LinearQuadraticRegulator<2, 1>{plant, {0.02, 0.4}, {12.0}, 5.05_ms}.K(); - EXPECT_NEAR(522.15314269, controller.K(0, 0), 1e-6); - EXPECT_NEAR(38.20138596, controller.K(0, 1), 1e-6); + EXPECT_NEAR(522.15314269, K(0, 0), 1e-6); + EXPECT_NEAR(38.20138596, K(0, 1), 1e-6); } TEST(LinearQuadraticRegulatorTest, ArmGains) { @@ -54,11 +54,12 @@ TEST(LinearQuadraticRegulatorTest, ArmGains) { motors, 1.0 / 3.0 * m * r * r, G); }(); - LinearQuadraticRegulator<2, 1> controller{ - plant, {0.01745, 0.08726}, {12.0}, 0.00505_s}; + Eigen::Matrix K = + LinearQuadraticRegulator<2, 1>{plant, {0.01745, 0.08726}, {12.0}, 5.05_ms} + .K(); - EXPECT_NEAR(19.16, controller.K(0, 0), 1e-1); - EXPECT_NEAR(3.32, controller.K(0, 1), 1e-1); + EXPECT_NEAR(19.16, K(0, 0), 1e-1); + EXPECT_NEAR(3.32, K(0, 1), 1e-1); } TEST(LinearQuadraticRegulatorTest, FourMotorElevator) { @@ -76,10 +77,99 @@ TEST(LinearQuadraticRegulatorTest, FourMotorElevator) { return frc::LinearSystemId::ElevatorSystem(motors, m, r, G); }(); - LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 0.020_s}; + Eigen::Matrix K = + LinearQuadraticRegulator<2, 1>{plant, {0.1, 0.2}, {12.0}, 20_ms}.K(); - EXPECT_NEAR(10.38, controller.K(0, 0), 1e-1); - EXPECT_NEAR(0.69, controller.K(0, 1), 1e-1); + EXPECT_NEAR(10.38, K(0, 0), 1e-1); + EXPECT_NEAR(0.69, K(0, 1), 1e-1); +} + +/** + * Returns feedback control gain for implicit model following. + * + * This is used to test the QRN overload of LQR. + * + * @tparam States Number of states. + * @tparam Inputs Number of inputs. + * @param A State matrix. + * @param B Input matrix. + * @param Q State cost matrix. + * @param R Input cost matrix. + * @param Aref Desired state matrix. + * @param dt Discretization timestep. + */ +template +Eigen::Matrix GetImplicitModelFollowingK( + const Eigen::Matrix& A, + const Eigen::Matrix& B, + const Eigen::Matrix& Q, + const Eigen::Matrix& R, + const Eigen::Matrix& Aref, units::second_t dt) { + // Discretize real dynamics + Eigen::Matrix discA; + Eigen::Matrix discB; + DiscretizeAB(A, B, dt, &discA, &discB); + + // Discretize desired dynamics + Eigen::Matrix discAref; + DiscretizeA(Aref, dt, &discAref); + + Eigen::Matrix Qimf = + (discA - discAref).transpose() * Q * (discA - discAref); + Eigen::Matrix Rimf = + discB.transpose() * Q * discB + R; + Eigen::Matrix Nimf = + (discA - discAref).transpose() * Q * discB; + + return LinearQuadraticRegulator{A, B, Qimf, Rimf, Nimf, dt} + .K(); +} + +TEST(LinearQuadraticRegulatorTest, MatrixOverloadsWithSingleIntegrator) { + Eigen::Matrix A{Eigen::Matrix::Zero()}; + Eigen::Matrix B{Eigen::Matrix::Identity()}; + Eigen::Matrix Q{Eigen::Matrix::Identity()}; + Eigen::Matrix R{Eigen::Matrix::Identity()}; + + // QR overload + Eigen::Matrix K = + LinearQuadraticRegulator<2, 2>{A, B, Q, R, 5_ms}.K(); + EXPECT_NEAR(0.99750312499512261, K(0, 0), 1e-10); + EXPECT_NEAR(0.0, K(0, 1), 1e-10); + EXPECT_NEAR(0.0, K(1, 0), 1e-10); + EXPECT_NEAR(0.99750312499512261, K(1, 1), 1e-10); + + // QRN overload + Eigen::Matrix N{Eigen::Matrix::Identity()}; + Eigen::Matrix Kimf = + LinearQuadraticRegulator<2, 2>{A, B, Q, R, N, 5_ms}.K(); + EXPECT_NEAR(1.0, Kimf(0, 0), 1e-10); + EXPECT_NEAR(0.0, Kimf(0, 1), 1e-10); + EXPECT_NEAR(0.0, Kimf(1, 0), 1e-10); + EXPECT_NEAR(1.0, Kimf(1, 1), 1e-10); +} + +TEST(LinearQuadraticRegulatorTest, MatrixOverloadsWithDoubleIntegrator) { + double Kv = 3.02; + double Ka = 0.642; + + Eigen::Matrix A{{0, 1}, {0, -Kv / Ka}}; + Eigen::Matrix B{{0}, {1.0 / Ka}}; + Eigen::Matrix Q{{1, 0}, {0, 0.2}}; + Eigen::Matrix R{0.25}; + + // QR overload + Eigen::Matrix K = + LinearQuadraticRegulator<2, 1>{A, B, Q, R, 5_ms}.K(); + EXPECT_NEAR(1.9960017786537287, K(0, 0), 1e-10); + EXPECT_NEAR(0.51182128351092726, K(0, 1), 1e-10); + + // QRN overload + Eigen::Matrix Aref{{0, 1}, {0, -Kv / (Ka * 2.0)}}; + Eigen::Matrix Kimf = + GetImplicitModelFollowingK<2, 1>(A, B, Q, R, Aref, 5_ms); + EXPECT_NEAR(0.0, Kimf(0, 0), 1e-10); + EXPECT_NEAR(-5.367540084534802e-05, Kimf(0, 1), 1e-10); } TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {