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

[wpimath] Clean up VecBuilder and MatBuilder (#5906)

Browse Source
This commit is contained in:
Joseph Eng
2023-11-14 12:23:50 -08:00
committed by GitHub
parent e117274a67
commit 79dd795bc0
29 changed files with 369 additions and 380 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java
View File

@@ -18,29 +18,28 @@ import org.junit.jupiter.api.Test;
class MatrixTest {
@Test
void testMatrixMultiplication() {
var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 0.0, 1.0);
var mat2 = Matrix.mat(Nat.N2(), Nat.N2()).fill(3.0, 0.0, 0.0, 2.5);
var mat1 = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 0.0, 1.0);
var mat2 = MatBuilder.fill(Nat.N2(), Nat.N2(), 3.0, 0.0, 0.0, 2.5);
Matrix<N2, N2> result = mat1.times(mat2);
assertEquals(result, Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 2.5, 0.0, 2.5));
assertEquals(result, MatBuilder.fill(Nat.N2(), Nat.N2(), 6.0, 2.5, 0.0, 2.5));
var mat3 = Matrix.mat(Nat.N2(), Nat.N3()).fill(1.0, 3.0, 0.5, 2.0, 4.3, 1.2);
var mat3 = MatBuilder.fill(Nat.N2(), Nat.N3(), 1.0, 3.0, 0.5, 2.0, 4.3, 1.2);
var mat4 =
Matrix.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);
MatBuilder.fill(
Nat.N3(), Nat.N4(), 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(
Matrix.mat(Nat.N2(), Nat.N4())
.fill(12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53)
MatBuilder.fill(Nat.N2(), Nat.N4(), 12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53)
.isEqual(result2, 1E-9));
}
@Test
void testMatrixVectorMultiplication() {
var mat = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 1.0, 0.0, 1.0);
var mat = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 1.0, 0.0, 1.0);
var vec = VecBuilder.fill(3.0, 2.0);
@@ -54,19 +53,19 @@ class MatrixTest {
Matrix<N1, N3> transpose = vec.transpose();
assertEquals(Matrix.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0), transpose);
assertEquals(MatBuilder.fill(Nat.N1(), Nat.N3(), 1.0, 2.0, 3.0), transpose);
}
@Test
void testSolve() {
var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0);
var mat1 = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 2.0, 3.0, 4.0);
var vec1 = VecBuilder.fill(1.0, 2.0);
var solve1 = mat1.solve(vec1);
assertEquals(VecBuilder.fill(0.0, 0.5), solve1);
var mat2 = Matrix.mat(Nat.N3(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
var mat2 = MatBuilder.fill(Nat.N3(), Nat.N2(), 1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
var vec2 = VecBuilder.fill(1.0, 2.0, 3.0);
var solve2 = mat2.solve(vec2);
@@ -76,7 +75,7 @@ class MatrixTest {
@Test
void testInverse() {
var mat = Matrix.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 mat = MatBuilder.fill(Nat.N3(), Nat.N3(), 1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5);
var inv = mat.inv();
@@ -87,33 +86,33 @@ class MatrixTest {
@Test
void testUninvertableMatrix() {
var singularMatrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 2.0, 1.0);
var singularMatrix = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 2.0, 1.0);
assertThrows(SingularMatrixException.class, singularMatrix::inv);
}
@Test
void testMatrixScalarArithmetic() {
var mat = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0);
var mat = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 2.0, 3.0, 4.0);
assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(3.0, 4.0, 5.0, 6.0), mat.plus(2.0));
assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 3.0, 4.0, 5.0, 6.0), mat.plus(2.0));
assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 2.0, 3.0), mat.minus(1.0));
assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 1.0, 2.0, 3.0), mat.minus(1.0));
assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 4.0, 6.0, 8.0), mat.times(2.0));
assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 4.0, 6.0, 8.0), mat.times(2.0));
assertTrue(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.5, 1.0, 1.5, 2.0).isEqual(mat.div(2.0), 1E-3));
assertTrue(MatBuilder.fill(Nat.N2(), Nat.N2(), 0.5, 1.0, 1.5, 2.0).isEqual(mat.div(2.0), 1E-3));
}
@Test
void testMatrixMatrixArithmetic() {
var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0);
var mat1 = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 2.0, 3.0, 4.0);
var mat2 = Matrix.mat(Nat.N2(), Nat.N2()).fill(5.0, 6.0, 7.0, 8.0);
var mat2 = MatBuilder.fill(Nat.N2(), Nat.N2(), 5.0, 6.0, 7.0, 8.0);
assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(-4.0, -4.0, -4.0, -4.0), mat1.minus(mat2));
assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), -4.0, -4.0, -4.0, -4.0), mat1.minus(mat2));
assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 8.0, 10.0, 12.0), mat1.plus(mat2));
assertEquals(MatBuilder.fill(Nat.N2(), Nat.N2(), 6.0, 8.0, 10.0, 12.0), mat1.plus(mat2));
}
@Test
@@ -121,14 +120,14 @@ class MatrixTest {
var matrix = Matrix.eye(Nat.N2());
var result = matrix.exp();
assertTrue(result.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9));
assertTrue(result.isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), Math.E, 0, 0, Math.E), 1E-9));
matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4);
matrix = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 2, 3, 4);
result = matrix.times(0.01).exp();
assertTrue(
result.isEqual(
Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912, 0.03076368, 1.04111993),
MatBuilder.fill(Nat.N2(), Nat.N2(), 1.01035625, 0.02050912, 0.03076368, 1.04111993),
1E-8));
}
}

