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

[build] Apply spotless for java formatting (#1768)

Browse Source 
Update checkstyle config to be compatible with spotless.

Co-authored-by: Austin Shalit <austinshalit@gmail.com>
This commit is contained in:
Peter Johnson
2020-12-29 22:45:16 -08:00
committed by GitHub
parent e563a0b7db
commit a751fa22d2
883 changed files with 16526 additions and 17751 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
View File

@@ -4,13 +4,12 @@
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;
import org.ejml.simple.SimpleMatrix;
import org.junit.jupiter.api.Test;
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public class DrakeTest {
public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) {
@@ -21,8 +20,8 @@ public class DrakeTest {
}
}
private boolean solveDAREandVerify(SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q,
SimpleMatrix R) {
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
@@ -30,11 +29,19 @@ public class DrakeTest {
assertMatrixEqual(X, X.transpose());
// Verify that this is a solution to the DARE.
SimpleMatrix Y = A.transpose().mult(X).mult(A)
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))
.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()));
@@ -47,20 +54,21 @@ public class DrakeTest {
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});
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});
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));
}
}

4
wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java
View File

@@ -4,10 +4,10 @@
package edu.wpi.first.math;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import org.junit.jupiter.api.Test;
public class WPIMathJNITest {
@Test
public void testLink() {

56
wpimath/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java
View File

@@ -4,20 +4,19 @@
package edu.wpi.first.wpilibj;
import java.util.Random;
import java.util.function.DoubleFunction;
import java.util.stream.Stream;
import org.junit.jupiter.api.Test;
import org.junit.jupiter.params.ParameterizedTest;
import org.junit.jupiter.params.provider.Arguments;
import org.junit.jupiter.params.provider.MethodSource;
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 static org.junit.jupiter.params.provider.Arguments.arguments;
import java.util.Random;
import java.util.function.DoubleFunction;
import java.util.stream.Stream;
import org.junit.jupiter.api.Test;
import org.junit.jupiter.params.ParameterizedTest;
import org.junit.jupiter.params.provider.Arguments;
import org.junit.jupiter.params.provider.MethodSource;
class LinearFilterTest {
private static final double kFilterStep = 0.005;
private static final double kFilterTime = 2.0;
@@ -46,9 +45,7 @@ class LinearFilterTest {
assertThrows(IllegalArgumentException.class, () -> LinearFilter.movingAverage(0));
}
/**
* Test if the filter reduces the noise produced by a signal generator.
*/
/** Test if the filter reduces the noise produced by a signal generator. */
@ParameterizedTest
@MethodSource("noiseFilterProvider")
void noiseReduceTest(final LinearFilter filter) {
@@ -65,25 +62,25 @@ class LinearFilterTest {
noiseGenError += Math.abs(noise - theory);
}
assertTrue(noiseGenError > filterError,
"Filter should have reduced noise accumulation from " + noiseGenError
+ " but failed. The filter error was " + filterError);
assertTrue(
noiseGenError > filterError,
"Filter should have reduced noise accumulation from "
+ noiseGenError
+ " but failed. The filter error was "
+ filterError);
}
static Stream<LinearFilter> noiseFilterProvider() {
return Stream.of(
LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
LinearFilter.movingAverage(kMovAvgTaps)
);
LinearFilter.movingAverage(kMovAvgTaps));
}
/**
* Test if the linear filters produce consistent output for a given data set.
*/
/** Test if the linear filters produce consistent output for a given data set. */
@ParameterizedTest
@MethodSource("outputFilterProvider")
void outputTest(final LinearFilter filter, final DoubleFunction<Double> data,
final double expectedOutput) {
void outputTest(
final LinearFilter filter, final DoubleFunction<Double> data, final double expectedOutput) {
double filterOutput = 0.0;
for (double t = 0.0; t < kFilterTime; t += kFilterStep) {
filterOutput = filter.calculate(data.apply(t));
@@ -94,18 +91,21 @@ class LinearFilterTest {
static Stream<Arguments> outputFilterProvider() {
return Stream.of(
arguments(LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
arguments(
LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
(DoubleFunction<Double>) LinearFilterTest::getData,
kSinglePoleIIRExpectedOutput),
arguments(LinearFilter.highPass(kHighPassTimeConstant, kFilterStep),
arguments(
LinearFilter.highPass(kHighPassTimeConstant, kFilterStep),
(DoubleFunction<Double>) LinearFilterTest::getData,
kHighPassExpectedOutput),
arguments(LinearFilter.movingAverage(kMovAvgTaps),
arguments(
LinearFilter.movingAverage(kMovAvgTaps),
(DoubleFunction<Double>) LinearFilterTest::getData,
kMovAvgExpectedOutput),
arguments(LinearFilter.movingAverage(kMovAvgTaps),
arguments(
LinearFilter.movingAverage(kMovAvgTaps),
(DoubleFunction<Double>) LinearFilterTest::getPulseData,
0.0)
);
0.0));
}
}

4
wpimath/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java
View File

@@ -4,10 +4,10 @@
package edu.wpi.first.wpilibj;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
public class MedianFilterTest {
@Test
void medianFilterNotFullTestEven() {

28
wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
View File

@@ -4,22 +4,21 @@
package edu.wpi.first.wpilibj;
import java.lang.reflect.Constructor;
import java.lang.reflect.InvocationTargetException;
import java.lang.reflect.Modifier;
import java.util.Arrays;
import java.util.stream.Stream;
import org.junit.jupiter.api.DynamicTest;
import org.junit.jupiter.api.Test;
import org.junit.jupiter.api.TestFactory;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
import static org.junit.jupiter.api.DynamicTest.dynamicTest;
import java.lang.reflect.Constructor;
import java.lang.reflect.InvocationTargetException;
import java.lang.reflect.Modifier;
import java.util.Arrays;
import java.util.stream.Stream;
import org.junit.jupiter.api.DynamicTest;
import org.junit.jupiter.api.Test;
import org.junit.jupiter.api.TestFactory;
@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
public abstract class UtilityClassTest<T> {
private final Class<T> m_clazz;
@@ -30,8 +29,7 @@ public abstract class UtilityClassTest<T> {
@Test
public void singleConstructorTest() {
assertEquals(1, m_clazz.getDeclaredConstructors().length,
"More than one constructor defined");
assertEquals(1, m_clazz.getDeclaredConstructors().length, "More than one constructor defined");
}
@Test
@@ -52,7 +50,9 @@ public abstract class UtilityClassTest<T> {
Stream<DynamicTest> publicMethodsStaticTestFactory() {
return Arrays.stream(m_clazz.getDeclaredMethods())
.filter(method -> Modifier.isPublic(method.getModifiers()))
.map(method -> dynamicTest(method.getName(),
() -> assertTrue(Modifier.isStatic(method.getModifiers()))));
.map(
method ->
dynamicTest(
method.getName(), () -> assertTrue(Modifier.isStatic(method.getModifiers()))));
}
}

34
wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java
View File

@@ -4,31 +4,25 @@
package edu.wpi.first.wpilibj.controller;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N2;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class ControlAffinePlantInversionFeedforwardTest {
@SuppressWarnings("LocalVariableName")
@Test
void testCalculate() {
ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
new ControlAffinePlantInversionFeedforward<N2, N1>(
Nat.N2(),
Nat.N1(),
this::getDynamics,
0.02);
new ControlAffinePlantInversionFeedforward<N2, N1>(
Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
assertEquals(48.0, feedforward.calculate(
VecBuilder.fill(2, 2),
VecBuilder.fill(3, 3)).get(0, 0),
1e-6);
assertEquals(
48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);
}
@SuppressWarnings("LocalVariableName")
@@ -36,22 +30,20 @@ class ControlAffinePlantInversionFeedforwardTest {
void testCalculateState() {
ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
new ControlAffinePlantInversionFeedforward<N2, N1>(
Nat.N2(),
Nat.N1(),
this::getDynamics,
0.02);
Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
assertEquals(48.0, feedforward.calculate(
VecBuilder.fill(2, 2),
VecBuilder.fill(3, 3)).get(0, 0),
1e-6);
assertEquals(
48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);
}
@SuppressWarnings("ParameterName")
protected Matrix<N2, N1> getDynamics(Matrix<N2, N1> x, Matrix<N1, N1> u) {
var result = new Matrix<>(Nat.N2(), Nat.N1());
result = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x)
result =
Matrix.mat(Nat.N2(), Nat.N2())
.fill(1.000, 0, 0, 1.000)
.times(x)
.plus(VecBuilder.fill(0, 1).times(u));
return result;

15
wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforwardTest.java
View File

@@ -4,15 +4,14 @@
package edu.wpi.first.wpilibj.controller;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N2;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class LinearPlantInversionFeedforwardTest {
@SuppressWarnings("LocalVariableName")
@@ -22,11 +21,11 @@ class LinearPlantInversionFeedforwardTest {
Matrix<N2, N1> B = VecBuilder.fill(0, 1);
LinearPlantInversionFeedforward<N2, N1, N1> feedforward =
new LinearPlantInversionFeedforward<N2, N1, N1>(A, B, 0.02);
new LinearPlantInversionFeedforward<N2, N1, N1>(A, B, 0.02);
assertEquals(47.502599, feedforward.calculate(
VecBuilder.fill(2, 2),
VecBuilder.fill(3, 3)).get(0, 0),
0.002);
assertEquals(
47.502599,
feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0),
0.002);
}
}

43
wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulatorTest.java
View File

@@ -4,7 +4,7 @@
package edu.wpi.first.wpilibj.controller;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.system.LinearSystem;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
@@ -12,9 +12,7 @@ import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N2;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
public class LinearQuadraticRegulatorTest {
public static LinearSystem<N2, N1, N1> elevatorPlant;
@@ -55,8 +53,7 @@ public class LinearQuadraticRegulatorTest {
var rElms = VecBuilder.fill(12.0);
var dt = 0.00505;
var controller = new LinearQuadraticRegulator<>(
elevatorPlant, qElms, rElms, dt);
var controller = new LinearQuadraticRegulator<>(elevatorPlant, qElms, rElms, dt);
var k = controller.getK();
@@ -69,21 +66,15 @@ public class LinearQuadraticRegulatorTest {
var dt = 0.020;
var plant = LinearSystemId.createElevatorSystem(
DCMotor.getVex775Pro(4),
8.0,
0.75 * 25.4 / 1000.0,
14.67);
var plant =
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 regulator =
new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt);
assertEquals(10.381, regulator.getK().get(0, 0), 1e-2);
assertEquals(0.6929, regulator.getK().get(0, 1), 1e-2);
}
@Test
@@ -102,8 +93,7 @@ public class LinearQuadraticRegulatorTest {
var rElms = VecBuilder.fill(12.0);
var dt = 0.00505;
var controller = new LinearQuadraticRegulator<>(
plant, qElms, rElms, dt);
var controller = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt);
var k = controller.getK();
@@ -115,17 +105,12 @@ public class LinearQuadraticRegulatorTest {
public void testLatencyCompensate() {
var dt = 0.02;
var plant = LinearSystemId.createElevatorSystem(
DCMotor.getVex775Pro(4),
8.0,
0.75 * 25.4 / 1000.0,
14.67);
var plant =
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 regulator =
new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt);
regulator.latencyCompensate(plant, dt, 0.01);

69
wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java
View File

@@ -4,11 +4,8 @@
package edu.wpi.first.wpilibj.controller;
import java.util.ArrayList;
import java.util.List;
import java.util.Random;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.wpilibj.estimator.KalmanFilter;
import edu.wpi.first.wpilibj.system.LinearSystem;
@@ -21,35 +18,34 @@ import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N2;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import java.util.ArrayList;
import java.util.List;
import java.util.Random;
import org.junit.jupiter.api.Test;
public class LinearSystemLoopTest {
public static final double kDt = 0.00505;
private static final double kPositionStddev = 0.0001;
private static final Random random = new Random();
LinearSystem<N2, N1, N1> m_plant = LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5,
0.0181864, 1.0);
LinearSystem<N2, N1, N1> m_plant =
LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5, 0.0181864, 1.0);
KalmanFilter<N2, N1, N1> m_observer = new KalmanFilter<>(Nat.N2(), Nat.N1(), m_plant,
VecBuilder.fill(0.05, 1.0),
VecBuilder.fill(0.0001), kDt);
KalmanFilter<N2, N1, N1> m_observer =
new KalmanFilter<>(
Nat.N2(), Nat.N1(), m_plant, VecBuilder.fill(0.05, 1.0), VecBuilder.fill(0.0001), kDt);
LinearQuadraticRegulator<N2, N1, N1> m_controller = new LinearQuadraticRegulator<>(
m_plant, VecBuilder.fill(0.02, 0.4), VecBuilder.fill(12.0),
0.00505);
LinearQuadraticRegulator<N2, N1, N1> m_controller =
new LinearQuadraticRegulator<>(
m_plant, VecBuilder.fill(0.02, 0.4), VecBuilder.fill(12.0), 0.00505);
private final LinearSystemLoop<N2, N1, N1> m_loop =
new LinearSystemLoop<>(m_plant, m_controller, m_observer, 12, 0.00505);
@SuppressWarnings("LocalVariableName")
private static void updateTwoState(LinearSystem<N2, N1, N1> plant, LinearSystemLoop<N2, N1, N1>
loop, double noise) {
Matrix<N1, N1> y = plant.calculateY(loop.getXHat(), loop.getU()).plus(
VecBuilder.fill(noise)
);
private static void updateTwoState(
LinearSystem<N2, N1, N1> plant, LinearSystemLoop<N2, N1, N1> loop, double noise) {
Matrix<N1, N1> y = plant.calculateY(loop.getXHat(), loop.getU()).plus(VecBuilder.fill(noise));
loop.correct(y);
loop.predict(kDt);
@@ -67,10 +63,11 @@ public class LinearSystemLoopTest {
TrapezoidProfile profile;
TrapezoidProfile.State state;
for (int i = 0; i < 1000; i++) {
profile = new TrapezoidProfile(
constraints, new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)),
new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0))
);
profile =
new TrapezoidProfile(
constraints,
new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)),
new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0)));
state = profile.calculate(kDt);
m_loop.setNextR(VecBuilder.fill(state.position, state.velocity));
@@ -82,24 +79,22 @@ public class LinearSystemLoopTest {
assertEquals(2.0, m_loop.getXHat(0), 0.05);
assertEquals(0.0, m_loop.getXHat(1), 0.5);
}
@Test
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
public void testFlywheelEnabled() {
LinearSystem<N1, N1, N1> plant = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2),
0.00289, 1.0);
KalmanFilter<N1, N1, N1> observer = new KalmanFilter<>(Nat.N1(), Nat.N1(), plant,
VecBuilder.fill(1.0),
VecBuilder.fill(kPositionStddev), kDt);
LinearSystem<N1, N1, N1> plant =
LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00289, 1.0);
KalmanFilter<N1, N1, N1> observer =
new KalmanFilter<>(
Nat.N1(), Nat.N1(), plant, VecBuilder.fill(1.0), VecBuilder.fill(kPositionStddev), kDt);
var qElms = VecBuilder.fill(9.0);
var rElms = VecBuilder.fill(12.0);
var controller = new LinearQuadraticRegulator<>(
plant, qElms, rElms, kDt);
var controller = new LinearQuadraticRegulator<>(plant, qElms, rElms, kDt);
var feedforward = new LinearPlantInversionFeedforward<>(plant, kDt);
@@ -125,9 +120,10 @@ public class LinearSystemLoopTest {
loop.setNextR(references);
Matrix<N1, N1> y = plant.calculateY(loop.getXHat(), loop.getU()).plus(
VecBuilder.fill(random.nextGaussian() * kPositionStddev)
);
Matrix<N1, N1> y =
plant
.calculateY(loop.getXHat(), loop.getU())
.plus(VecBuilder.fill(random.nextGaussian() * kPositionStddev));
loop.correct(y);
loop.predict(kDt);
@@ -158,5 +154,4 @@ public class LinearSystemLoopTest {
assertEquals(0.0, loop.getError(0), 0.1);
}
}

24
wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/AngleStatisticsTest.java
View File

@@ -4,38 +4,36 @@
package edu.wpi.first.wpilibj.estimator;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.VecBuilder;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import org.junit.jupiter.api.Test;
public class AngleStatisticsTest {
@Test
public 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
);
var sigmas =
Matrix.mat(Nat.N3(), Nat.N2()).fill(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());
assertTrue(AngleStatistics.angleMean(sigmas, weights, 1).isEqual(
VecBuilder.fill(1.1, Math.toRadians(1), 1.5), 1e-6));
assertTrue(
AngleStatistics.angleMean(sigmas, weights, 1)
.isEqual(VecBuilder.fill(1.1, Math.toRadians(1), 1.5), 1e-6));
}
@Test
public void testResidual() {
var first = VecBuilder.fill(1, Math.toRadians(1), 2);
var second = VecBuilder.fill(1, Math.toRadians(359), 1);
assertTrue(AngleStatistics.angleResidual(first, second, 1).isEqual(
VecBuilder.fill(0, Math.toRadians(2), 1), 1e-6));
assertTrue(
AngleStatistics.angleResidual(first, second, 1)
.isEqual(VecBuilder.fill(0, Math.toRadians(2), 1), 1e-6));
}
@Test

80
wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/DifferentialDrivePoseEstimatorTest.java
View File

@@ -4,10 +4,7 @@
package edu.wpi.first.wpilibj.estimator;
import java.util.List;
import java.util.Random;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -20,30 +17,36 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpiutil.math.MatBuilder;
import edu.wpi.first.wpiutil.math.Nat;
import static org.junit.jupiter.api.Assertions.assertEquals;
import java.util.List;
import java.util.Random;
import org.junit.jupiter.api.Test;
public class DifferentialDrivePoseEstimatorTest {
@SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength",
"PMD.AvoidInstantiatingObjectsInLoops"})
@SuppressWarnings({
"LocalVariableName",
"PMD.ExcessiveMethodLength",
"PMD.AvoidInstantiatingObjectsInLoops"
})
@Test
public void testAccuracy() {
var estimator = new DifferentialDrivePoseEstimator(new Rotation2d(), new Pose2d(),
var estimator =
new DifferentialDrivePoseEstimator(
new Rotation2d(),
new Pose2d(),
new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02),
new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.01, 0.01, 0.001),
new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01));
var traj = TrajectoryGenerator.generateTrajectory(
var traj =
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))
),
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
new TrajectoryConfig(10, 5));
var kinematics = new DifferentialDriveKinematics(1);
var rand = new Random(4915);
@@ -63,24 +66,26 @@ public class DifferentialDrivePoseEstimatorTest {
DifferentialDriveWheelSpeeds input;
while (t <= traj.getTotalTimeSeconds()) {
groundtruthState = traj.sample(t);
input = kinematics.toWheelSpeeds(new ChassisSpeeds(
groundtruthState.velocityMetersPerSecond, 0.0,
// ds/dt * dtheta/ds = dtheta/dt
groundtruthState.velocityMetersPerSecond * groundtruthState.curvatureRadPerMeter
));
input =
kinematics.toWheelSpeeds(
new ChassisSpeeds(
groundtruthState.velocityMetersPerSecond,
0.0,
// ds/dt * dtheta/ds = dtheta/dt
groundtruthState.velocityMetersPerSecond
* groundtruthState.curvatureRadPerMeter));
if (lastVisionUpdateTime + visionUpdateRate + rand.nextGaussian() * 0.4 < t) {
if (lastVisionPose != null) {
estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
}
var groundPose = groundtruthState.poseMeters;
lastVisionPose = new Pose2d(
lastVisionPose =
new Pose2d(
new Translation2d(
groundPose.getTranslation().getX() + rand.nextGaussian() * 0.1,
groundPose.getTranslation().getY() + rand.nextGaussian() * 0.1
),
new Rotation2d(rand.nextGaussian() * 0.01).plus(groundPose.getRotation())
);
groundPose.getTranslation().getX() + rand.nextGaussian() * 0.1,
groundPose.getTranslation().getY() + rand.nextGaussian() * 0.1),
new Rotation2d(rand.nextGaussian() * 0.01).plus(groundPose.getRotation()));
lastVisionUpdateTime = t;
}
@@ -91,15 +96,16 @@ public class DifferentialDrivePoseEstimatorTest {
distanceRight += input.rightMetersPerSecond * dt;
var rotNoise = new Rotation2d(rand.nextGaussian() * 0.001);
var xHat = estimator.updateWithTime(
var xHat =
estimator.updateWithTime(
t,
groundtruthState.poseMeters.getRotation().plus(rotNoise),
input,
distanceLeft, distanceRight
);
distanceLeft,
distanceRight);
double error =
groundtruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
groundtruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
@@ -108,13 +114,7 @@ public class DifferentialDrivePoseEstimatorTest {
t += dt;
}
assertEquals(
0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.035,
"Incorrect mean error"
);
assertEquals(
0.0, maxError, 0.055,
"Incorrect max error"
);
assertEquals(0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.035, "Incorrect mean error");
assertEquals(0.0, maxError, 0.055, "Incorrect max error");
}
}

127
wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java
View File

@@ -4,11 +4,8 @@
package edu.wpi.first.wpilibj.estimator;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -25,9 +22,10 @@ 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.N5;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
@SuppressWarnings("ParameterName")
public class ExtendedKalmanFilterTest {
@@ -41,8 +39,9 @@ public class ExtendedKalmanFilterTest {
final var J = 5.6; // Robot moment of inertia
final var C1 =
-Math.pow(gr, 2) * motors.m_KtNMPerAmp / (
motors.m_KvRadPerSecPerVolt * motors.m_rOhms * r * r);
-Math.pow(gr, 2)
* motors.m_KtNMPerAmp
/ (motors.m_KvRadPerSecPerVolt * motors.m_rOhms * r * r);
final var C2 = gr * motors.m_KtNMPerAmp / (motors.m_rOhms * r);
final var k1 = 1.0 / m + rb * rb / J;
final var k2 = 1.0 / m - rb * rb / J;
@@ -67,13 +66,7 @@ public class ExtendedKalmanFilterTest {
}
public static Matrix<N5, N1> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
return VecBuilder.fill(
x.get(0, 0),
x.get(1, 0),
x.get(2, 0),
x.get(3, 0),
x.get(4, 0)
);
return VecBuilder.fill(x.get(0, 0), x.get(1, 0), x.get(2, 0), x.get(3, 0), x.get(4, 0));
}
@SuppressWarnings("LocalVariableName")
@@ -81,49 +74,59 @@ public class ExtendedKalmanFilterTest {
public void testInit() {
double dtSeconds = 0.00505;
assertDoesNotThrow(() -> {
ExtendedKalmanFilter<N5, N2, N3> observer =
new ExtendedKalmanFilter<>(Nat.N5(), Nat.N2(), Nat.N3(),
assertDoesNotThrow(
() -> {
ExtendedKalmanFilter<N5, N2, N3> observer =
new ExtendedKalmanFilter<>(
Nat.N5(),
Nat.N2(),
Nat.N3(),
ExtendedKalmanFilterTest::getDynamics,
ExtendedKalmanFilterTest::getLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
VecBuilder.fill(0.0001, 0.01, 0.01), dtSeconds);
VecBuilder.fill(0.0001, 0.01, 0.01),
dtSeconds);
Matrix<N2, N1> u = VecBuilder.fill(12.0, 12.0);
observer.predict(u, dtSeconds);
Matrix<N2, N1> u = VecBuilder.fill(12.0, 12.0);
observer.predict(u, dtSeconds);
var localY = getLocalMeasurementModel(observer.getXhat(), u);
observer.correct(u, localY);
var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
var R = StateSpaceUtil.makeCostMatrix(
VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
observer.correct(Nat.N5(),
u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
});
var localY = getLocalMeasurementModel(observer.getXhat(), u);
observer.correct(u, localY);
var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
observer.correct(
Nat.N5(), u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
});
}
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops",
"PMD.ExcessiveMethodLength"})
@SuppressWarnings({
"LocalVariableName",
"PMD.AvoidInstantiatingObjectsInLoops",
"PMD.ExcessiveMethodLength"
})
@Test
public void testConvergence() {
double dtSeconds = 0.00505;
double rbMeters = 0.8382 / 2.0; // Robot radius
ExtendedKalmanFilter<N5, N2, N3> observer =
new ExtendedKalmanFilter<>(Nat.N5(), Nat.N2(), Nat.N3(),
ExtendedKalmanFilterTest::getDynamics,
ExtendedKalmanFilterTest::getLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
VecBuilder.fill(0.001, 0.01, 0.01), dtSeconds);
new ExtendedKalmanFilter<>(
Nat.N5(),
Nat.N2(),
Nat.N3(),
ExtendedKalmanFilterTest::getDynamics,
ExtendedKalmanFilterTest::getLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
VecBuilder.fill(0.001, 0.01, 0.01),
dtSeconds);
List<Pose2d> waypoints = Arrays.asList(new Pose2d(2.75, 22.521, new Rotation2d()),
new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
var trajectory = TrajectoryGenerator.generateTrajectory(
waypoints,
new TrajectoryConfig(8.8, 0.1)
);
List<Pose2d> waypoints =
Arrays.asList(
new Pose2d(2.75, 22.521, new Rotation2d()),
new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
var trajectory =
TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));
Matrix<N5, N1> r = new Matrix<>(Nat.N5(), Nat.N1());
@@ -136,14 +139,21 @@ public class ExtendedKalmanFilterTest {
List<Double> observerXs = new ArrayList<>();
List<Double> observerYs = new ArrayList<>();
var B = NumericalJacobian.numericalJacobianU(Nat.N5(), Nat.N2(),
ExtendedKalmanFilterTest::getDynamics, new Matrix<>(Nat.N5(), Nat.N1()), u);
var B =
NumericalJacobian.numericalJacobianU(
Nat.N5(),
Nat.N2(),
ExtendedKalmanFilterTest::getDynamics,
new Matrix<>(Nat.N5(), Nat.N1()),
u);
observer.setXhat(VecBuilder.fill(
trajectory.getInitialPose().getTranslation().getX(),
trajectory.getInitialPose().getTranslation().getY(),
trajectory.getInitialPose().getRotation().getRadians(),
0.0, 0.0));
observer.setXhat(
VecBuilder.fill(
trajectory.getInitialPose().getTranslation().getX(),
trajectory.getInitialPose().getTranslation().getY(),
trajectory.getInitialPose().getRotation().getRadians(),
0.0,
0.0));
var groundTruthX = observer.getXhat();
@@ -159,19 +169,17 @@ public class ExtendedKalmanFilterTest {
nextR.set(3, 0, vl);
nextR.set(4, 0, vr);
var localY =
getLocalMeasurementModel(groundTruthX, u);
var localY = getLocalMeasurementModel(groundTruthX, u);
var whiteNoiseStdDevs = VecBuilder.fill(0.0001, 0.5, 0.5);
observer.correct(u,
localY.plus(StateSpaceUtil.makeWhiteNoiseVector(whiteNoiseStdDevs)));
observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(whiteNoiseStdDevs)));
Matrix<N5, N1> rdot = nextR.minus(r).div(dtSeconds);
u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
observer.predict(u, dtSeconds);
groundTruthX = RungeKutta.rungeKutta(ExtendedKalmanFilterTest::getDynamics,
groundTruthX, u, dtSeconds);
groundTruthX =
RungeKutta.rungeKutta(ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dtSeconds);
r = nextR;
@@ -186,8 +194,7 @@ public class ExtendedKalmanFilterTest {
observer.correct(u, localY);
var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
var R = StateSpaceUtil.makeCostMatrix(
VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
observer.correct(Nat.N5(), u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
// var chartBuilder = new XYChartBuilder();

188
wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/KalmanFilterTest.java
View File

@@ -4,11 +4,8 @@
package edu.wpi.first.wpilibj.estimator;
import java.util.ArrayList;
import java.util.List;
import java.util.Random;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -24,9 +21,10 @@ 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.N6;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import java.util.ArrayList;
import java.util.List;
import java.util.Random;
import org.junit.jupiter.api.Test;
public class KalmanFilterTest {
private static LinearSystem<N2, N1, N1> elevatorPlant;
@@ -49,28 +47,19 @@ public class KalmanFilterTest {
// A swerve drive system where the states are [x, y, theta, vx, vy, vTheta]^T,
// Y is [x, y, theta]^T and u is [ax, ay, alpha}^T
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
),
new Matrix<>(Nat.N3(), Nat.N3())); // D
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),
new Matrix<>(Nat.N3(), Nat.N3())); // D
@Test
@SuppressWarnings("LocalVariableName")
@@ -88,13 +77,15 @@ public class KalmanFilterTest {
var random = new Random();
var filter = new KalmanFilter<>(Nat.N6(), Nat.N3(),
m_swerveObserverSystem,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
// weights
VecBuilder.fill(2, 2, 2), // measurement weights
0.020
);
var filter =
new KalmanFilter<>(
Nat.N6(),
Nat.N3(),
m_swerveObserverSystem,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
// weights
VecBuilder.fill(2, 2, 2), // measurement weights
0.020);
List<Double> xhatsX = new ArrayList<>();
List<Double> measurementsX = new ArrayList<>();
@@ -104,8 +95,8 @@ public class KalmanFilterTest {
Matrix<N3, N1> measurement;
for (int i = 0; i < 100; i++) {
// the robot is at [0, 0, 0] so we just park here
measurement = VecBuilder.fill(
random.nextGaussian(), random.nextGaussian(), random.nextGaussian());
measurement =
VecBuilder.fill(random.nextGaussian(), random.nextGaussian(), random.nextGaussian());
filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
// we continue to not accelerate
@@ -117,14 +108,14 @@ public class KalmanFilterTest {
xhatsY.add(filter.getXhat(1));
}
//var chart = new XYChartBuilder().build();
//chart.addSeries("Xhat, x/y", xhatsX, xhatsY);
//chart.addSeries("Measured position, x/y", measurementsX, measurementsY);
//try {
// var chart = new XYChartBuilder().build();
// chart.addSeries("Xhat, x/y", xhatsX, xhatsY);
// chart.addSeries("Measured position, x/y", measurementsX, measurementsY);
// try {
// new SwingWrapper<>(chart).displayChart();
// Thread.sleep(10000000);
//} catch (Exception ign) {
//}
// } catch (Exception ign) {
// }
assertEquals(0.0, filter.getXhat(0), 0.3);
assertEquals(0.0, filter.getXhat(0), 0.3);
}
@@ -135,13 +126,15 @@ public class KalmanFilterTest {
var random = new Random();
var filter = new KalmanFilter<>(Nat.N6(), Nat.N3(),
m_swerveObserverSystem,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
// weights
VecBuilder.fill(4, 4, 4), // measurement weights
0.020
);
var filter =
new KalmanFilter<>(
Nat.N6(),
Nat.N3(),
m_swerveObserverSystem,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
// weights
VecBuilder.fill(4, 4, 4), // measurement weights
0.020);
List<Double> xhatsX = new ArrayList<>();
List<Double> measurementsX = new ArrayList<>();
@@ -154,11 +147,12 @@ public class KalmanFilterTest {
for (int i = 0; i < 300; i++) {
// the robot is at [0, 0, 0] so we just park here
var measurement = VecBuilder.fill(
random.nextGaussian() / 10d,
random.nextGaussian() / 10d,
random.nextGaussian() / 4d // std dev of [1, 1, 1]
);
var measurement =
VecBuilder.fill(
random.nextGaussian() / 10d,
random.nextGaussian() / 10d,
random.nextGaussian() / 4d // std dev of [1, 1, 1]
);
filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
@@ -171,13 +165,13 @@ public class KalmanFilterTest {
xhatsY.add(filter.getXhat(1));
}
//var chart = new XYChartBuilder().build();
//chart.addSeries("Xhat, x/y", xhatsX, xhatsY);
//chart.addSeries("Measured position, x/y", measurementsX, measurementsY);
//try {
// var chart = new XYChartBuilder().build();
// chart.addSeries("Xhat, x/y", xhatsX, xhatsY);
// chart.addSeries("Measured position, x/y", measurementsX, measurementsY);
// try {
// new SwingWrapper<>(chart).displayChart();
// Thread.sleep(10000000);
//} catch (Exception ign) {}
// } catch (Exception ign) {}
assertEquals(0.0, filter.getXhat(0), 0.2);
assertEquals(0.0, filter.getXhat(1), 0.2);
@@ -189,41 +183,41 @@ public class KalmanFilterTest {
var random = new Random();
var filter = new KalmanFilter<>(Nat.N6(), Nat.N3(),
m_swerveObserverSystem,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
// weights
VecBuilder.fill(4, 4, 4), // measurement weights
0.020
);
var filter =
new KalmanFilter<>(
Nat.N6(),
Nat.N3(),
m_swerveObserverSystem,
VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
// weights
VecBuilder.fill(4, 4, 4), // measurement weights
0.020);
List<Double> xhatsX = new ArrayList<>();
List<Double> measurementsX = new ArrayList<>();
List<Double> xhatsY = new ArrayList<>();
List<Double> measurementsY = new ArrayList<>();
var trajectory = TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(0, 0, new Rotation2d()),
new Pose2d(5, 5, new Rotation2d())
), new TrajectoryConfig(2, 2)
);
var trajectory =
TrajectoryGenerator.generateTrajectory(
List.of(new Pose2d(0, 0, new Rotation2d()), new Pose2d(5, 5, new Rotation2d())),
new TrajectoryConfig(2, 2));
var time = 0.0;
var lastVelocity = VecBuilder.fill(0.0, 0.0, 0.0);
while (time <= trajectory.getTotalTimeSeconds()) {
var sample = trajectory.sample(time);
var measurement = VecBuilder.fill(
sample.poseMeters.getTranslation().getX() + random.nextGaussian() / 5d,
sample.poseMeters.getTranslation().getY() + random.nextGaussian() / 5d,
sample.poseMeters.getRotation().getRadians() + random.nextGaussian() / 3d
);
var measurement =
VecBuilder.fill(
sample.poseMeters.getTranslation().getX() + random.nextGaussian() / 5d,
sample.poseMeters.getTranslation().getY() + random.nextGaussian() / 5d,
sample.poseMeters.getRotation().getRadians() + random.nextGaussian() / 3d);
var velocity = VecBuilder.fill(
sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getCos(),
sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getSin(),
sample.curvatureRadPerMeter * sample.velocityMetersPerSecond
);
var velocity =
VecBuilder.fill(
sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getCos(),
sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getSin(),
sample.curvatureRadPerMeter * sample.velocityMetersPerSecond);
var u = (velocity.minus(lastVelocity)).div(0.020);
lastVelocity = velocity;
@@ -238,18 +232,22 @@ public class KalmanFilterTest {
time += 0.020;
}
//var chart = new XYChartBuilder().build();
//chart.addSeries("Xhat, x/y", xhatsX, xhatsY);
//chart.addSeries("Measured position, x/y", measurementsX, measurementsY);
//try {
// var chart = new XYChartBuilder().build();
// chart.addSeries("Xhat, x/y", xhatsX, xhatsY);
// chart.addSeries("Measured position, x/y", measurementsX, measurementsY);
// try {
// new SwingWrapper<>(chart).displayChart();
// Thread.sleep(10000000);
//} catch (Exception ign) {
//}
// } catch (Exception ign) {
// }
assertEquals(trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters
.getTranslation().getX(), filter.getXhat(0), 0.2);
assertEquals(trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters
.getTranslation().getY(), filter.getXhat(1), 0.2);
assertEquals(
trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters.getTranslation().getX(),
filter.getXhat(0),
0.2);
assertEquals(
trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters.getTranslation().getY(),
filter.getXhat(1),
0.2);
}
}

98
wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MecanumDrivePoseEstimatorTest.java
View File

@@ -4,10 +4,7 @@
package edu.wpi.first.wpilibj.estimator;
import java.util.List;
import java.util.Random;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -17,35 +14,42 @@ import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpiutil.math.VecBuilder;
import static org.junit.jupiter.api.Assertions.assertEquals;
import java.util.List;
import java.util.Random;
import org.junit.jupiter.api.Test;
public class MecanumDrivePoseEstimatorTest {
@Test
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops",
"PMD.ExcessiveMethodLength"})
@SuppressWarnings({
"LocalVariableName",
"PMD.AvoidInstantiatingObjectsInLoops",
"PMD.ExcessiveMethodLength"
})
public void testAccuracy() {
var kinematics = new MecanumDriveKinematics(
var kinematics =
new MecanumDriveKinematics(
new Translation2d(1, 1), new Translation2d(1, -1),
new Translation2d(-1, -1), new Translation2d(-1, 1));
var estimator = new MecanumDrivePoseEstimator(
new Rotation2d(), new Pose2d(), kinematics,
var estimator =
new MecanumDrivePoseEstimator(
new Rotation2d(),
new Pose2d(),
kinematics,
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.05),
VecBuilder.fill(0.1, 0.1, 0.1)
);
VecBuilder.fill(0.1, 0.1, 0.1));
var trajectory = TrajectoryGenerator.generateTrajectory(
List.of(new Pose2d(),
new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
new TrajectoryConfig(0.5, 2)
);
var trajectory =
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(),
new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
new TrajectoryConfig(0.5, 2));
var rand = new Random(5190);
@@ -66,35 +70,42 @@ public class MecanumDrivePoseEstimatorTest {
estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
}
lastVisionPose = new Pose2d(
lastVisionPose =
new Pose2d(
new Translation2d(
groundTruthState.poseMeters.getTranslation().getX()
+ rand.nextGaussian() * 0.1,
groundTruthState.poseMeters.getTranslation().getY()
+ rand.nextGaussian() * 0.1
),
new Rotation2d(
rand.nextGaussian() * 0.1).plus(groundTruthState.poseMeters.getRotation())
);
groundTruthState.poseMeters.getTranslation().getX() + rand.nextGaussian() * 0.1,
groundTruthState.poseMeters.getTranslation().getY()
+ rand.nextGaussian() * 0.1),
new Rotation2d(rand.nextGaussian() * 0.1)
.plus(groundTruthState.poseMeters.getRotation()));
lastVisionUpdateTime = t;
}
var wheelSpeeds = kinematics.toWheelSpeeds(new ChassisSpeeds(
groundTruthState.velocityMetersPerSecond, 0,
groundTruthState.velocityMetersPerSecond * groundTruthState.curvatureRadPerMeter));
var wheelSpeeds =
kinematics.toWheelSpeeds(
new ChassisSpeeds(
groundTruthState.velocityMetersPerSecond,
0,
groundTruthState.velocityMetersPerSecond
* groundTruthState.curvatureRadPerMeter));
wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
var xHat = estimator.updateWithTime(t,
groundTruthState.poseMeters.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05)), wheelSpeeds);
var xHat =
estimator.updateWithTime(
t,
groundTruthState
.poseMeters
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05)),
wheelSpeeds);
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
@@ -104,12 +115,7 @@ public class MecanumDrivePoseEstimatorTest {
}
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25,
"Incorrect mean error"
);
assertEquals(
0.0, maxError, 0.42,
"Incorrect max error"
);
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, "Incorrect mean error");
assertEquals(0.0, maxError, 0.42, "Incorrect max error");
}
}

