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

[wpimath] Move math functionality into new wpimath library (#2629)

Browse Source 
The wpimath library is a new library designed to separate the reusable math functionality
from the common utility library (wpiutil) and the hardware-dependent library (wpilibc/j).

Package names / include file names were NOT changed to minimize breakage.  In a future year
it would be good to revamp these for a more uniform user experience and to reduce the risk
of accidental naming conflicts.

While theoretically all of this functionality could be placed into wpiutil, several pieces
of this library (e.g. DARE) are very time-consuming to compile, so it's nice to avoid this
expense for users who only want cscore or ntcore.  It also allows for easy future separation
of build tasks vs number of workers on memory-constrained machines.

This moves the following functionality from wpiutil into wpimath:
- Eigen
- ejml
- Drake
- DARE
- wpiutil.math package (Matrix etc)
- units

And the following functionality from wpilibc/j into wpimath:
- Geometry
- Kinematics
- Spline
- Trajectory
- LinearFilter
- MedianFilter
- Feed-forward controllers
This commit is contained in:
Peter Johnson
2020-08-06 23:57:39 -07:00
committed by GitHub
parent ad817d4f23
commit 42993b15c6
463 changed files with 1006 additions and 399 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
wpiutil/src/test/java/edu/wpi/first/wpiutil/math/DrakeJNITest.java
View File

@@ -1,19 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.wpiutil.math;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
public class DrakeJNITest {
@Test
public void testLink() {
assertDoesNotThrow(DrakeJNI::forceLoad);
}
}

69
wpiutil/src/test/java/edu/wpi/first/wpiutil/math/DrakeTest.java
View File

@@ -1,69 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.wpiutil.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));
}
}

22
wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java
View File

@@ -1,22 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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.wpiutil.math;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
class MathUtilTest {
@Test
void testAngleNormalize() {
assertEquals(MathUtil.normalizeAngle(5 * Math.PI), Math.PI);
assertEquals(MathUtil.normalizeAngle(-5 * Math.PI), Math.PI);
assertEquals(MathUtil.normalizeAngle(Math.PI / 2), Math.PI / 2);
assertEquals(MathUtil.normalizeAngle(-Math.PI / 2), -Math.PI / 2);
}
}

210
wpiutil/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java
View File