24
wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
View File

@@ -61,48 +61,48 @@ class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
// First eigenvalue is uncontrollable and unstable.
// Second eigenvalue is controllable and stable.
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5);
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.2, 0, 0, 0.5);
assertFalse(StateSpaceUtil.isStabilizable(A, B));
// First eigenvalue is uncontrollable and marginally stable.
// Second eigenvalue is controllable and stable.
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5);
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5);
assertFalse(StateSpaceUtil.isStabilizable(A, B));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and stable.
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5);
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5);
assertTrue(StateSpaceUtil.isStabilizable(A, B));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and unstable.
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2);
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2);
assertTrue(StateSpaceUtil.isStabilizable(A, B));
}
@Test
void testIsDetectable() {
Matrix<N2, N2> A;
Matrix<N1, N2> C = Matrix.mat(Nat.N1(), Nat.N2()).fill(0, 1);
Matrix<N1, N2> C = MatBuilder.fill(Nat.N1(), Nat.N2(), 0, 1);
// First eigenvalue is unobservable and unstable.
// Second eigenvalue is observable and stable.
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5);
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1.2, 0, 0, 0.5);
assertFalse(StateSpaceUtil.isDetectable(A, C));
// First eigenvalue is unobservable and marginally stable.
// Second eigenvalue is observable and stable.
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5);
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.5);
assertFalse(StateSpaceUtil.isDetectable(A, C));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and stable.
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5);
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 0.5);
assertTrue(StateSpaceUtil.isDetectable(A, C));
// First eigenvalue is unobservable and stable.
// Second eigenvalue is observable and unstable.
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2);
A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.2, 0, 0, 1.2);
assertTrue(StateSpaceUtil.isDetectable(A, C));
}
@@ -143,14 +143,14 @@ class StateSpaceUtilTest extends UtilityClassTest<StateSpaceUtil> {
var wrappedResult = wrappedMatrix.exp();
assertTrue(
wrappedResult.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9));
wrappedResult.isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), Math.E, 0, 0, Math.E), 1E-9));
var matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4);
var matrix = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 2, 3, 4);
wrappedResult = matrix.times(0.01).exp();
assertTrue(
wrappedResult.isEqual(
Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912, 0.03076368, 1.04111993),
MatBuilder.fill(Nat.N2(), Nat.N2(), 1.01035625, 0.02050912, 0.03076368, 1.04111993),
1E-8));
}