34
wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPointsTest.java
View File

@@ -4,36 +4,40 @@
package edu.wpi.first.wpilibj.estimator;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.VecBuilder;
import static org.junit.jupiter.api.Assertions.assertTrue;
import org.junit.jupiter.api.Test;
public class MerweScaledSigmaPointsTest {
@Test
public void testZeroMeanPoints() {
var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
var points = merweScaledSigmaPoints.sigmaPoints(VecBuilder.fill(0, 0),
Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1));
var points =
merweScaledSigmaPoints.sigmaPoints(
VecBuilder.fill(0, 0), Matrix.mat(Nat.N2(), Nat.N2()).fill(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
), 1E-6));
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),
1E-6));
}
@Test
public void testNonzeroMeanPoints() {
var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
var points = merweScaledSigmaPoints.sigmaPoints(VecBuilder.fill(1, 2),
Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 10));
var points =
merweScaledSigmaPoints.sigmaPoints(
VecBuilder.fill(1, 2), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 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
), 1E-6));
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),
1E-6));
}
}

100
wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/SwerveDrivePoseEstimatorTest.java
View File

@@ -4,10 +4,7 @@
package edu.wpi.first.wpilibj.estimator;
import java.util.List;
import java.util.Random;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -17,38 +14,42 @@ import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpiutil.math.VecBuilder;
import static org.junit.jupiter.api.Assertions.assertEquals;
import java.util.List;
import java.util.Random;
import org.junit.jupiter.api.Test;
public class SwerveDrivePoseEstimatorTest {
@Test
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops",
"PMD.ExcessiveMethodLength"})
@SuppressWarnings({
"LocalVariableName",
"PMD.AvoidInstantiatingObjectsInLoops",
"PMD.ExcessiveMethodLength"
})
public void testAccuracy() {
var kinematics = new SwerveDriveKinematics(
var kinematics =
new SwerveDriveKinematics(
new Translation2d(1, 1),
new Translation2d(1, -1),
new Translation2d(-1, -1),
new Translation2d(-1, 1)
);
var estimator = new SwerveDrivePoseEstimator(
new Rotation2d(), new Pose2d(), kinematics,
new Translation2d(-1, 1));
var estimator =
new SwerveDrivePoseEstimator(
new Rotation2d(),
new Pose2d(),
kinematics,
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.005),
VecBuilder.fill(0.1, 0.1, 0.1)
);
VecBuilder.fill(0.1, 0.1, 0.1));
var trajectory = TrajectoryGenerator.generateTrajectory(
var trajectory =
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))
),
new TrajectoryConfig(0.5, 2)
);
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
new TrajectoryConfig(0.5, 2));
var rand = new Random(4915);
@@ -69,39 +70,41 @@ public class SwerveDrivePoseEstimatorTest {
estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
}
lastVisionPose = new Pose2d(
lastVisionPose =
new Pose2d(
new Translation2d(
groundTruthState.poseMeters.getTranslation().getX()
+ rand.nextGaussian() * 0.1,
groundTruthState.poseMeters.getTranslation().getY()
+ rand.nextGaussian() * 0.1
),
new Rotation2d(
rand.nextGaussian() * 0.1).plus(groundTruthState.poseMeters.getRotation())
);
groundTruthState.poseMeters.getTranslation().getX() + rand.nextGaussian() * 0.1,
groundTruthState.poseMeters.getTranslation().getY()
+ rand.nextGaussian() * 0.1),
new Rotation2d(rand.nextGaussian() * 0.1)
.plus(groundTruthState.poseMeters.getRotation()));
lastVisionUpdateTime = t;
}
var moduleStates = kinematics.toSwerveModuleStates(new ChassisSpeeds(
groundTruthState.velocityMetersPerSecond,
0.0,
groundTruthState.velocityMetersPerSecond * groundTruthState.curvatureRadPerMeter)
);
var moduleStates =
kinematics.toSwerveModuleStates(
new ChassisSpeeds(
groundTruthState.velocityMetersPerSecond,
0.0,
groundTruthState.velocityMetersPerSecond
* groundTruthState.curvatureRadPerMeter));
for (var moduleState : moduleStates) {
moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1;
}
var xHat = estimator.updateWithTime(
var xHat =
estimator.updateWithTime(
t,
groundTruthState.poseMeters.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05)),
groundTruthState
.poseMeters
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05)),
moduleStates);
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
@@ -111,12 +114,7 @@ public class SwerveDrivePoseEstimatorTest {
}
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25,
"Incorrect mean error"
);
assertEquals(
0.0, maxError, 0.42,
"Incorrect max error"
);
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, "Incorrect mean error");
assertEquals(0.0, maxError, 0.42, "Incorrect max error");
}
}