@@ -1,210 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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.wpiutil.math;
import org.ejml.data.SingularMatrixException;
import org.ejml.dense.row.MatrixFeatures_DDRM;
import org.ejml.simple.SimpleMatrix;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N2;
import edu.wpi.first.wpiutil.math.numbers.N3;
import edu.wpi.first.wpiutil.math.numbers.N4;
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
public class MatrixTest {
@Test
void testMatrixMultiplication() {
var mat1 = MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(2.0, 1.0,
0.0, 1.0);
var mat2 = MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(3.0, 0.0,
0.0, 2.5);
Matrix<N2, N2> result = mat1.times(mat2);
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(6.0, 2.5,
0.0, 2.5).getStorage().getDDRM(),
result.getStorage().getDDRM()
));
var mat3 = MatrixUtils.mat(Nat.N2(), Nat.N3())
.fill(1.0, 3.0, 0.5,
2.0, 4.3, 1.2);
var mat4 = MatrixUtils.mat(Nat.N3(), Nat.N4())
.fill(3.0, 1.5, 2.0, 4.5,
2.3, 1.0, 1.6, 3.1,
5.2, 2.1, 2.0, 1.0);
Matrix<N2, N4> result2 = mat3.times(mat4);
assertTrue(MatrixFeatures_DDRM.isIdentical(
MatrixUtils.mat(Nat.N2(), Nat.N4())
.fill(12.5, 5.55, 7.8, 14.3,
22.13, 9.82, 13.28, 23.53).getStorage().getDDRM(),
result2.getStorage().getDDRM(),
1E-9
));
}
@Test
void testMatrixVectorMultiplication() {
var mat = MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(1.0, 1.0,
0.0, 1.0);
var vec = MatrixUtils.vec(Nat.N2())
.fill(3.0,
2.0);
Matrix<N2, N1> result = mat.times(vec);
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.vec(Nat.N2())
.fill(5.0,
2.0).getStorage().getDDRM(),
result.getStorage().getDDRM()
));
}
@Test
void testTranspose() {
Matrix<N3, N1> vec = MatrixUtils.vec(Nat.N3())
.fill(1.0,
2.0,
3.0);
Matrix<N1, N3> transpose = vec.transpose();
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0).getStorage()
.getDDRM(),
transpose.getStorage().getDDRM()
));
}
@Test
void testInverse() {
var mat = MatrixUtils.mat(Nat.N3(), Nat.N3())
.fill(1.0, 3.0, 2.0,
5.0, 2.0, 1.5,
0.0, 1.3, 2.5);
var inv = mat.inv();
assertTrue(MatrixFeatures_DDRM.isIdentical(
MatrixUtils.eye(Nat.N3()).getStorage().getDDRM(),
mat.times(inv).getStorage().getDDRM(),
1E-9
));
assertTrue(MatrixFeatures_DDRM.isIdentical(
MatrixUtils.eye(Nat.N3()).getStorage().getDDRM(),
inv.times(mat).getStorage().getDDRM(),
1E-9
));
}
@Test
void testUninvertableMatrix() {
var singularMatrix = MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(2.0, 1.0,
2.0, 1.0);
assertThrows(SingularMatrixException.class, singularMatrix::inv);
}
@Test
void testMatrixScalarArithmetic() {
var mat = MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(1.0, 2.0,
3.0, 4.0);
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(3.0, 4.0,
5.0, 6.0).getStorage().getDDRM(),
mat.plus(2.0).getStorage().getDDRM()
));
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(0.0, 1.0,
2.0, 3.0).getStorage().getDDRM(),
mat.minus(1.0).getStorage().getDDRM()
));
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(2.0, 4.0,
6.0, 8.0).getStorage().getDDRM(),
mat.times(2.0).getStorage().getDDRM()
));
assertTrue(MatrixFeatures_DDRM.isIdentical(
MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(0.5, 1.0,
1.5, 2.0).getStorage().getDDRM(),
mat.div(2.0).getStorage().getDDRM(),
1E-3
));
}
@Test
void testMatrixMatrixArithmetic() {
var mat1 = MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(1.0, 2.0,
3.0, 4.0);
var mat2 = MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(5.0, 6.0,
7.0, 8.0);
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(-4.0, -4.0,
-4.0, -4.0).getStorage().getDDRM(),
mat1.minus(mat2).getStorage().getDDRM()
));
assertTrue(MatrixFeatures_DDRM.isEquals(
MatrixUtils.mat(Nat.N2(), Nat.N2())
.fill(6.0, 8.0,
10.0, 12.0).getStorage().getDDRM(),
mat1.plus(mat2).getStorage().getDDRM()
));
}
@Test
void testMatrixExponential() {
SimpleMatrix matrix = MatrixUtils.eye(Nat.N2()).getStorage();
var result = SimpleMatrixUtils.expm(matrix);
assertTrue(MatrixFeatures_DDRM.isIdentical(
result.getDDRM(),
new SimpleMatrix(2, 2, true, new double[]{Math.E, 0, 0, Math.E}).getDDRM(),
1E-9
));
matrix = new SimpleMatrix(2, 2, true, new double[]{1, 2, 3, 4});
result = SimpleMatrixUtils.expm(matrix.scale(0.01));
assertTrue(MatrixFeatures_DDRM.isIdentical(
result.getDDRM(),
new SimpleMatrix(2, 2, true, new double[]{1.01035625, 0.02050912,
0.03076368, 1.04111993}).getDDRM(),
1E-8
));
}
}