6
wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java
View File

@@ -6,6 +6,7 @@ package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
@@ -35,13 +36,12 @@ class ControlAffinePlantInversionFeedforwardTest {
}
protected Matrix<N2, N1> getDynamics(Matrix<N2, N1> x, Matrix<N1, N1> u) {
return Matrix.mat(Nat.N2(), Nat.N2())
.fill(1.000, 0, 0, 1.000)
return MatBuilder.fill(Nat.N2(), Nat.N2(), 1.000, 0, 0, 1.000)
.times(x)
.plus(VecBuilder.fill(0, 1).times(u));
}
protected Matrix<N2, N1> getStateDynamics(Matrix<N2, N1> x) {
return Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x);
return MatBuilder.fill(Nat.N2(), Nat.N2(), 1.000, 0, 0, 1.000).times(x);
}
}

124
wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiterTest.java
View File

@@ -9,8 +9,10 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.plant.LinearSystemId;
import org.junit.jupiter.api.Test;
@@ -26,52 +28,38 @@ class DifferentialDriveAccelerationLimiterTest {
var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, maxA, maxAlpha);
var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
Matrix<N2, N1> xAccelLimiter = VecBuilder.fill(0.0, 0.0);
// Ensure voltage exceeds acceleration before limiting
{
final var accels =
plant
.getA()
.times(xAccelLimiter)
.plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0)));
plant.getA().times(xAccelLimiter).plus(plant.getB().times(VecBuilder.fill(12.0, 12.0)));
final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
assertTrue(Math.abs(a) > maxA);
}
{
final var accels =
plant
.getA()
.times(xAccelLimiter)
.plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0)));
plant.getA().times(xAccelLimiter).plus(plant.getB().times(VecBuilder.fill(-12.0, 12.0)));
final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
assertTrue(Math.abs(alpha) > maxAlpha);
}
// Forward
var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
var u = VecBuilder.fill(12.0, 12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
final var voltages =
accelLimiter.calculate(
xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
xAccelLimiter =
plant.calculateX(
xAccelLimiter,
new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
dt);
plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
final var accels =
plant
.getA()
.times(xAccelLimiter)
.plus(
plant
.getB()
.times(
new MatBuilder<>(Nat.N2(), Nat.N1())
.fill(voltages.left, voltages.right)));
.plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right)));
final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
assertTrue(Math.abs(a) <= maxA);
@@ -79,28 +67,20 @@ class DifferentialDriveAccelerationLimiterTest {
}
// Backward
u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
u = VecBuilder.fill(-12.0, -12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
final var voltages =
accelLimiter.calculate(
xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
xAccelLimiter =
plant.calculateX(
xAccelLimiter,
new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
dt);
plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
final var accels =
plant
.getA()
.times(xAccelLimiter)
.plus(
plant
.getB()
.times(
new MatBuilder<>(Nat.N2(), Nat.N1())
.fill(voltages.left, voltages.right)));
.plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right)));
final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
assertTrue(Math.abs(a) <= maxA);
@@ -108,28 +88,20 @@ class DifferentialDriveAccelerationLimiterTest {
}
// Rotate CCW
u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
u = VecBuilder.fill(-12.0, 12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
final var voltages =
accelLimiter.calculate(
xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
xAccelLimiter =
plant.calculateX(
xAccelLimiter,
new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
dt);
plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
final var accels =
plant
.getA()
.times(xAccelLimiter)
.plus(
plant
.getB()
.times(
new MatBuilder<>(Nat.N2(), Nat.N1())
.fill(voltages.left, voltages.right)));
.plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right)));
final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
final double alpha = (accels.get(1, 0) - accels.get(0, 0)) / trackwidth;
assertTrue(Math.abs(a) <= maxA);
@@ -148,21 +120,18 @@ class DifferentialDriveAccelerationLimiterTest {
// unconstrained systems should match
var accelLimiter = new DifferentialDriveAccelerationLimiter(plant, trackwidth, 1e3, 1e3);
var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
Matrix<N2, N1> xAccelLimiter = VecBuilder.fill(0.0, 0.0);
// Forward
var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
var u = VecBuilder.fill(12.0, 12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
final var voltages =
accelLimiter.calculate(
xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
xAccelLimiter =
plant.calculateX(
xAccelLimiter,
new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
dt);
plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5);
assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
@@ -171,17 +140,14 @@ class DifferentialDriveAccelerationLimiterTest {
// Backward
x.fill(0.0);
xAccelLimiter.fill(0.0);
u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
u = VecBuilder.fill(-12.0, -12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
final var voltages =
accelLimiter.calculate(
xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
xAccelLimiter =
plant.calculateX(
xAccelLimiter,
new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
dt);
plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5);
assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
@@ -190,17 +156,14 @@ class DifferentialDriveAccelerationLimiterTest {
// Rotate CCW
x.fill(0.0);
xAccelLimiter.fill(0.0);
u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
u = VecBuilder.fill(-12.0, 12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
final var voltages =
accelLimiter.calculate(
xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
xAccelLimiter =
plant.calculateX(
xAccelLimiter,
new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
dt);
plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
assertEquals(x.get(0, 0), xAccelLimiter.get(0, 0), 1e-5);
assertEquals(x.get(1, 0), xAccelLimiter.get(1, 0), 1e-5);
@@ -220,16 +183,13 @@ class DifferentialDriveAccelerationLimiterTest {
var accelLimiter =
new DifferentialDriveAccelerationLimiter(plant, trackwidth, minA, maxA, maxAlpha);
var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
var xAccelLimiter = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
Matrix<N2, N1> xAccelLimiter = VecBuilder.fill(0.0, 0.0);
// Ensure voltage exceeds acceleration before limiting
{
final var accels =
plant
.getA()
.times(xAccelLimiter)
.plus(plant.getB().times(new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0)));
plant.getA().times(xAccelLimiter).plus(plant.getB().times(VecBuilder.fill(12.0, 12.0)));
final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
assertTrue(Math.abs(a) > maxA);
assertTrue(Math.abs(a) > -minA);
@@ -237,55 +197,39 @@ class DifferentialDriveAccelerationLimiterTest {
// a should always be within [minA, maxA]
// Forward
var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
var u = VecBuilder.fill(12.0, 12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
final var voltages =
accelLimiter.calculate(
xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
xAccelLimiter =
plant.calculateX(
xAccelLimiter,
new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
dt);
plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
final var accels =
plant
.getA()
.times(xAccelLimiter)
.plus(
plant
.getB()
.times(
new MatBuilder<>(Nat.N2(), Nat.N1())
.fill(voltages.left, voltages.right)));
.plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right)));
final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
assertTrue(minA <= a && a <= maxA);
}
// Backward
u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
u = VecBuilder.fill(-12.0, -12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
final var voltages =
accelLimiter.calculate(
xAccelLimiter.get(0, 0), xAccelLimiter.get(1, 0), u.get(0, 0), u.get(1, 0));
xAccelLimiter =
plant.calculateX(
xAccelLimiter,
new MatBuilder<>(Nat.N2(), Nat.N1()).fill(voltages.left, voltages.right),
dt);
plant.calculateX(xAccelLimiter, VecBuilder.fill(voltages.left, voltages.right), dt);
final var accels =
plant
.getA()
.times(xAccelLimiter)
.plus(
plant
.getB()
.times(
new MatBuilder<>(Nat.N2(), Nat.N1())
.fill(voltages.left, voltages.right)));
.plus(plant.getB().times(VecBuilder.fill(voltages.left, voltages.right)));
final double a = (accels.get(0, 0) + accels.get(1, 0)) / 2.0;
assertTrue(minA <= a && a <= maxA);
}

6
wpimath/src/test/java/edu/wpi/first/math/controller/ElevatorFeedforwardTest.java
View File

@@ -6,7 +6,7 @@ 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.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
@@ -27,8 +27,8 @@ class ElevatorFeedforwardTest {
assertEquals(6.5, m_elevatorFF.calculate(2, 1), 0.002);
assertEquals(-0.5, m_elevatorFF.calculate(-2, 1), 0.002);
var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-kv / ka);
var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / ka);
var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -kv / ka);
var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / ka);
final double dt = 0.02;
var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);

25
wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java
View File

@@ -7,8 +7,9 @@ package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.plant.LinearSystemId;
import org.junit.jupiter.api.Test;
@@ -22,11 +23,11 @@ class ImplicitModelFollowerTest {
var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plant);
var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
Matrix<N2, N1> xImf = VecBuilder.fill(0.0, 0.0);
// Forward
var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
var u = VecBuilder.fill(12.0, 12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
@@ -36,7 +37,7 @@ class ImplicitModelFollowerTest {
}
// Backward
u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
u = VecBuilder.fill(-12.0, -12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
@@ -46,7 +47,7 @@ class ImplicitModelFollowerTest {
}
// Rotate CCW
u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
u = VecBuilder.fill(-12.0, 12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
@@ -67,11 +68,11 @@ class ImplicitModelFollowerTest {
var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plantRef);
var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
var xImf = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0.0, 0.0);
Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
Matrix<N2, N1> xImf = VecBuilder.fill(0.0, 0.0);
// Forward
var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(12.0, 12.0);
var u = VecBuilder.fill(12.0, 12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
@@ -83,7 +84,7 @@ class ImplicitModelFollowerTest {
// Backward
x.fill(0.0);
xImf.fill(0.0);
u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, -12.0);
u = VecBuilder.fill(-12.0, -12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);
@@ -95,7 +96,7 @@ class ImplicitModelFollowerTest {
// Rotate CCW
x.fill(0.0);
xImf.fill(0.0);
u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(-12.0, 12.0);
u = VecBuilder.fill(-12.0, 12.0);
for (double t = 0.0; t < 3.0; t += dt) {
x = plant.calculateX(x, u, dt);
xImf = plant.calculateX(xImf, imf.calculate(xImf, u), dt);

13
wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java
View File

@@ -87,9 +87,14 @@ class LTVDifferentialDriveControllerTest {
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
var x =
new MatBuilder<>(Nat.N5(), Nat.N1())
.fill(
robotPose.getX(), robotPose.getY(), robotPose.getRotation().getRadians(), 0.0, 0.0);
MatBuilder.fill(
Nat.N5(),
Nat.N1(),
robotPose.getX(),
robotPose.getY(),
robotPose.getRotation().getRadians(),
0.0,
0.0);
final var totalTime = trajectory.getTotalTimeSeconds();
for (int i = 0; i < (totalTime / kDt); ++i) {
@@ -105,7 +110,7 @@ class LTVDifferentialDriveControllerTest {
NumericalIntegration.rkdp(
LTVDifferentialDriveControllerTest::dynamics,
x,
new MatBuilder<>(Nat.N2(), Nat.N1()).fill(output.left, output.right),
VecBuilder.fill(output.left, output.right),
kDt);
}

3
wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java
View File

@@ -6,6 +6,7 @@ package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
@@ -16,7 +17,7 @@ import org.junit.jupiter.api.Test;
class LinearPlantInversionFeedforwardTest {
@Test
void testCalculate() {
Matrix<N2, N2> A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
Matrix<N2, N2> A = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
Matrix<N2, N1> B = VecBuilder.fill(0, 1);
LinearPlantInversionFeedforward<N2, N1, N1> feedforward =

21
wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
View File

@@ -6,6 +6,7 @@ package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.Num;
@@ -111,10 +112,10 @@ class LinearQuadraticRegulatorTest {
@Test
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);
var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 0, 0, 0);
var B = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
var Q = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
var R = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
// QR overload
var K = new LinearQuadraticRegulator<>(A, B, Q, R, 0.005).getK();
@@ -124,7 +125,7 @@ class LinearQuadraticRegulatorTest {
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 N = MatBuilder.fill(Nat.N2(), Nat.N2(), 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);
@@ -137,10 +138,10 @@ class LinearQuadraticRegulatorTest {
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);
var A = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -Kv / Ka);
var B = MatBuilder.fill(Nat.N2(), Nat.N1(), 0, 1.0 / Ka);
var Q = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 0.2);
var R = MatBuilder.fill(Nat.N1(), Nat.N1(), 0.25);
// QR overload
var K = new LinearQuadraticRegulator<>(A, B, Q, R, 0.005).getK();
@@ -148,7 +149,7 @@ class LinearQuadraticRegulatorTest {
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 * 5.0));
var Aref = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -Kv / (Ka * 5.0));
var Kimf = getImplicitModelFollowingK(A, B, Q, R, Aref, 0.005);
assertEquals(0.0, Kimf.get(0, 0), 1e-10);
assertEquals(-6.9190500116751458e-05, Kimf.get(0, 1), 1e-10);

6
wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
View File

@@ -6,7 +6,7 @@ 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.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
@@ -20,8 +20,8 @@ class SimpleMotorFeedforwardTest {
double Ka = 0.6;
double dt = 0.02;
var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-Kv / Ka);
var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / Ka);
var A = MatBuilder.fill(Nat.N1(), Nat.N1(), -Kv / Ka);
var B = MatBuilder.fill(Nat.N1(), Nat.N1(), 1.0 / Ka);
var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka);

3
wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java
View File

@@ -6,6 +6,7 @@ package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
@@ -16,7 +17,7 @@ class AngleStatisticsTest {
void testMean() {
// 3 states, 2 sigmas
var sigmas =
Matrix.mat(Nat.N3(), Nat.N2()).fill(1, 1.2, Math.toRadians(359), Math.toRadians(3), 1, 2);
MatBuilder.fill(Nat.N3(), Nat.N2(), 1, 1.2, Math.toRadians(359), Math.toRadians(3), 1, 2);
// Weights need to produce the mean of the sigmas
var weights = new Matrix<>(Nat.N2(), Nat.N1());
weights.fill(1.0 / sigmas.getNumCols());

21
wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
View File

@@ -7,6 +7,7 @@ package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
@@ -47,16 +48,16 @@ class KalmanFilterTest {
// Y is [x, y, theta]ᵀ and u is [ax, ay, alpha}ᵀ
LinearSystem<N6, N3, N3> m_swerveObserverSystem =
new LinearSystem<>(
Matrix.mat(Nat.N6(), Nat.N6())
.fill( // A
0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0),
Matrix.mat(Nat.N6(), Nat.N3())
.fill( // B
0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1),
Matrix.mat(Nat.N3(), Nat.N6())
.fill( // C
1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0),
MatBuilder.fill(
Nat.N6(), Nat.N6(), // A
0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0),
MatBuilder.fill(
Nat.N6(), Nat.N3(), // B
0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1),
MatBuilder.fill(
Nat.N3(), Nat.N6(), // C
1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0),
new Matrix<>(Nat.N3(), Nat.N3())); // D
@Test

37
wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
View File

@@ -6,7 +6,7 @@ package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import org.junit.jupiter.api.Test;
@@ -17,13 +17,23 @@ class MerweScaledSigmaPointsTest {
var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
var points =
merweScaledSigmaPoints.squareRootSigmaPoints(
VecBuilder.fill(0, 0), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1));
VecBuilder.fill(0, 0), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1));
assertTrue(
points.isEqual(
Matrix.mat(Nat.N2(), Nat.N5())
.fill(
0.0, 0.00173205, 0.0, -0.00173205, 0.0, 0.0, 0.0, 0.00173205, 0.0, -0.00173205),
MatBuilder.fill(
Nat.N2(),
Nat.N5(),
0.0,
0.00173205,
0.0,
-0.00173205,
0.0,
0.0,
0.0,
0.00173205,
0.0,
-0.00173205),
1E-6));
}
@@ -32,12 +42,23 @@ class MerweScaledSigmaPointsTest {
var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
var points =
merweScaledSigmaPoints.squareRootSigmaPoints(
VecBuilder.fill(1, 2), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, Math.sqrt(10)));
VecBuilder.fill(1, 2), MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, Math.sqrt(10)));
assertTrue(
points.isEqual(
Matrix.mat(Nat.N2(), Nat.N5())
.fill(1.0, 1.00173205, 1.0, 0.99826795, 1.0, 2.0, 2.0, 2.00547723, 2.0, 1.99452277),
MatBuilder.fill(
Nat.N2(),
Nat.N5(),
1.0,
1.00173205,
1.0,
0.99826795,
1.0,
2.0,
2.0,
2.00547723,
2.0,
1.99452277),
1E-6));
}
}

3
wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
View File

@@ -8,6 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.StateSpaceUtil;
@@ -264,7 +265,7 @@ class UnscentedKalmanFilterTest {
VecBuilder.fill(0.0, 0.0),
dtSeconds);
var P = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 1.0, 2.0);
var P = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 1.0, 2.0);
observer.setP(P);
assertTrue(observer.getP().isEqual(P, 1e-9));