300
wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java
View File

@@ -4,11 +4,9 @@
package edu.wpi.first.wpilibj.estimator;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
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.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -27,11 +25,10 @@ import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N2;
import edu.wpi.first.wpiutil.math.numbers.N4;
import edu.wpi.first.wpiutil.math.numbers.N6;
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 java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
public class UnscentedKalmanFilterTest {
@SuppressWarnings({"LocalVariableName", "ParameterName"})
@@ -44,8 +41,10 @@ public class UnscentedKalmanFilterTest {
var m = 63.503;
var J = 5.6;
var C1 = -Math.pow(gHigh, 2) * motors.m_KtNMPerAmp
/ (motors.m_KvRadPerSecPerVolt * motors.m_rOhms * r * r);
var C1 =
-Math.pow(gHigh, 2)
* motors.m_KtNMPerAmp
/ (motors.m_KvRadPerSecPerVolt * motors.m_rOhms * r * r);
var C2 = gHigh * motors.m_KtNMPerAmp / (motors.m_rOhms * r);
var c = x.get(2, 0);
@@ -63,13 +62,12 @@ public class UnscentedKalmanFilterTest {
var w = (vr - vl) / (2.0 * rb);
return VecBuilder.fill(
xvel * c,
xvel * s,
-s * w,
c * w,
k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)),
k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr))
);
xvel * c,
xvel * s,
-s * w,
c * w,
k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)),
k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr)));
}
@SuppressWarnings("ParameterName")
@@ -85,25 +83,31 @@ public class UnscentedKalmanFilterTest {
@Test
@SuppressWarnings("LocalVariableName")
public void testInit() {
assertDoesNotThrow(() -> {
UnscentedKalmanFilter<N6, N2, N4> observer = new UnscentedKalmanFilter<>(
Nat.N6(), Nat.N4(),
UnscentedKalmanFilterTest::getDynamics,
UnscentedKalmanFilterTest::getLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
0.00505);
assertDoesNotThrow(
() -> {
UnscentedKalmanFilter<N6, N2, N4> observer =
new UnscentedKalmanFilter<>(
Nat.N6(),
Nat.N4(),
UnscentedKalmanFilterTest::getDynamics,
UnscentedKalmanFilterTest::getLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
0.00505);
var u = VecBuilder.fill(12.0, 12.0);
observer.predict(u, 0.00505);
var u = VecBuilder.fill(12.0, 12.0);
observer.predict(u, 0.00505);
var localY = getLocalMeasurementModel(observer.getXhat(), u);
observer.correct(u, localY);
});
var localY = getLocalMeasurementModel(observer.getXhat(), u);
observer.correct(u, localY);
});
}
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops",
"PMD.ExcessiveMethodLength"})
@SuppressWarnings({
"LocalVariableName",
"PMD.AvoidInstantiatingObjectsInLoops",
"PMD.ExcessiveMethodLength"
})
@Test
public void testConvergence() {
double dtSeconds = 0.00505;
@@ -125,39 +129,46 @@ public class UnscentedKalmanFilterTest {
List<Double> timeData = new ArrayList<>();
List<Matrix<?, ?>> rdots = new ArrayList<>();
UnscentedKalmanFilter<N6, N2, N4> observer = new UnscentedKalmanFilter<>(
Nat.N6(), Nat.N4(),
UnscentedKalmanFilterTest::getDynamics,
UnscentedKalmanFilterTest::getLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
dtSeconds);
UnscentedKalmanFilter<N6, N2, N4> observer =
new UnscentedKalmanFilter<>(
Nat.N6(),
Nat.N4(),
UnscentedKalmanFilterTest::getDynamics,
UnscentedKalmanFilterTest::getLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
dtSeconds);
List<Pose2d> waypoints = Arrays.asList(new Pose2d(2.75, 22.521, new Rotation2d()),
new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
var trajectory = TrajectoryGenerator.generateTrajectory(
waypoints,
new TrajectoryConfig(8.8, 0.1)
);
List<Pose2d> waypoints =
Arrays.asList(
new Pose2d(2.75, 22.521, new Rotation2d()),
new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
var trajectory =
TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));
Matrix<N6, N1> nextR;
Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
var B = NumericalJacobian.numericalJacobianU(Nat.N6(), Nat.N2(),
UnscentedKalmanFilterTest::getDynamics, new Matrix<>(Nat.N6(), Nat.N1()), u);
var B =
NumericalJacobian.numericalJacobianU(
Nat.N6(),
Nat.N2(),
UnscentedKalmanFilterTest::getDynamics,
new Matrix<>(Nat.N6(), Nat.N1()),
u);
observer.setXhat(VecBuilder.fill(2.75, 22.521, 1.0, 0.0, 0.0, 0.0)); // TODO not hard code this
var ref = trajectory.sample(0.0);
Matrix<N6, N1> r = VecBuilder.fill(
ref.poseMeters.getTranslation().getX(),
ref.poseMeters.getTranslation().getY(),
ref.poseMeters.getRotation().getCos(),
ref.poseMeters.getRotation().getSin(),
ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)),
ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters))
);
Matrix<N6, N1> r =
VecBuilder.fill(
ref.poseMeters.getTranslation().getX(),
ref.poseMeters.getTranslation().getY(),
ref.poseMeters.getRotation().getCos(),
ref.poseMeters.getRotation().getSin(),
ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)),
ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters)));
nextR = r.copy();
var trueXhat = observer.getXhat();
@@ -176,13 +187,10 @@ public class UnscentedKalmanFilterTest {
nextR.set(4, 0, vl);
nextR.set(5, 0, vr);
Matrix<N4, N1> localY =
getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
Matrix<N4, N1> localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
var noiseStdDev = VecBuilder.fill(0.001, 0.001, 0.5, 0.5);
observer.correct(u,
localY.plus(StateSpaceUtil.makeWhiteNoiseVector(
noiseStdDev)));
observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));
var rdot = nextR.minus(r).div(dtSeconds);
u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
@@ -208,20 +216,25 @@ public class UnscentedKalmanFilterTest {
r = nextR;
observer.predict(u, dtSeconds);
trueXhat = RungeKutta.rungeKutta(UnscentedKalmanFilterTest::getDynamics,
trueXhat, u, dtSeconds);
trueXhat =
RungeKutta.rungeKutta(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds);
}
var localY = getLocalMeasurementModel(trueXhat, u);
observer.correct(u, localY);
var globalY = getGlobalMeasurementModel(trueXhat, u);
var R = StateSpaceUtil.makeCostMatrix(
VecBuilder.fill(0.01, 0.01, 0.0001, 0.0001, 0.5, 0.5));
observer.correct(Nat.N6(), u, globalY,
UnscentedKalmanFilterTest::getGlobalMeasurementModel, R,
var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.0001, 0.5, 0.5));
observer.correct(
Nat.N6(),
u,
globalY,
UnscentedKalmanFilterTest::getGlobalMeasurementModel,
R,
(sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)),
Matrix::minus, Matrix::minus, Matrix::plus);
Matrix::minus,
Matrix::minus,
Matrix::plus);
final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
@@ -284,12 +297,15 @@ public class UnscentedKalmanFilterTest {
public void testLinearUKF() {
var dt = 0.020;
var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
var observer = new UnscentedKalmanFilter<>(Nat.N1(), Nat.N1(),
(x, u) -> plant.getA().times(x).plus(plant.getB().times(u)),
plant::calculateY,
VecBuilder.fill(0.05),
VecBuilder.fill(1.0),
dt);
var observer =
new UnscentedKalmanFilter<>(
Nat.N1(),
Nat.N1(),
(x, u) -> plant.getA().times(x).plus(plant.getB().times(u)),
plant::calculateY,
VecBuilder.fill(0.05),
VecBuilder.fill(1.0),
dt);
var time = new ArrayList<Double>();
var refData = new ArrayList<Double>();
@@ -340,56 +356,92 @@ public class UnscentedKalmanFilterTest {
@Test
public void testUnscentedTransform() {
// From FilterPy
var ret = UnscentedKalmanFilter.unscentedTransform(Nat.N4(), Nat.N4(),
Matrix.mat(Nat.N4(), Nat.N9()).fill(
-0.9, -0.822540333075852, -0.8922540333075852, -0.9,
-0.9, -0.9774596669241481, -0.9077459666924148, -0.9, -0.9,
1.0, 1.0, 1.077459666924148, 1.0, 1.0, 1.0, 0.9225403330758519, 1.0, 1.0,
-0.9, -0.9, -0.9, -0.822540333075852, -0.8922540333075852, -0.9,
-0.9, -0.9774596669241481, -0.9077459666924148,
1.0, 1.0, 1.0, 1.0, 1.077459666924148, 1.0, 1.0, 1.0, 0.9225403330758519
),
VecBuilder.fill(
-132.33333333,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667
),
VecBuilder.fill(
-129.34333333,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667
), (sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)), Matrix::minus
);
var ret =
UnscentedKalmanFilter.unscentedTransform(
Nat.N4(),
Nat.N4(),
Matrix.mat(Nat.N4(), Nat.N9())
.fill(
-0.9,
-0.822540333075852,
-0.8922540333075852,
-0.9,
-0.9,
-0.9774596669241481,
-0.9077459666924148,
-0.9,
-0.9,
1.0,
1.0,
1.077459666924148,
1.0,
1.0,
1.0,
0.9225403330758519,
1.0,
1.0,
-0.9,
-0.9,
-0.9,
-0.822540333075852,
-0.8922540333075852,
-0.9,
-0.9,
-0.9774596669241481,
-0.9077459666924148,
1.0,
1.0,
1.0,
1.0,
1.077459666924148,
1.0,
1.0,
1.0,
0.9225403330758519),
VecBuilder.fill(
-132.33333333,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667),
VecBuilder.fill(
-129.34333333,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667,
16.66666667),
(sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)),
Matrix::minus);
assertTrue(VecBuilder.fill(-0.9, 1, -0.9, 1).isEqual(ret.getFirst(), 1E-5));
assertTrue(
VecBuilder.fill(-0.9, 1, -0.9, 1).isEqual(
ret.getFirst(), 1E-5
));
assertTrue(
Matrix.mat(Nat.N4(), Nat.N4()).fill(
2.02000002e-01, 2.00000500e-02, -2.69044710e-29,
-4.59511477e-29,
2.00000500e-02, 2.00001000e-01, -2.98781068e-29,
-5.12759588e-29,
-2.73372625e-29, -3.09882635e-29, 2.02000002e-01,
2.00000500e-02,
-4.67065917e-29, -5.10705197e-29, 2.00000500e-02,
2.00001000e-01
).isEqual(
ret.getSecond(), 1E-5
));
Matrix.mat(Nat.N4(), Nat.N4())
.fill(
2.02000002e-01,
2.00000500e-02,
-2.69044710e-29,
-4.59511477e-29,
2.00000500e-02,
2.00001000e-01,
-2.98781068e-29,
-5.12759588e-29,
-2.73372625e-29,
-3.09882635e-29,
2.02000002e-01,
2.00000500e-02,
-4.67065917e-29,
-5.10705197e-29,
2.00000500e-02,
2.00001000e-01)
.isEqual(ret.getSecond(), 1E-5));
}
}

