Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Move DrakeJNI to edu.wpi.first.math.WPIMathJNI

Browse Source
This commit is contained in:
Peter Johnson
2020-08-07 09:38:13 -07:00
parent 42993b15c6
commit 8dc3d23831
6 changed files with 17 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java Normal file
View File

@@ -0,0 +1,69 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.math;
import org.ejml.simple.SimpleMatrix;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public class DrakeTest {
public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) {
for (int i = 0; i < A.numRows(); i++) {
for (int j = 0; j < A.numCols(); j++) {
assertEquals(A.get(i, j), B.get(i, j), 1e-4);
}
}
}
private boolean solveDAREandVerify(SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q,
SimpleMatrix R) {
var X = Drake.discreteAlgebraicRiccatiEquation(A, B, Q, R);
// expect that x is the same as it's transpose
assertEquals(X.numRows(), X.numCols());
assertMatrixEqual(X, X.transpose());
// Verify that this is a solution to the DARE.
SimpleMatrix Y = A.transpose().mult(X).mult(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))
.plus(Q);
assertMatrixEqual(Y, new SimpleMatrix(Y.numRows(), Y.numCols()));
return true;
}
@Test
public void testDiscreteAlgebraicRicattiEquation() {
int n1 = 4;
int m1 = 1;
// we know from Scipy that this should be [[0.05048525 0.10097051 0.20194102 0.40388203]]
SimpleMatrix A1 = new SimpleMatrix(n1, n1, true, new double[]{0.5, 1, 0, 0, 0, 0, 1,
0, 0, 0, 0, 1, 0, 0, 0, 0}).transpose();
SimpleMatrix B1 = new SimpleMatrix(n1, m1, true, new double[]{0, 0, 0, 1});
SimpleMatrix Q1 = new SimpleMatrix(n1, n1, true, new double[]{1, 0,
0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0});
SimpleMatrix R1 = new SimpleMatrix(m1, m1, true, new double[]{0.25});
assertTrue(solveDAREandVerify(A1, B1, Q1, R1));
SimpleMatrix A2 = new SimpleMatrix(2, 2, true, new double[]{1, 1, 0, 1});
SimpleMatrix B2 = new SimpleMatrix(2, 1, true, new double[]{0, 1});
SimpleMatrix Q2 = new SimpleMatrix(2, 2, true, new double[]{1, 0, 0, 0});
SimpleMatrix R2 = new SimpleMatrix(1, 1, true, new double[]{0.3});
assertTrue(solveDAREandVerify(A2, B2, Q2, R2));
}
}

19
wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java Normal file
View File

@@ -0,0 +1,19 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2020 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.math;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
public class WPIMathJNITest {
@Test
public void testLink() {
assertDoesNotThrow(WPIMathJNI::forceLoad);
}
}