3
wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
View File

@@ -93,8 +93,7 @@ class Rotation3dTest {
assertEquals(expected2, rot2);
// Matrix that isn't orthogonal
final var R3 =
new MatBuilder<>(Nat.N3(), Nat.N3()).fill(1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0);
final var R3 = MatBuilder.fill(Nat.N3(), Nat.N3(), 1.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0);
assertThrows(IllegalArgumentException.class, () -> new Rotation3d(R3));
// Matrix that's orthogonal but not special orthogonal

18
wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
View File

@@ -19,7 +19,7 @@ class DiscretizationTest {
// analytically,
@Test
void testDiscretizeA() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, 0);
final var x0 = VecBuilder.fill(1, 1);
final var discA = Discretization.discretizeA(contA, 1.0);
@@ -37,8 +37,8 @@ class DiscretizationTest {
// analytically,
@Test
void testDiscretizeAB() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
final var contB = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0, 1);
final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, 0);
final var contB = MatBuilder.fill(Nat.N2(), Nat.N1(), 0, 1);
final var x0 = VecBuilder.fill(1, 1);
final var u = VecBuilder.fill(1);
@@ -63,8 +63,8 @@ class DiscretizationTest {
// 0
@Test
void testDiscretizeSlowModelAQ() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, 0);
final var contQ = MatBuilder.fill(Nat.N2(), Nat.N2(), 1, 0, 0, 1);
final double dt = 1.0;
@@ -95,8 +95,8 @@ class DiscretizationTest {
// 0
@Test
void testDiscretizeFastModelAQ() {
final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1406.29);
final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1);
final var contA = MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -1406.29);
final var contQ = MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0025, 0, 0, 1);
final var dt = 0.005;
@@ -125,8 +125,8 @@ class DiscretizationTest {
// Test that DiscretizeR() works
@Test
void testDiscretizeR() {
var contR = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 0.0, 0.0, 1.0);
var discRTruth = Matrix.mat(Nat.N2(), Nat.N2()).fill(4.0, 0.0, 0.0, 2.0);
var contR = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 0.0, 0.0, 1.0);
var discRTruth = MatBuilder.fill(Nat.N2(), Nat.N2(), 4.0, 0.0, 0.0, 2.0);
var discR = Discretization.discretizeR(contR, 0.5);