19
wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java
View File

@@ -4,28 +4,26 @@
package edu.wpi.first.wpilibj.geometry;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import org.junit.jupiter.api.Test;
class Pose2dTest {
private static final double kEpsilon = 1E-9;
@Test
void testTransformBy() {
var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
var transformation = new Transform2d(new Translation2d(5.0, 0.0),
Rotation2d.fromDegrees(5.0));
var transformation = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0));
var transformed = initial.plus(transformation);
assertAll(
() -> assertEquals(transformed.getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon),
() -> assertEquals(transformed.getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon),
() -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon)
);
() -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon));
}
@Test
@@ -36,11 +34,9 @@ class Pose2dTest {
var finalRelativeToInitial = last.relativeTo(initial);
assertAll(
() -> assertEquals(finalRelativeToInitial.getX(), 5.0 * Math.sqrt(2.0),
kEpsilon),
() -> assertEquals(finalRelativeToInitial.getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
() -> assertEquals(finalRelativeToInitial.getY(), 0.0, kEpsilon),
() -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon)
);
() -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon));
}
@Test
@@ -66,7 +62,6 @@ class Pose2dTest {
assertAll(
() -> assertEquals(transform.getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
() -> assertEquals(transform.getY(), 0.0, kEpsilon),
() -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon)
);
() -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon));
}
}

