Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Make LTV controller constructors use faster DARE solver (#5543)

Browse Source 
Made JNI modifications to expose the faster function, made the API use
the typesafe Matrix API, and synchronized the documentation with C++.

Sped up C++ LTV diff drive test from 20 ms to 15 ms.
Sped up C++ LTV unicycle test from 15 ms to 10 ms.
This commit is contained in:
Tyler Veness
2023-08-17 13:56:15 -07:00
committed by GitHub
parent 6953a303b3
commit 0cf6e37dc1
9 changed files with 596 additions and 220 deletions
Download Patch File Download Diff File Expand all files Collapse all files

239
wpimath/src/test/java/edu/wpi/first/math/DARETest.java
View File

@@ -16,7 +16,8 @@ class DARETest extends UtilityClassTest<DARE> {
super(DARE.class);
}
public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) {
public static <R extends Num, C extends Num> void assertMatrixEqual(
Matrix<R, C> A, Matrix<R, C> B) {
for (int i = 0; i < A.getNumRows(); i++) {
for (int j = 0; j < A.getNumCols(); j++) {
assertEquals(A.get(i, j), B.get(i, j), 1e-4);
@@ -24,39 +25,45 @@ class DARETest extends UtilityClassTest<DARE> {
}
}
void assertDARESolution(
SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R, SimpleMatrix X) {
<States extends Num, Inputs extends Num> void assertDARESolution(
Matrix<States, States> A,
Matrix<States, Inputs> B,
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R,
Matrix<States, States> X) {
// Check that X is the solution to the DARE
// Y = AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
var Y =
(A.transpose().mult(X).mult(A))
(A.transpose().times(X).times(A))
.minus(X)
.minus(
(A.transpose().mult(X).mult(B))
.mult((B.transpose().mult(X).mult(B).plus(R)).invert())
.mult(B.transpose().mult(X).mult(A)))
(A.transpose().times(X).times(B))
.times((B.transpose().times(X).times(B).plus(R)).inv())
.times(B.transpose().times(X).times(A)))
.plus(Q);
assertMatrixEqual(new SimpleMatrix(Y.getNumRows(), Y.getNumCols()), Y);
assertMatrixEqual(
new Matrix<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
}
void assertDARESolution(
SimpleMatrix A,
SimpleMatrix B,
SimpleMatrix Q,
SimpleMatrix R,
SimpleMatrix N,
SimpleMatrix X) {
<States extends Num, Inputs extends Num> void assertDARESolution(
Matrix<States, States> A,
Matrix<States, Inputs> B,
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R,
Matrix<States, Inputs> N,
Matrix<States, States> X) {
// Check that X is the solution to the DARE
// Y = AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
var Y =
(A.transpose().mult(X).mult(A))
(A.transpose().times(X).times(A))
.minus(X)
.minus(
(A.transpose().mult(X).mult(B).plus(N))
.mult((B.transpose().mult(X).mult(B).plus(R)).invert())
.mult(B.transpose().mult(X).mult(A).plus(N.transpose())))
(A.transpose().times(X).times(B).plus(N))
.times((B.transpose().times(X).times(B).plus(R)).inv())
.times(B.transpose().times(X).times(A).plus(N.transpose())))
.plus(Q);
assertMatrixEqual(new SimpleMatrix(Y.getNumRows(), Y.getNumCols()), Y);
assertMatrixEqual(
new Matrix<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
}
@Test
@@ -65,12 +72,13 @@ class DARETest extends UtilityClassTest<DARE> {
// Riccati Equation"
var A =
new SimpleMatrix(
4, 4, true, new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
var B = new SimpleMatrix(4, 1, true, new double[] {0, 0, 0, 1});
new Matrix<>(
Nat.N4(), Nat.N4(), new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
var B = new Matrix<>(Nat.N4(), Nat.N1(), new double[] {0, 0, 0, 1});
var Q =
new SimpleMatrix(4, 4, true, new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
var R = new SimpleMatrix(1, 1, true, new double[] {0.25});
new Matrix<>(
Nat.N4(), Nat.N4(), new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.25});
var X = DARE.dare(A, B, Q, R);
assertMatrixEqual(X, X.transpose());
@@ -83,19 +91,20 @@ class DARETest extends UtilityClassTest<DARE> {
// Riccati Equation"
var A =
new SimpleMatrix(
4, 4, true, new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
var B = new SimpleMatrix(4, 1, true, new double[] {0, 0, 0, 1});
new Matrix<>(
Nat.N4(), Nat.N4(), new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
var B = new Matrix<>(Nat.N4(), Nat.N1(), new double[] {0, 0, 0, 1});
var Q =
new SimpleMatrix(4, 4, true, new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
var R = new SimpleMatrix(1, 1, true, new double[] {0.25});
new Matrix<>(
Nat.N4(), Nat.N4(), new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.25});
var Aref =
new SimpleMatrix(
4, 4, true, new double[] {0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
Q = A.minus(Aref).transpose().mult(Q).mult(A.minus(Aref));
R = B.transpose().mult(Q).mult(B).plus(R);
var N = A.minus(Aref).transpose().mult(Q).mult(B);
new Matrix<>(
Nat.N4(), Nat.N4(), new double[] {0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0});
Q = A.minus(Aref).transpose().times(Q).times(A.minus(Aref));
R = B.transpose().times(Q).times(B).plus(R);
var N = A.minus(Aref).transpose().times(Q).times(B);
var X = DARE.dare(A, B, Q, R, N);
assertMatrixEqual(X, X.transpose());
@@ -104,10 +113,10 @@ class DARETest extends UtilityClassTest<DARE> {
@Test
void testInvertibleA_ABQR() {
var A = new SimpleMatrix(2, 2, true, new double[] {1, 1, 0, 1});
var B = new SimpleMatrix(2, 1, true, new double[] {0, 1});
var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 0});
var R = new SimpleMatrix(1, 1, true, new double[] {0.3});
var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 1, 0, 1});
var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 0});
var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.3});
var X = DARE.dare(A, B, Q, R);
assertMatrixEqual(X, X.transpose());
@@ -116,15 +125,15 @@ class DARETest extends UtilityClassTest<DARE> {
@Test
void testInvertibleA_ABQRN() {
var A = new SimpleMatrix(2, 2, true, new double[] {1, 1, 0, 1});
var B = new SimpleMatrix(2, 1, true, new double[] {0, 1});
var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 0});
var R = new SimpleMatrix(1, 1, true, new double[] {0.3});
var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 1, 0, 1});
var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 0});
var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {0.3});
var Aref = new SimpleMatrix(2, 2, true, new double[] {0.5, 1, 0, 1});
Q = A.minus(Aref).transpose().mult(Q).mult(A.minus(Aref));
R = B.transpose().mult(Q).mult(B).plus(R);
var N = A.minus(Aref).transpose().mult(Q).mult(B);
var Aref = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0.5, 1, 0, 1});
Q = A.minus(Aref).transpose().times(Q).times(A.minus(Aref));
R = B.transpose().times(Q).times(B).plus(R);
var N = A.minus(Aref).transpose().times(Q).times(B);
var X = DARE.dare(A, B, Q, R, N);
assertMatrixEqual(X, X.transpose());
@@ -135,10 +144,10 @@ class DARETest extends UtilityClassTest<DARE> {
void testFirstGeneralizedEigenvalueOfSTIsStable_ABQR() {
// The first generalized eigenvalue of (S, T) is stable
var A = new SimpleMatrix(2, 2, true, new double[] {0, 1, 0, 0});
var B = new SimpleMatrix(2, 1, true, new double[] {0, 1});
var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 1});
var R = new SimpleMatrix(1, 1, true, new double[] {1});
var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0, 1, 0, 0});
var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 1});
var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {1});
var X = DARE.dare(A, B, Q, R);
assertMatrixEqual(X, X.transpose());
@@ -149,15 +158,15 @@ class DARETest extends UtilityClassTest<DARE> {
void testFirstGeneralizedEigenvalueOfSTIsStable_ABQRN() {
// The first generalized eigenvalue of (S, T) is stable
var A = new SimpleMatrix(2, 2, true, new double[] {0, 1, 0, 0});
var B = new SimpleMatrix(2, 1, true, new double[] {0, 1});
var Q = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 1});
var R = new SimpleMatrix(1, 1, true, new double[] {1});
var A = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0, 1, 0, 0});
var B = new Matrix<>(Nat.N2(), Nat.N1(), new double[] {0, 1});
var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {1, 0, 0, 1});
var R = new Matrix<>(Nat.N1(), Nat.N1(), new double[] {1});
var Aref = new SimpleMatrix(2, 2, true, new double[] {0, 0.5, 0, 0});
Q = A.minus(Aref).transpose().mult(Q).mult(A.minus(Aref));
R = B.transpose().mult(Q).mult(B).plus(R);
var N = A.minus(Aref).transpose().mult(Q).mult(B);
var Aref = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {0, 0.5, 0, 0});
Q = A.minus(Aref).transpose().times(Q).times(A.minus(Aref));
R = B.transpose().times(Q).times(B).plus(R);
var N = A.minus(Aref).transpose().times(Q).times(B);
var X = DARE.dare(A, B, Q, R, N);
assertMatrixEqual(X, X.transpose());
@@ -166,10 +175,10 @@ class DARETest extends UtilityClassTest<DARE> {
@Test
void testIdentitySystem_ABQR() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.identity(2);
var A = Matrix.eye(Nat.N2());
var B = Matrix.eye(Nat.N2());
var Q = Matrix.eye(Nat.N2());
var R = Matrix.eye(Nat.N2());
var X = DARE.dare(A, B, Q, R);
assertMatrixEqual(X, X.transpose());
@@ -178,11 +187,11 @@ class DARETest extends UtilityClassTest<DARE> {
@Test
void testIdentitySystem_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.identity(2);
var N = SimpleMatrix.identity(2);
var A = Matrix.eye(Nat.N2());
var B = Matrix.eye(Nat.N2());
var Q = Matrix.eye(Nat.N2());
var R = Matrix.eye(Nat.N2());
var N = Matrix.eye(Nat.N2());
var X = DARE.dare(A, B, Q, R, N);
assertMatrixEqual(X, X.transpose());
@@ -191,10 +200,10 @@ class DARETest extends UtilityClassTest<DARE> {
@Test
void testMoreInputsThanStates_ABQR() {
var A = SimpleMatrix.identity(2);
var B = new SimpleMatrix(2, 3, true, new double[] {1, 0, 0, 0, 0.5, 0.3});
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.identity(3);
var A = Matrix.eye(Nat.N2());
var B = new Matrix<>(Nat.N2(), Nat.N3(), new double[] {1, 0, 0, 0, 0.5, 0.3});
var Q = Matrix.eye(Nat.N2());
var R = Matrix.eye(Nat.N3());
var X = DARE.dare(A, B, Q, R);
assertMatrixEqual(X, X.transpose());
@@ -203,11 +212,11 @@ class DARETest extends UtilityClassTest<DARE> {
@Test
void testMoreInputsThanStates_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = new SimpleMatrix(2, 3, true, new double[] {1, 0, 0, 0, 0.5, 0.3});
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.identity(3);
var N = new SimpleMatrix(2, 3, true, new double[] {1, 0, 0, 0, 1, 0});
var A = Matrix.eye(Nat.N2());
var B = new Matrix<>(Nat.N2(), Nat.N3(), new double[] {1, 0, 0, 0, 0.5, 0.3});
var Q = Matrix.eye(Nat.N2());
var R = Matrix.eye(Nat.N3());
var N = new Matrix<>(Nat.N2(), Nat.N3(), new double[] {1, 0, 0, 0, 1, 0});
var X = DARE.dare(A, B, Q, R, N);
assertMatrixEqual(X, X.transpose());
@@ -216,90 +225,90 @@ class DARETest extends UtilityClassTest<DARE> {
@Test
void testQNotSymmetricPositiveSemidefinite_ABQR() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = SimpleMatrix.diag(-1.0, -1.0);
var R = SimpleMatrix.identity(2);
var A = Matrix.eye(Nat.N2());
var B = Matrix.eye(Nat.N2());
var Q = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
var R = Matrix.eye(Nat.N2());
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
}
@Test
void testQNotSymmetricPositiveSemidefinite_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.diag(-1.0, -1.0);
var N = SimpleMatrix.diag(2.0, 2.0);
var A = Matrix.eye(Nat.N2());
var B = Matrix.eye(Nat.N2());
var Q = Matrix.eye(Nat.N2());
var R = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
var N = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {2.0, 0.0, 0.0, 2.0});
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
}
@Test
void testRNotSymmetricPositiveDefinite_ABQR() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = SimpleMatrix.identity(2);
var A = Matrix.eye(Nat.N2());
var B = Matrix.eye(Nat.N2());
var Q = Matrix.eye(Nat.N2());
var R1 = new SimpleMatrix(2, 2);
var R1 = new Matrix<>(Nat.N2(), Nat.N2());
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R1));
var R2 = SimpleMatrix.diag(-1.0, -1.0);
var R2 = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R2));
}
@Test
void testRNotSymmetricPositiveDefinite_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = SimpleMatrix.identity(2);
var N = SimpleMatrix.identity(2);
var A = Matrix.eye(Nat.N2());
var B = Matrix.eye(Nat.N2());
var Q = Matrix.eye(Nat.N2());
var N = Matrix.eye(Nat.N2());
var R1 = new SimpleMatrix(2, 2);
var R1 = new Matrix<>(Nat.N2(), Nat.N2());
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R1, N));
var R2 = SimpleMatrix.diag(-1.0, -1.0);
var R2 = new Matrix<>(Nat.N2(), Nat.N2(), new double[] {-1.0, 0.0, 0.0, -1.0});
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R2, N));
}
@Test
void testABNotStabilizable_ABQR() {
var A = SimpleMatrix.identity(2);
var B = new SimpleMatrix(2, 2);
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.identity(2);
var A = Matrix.eye(Nat.N2());
var B = new Matrix<>(Nat.N2(), Nat.N2());
var Q = Matrix.eye(Nat.N2());
var R = Matrix.eye(Nat.N2());
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
}
@Test
void testABNotStabilizable_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = new SimpleMatrix(2, 2);
var Q = SimpleMatrix.identity(2);
var R = SimpleMatrix.identity(2);
var N = SimpleMatrix.identity(2);
var A = Matrix.eye(Nat.N2());
var B = new Matrix<>(Nat.N2(), Nat.N2());
var Q = Matrix.eye(Nat.N2());
var R = Matrix.eye(Nat.N2());
var N = Matrix.eye(Nat.N2());
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
}
@Test
void testACNotDetectable_ABQR() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = new SimpleMatrix(2, 2);
var R = SimpleMatrix.identity(2);
var A = Matrix.eye(Nat.N2());
var B = Matrix.eye(Nat.N2());
var Q = new Matrix<>(Nat.N2(), Nat.N2());
var R = Matrix.eye(Nat.N2());
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R));
}
@Test
void testACNotDetectable_ABQRN() {
var A = SimpleMatrix.identity(2);
var B = SimpleMatrix.identity(2);
var Q = new SimpleMatrix(2, 2);
var R = SimpleMatrix.identity(2);
var N = new SimpleMatrix(2, 2);
var A = Matrix.eye(Nat.N2());
var B = Matrix.eye(Nat.N2());
var Q = new Matrix<>(Nat.N2(), Nat.N2());
var R = Matrix.eye(Nat.N2());
var N = new Matrix<>(Nat.N2(), Nat.N2());
assertThrows(IllegalArgumentException.class, () -> DARE.dare(A, B, Q, R, N));
}