17
wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java
View File

@@ -7,6 +7,7 @@ package edu.wpi.first.math.system;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
@@ -23,31 +24,31 @@ class LinearSystemIDTest {
model
.getA()
.isEqual(
Matrix.mat(Nat.N2(), Nat.N2()).fill(-10.14132, 3.06598, 3.06598, -10.14132),
MatBuilder.fill(Nat.N2(), Nat.N2(), -10.14132, 3.06598, 3.06598, -10.14132),
0.001));
assertTrue(
model
.getB()
.isEqual(
Matrix.mat(Nat.N2(), Nat.N2()).fill(4.2590, -1.28762, -1.2876, 4.2590), 0.001));
MatBuilder.fill(Nat.N2(), Nat.N2(), 4.2590, -1.28762, -1.2876, 4.2590), 0.001));
assertTrue(
model.getC().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0), 0.001));
model.getC().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 1.0, 0.0, 0.0, 1.0), 0.001));
assertTrue(
model.getD().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 0.0, 0.0, 0.0), 0.001));
model.getD().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0.0, 0.0, 0.0, 0.0), 0.001));
}
@Test
void testElevatorSystem() {
var model = LinearSystemId.createElevatorSystem(DCMotor.getNEO(2), 5, 0.05, 12);
assertTrue(
model.getA().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -99.05473), 0.001));
model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -99.05473), 0.001));
assertTrue(model.getB().isEqual(VecBuilder.fill(0, 20.8), 0.001));
assertTrue(model.getC().isEqual(Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0), 0.001));
assertTrue(model.getC().isEqual(MatBuilder.fill(Nat.N1(), Nat.N2(), 1, 0), 0.001));
assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
}
@@ -68,7 +69,7 @@ class LinearSystemIDTest {
void testDCMotorSystem() {
var model = LinearSystemId.createDCMotorSystem(DCMotor.getNEO(2), 0.00032, 1.0);
assertTrue(
model.getA().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -26.87032), 0.001));
model.getA().isEqual(MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -26.87032), 0.001));
assertTrue(model.getB().isEqual(VecBuilder.fill(0, 1354.166667), 0.001));
@@ -85,7 +86,7 @@ class LinearSystemIDTest {
var ka = 0.5;
var model = LinearSystemId.identifyPositionSystem(kv, ka);
assertEquals(model.getA(), Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kv / ka));
assertEquals(model.getA(), MatBuilder.fill(Nat.N2(), Nat.N2(), 0, 1, 0, -kv / ka));
assertEquals(model.getB(), VecBuilder.fill(0, 1 / ka));
}

8
wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
View File

@@ -14,8 +14,8 @@ import org.junit.jupiter.api.Test;
class RungeKuttaTimeVaryingTest {
private static Matrix<N1, N1> rungeKuttaTimeVaryingSolution(double t) {
return new MatBuilder<>(Nat.N1(), Nat.N1())
.fill(12.0 * Math.exp(t) / Math.pow(Math.exp(t) + 1.0, 2.0));
return MatBuilder.fill(
Nat.N1(), Nat.N1(), 12.0 * Math.exp(t) / Math.pow(Math.exp(t) + 1.0, 2.0));
}
// Tests RK4 with a time varying solution. From
@@ -32,8 +32,8 @@ class RungeKuttaTimeVaryingTest {
final var y1 =
RungeKuttaTimeVarying.rungeKuttaTimeVarying(
(Double t, Matrix<N1, N1> x) -> {
return new MatBuilder<>(Nat.N1(), Nat.N1())
.fill(x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0));
return MatBuilder.fill(
Nat.N1(), Nat.N1(), x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0));
},
5.0,
y0,