13
wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java
View File

@@ -4,12 +4,12 @@
package edu.wpi.first.wpilibj.geometry;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import org.junit.jupiter.api.Test;
class Rotation2dTest {
private static final double kEpsilon = 1E-9;
@@ -20,8 +20,7 @@ class Rotation2dTest {
assertAll(
() -> assertEquals(one.getDegrees(), 60.0, kEpsilon),
() -> assertEquals(two.getDegrees(), 45.0, kEpsilon)
);
() -> assertEquals(two.getDegrees(), 45.0, kEpsilon));
}
@Test
@@ -31,8 +30,7 @@ class Rotation2dTest {
assertAll(
() -> assertEquals(one.getRadians(), Math.PI / 4, kEpsilon),
() -> assertEquals(two.getRadians(), Math.PI / 6, kEpsilon)
);
() -> assertEquals(two.getRadians(), Math.PI / 6, kEpsilon));
}
@Test
@@ -42,8 +40,7 @@ class Rotation2dTest {
assertAll(
() -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon),
() -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon)
);
() -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon));
}
@Test

21
wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java
View File

@@ -4,30 +4,29 @@
package edu.wpi.first.wpilibj.geometry;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class Transform2dTest {
private static final double kEpsilon = 1E-9;
@Test
void testInverse() {
var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
var transformation = new Transform2d(new Translation2d(5.0, 0.0),
Rotation2d.fromDegrees(5.0));
var transformation = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0));
var transformed = initial.plus(transformation);
var untransformed = transformed.plus(transformation.inverse());
assertAll(
() -> assertEquals(initial.getX(), untransformed.getX(),
kEpsilon),
() -> assertEquals(initial.getY(), untransformed.getY(),
kEpsilon),
() -> assertEquals(initial.getRotation().getDegrees(),
untransformed.getRotation().getDegrees(), kEpsilon)
);
() -> assertEquals(initial.getX(), untransformed.getX(), kEpsilon),
() -> assertEquals(initial.getY(), untransformed.getY(), kEpsilon),
() ->
assertEquals(
initial.getRotation().getDegrees(),
untransformed.getRotation().getDegrees(),
kEpsilon));
}
}

25
wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java
View File

@@ -4,12 +4,12 @@
package edu.wpi.first.wpilibj.geometry;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import org.junit.jupiter.api.Test;
class Translation2dTest {
private static final double kEpsilon = 1E-9;
@@ -22,8 +22,7 @@ class Translation2dTest {
assertAll(
() -> assertEquals(sum.getX(), 3.0, kEpsilon),
() -> assertEquals(sum.getY(), 8.0, kEpsilon)
);
() -> assertEquals(sum.getY(), 8.0, kEpsilon));
}
@Test
@@ -35,8 +34,7 @@ class Translation2dTest {
assertAll(
() -> assertEquals(difference.getX(), -1.0, kEpsilon),
() -> assertEquals(difference.getY(), -2.0, kEpsilon)
);
() -> assertEquals(difference.getY(), -2.0, kEpsilon));
}
@Test
@@ -46,8 +44,7 @@ class Translation2dTest {
assertAll(
() -> assertEquals(rotated.getX(), 0.0, kEpsilon),
() -> assertEquals(rotated.getY(), 3.0, kEpsilon)
);
() -> assertEquals(rotated.getY(), 3.0, kEpsilon));
}
@Test
@@ -57,8 +54,7 @@ class Translation2dTest {
assertAll(
() -> assertEquals(mult.getX(), 9.0, kEpsilon),
() -> assertEquals(mult.getY(), 15.0, kEpsilon)
);
() -> assertEquals(mult.getY(), 15.0, kEpsilon));
}
@Test
@@ -68,8 +64,7 @@ class Translation2dTest {
assertAll(
() -> assertEquals(div.getX(), 1.5, kEpsilon),
() -> assertEquals(div.getY(), 2.5, kEpsilon)
);
() -> assertEquals(div.getY(), 2.5, kEpsilon));
}
@Test
@@ -92,8 +87,7 @@ class Translation2dTest {
assertAll(
() -> assertEquals(inverted.getX(), 4.5, kEpsilon),
() -> assertEquals(inverted.getY(), -7, kEpsilon)
);
() -> assertEquals(inverted.getY(), -7, kEpsilon));
}
@Test
@@ -118,7 +112,6 @@ class Translation2dTest {
() -> assertEquals(one.getX(), 1.0, kEpsilon),
() -> assertEquals(one.getY(), 1.0, kEpsilon),
() -> assertEquals(two.getX(), 1.0, kEpsilon),
() -> assertEquals(two.getY(), Math.sqrt(3), kEpsilon)
);
() -> assertEquals(two.getY(), Math.sqrt(3), kEpsilon));
}
}

16
wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java
View File

@@ -4,12 +4,12 @@
package edu.wpi.first.wpilibj.geometry;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import org.junit.jupiter.api.Test;
class Twist2dTest {
private static final double kEpsilon = 1E-9;
@@ -21,8 +21,7 @@ class Twist2dTest {
assertAll(
() -> assertEquals(straightPose.getX(), 5.0, kEpsilon),
() -> assertEquals(straightPose.getY(), 0.0, kEpsilon),
() -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon)
);
() -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon));
}
@Test
@@ -33,8 +32,7 @@ class Twist2dTest {
assertAll(
() -> assertEquals(quarterCirclePose.getX(), 5.0, kEpsilon),
() -> assertEquals(quarterCirclePose.getY(), 5.0, kEpsilon),
() -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon)
);
() -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon));
}
@Test
@@ -45,8 +43,7 @@ class Twist2dTest {
assertAll(
() -> assertEquals(diagonalPose.getX(), 2.0, kEpsilon),
() -> assertEquals(diagonalPose.getY(), 2.0, kEpsilon),
() -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon)
);
() -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon));
}
@Test
@@ -72,7 +69,6 @@ class Twist2dTest {
assertAll(
() -> assertEquals(twist.dx, 5.0 / 2.0 * Math.PI, kEpsilon),
() -> assertEquals(twist.dy, 0.0, kEpsilon),
() -> assertEquals(twist.dtheta, Math.PI / 2.0, kEpsilon)
);
() -> assertEquals(twist.dtheta, Math.PI / 2.0, kEpsilon));
}
}

15
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java
View File

@@ -4,26 +4,23 @@
package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import org.junit.jupiter.api.Test;
class ChassisSpeedsTest {
private static final double kEpsilon = 1E-9;
@Test
void testFieldRelativeConstruction() {
final var chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0)
);
final var chassisSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0));
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
);
() -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
}

26
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java
View File

@@ -4,15 +4,15 @@
package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class DifferentialDriveKinematicsTest {
private static final double kEpsilon = 1E-9;
private final DifferentialDriveKinematics m_kinematics
= new DifferentialDriveKinematics(0.381 * 2);
private final DifferentialDriveKinematics m_kinematics =
new DifferentialDriveKinematics(0.381 * 2);
@Test
void testInverseKinematicsForZeros() {
@@ -21,8 +21,7 @@ class DifferentialDriveKinematicsTest {
assertAll(
() -> assertEquals(0.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)
);
() -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond, kEpsilon));
}
@Test
@@ -33,8 +32,7 @@ class DifferentialDriveKinematicsTest {
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
);
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
@Test
@@ -44,8 +42,7 @@ class DifferentialDriveKinematicsTest {
assertAll(
() -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
() -> assertEquals(3.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)
);
() -> assertEquals(3.0, wheelSpeeds.rightMetersPerSecond, kEpsilon));
}
@Test
@@ -56,8 +53,7 @@ class DifferentialDriveKinematicsTest {
assertAll(
() -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
);
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
@Test
@@ -67,8 +63,7 @@ class DifferentialDriveKinematicsTest {
assertAll(
() -> assertEquals(-0.381 * Math.PI, wheelSpeeds.leftMetersPerSecond, kEpsilon),
() -> assertEquals(+0.381 * Math.PI, wheelSpeeds.rightMetersPerSecond, kEpsilon)
);
() -> assertEquals(+0.381 * Math.PI, wheelSpeeds.rightMetersPerSecond, kEpsilon));
}
@Test
@@ -79,7 +74,6 @@ class DifferentialDriveKinematicsTest {
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(-Math.PI, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
);
() -> assertEquals(-Math.PI, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
}

16
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
View File

@@ -4,18 +4,17 @@
package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import org.junit.jupiter.api.Test;
class DifferentialDriveOdometryTest {
private static final double kEpsilon = 1E-9;
private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(
new Rotation2d());
private final DifferentialDriveOdometry m_odometry =
new DifferentialDriveOdometry(new Rotation2d());
@Test
void testOdometryWithEncoderDistances() {
@@ -25,7 +24,6 @@ class DifferentialDriveOdometryTest {
assertAll(
() -> assertEquals(pose.getX(), 5.0, kEpsilon),
() -> assertEquals(pose.getY(), 5.0, kEpsilon),
() -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon)
);
() -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon));
}
}

47
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java
View File

@@ -4,13 +4,12 @@
package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import org.junit.jupiter.api.Test;
class MecanumDriveKinematicsTest {
private static final double kEpsilon = 1E-9;
@@ -37,8 +36,7 @@ class MecanumDriveKinematicsTest {
() -> assertEquals(3.536, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(3.536, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(3.536, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(3.536, moduleStates.rearRightMetersPerSecond, 0.1)
);
() -> assertEquals(3.536, moduleStates.rearRightMetersPerSecond, 0.1));
}
@Test
@@ -55,8 +53,7 @@ class MecanumDriveKinematicsTest {
assertAll(
() -> assertEquals(5, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)
);
() -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
@@ -74,8 +71,7 @@ class MecanumDriveKinematicsTest {
() -> assertEquals(-2.828427, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(2.828427, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(2.828427, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(-2.828427, moduleStates.rearRightMetersPerSecond, 0.1)
);
() -> assertEquals(-2.828427, moduleStates.rearRightMetersPerSecond, 0.1));
}
@Test
@@ -92,8 +88,7 @@ class MecanumDriveKinematicsTest {
assertAll(
() -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(4, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)
);
() -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
@@ -111,8 +106,7 @@ class MecanumDriveKinematicsTest {
() -> assertEquals(-106.629191, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(106.629191, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(-106.629191, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(106.629191, moduleStates.rearRightMetersPerSecond, 0.1)
);
() -> assertEquals(106.629191, moduleStates.rearRightMetersPerSecond, 0.1));
}
@Test
@@ -128,8 +122,7 @@ class MecanumDriveKinematicsTest {
assertAll(
() -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(2 * Math.PI, moduleStates.omegaRadiansPerSecond, 0.1)
);
() -> assertEquals(2 * Math.PI, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
@@ -147,8 +140,7 @@ class MecanumDriveKinematicsTest {
() -> assertEquals(-17.677670, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(20.506097, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(-13.435, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(16.26, moduleStates.rearRightMetersPerSecond, 0.1)
);
() -> assertEquals(16.26, moduleStates.rearRightMetersPerSecond, 0.1));
}
@Test
@@ -164,8 +156,7 @@ class MecanumDriveKinematicsTest {
assertAll(
() -> assertEquals(2, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(3, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
);
() -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
@@ -183,8 +174,7 @@ class MecanumDriveKinematicsTest {
() -> assertEquals(0, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(16.971, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(-16.971, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(33.941, moduleStates.rearRightMetersPerSecond, 0.1)
);
() -> assertEquals(33.941, moduleStates.rearRightMetersPerSecond, 0.1));
}
@Test
@@ -200,8 +190,7 @@ class MecanumDriveKinematicsTest {
assertAll(
() -> assertEquals(12, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(-12, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
);
() -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
@@ -219,8 +208,7 @@ class MecanumDriveKinematicsTest {
() -> assertEquals(2.12, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(21.92, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(-12.02, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(36.06, moduleStates.rearRightMetersPerSecond, 0.1)
);
() -> assertEquals(36.06, moduleStates.rearRightMetersPerSecond, 0.1));
}
@Test
@@ -237,8 +225,7 @@ class MecanumDriveKinematicsTest {
assertAll(
() -> assertEquals(17, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(-10, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
);
() -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
@@ -252,8 +239,6 @@ class MecanumDriveKinematicsTest {
() -> assertEquals(5.0 * factor, wheelSpeeds.frontLeftMetersPerSecond, kEpsilon),
() -> assertEquals(6.0 * factor, wheelSpeeds.frontRightMetersPerSecond, kEpsilon),
() -> assertEquals(4.0 * factor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon),
() -> assertEquals(7.0 * factor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon)
);
() -> assertEquals(7.0 * factor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon));
}
}

28
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
View File

@@ -4,14 +4,13 @@
package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class MecanumDriveOdometryTest {
private final Translation2d m_fl = new Translation2d(12, 12);
@@ -19,12 +18,11 @@ class MecanumDriveOdometryTest {
private final Translation2d m_bl = new Translation2d(-12, 12);
private final Translation2d m_br = new Translation2d(-12, -12);
private final MecanumDriveKinematics m_kinematics =
new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
new Rotation2d());
private final MecanumDriveOdometry m_odometry =
new MecanumDriveOdometry(m_kinematics, new Rotation2d());
@Test
void testMultipleConsecutiveUpdates() {
@@ -36,8 +34,7 @@ class MecanumDriveOdometryTest {
assertAll(
() -> assertEquals(secondPose.getX(), 0.0, 0.01),
() -> assertEquals(secondPose.getY(), 0.0, 0.01),
() -> assertEquals(secondPose.getRotation().getDegrees(), 0.0, 0.01)
);
() -> assertEquals(secondPose.getRotation().getDegrees(), 0.0, 0.01));
}
@Test
@@ -51,8 +48,7 @@ class MecanumDriveOdometryTest {
assertAll(
() -> assertEquals(5.0 / 10.0, pose.getX(), 0.01),
() -> assertEquals(0, pose.getY(), 0.01),
() -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)
);
() -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01));
}
@Test
@@ -67,8 +63,7 @@ class MecanumDriveOdometryTest {
assertAll(
() -> assertEquals(12.0, pose.getX(), 0.01),
() -> assertEquals(12.0, pose.getY(), 0.01),
() -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
);
() -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01));
}
@Test
@@ -76,16 +71,13 @@ class MecanumDriveOdometryTest {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536,
3.536, 3.536);
var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds());
var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
assertAll(
() -> assertEquals(5.0, pose.getX(), 0.1),
() -> assertEquals(0.00, pose.getY(), 0.1),
() -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
);
() -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1));
}
}

74
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java
View File

@@ -4,13 +4,12 @@
package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class SwerveDriveKinematicsTest {
private static final double kEpsilon = 1E-9;
@@ -37,8 +36,7 @@ class SwerveDriveKinematicsTest {
() -> assertEquals(0.0, moduleStates[0].angle.getRadians(), kEpsilon),
() -> assertEquals(0.0, moduleStates[1].angle.getRadians(), kEpsilon),
() -> assertEquals(0.0, moduleStates[2].angle.getRadians(), kEpsilon),
() -> assertEquals(0.0, moduleStates[3].angle.getRadians(), kEpsilon)
);
() -> assertEquals(0.0, moduleStates[3].angle.getRadians(), kEpsilon));
}
@Test
@@ -49,8 +47,7 @@ class SwerveDriveKinematicsTest {
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
);
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
@Test
@@ -67,8 +64,7 @@ class SwerveDriveKinematicsTest {
() -> assertEquals(90.0, moduleStates[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
() -> assertEquals(90.0, moduleStates[3].angle.getDegrees(), kEpsilon)
);
() -> assertEquals(90.0, moduleStates[3].angle.getDegrees(), kEpsilon));
}
@Test
@@ -79,8 +75,7 @@ class SwerveDriveKinematicsTest {
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
);
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
@Test
@@ -104,8 +99,7 @@ class SwerveDriveKinematicsTest {
() -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon),
() -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)
);
() -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon));
}
@Test
@@ -120,8 +114,7 @@ class SwerveDriveKinematicsTest {
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)
);
() -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1));
}
@Test
@@ -147,8 +140,7 @@ class SwerveDriveKinematicsTest {
() -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(0.0, moduleStates[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(-90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
() -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)
);
() -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon));
}
@Test
@@ -172,23 +164,27 @@ class SwerveDriveKinematicsTest {
assertAll(
() -> assertEquals(75.398, chassisSpeeds.vxMetersPerSecond, 0.1),
() -> assertEquals(-75.398, chassisSpeeds.vyMetersPerSecond, 0.1),
() -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)
);
() -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1));
}
private void assertModuleState(SwerveModuleState expected, SwerveModuleState actual,
SwerveModuleState tolerance) {
private void assertModuleState(
SwerveModuleState expected, SwerveModuleState actual, SwerveModuleState tolerance) {
assertAll(
() -> assertEquals(expected.speedMetersPerSecond, actual.speedMetersPerSecond,
tolerance.speedMetersPerSecond),
() -> assertEquals(expected.angle.getDegrees(), actual.angle.getDegrees(),
tolerance.angle.getDegrees())
);
() ->
assertEquals(
expected.speedMetersPerSecond,
actual.speedMetersPerSecond,
tolerance.speedMetersPerSecond),
() ->
assertEquals(
expected.angle.getDegrees(),
actual.angle.getDegrees(),
tolerance.angle.getDegrees()));
}
/**
* Test the rotation of the robot about a non-central point with
* both linear and angular velocities.
* Test the rotation of the robot about a non-central point with both linear and angular
* velocities.
*/
@Test
void testOffCenterCORRotationAndTranslationInverseKinematics() {
@@ -198,12 +194,13 @@ class SwerveDriveKinematicsTest {
// By equation (13.14) from state-space guide, our wheels/angles will be as follows,
// (+-1 degree or speed):
SwerveModuleState[] expectedStates = new SwerveModuleState[]{
new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19)),
new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81)),
new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44)),
new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56))
};
SwerveModuleState[] expectedStates =
new SwerveModuleState[] {
new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19)),
new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81)),
new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44)),
new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56))
};
var stateTolerance = new SwerveModuleState(0.1, Rotation2d.fromDegrees(0.1));
for (int i = 0; i < expectedStates.length; i++) {
@@ -232,8 +229,7 @@ class SwerveDriveKinematicsTest {
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, 0.1),
() -> assertEquals(-33.0, chassisSpeeds.vyMetersPerSecond, 0.1),
() -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond, 0.1)
);
() -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond, 0.1));
}
@Test
@@ -252,8 +248,6 @@ class SwerveDriveKinematicsTest {
() -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon),
() -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon),
() -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon)
);
() -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon));
}
}

51
wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
View File

@@ -4,14 +4,13 @@
package edu.wpi.first.wpilibj.kinematics;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class SwerveDriveOdometryTest {
private final Translation2d m_fl = new Translation2d(12, 12);
@@ -22,29 +21,32 @@ class SwerveDriveOdometryTest {
private final SwerveDriveKinematics m_kinematics =
new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
new Rotation2d());
private final SwerveDriveOdometry m_odometry =
new SwerveDriveOdometry(m_kinematics, new Rotation2d());
@Test
void testTwoIterations() {
// 5 units/sec in the x axis (forward)
final SwerveModuleState[] wheelSpeeds = {
new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
new SwerveModuleState(5, Rotation2d.fromDegrees(0))
};
new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
new SwerveModuleState(5, Rotation2d.fromDegrees(0))
};
m_odometry.updateWithTime(0.0, new Rotation2d(),
new SwerveModuleState(), new SwerveModuleState(),
new SwerveModuleState(), new SwerveModuleState());
m_odometry.updateWithTime(
0.0,
new Rotation2d(),
new SwerveModuleState(),
new SwerveModuleState(),
new SwerveModuleState(),
new SwerveModuleState());
var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
assertAll(
() -> assertEquals(5.0 / 10.0, pose.getX(), 0.01),
() -> assertEquals(0, pose.getY(), 0.01),
() -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)
);
() -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01));
}
@Test
@@ -56,11 +58,11 @@ class SwerveDriveOdometryTest {
// Module 3: speed 42.14888838624436 angle -26.565051177077986
final SwerveModuleState[] wheelSpeeds = {
new SwerveModuleState(18.85, Rotation2d.fromDegrees(90.0)),
new SwerveModuleState(42.15, Rotation2d.fromDegrees(26.565)),
new SwerveModuleState(18.85, Rotation2d.fromDegrees(-90)),
new SwerveModuleState(42.15, Rotation2d.fromDegrees(-26.565))
};
new SwerveModuleState(18.85, Rotation2d.fromDegrees(90.0)),
new SwerveModuleState(42.15, Rotation2d.fromDegrees(26.565)),
new SwerveModuleState(18.85, Rotation2d.fromDegrees(-90)),
new SwerveModuleState(42.15, Rotation2d.fromDegrees(-26.565))
};
final var zero = new SwerveModuleState();
m_odometry.updateWithTime(0.0, new Rotation2d(), zero, zero, zero, zero);
@@ -69,8 +71,7 @@ class SwerveDriveOdometryTest {
assertAll(
() -> assertEquals(12.0, pose.getX(), 0.01),
() -> assertEquals(12.0, pose.getY(), 0.01),
() -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
);
() -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01));
}
@Test
@@ -86,8 +87,6 @@ class SwerveDriveOdometryTest {
assertAll(
() -> assertEquals(1.0, pose.getX(), 0.1),
() -> assertEquals(0.00, pose.getY(), 0.1),
() -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
);
() -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1));
}
}

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

@@ -4,12 +4,9 @@
package edu.wpi.first.wpilibj.math;
import java.util.ArrayList;
import java.util.List;
import org.ejml.dense.row.MatrixFeatures_DDRM;
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.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -19,16 +16,16 @@ import edu.wpi.first.wpiutil.math.SimpleMatrixUtils;
import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import edu.wpi.first.wpiutil.math.numbers.N2;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import java.util.ArrayList;
import java.util.List;
import org.ejml.dense.row.MatrixFeatures_DDRM;
import org.ejml.simple.SimpleMatrix;
import org.junit.jupiter.api.Test;
public class StateSpaceUtilTest {
@Test
public void testCostArray() {
var mat = StateSpaceUtil.makeCostMatrix(
VecBuilder.fill(1.0, 2.0, 3.0));
var mat = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(1.0, 2.0, 3.0));
assertEquals(1.0, mat.get(0, 0), 1e-3);
assertEquals(0.0, mat.get(0, 1), 1e-3);
@@ -43,8 +40,7 @@ public class StateSpaceUtilTest {
@Test
public void testCovArray() {
var mat = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(),
VecBuilder.fill(1.0, 2.0, 3.0));
var mat = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), VecBuilder.fill(1.0, 2.0, 3.0));
assertEquals(1.0, mat.get(0, 0), 1e-3);
assertEquals(0.0, mat.get(0, 1), 1e-3);
@@ -123,8 +119,7 @@ public class StateSpaceUtilTest {
var x1Discrete = discA.times(x0);
// We now have pos = vel = 1 and accel = 0, which should give us:
var x1Truth = VecBuilder.fill(x0.get(0, 0) + 1.0 * x0.get(1, 0),
x0.get(1, 0));
var x1Truth = VecBuilder.fill(x0.get(0, 0) + 1.0 * x0.get(1, 0), x0.get(1, 0));
assertTrue(x1Truth.isEqual(x1Discrete, 1E-4));
}
@@ -141,8 +136,9 @@ public class StateSpaceUtilTest {
var x1Discrete = abPair.getFirst().times(x0).plus(abPair.getSecond().times(u));
// We now have pos = vel = accel = 1, which should give us:
var x1Truth = VecBuilder.fill(x0.get(0, 0) + x0.get(1, 0) + 0.5 * u.get(0, 0), x0.get(0, 0)
+ u.get(0, 0));
var x1Truth =
VecBuilder.fill(
x0.get(0, 0) + x0.get(1, 0) + 0.5 * u.get(0, 0), x0.get(0, 0) + u.get(0, 0));
assertTrue(x1Truth.isEqual(x1Discrete, 1E-4));
}
@@ -152,14 +148,16 @@ public class StateSpaceUtilTest {
Matrix<N2, N2> wrappedMatrix = Matrix.eye(Nat.N2());
var wrappedResult = wrappedMatrix.exp();
assertTrue(wrappedResult.isEqual(
Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9));
assertTrue(
wrappedResult.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9));
var matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(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), 1E-8));
assertTrue(
wrappedResult.isEqual(
Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912, 0.03076368, 1.04111993),
1E-8));
}
@Test
@@ -167,21 +165,22 @@ public class StateSpaceUtilTest {
SimpleMatrix matrix = SimpleMatrixUtils.eye(2);
var result = SimpleMatrixUtils.exp(matrix);
assertTrue(MatrixFeatures_DDRM.isIdentical(
result.getDDRM(),
new SimpleMatrix(2, 2, true, new double[]{Math.E, 0, 0, Math.E}).getDDRM(),
1E-9
));
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});
matrix = new SimpleMatrix(2, 2, true, new double[] {1, 2, 3, 4});
result = SimpleMatrixUtils.exp(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
));
assertTrue(
MatrixFeatures_DDRM.isIdentical(
result.getDDRM(),
new SimpleMatrix(
2, 2, true, new double[] {1.01035625, 0.02050912, 0.03076368, 1.04111993})
.getDDRM(),
1E-8));
}
@Test
@@ -192,5 +191,4 @@ public class StateSpaceUtilTest {
assertEquals(pose.getTranslation().getY(), vector.get(1, 0), 1e-6);
assertEquals(pose.getRotation().getRadians(), vector.get(2, 0), 1e-6);
}
}

87
wpimath/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
View File

@@ -4,22 +4,19 @@
package edu.wpi.first.wpilibj.spline;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
import static org.junit.jupiter.api.Assertions.assertAll;
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.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
class CubicHermiteSplineTest {
private static final double kMaxDx = 0.127;
@@ -29,15 +26,15 @@ class CubicHermiteSplineTest {
@SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"})
private void run(Pose2d a, List<Translation2d> waypoints, Pose2d b) {
// Start the timer.
//var start = System.nanoTime();
// var start = System.nanoTime();
// Generate and parameterize the spline.
var controlVectors =
SplineHelper.getCubicControlVectorsFromWaypoints(a,
waypoints.toArray(new Translation2d[0]), b);
var splines
= SplineHelper.getCubicSplinesFromControlVectors(
controlVectors[0], waypoints.toArray(new Translation2d[0]), controlVectors[1]);
SplineHelper.getCubicControlVectorsFromWaypoints(
a, waypoints.toArray(new Translation2d[0]), b);
var splines =
SplineHelper.getCubicSplinesFromControlVectors(
controlVectors[0], waypoints.toArray(new Translation2d[0]), controlVectors[1]);
var poses = new ArrayList<PoseWithCurvature>();
@@ -48,10 +45,10 @@ class CubicHermiteSplineTest {
}
// End the timer.
//var end = System.nanoTime();
// var end = System.nanoTime();
// Calculate the duration (used when benchmarking)
//var durationMicroseconds = (end - start) / 1000.0;
// var durationMicroseconds = (end - start) / 1000.0;
for (int i = 0; i < poses.size() - 1; i++) {
var p0 = poses.get(i);
@@ -62,19 +59,18 @@ class CubicHermiteSplineTest {
assertAll(
() -> assertTrue(Math.abs(twist.dx) < kMaxDx),
() -> assertTrue(Math.abs(twist.dy) < kMaxDy),
() -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta)
);
() -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta));
}
// Check first point
assertAll(
() -> assertEquals(a.getX(),
poses.get(0).poseMeters.getX(), 1E-9),
() -> assertEquals(a.getY(),
poses.get(0).poseMeters.getY(), 1E-9),
() -> assertEquals(a.getRotation().getRadians(),
poses.get(0).poseMeters.getRotation().getRadians(), 1E-9)
);
() -> assertEquals(a.getX(), poses.get(0).poseMeters.getX(), 1E-9),
() -> assertEquals(a.getY(), poses.get(0).poseMeters.getY(), 1E-9),
() ->
assertEquals(
a.getRotation().getRadians(),
poses.get(0).poseMeters.getRotation().getRadians(),
1E-9));
// Check interior waypoints
boolean interiorsGood = true;
@@ -92,13 +88,13 @@ class CubicHermiteSplineTest {
// Check last point
assertAll(
() -> assertEquals(b.getX(),
poses.get(poses.size() - 1).poseMeters.getX(), 1E-9),
() -> assertEquals(b.getY(),
poses.get(poses.size() - 1).poseMeters.getY(), 1E-9),
() -> assertEquals(b.getRotation().getRadians(),
poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9)
);
() -> assertEquals(b.getX(), poses.get(poses.size() - 1).poseMeters.getX(), 1E-9),
() -> assertEquals(b.getY(), poses.get(poses.size() - 1).poseMeters.getY(), 1E-9),
() ->
assertEquals(
b.getRotation().getRadians(),
poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(),
1E-9));
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@@ -148,12 +144,19 @@ class CubicHermiteSplineTest {
@Test
void testMalformed() {
assertThrows(MalformedSplineException.class, () -> run(
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new ArrayList<>(), new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
assertThrows(MalformedSplineException.class, () -> run(
new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
Arrays.asList(new Translation2d(10, 10.5)),
new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
assertThrows(
MalformedSplineException.class,
() ->
run(
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new ArrayList<>(),
new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
assertThrows(
MalformedSplineException.class,
() ->
run(
new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
Arrays.asList(new Translation2d(10, 10.5)),
new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
}
}

69
wpimath/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
View File

@@ -4,38 +4,36 @@
package edu.wpi.first.wpilibj.spline;
import java.util.List;
import org.junit.jupiter.api.Test;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
import static org.junit.jupiter.api.Assertions.assertAll;
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.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
import java.util.List;
import org.junit.jupiter.api.Test;
class QuinticHermiteSplineTest {
private static final double kMaxDx = 0.127;
private static final double kMaxDy = 0.00127;
private static final double kMaxDtheta = 0.0872;
@SuppressWarnings({ "ParameterName", "PMD.UnusedLocalVariable" })
@SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"})
private void run(Pose2d a, Pose2d b) {
// Start the timer.
//var start = System.nanoTime();
// var start = System.nanoTime();
// Generate and parameterize the spline.
var spline = SplineHelper.getQuinticSplinesFromWaypoints(List.of(a, b))[0];
var poses = SplineParameterizer.parameterize(spline);
// End the timer.
//var end = System.nanoTime();
// var end = System.nanoTime();
// Calculate the duration (used when benchmarking)
//var durationMicroseconds = (end - start) / 1000.0;
// var durationMicroseconds = (end - start) / 1000.0;
for (int i = 0; i < poses.size() - 1; i++) {
var p0 = poses.get(i);
@@ -51,22 +49,23 @@ class QuinticHermiteSplineTest {
// Check first point
assertAll(
() -> assertEquals(
a.getX(), poses.get(0).poseMeters.getX(), 1E-9),
() -> assertEquals(
a.getY(), poses.get(0).poseMeters.getY(), 1E-9),
() -> assertEquals(
a.getRotation().getRadians(), poses.get(0).poseMeters.getRotation().getRadians(),
1E-9));
() -> assertEquals(a.getX(), poses.get(0).poseMeters.getX(), 1E-9),
() -> assertEquals(a.getY(), poses.get(0).poseMeters.getY(), 1E-9),
() ->
assertEquals(
a.getRotation().getRadians(),
poses.get(0).poseMeters.getRotation().getRadians(),
1E-9));
// Check last point
assertAll(
() -> assertEquals(b.getX(), poses.get(poses.size() - 1)
.poseMeters.getX(), 1E-9),
() -> assertEquals(b.getY(), poses.get(poses.size() - 1)
.poseMeters.getY(), 1E-9),
() -> assertEquals(b.getRotation().getRadians(),
poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9));
() -> assertEquals(b.getX(), poses.get(poses.size() - 1).poseMeters.getX(), 1E-9),
() -> assertEquals(b.getY(), poses.get(poses.size() - 1).poseMeters.getY(), 1E-9),
() ->
assertEquals(
b.getRotation().getRadians(),
poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(),
1E-9));
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@@ -91,13 +90,17 @@ class QuinticHermiteSplineTest {
@Test
void testMalformed() {
assertThrows(MalformedSplineException.class,
() -> run(
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
assertThrows(MalformedSplineException.class,
() -> run(
new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
assertThrows(
MalformedSplineException.class,
() ->
run(
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
assertThrows(
MalformedSplineException.class,
() ->
run(
new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
}
}

42
wpimath/src/test/java/edu/wpi/first/wpilibj/system/LinearSystemIDTest.java
View File

@@ -4,47 +4,51 @@
package edu.wpi.first.wpilibj.system;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.wpilibj.system.plant.DCMotor;
import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.VecBuilder;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import org.junit.jupiter.api.Test;
class LinearSystemIDTest {
@Test
public void testDrivetrainVelocitySystem() {
var model = LinearSystemId.createDrivetrainVelocitySystem(
DCMotor.getNEO(4), 70, 0.05, 0.4, 6.0, 6
);
assertTrue(model.getA().isEqual(Matrix.mat(Nat.N2(),
Nat.N2()).fill(-10.14132, 3.06598, 3.06598, -10.14132), 0.001));
var model =
LinearSystemId.createDrivetrainVelocitySystem(DCMotor.getNEO(4), 70, 0.05, 0.4, 6.0, 6);
assertTrue(
model
.getA()
.isEqual(
Matrix.mat(Nat.N2(), Nat.N2()).fill(-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));
assertTrue(
model
.getB()
.isEqual(
Matrix.mat(Nat.N2(), Nat.N2()).fill(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));
assertTrue(
model.getC().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(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));
assertTrue(
model.getD().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 0.0, 0.0, 0.0), 0.001));
}
@Test
public 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));
assertTrue(
model.getA().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(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(Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0), 0.001));
assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
}

20
wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java
View File

@@ -4,14 +4,13 @@
package edu.wpi.first.wpilibj.system;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpiutil.math.Matrix;
import edu.wpi.first.wpiutil.math.Nat;
import edu.wpi.first.wpiutil.math.VecBuilder;
import edu.wpi.first.wpiutil.math.numbers.N1;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
public class RungeKuttaTest {
@Test
@@ -21,12 +20,15 @@ public class RungeKuttaTest {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
//noinspection SuspiciousNameCombination
var y1 = RungeKutta.rungeKutta((Matrix<N1, N1> x) -> {
var y = new Matrix<>(Nat.N1(), Nat.N1());
y.set(0, 0, Math.exp(x.get(0, 0)));
return y; },
y0, 0.1
);
var y1 =
RungeKutta.rungeKutta(
(Matrix<N1, N1> x) -> {
var y = new Matrix<>(Nat.N1(), Nat.N1());
y.set(0, 0, Math.exp(x.get(0, 0)));
return y;
},
y0,
0.1);
assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
}

17
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java
View File

@@ -4,14 +4,12 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.Collections;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.wpilibj.trajectory.constraint.CentripetalAccelerationConstraint;
import edu.wpi.first.wpilibj.util.Units;
import static org.junit.jupiter.api.Assertions.assertTrue;
import java.util.Collections;
import org.junit.jupiter.api.Test;
class CentripetalAccelerationConstraintTest {
@SuppressWarnings("LocalVariableName")
@@ -20,8 +18,8 @@ class CentripetalAccelerationConstraintTest {
double maxCentripetalAcceleration = Units.feetToMeters(7.0); // 7 feet per second squared
var constraint = new CentripetalAccelerationConstraint(maxCentripetalAcceleration);
Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
Collections.singletonList(constraint));
Trajectory trajectory =
TrajectoryGeneratorTest.getTrajectory(Collections.singletonList(constraint));
var duration = trajectory.getTotalTimeSeconds();
var t = 0.0;
@@ -29,12 +27,11 @@ class CentripetalAccelerationConstraintTest {
while (t < duration) {
var point = trajectory.sample(t);
var centripetalAcceleration
= Math.pow(point.velocityMetersPerSecond, 2) * point.curvatureRadPerMeter;
var centripetalAcceleration =
Math.pow(point.velocityMetersPerSecond, 2) * point.curvatureRadPerMeter;
t += dt;
assertTrue(centripetalAcceleration <= maxCentripetalAcceleration + 0.05);
}
}
}

26
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java
View File

@@ -4,17 +4,15 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.Collections;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
import edu.wpi.first.wpilibj.util.Units;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertTrue;
import java.util.Collections;
import org.junit.jupiter.api.Test;
class DifferentialDriveKinematicsConstraintTest {
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
@@ -24,8 +22,8 @@ class DifferentialDriveKinematicsConstraintTest {
var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(27));
var constraint = new DifferentialDriveKinematicsConstraint(kinematics, maxVelocity);
Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
Collections.singletonList(constraint));
Trajectory trajectory =
TrajectoryGeneratorTest.getTrajectory(Collections.singletonList(constraint));
var duration = trajectory.getTotalTimeSeconds();
var t = 0.0;
@@ -33,18 +31,18 @@ class DifferentialDriveKinematicsConstraintTest {
while (t < duration) {
var point = trajectory.sample(t);
var chassisSpeeds = new ChassisSpeeds(
point.velocityMetersPerSecond, 0,
point.velocityMetersPerSecond * point.curvatureRadPerMeter
);
var chassisSpeeds =
new ChassisSpeeds(
point.velocityMetersPerSecond,
0,
point.velocityMetersPerSecond * point.curvatureRadPerMeter);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
t += dt;
assertAll(
() -> assertTrue(wheelSpeeds.leftMetersPerSecond <= maxVelocity + 0.05),
() -> assertTrue(wheelSpeeds.rightMetersPerSecond <= maxVelocity + 0.05)
);
() -> assertTrue(wheelSpeeds.rightMetersPerSecond <= maxVelocity + 0.05));
}
}
}

93
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java
View File

@@ -4,10 +4,9 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.ArrayList;
import java.util.Collections;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.geometry.Pose2d;
@@ -16,10 +15,9 @@ import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertTrue;
import java.util.ArrayList;
import java.util.Collections;
import org.junit.jupiter.api.Test;
class DifferentialDriveVoltageConstraintTest {
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
@@ -29,12 +27,10 @@ class DifferentialDriveVoltageConstraintTest {
var feedforward = new SimpleMotorFeedforward(1, 1, 3);
var kinematics = new DifferentialDriveKinematics(0.5);
double maxVoltage = 10;
var constraint = new DifferentialDriveVoltageConstraint(feedforward,
kinematics,
maxVoltage);
var constraint = new DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage);
Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
Collections.singletonList(constraint));
Trajectory trajectory =
TrajectoryGeneratorTest.getTrajectory(Collections.singletonList(constraint));
var duration = trajectory.getTotalTimeSeconds();
var t = 0.0;
@@ -42,10 +38,11 @@ class DifferentialDriveVoltageConstraintTest {
while (t < duration) {
var point = trajectory.sample(t);
var chassisSpeeds = new ChassisSpeeds(
point.velocityMetersPerSecond, 0,
point.velocityMetersPerSecond * point.curvatureRadPerMeter
);
var chassisSpeeds =
new ChassisSpeeds(
point.velocityMetersPerSecond,
0,
point.velocityMetersPerSecond * point.curvatureRadPerMeter);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
@@ -54,19 +51,26 @@ class DifferentialDriveVoltageConstraintTest {
// Not really a strictly-correct test as we're using the chassis accel instead of the
// wheel accel, but much easier than doing it "properly" and a reasonable check anyway
assertAll(
() -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond,
point.accelerationMetersPerSecondSq)
<= maxVoltage + 0.05),
() -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond,
point.accelerationMetersPerSecondSq)
>= -maxVoltage - 0.05),
() -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond,
point.accelerationMetersPerSecondSq)
<= maxVoltage + 0.05),
() -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond,
point.accelerationMetersPerSecondSq)
>= -maxVoltage - 0.05)
);
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.leftMetersPerSecond, point.accelerationMetersPerSecondSq)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.leftMetersPerSecond, point.accelerationMetersPerSecondSq)
>= -maxVoltage - 0.05),
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.rightMetersPerSecond, point.accelerationMetersPerSecondSq)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.rightMetersPerSecond, point.accelerationMetersPerSecondSq)
>= -maxVoltage - 0.05));
}
}
@@ -77,24 +81,25 @@ class DifferentialDriveVoltageConstraintTest {
// Large trackwidth - need to test with radius of curvature less than half of trackwidth
var kinematics = new DifferentialDriveKinematics(3);
double maxVoltage = 10;
var constraint = new DifferentialDriveVoltageConstraint(feedforward,
kinematics,
maxVoltage);
var constraint = new DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage);
var config = new TrajectoryConfig(12, 12).addConstraint(constraint);
// Radius of curvature should be ~1 meter.
assertDoesNotThrow(() -> TrajectoryGenerator.generateTrajectory(
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
new ArrayList<Translation2d>(),
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
config));
assertDoesNotThrow(() -> TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
new ArrayList<Translation2d>(),
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
config.setReversed(true)));
assertDoesNotThrow(
() ->
TrajectoryGenerator.generateTrajectory(
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
new ArrayList<Translation2d>(),
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
config));
assertDoesNotThrow(
() ->
TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
new ArrayList<Translation2d>(),
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
config.setReversed(true)));
}
}

56
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java
View File

@@ -4,9 +4,8 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.List;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -14,9 +13,8 @@ import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.trajectory.constraint.EllipticalRegionConstraint;
import edu.wpi.first.wpilibj.trajectory.constraint.MaxVelocityConstraint;
import edu.wpi.first.wpilibj.util.Units;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import java.util.List;
import org.junit.jupiter.api.Test;
public class EllipticalRegionConstraintTest {
@Test
@@ -24,11 +22,13 @@ public class EllipticalRegionConstraintTest {
// Create constraints
double maxVelocity = Units.feetToMeters(3.0);
var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
var regionConstraint = new EllipticalRegionConstraint(
new Translation2d(Units.feetToMeters(5.0), Units.feetToMeters(5.0)),
Units.feetToMeters(10.0), Units.feetToMeters(5.0), Rotation2d.fromDegrees(180.0),
maxVelocityConstraint
);
var regionConstraint =
new EllipticalRegionConstraint(
new Translation2d(Units.feetToMeters(5.0), Units.feetToMeters(5.0)),
Units.feetToMeters(10.0),
Units.feetToMeters(5.0),
Rotation2d.fromDegrees(180.0),
maxVelocityConstraint);
// Get trajectory
var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint));
@@ -53,22 +53,28 @@ public class EllipticalRegionConstraintTest {
double maxVelocity = Units.feetToMeters(3.0);
var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
var regionConstraintNoRotation = new EllipticalRegionConstraint(
new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
Units.feetToMeters(2.0), Units.feetToMeters(4.0), new Rotation2d(),
maxVelocityConstraint);
var regionConstraintNoRotation =
new EllipticalRegionConstraint(
new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
Units.feetToMeters(2.0),
Units.feetToMeters(4.0),
new Rotation2d(),
maxVelocityConstraint);
assertFalse(regionConstraintNoRotation.isPoseInRegion(new Pose2d(
Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d()
)));
assertFalse(
regionConstraintNoRotation.isPoseInRegion(
new Pose2d(Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d())));
var regionConstraintWithRotation = new EllipticalRegionConstraint(
new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
Units.feetToMeters(2.0), Units.feetToMeters(4.0), Rotation2d.fromDegrees(90.0),
maxVelocityConstraint);
var regionConstraintWithRotation =
new EllipticalRegionConstraint(
new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
Units.feetToMeters(2.0),
Units.feetToMeters(4.0),
Rotation2d.fromDegrees(90.0),
maxVelocityConstraint);
assertTrue(regionConstraintWithRotation.isPoseInRegion(new Pose2d(
Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d()
)));
assertTrue(
regionConstraintWithRotation.isPoseInRegion(
new Pose2d(Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d())));
}
}

35
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java
View File

@@ -4,9 +4,8 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.List;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -14,9 +13,8 @@ import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.trajectory.constraint.MaxVelocityConstraint;
import edu.wpi.first.wpilibj.trajectory.constraint.RectangularRegionConstraint;
import edu.wpi.first.wpilibj.util.Units;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import java.util.List;
import org.junit.jupiter.api.Test;
public class RectangularRegionConstraintTest {
@Test
@@ -24,11 +22,11 @@ public class RectangularRegionConstraintTest {
// Create constraints
double maxVelocity = Units.feetToMeters(3.0);
var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
var regionConstraint = new RectangularRegionConstraint(
new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)),
maxVelocityConstraint
);
var regionConstraint =
new RectangularRegionConstraint(
new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)),
maxVelocityConstraint);
// Get trajectory
var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint));
@@ -49,14 +47,15 @@ public class RectangularRegionConstraintTest {
void testIsPoseWithinRegion() {
double maxVelocity = Units.feetToMeters(3.0);
var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
var regionConstraint = new RectangularRegionConstraint(
new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)),
maxVelocityConstraint
);
var regionConstraint =
new RectangularRegionConstraint(
new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)),
maxVelocityConstraint);
assertFalse(regionConstraint.isPoseInRegion(new Pose2d()));
assertTrue(regionConstraint.isPoseInRegion(new Pose2d(Units.feetToMeters(3.0),
Units.feetToMeters(14.5), new Rotation2d())));
assertTrue(
regionConstraint.isPoseInRegion(
new Pose2d(Units.feetToMeters(3.0), Units.feetToMeters(14.5), new Rotation2d())));
}
}

62
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
View File

@@ -4,22 +4,20 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
import static edu.wpi.first.wpilibj.util.Units.feetToMeters;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
import static edu.wpi.first.wpilibj.util.Units.feetToMeters;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
class TrajectoryGeneratorTest {
static Trajectory getTrajectory(List<? extends TrajectoryConstraint> constraints) {
@@ -27,24 +25,26 @@ class TrajectoryGeneratorTest {
final double maxAccel = feetToMeters(12);
// 2018 cross scale auto waypoints.
var sideStart = new Pose2d(feetToMeters(1.54), feetToMeters(23.23),
Rotation2d.fromDegrees(-180));
var crossScale = new Pose2d(feetToMeters(23.7), feetToMeters(6.8),
Rotation2d.fromDegrees(-160));
var sideStart =
new Pose2d(feetToMeters(1.54), feetToMeters(23.23), Rotation2d.fromDegrees(-180));
var crossScale =
new Pose2d(feetToMeters(23.7), feetToMeters(6.8), Rotation2d.fromDegrees(-160));
var waypoints = new ArrayList<Pose2d>();
waypoints.add(sideStart);
waypoints.add(sideStart.plus(
new Transform2d(new Translation2d(feetToMeters(-13), feetToMeters(0)),
new Rotation2d())));
waypoints.add(sideStart.plus(
new Transform2d(new Translation2d(feetToMeters(-19.5), feetToMeters(5)),
Rotation2d.fromDegrees(-90))));
waypoints.add(
sideStart.plus(
new Transform2d(
new Translation2d(feetToMeters(-13), feetToMeters(0)), new Rotation2d())));
waypoints.add(
sideStart.plus(
new Transform2d(
new Translation2d(feetToMeters(-19.5), feetToMeters(5)),
Rotation2d.fromDegrees(-90))));
waypoints.add(crossScale);
TrajectoryConfig config = new TrajectoryConfig(maxVelocity, maxAccel)
.setReversed(true)
.addConstraints(constraints);
TrajectoryConfig config =
new TrajectoryConfig(maxVelocity, maxAccel).setReversed(true).addConstraints(constraints);
return TrajectoryGenerator.generateTrajectory(waypoints, config);
}
@@ -63,9 +63,9 @@ class TrajectoryGeneratorTest {
t += dt;
assertAll(
() -> assertTrue(Math.abs(point.velocityMetersPerSecond) < feetToMeters(12.0) + 0.05),
() -> assertTrue(Math.abs(point.accelerationMetersPerSecondSq) < feetToMeters(12.0)
+ 0.05)
);
() ->
assertTrue(
Math.abs(point.accelerationMetersPerSecondSq) < feetToMeters(12.0) + 0.05));
}
}
@@ -73,12 +73,10 @@ class TrajectoryGeneratorTest {
void testMalformedTrajectory() {
var traj =
TrajectoryGenerator.generateTrajectory(
Arrays.asList(
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(1, 0, Rotation2d.fromDegrees(180))
),
new TrajectoryConfig(feetToMeters(12), feetToMeters(12))
);
Arrays.asList(
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(1, 0, Rotation2d.fromDegrees(180))),
new TrajectoryConfig(feetToMeters(12), feetToMeters(12)));
assertEquals(traj.getStates().size(), 1);
assertEquals(traj.getTotalTimeSeconds(), 0);

20
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java
View File

@@ -4,26 +4,26 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.List;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import java.util.List;
import org.junit.jupiter.api.Test;
public class TrajectoryJsonTest {
@Test
void deserializeMatches() {
var config = List.of(new DifferentialDriveKinematicsConstraint(
new DifferentialDriveKinematics(20), 3));
var config =
List.of(new DifferentialDriveKinematicsConstraint(new DifferentialDriveKinematics(20), 3));
var trajectory = TrajectoryGeneratorTest.getTrajectory(config);
var deserialized =
assertDoesNotThrow(() ->
TrajectoryUtil.deserializeTrajectory(TrajectoryUtil.serializeTrajectory(trajectory)));
assertDoesNotThrow(
() ->
TrajectoryUtil.deserializeTrajectory(
TrajectoryUtil.serializeTrajectory(trajectory)));
assertEquals(trajectory.getStates(), deserialized.getStates());
}

35
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java
View File

@@ -4,32 +4,30 @@
package edu.wpi.first.wpilibj.trajectory;
import java.util.List;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Transform2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import static org.junit.jupiter.api.Assertions.assertEquals;
import java.util.List;
import org.junit.jupiter.api.Test;
class TrajectoryTransformTest {
@Test
void testTransformBy() {
var config = new TrajectoryConfig(3, 3);
var trajectory = TrajectoryGenerator.generateTrajectory(
new Pose2d(), List.of(), new Pose2d(1, 1, Rotation2d.fromDegrees(90)),
config
);
var trajectory =
TrajectoryGenerator.generateTrajectory(
new Pose2d(), List.of(), new Pose2d(1, 1, Rotation2d.fromDegrees(90)), config);
var transformedTrajectory = trajectory.transformBy(
new Transform2d(new Translation2d(1, 2), Rotation2d.fromDegrees(30)));
var transformedTrajectory =
trajectory.transformBy(
new Transform2d(new Translation2d(1, 2), Rotation2d.fromDegrees(30)));
// Test initial pose.
assertEquals(new Pose2d(1, 2, Rotation2d.fromDegrees(30)),
transformedTrajectory.sample(0).poseMeters);
assertEquals(
new Pose2d(1, 2, Rotation2d.fromDegrees(30)), transformedTrajectory.sample(0).poseMeters);
testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
}
@@ -37,11 +35,12 @@ class TrajectoryTransformTest {
@Test
void testRelativeTo() {
var config = new TrajectoryConfig(3, 3);
var trajectory = TrajectoryGenerator.generateTrajectory(
new Pose2d(1, 2, Rotation2d.fromDegrees(30.0)),
List.of(), new Pose2d(5, 7, Rotation2d.fromDegrees(90)),
config
);
var trajectory =
TrajectoryGenerator.generateTrajectory(
new Pose2d(1, 2, Rotation2d.fromDegrees(30.0)),
List.of(),
new Pose2d(5, 7, Rotation2d.fromDegrees(90)),
config);
var transformedTrajectory = trajectory.relativeTo(new Pose2d(1, 2, Rotation2d.fromDegrees(30)));

12
wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java
View File

@@ -4,12 +4,12 @@
package edu.wpi.first.wpilibj.trajectory;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertNotEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import org.junit.jupiter.api.Test;
@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
class TrapezoidProfileTest {
private static final double kDt = 0.01;
@@ -32,8 +32,9 @@ class TrapezoidProfileTest {
* @param eps Tolerance for whether values are near to each other.
*/
private static void assertNear(double val1, double val2, double eps) {
assertTrue(Math.abs(val1 - val2) <= eps, "Difference between " + val1 + " and " + val2
+ " is greater than " + eps);
assertTrue(
Math.abs(val1 - val2) <= eps,
"Difference between " + val1 + " and " + val2 + " is greater than " + eps);
}
/**
@@ -53,8 +54,7 @@ class TrapezoidProfileTest {
@Test
void reachesGoal() {
TrapezoidProfile.Constraints constraints =
new TrapezoidProfile.Constraints(1.75, 0.75);
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();

5
wpimath/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java
View File

@@ -4,11 +4,10 @@
package edu.wpi.first.wpilibj.util;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.UtilityClassTest;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class UnitsTest extends UtilityClassTest<Units> {
UnitsTest() {

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

@@ -4,10 +4,10 @@
package edu.wpi.first.wpiutil.math;
import org.junit.jupiter.api.Test;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class MathUtilTest {
@Test
void testAngleNormalize() {

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

@@ -4,55 +4,43 @@
package edu.wpi.first.wpiutil.math;
import org.ejml.data.SingularMatrixException;
import org.junit.jupiter.api.Test;
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.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.assertEquals;
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
import org.ejml.data.SingularMatrixException;
import org.junit.jupiter.api.Test;
public 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 = 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);
Matrix<N2, N2> result = mat1.times(mat2);
assertEquals(result, Matrix.mat(Nat.N2(), Nat.N2()).fill(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 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);
var mat3 = Matrix.mat(Nat.N2(), Nat.N3()).fill(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);
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).isEqual(
result2,
1E-9
));
assertTrue(
Matrix.mat(Nat.N2(), Nat.N4())
.fill(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 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 1.0, 0.0, 1.0);
var vec = VecBuilder.fill(3.0, 2.0);
@@ -62,10 +50,7 @@ public class MatrixTest {
@Test
void testTranspose() {
Matrix<N3, N1> vec = VecBuilder
.fill(1.0,
2.0,
3.0);
Matrix<N3, N1> vec = VecBuilder.fill(1.0, 2.0, 3.0);
Matrix<N1, N3> transpose = vec.transpose();
@@ -91,68 +76,44 @@ public 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 = 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 inv = mat.inv();
assertTrue(Matrix.eye(Nat.N3()).isEqual(
mat.times(inv),
1E-9
));
assertTrue(Matrix.eye(Nat.N3()).isEqual(mat.times(inv), 1E-9));
assertTrue(Matrix.eye(Nat.N3()).isEqual(
inv.times(mat),
1E-9
));
assertTrue(Matrix.eye(Nat.N3()).isEqual(inv.times(mat), 1E-9));
}
@Test
void testUninvertableMatrix() {
var singularMatrix = Matrix.mat(Nat.N2(), Nat.N2())
.fill(2.0, 1.0,
2.0, 1.0);
var singularMatrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(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 = Matrix.mat(Nat.N2(), Nat.N2()).fill(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(Matrix.mat(Nat.N2(), Nat.N2()).fill(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(Matrix.mat(Nat.N2(), Nat.N2()).fill(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(Matrix.mat(Nat.N2(), Nat.N2()).fill(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 = Matrix.mat(Nat.N2(), Nat.N2()).fill(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 = Matrix.mat(Nat.N2(), Nat.N2()).fill(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(Matrix.mat(Nat.N2(), Nat.N2()).fill(-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(Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 8.0, 10.0, 12.0), mat1.plus(mat2));
}
@Test
@@ -165,7 +126,9 @@ public class MatrixTest {
matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(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), 1E-8));
assertTrue(
result.isEqual(
Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912, 0.03076368, 1.04111993),
1E-8));
}
}