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

Merge branch '2022'

Browse Source
This commit is contained in:
Peter Johnson
2021-05-09 14:15:40 -07:00
parent ba08baabb9 a3cd90dd71
commit 3cc2da3328
765 changed files with 5914 additions and 13714 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java Normal file
View File

@@ -0,0 +1,52 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
class MathUtilTest {
@Test
void testInputModulus() {
// These tests check error wrapping. That is, the result of wrapping the
// result of an angle reference minus the measurement.
// Test symmetric range
assertEquals(-20.0, MathUtil.inputModulus(170.0 - (-170.0), -180.0, 180.0));
assertEquals(-20.0, MathUtil.inputModulus(170.0 + 360.0 - (-170.0), -180.0, 180.0));
assertEquals(-20.0, MathUtil.inputModulus(170.0 - (-170.0 + 360.0), -180.0, 180.0));
assertEquals(20.0, MathUtil.inputModulus(-170.0 - 170.0, -180.0, 180.0));
assertEquals(20.0, MathUtil.inputModulus(-170.0 + 360.0 - 170.0, -180.0, 180.0));
assertEquals(20.0, MathUtil.inputModulus(-170.0 - (170.0 + 360.0), -180.0, 180.0));
// Test range start at zero
assertEquals(340.0, MathUtil.inputModulus(170.0 - 190.0, 0.0, 360.0));
assertEquals(340.0, MathUtil.inputModulus(170.0 + 360.0 - 190.0, 0.0, 360.0));
assertEquals(340.0, MathUtil.inputModulus(170.0 - (190.0 + 360), 0.0, 360.0));
// Test asymmetric range that doesn't start at zero
assertEquals(-20.0, MathUtil.inputModulus(170.0 - (-170.0), -170.0, 190.0));
// Test range with both positive endpoints
assertEquals(2.0, MathUtil.inputModulus(0.0, 1.0, 3.0));
assertEquals(3.0, MathUtil.inputModulus(1.0, 1.0, 3.0));
assertEquals(2.0, MathUtil.inputModulus(2.0, 1.0, 3.0));
assertEquals(3.0, MathUtil.inputModulus(3.0, 1.0, 3.0));
assertEquals(2.0, MathUtil.inputModulus(4.0, 1.0, 3.0));
}
@Test
void testAngleModulus() {
assertEquals(MathUtil.angleModulus(Math.toRadians(-2000)), Math.toRadians(160), 1e-6);
assertEquals(MathUtil.angleModulus(Math.toRadians(358)), Math.toRadians(-2), 1e-6);
assertEquals(MathUtil.angleModulus(Math.toRadians(360)), 0, 1e-6);
assertEquals(MathUtil.angleModulus(5 * Math.PI), Math.PI);
assertEquals(MathUtil.angleModulus(-5 * Math.PI), Math.PI);
assertEquals(MathUtil.angleModulus(Math.PI / 2), Math.PI / 2);
assertEquals(MathUtil.angleModulus(-Math.PI / 2), -Math.PI / 2);
}
}

134
wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java Normal file
View File

@@ -0,0 +1,134 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertThrows;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N4;
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);
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);
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));
}
@Test
void testMatrixVectorMultiplication() {
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);
Matrix<N2, N1> result = mat.times(vec);
assertEquals(VecBuilder.fill(5.0, 2.0), result);
}
@Test
void testTranspose() {
Matrix<N3, N1> vec = VecBuilder.fill(1.0, 2.0, 3.0);
Matrix<N1, N3> transpose = vec.transpose();
assertEquals(Matrix.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0), transpose);
}
@Test
void testSolve() {
var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0);
var vec1 = VecBuilder.fill(1.0, 2.0);
var solve1 = mat1.solve(vec1);
assertEquals(VecBuilder.fill(0.0, 0.5), solve1);
var mat2 = Matrix.mat(Nat.N3(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
var vec2 = VecBuilder.fill(1.0, 2.0, 3.0);
var solve2 = mat2.solve(vec2);
assertEquals(VecBuilder.fill(0.0, 0.5), solve2);
}
@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 inv = mat.inv();
assertTrue(Matrix.eye(Nat.N3()).isEqual(mat.times(inv), 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);
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);
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));
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 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(6.0, 8.0, 10.0, 12.0), mat1.plus(mat2));
}
@Test
void testMatrixExponential() {
var matrix = Matrix.eye(Nat.N2());
var result = matrix.exp();
assertTrue(result.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9));
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));
}
}

60
wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java Normal file
View File

@@ -0,0 +1,60 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
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);
assertEquals(
48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);
}
@SuppressWarnings("LocalVariableName")
@Test
void testCalculateState() {
ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
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);
}
@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)
.plus(VecBuilder.fill(0, 1).times(u));
return result;
}
@SuppressWarnings("ParameterName")
protected Matrix<N2, N1> getStateDynamics(Matrix<N2, N1> x) {
var result = new Matrix<>(Nat.N2(), Nat.N1());
result = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x);
return result;
}
}

31
wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java Normal file
View File

@@ -0,0 +1,31 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import org.junit.jupiter.api.Test;
class LinearPlantInversionFeedforwardTest {
@SuppressWarnings("LocalVariableName")
@Test
void testCalculate() {
Matrix<N2, N2> A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
Matrix<N2, N1> B = VecBuilder.fill(0, 1);
LinearPlantInversionFeedforward<N2, N1, N1> feedforward =
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);
}
}

120
wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java Normal file
View File

@@ -0,0 +1,120 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import org.junit.jupiter.api.Test;
public class LinearQuadraticRegulatorTest {
public static LinearSystem<N2, N1, N1> elevatorPlant;
static LinearSystem<N2, N1, N1> armPlant;
static {
createArm();
createElevator();
}
@SuppressWarnings("LocalVariableName")
public static void createArm() {
var motors = DCMotor.getVex775Pro(2);
var m = 4.0;
var r = 0.4;
var J = 1d / 3d * m * r * r;
var G = 100.0;
armPlant = LinearSystemId.createSingleJointedArmSystem(motors, J, G);
}
@SuppressWarnings("LocalVariableName")
public static void createElevator() {
var motors = DCMotor.getVex775Pro(2);
var m = 5.0;
var r = 0.0181864;
var G = 1.0;
elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G);
}
@Test
@SuppressWarnings("LocalVariableName")
public void testLQROnElevator() {
var qElms = VecBuilder.fill(0.02, 0.4);
var rElms = VecBuilder.fill(12.0);
var dt = 0.00505;
var controller = new LinearQuadraticRegulator<>(elevatorPlant, qElms, rElms, dt);
var k = controller.getK();
assertEquals(522.153, k.get(0, 0), 0.1);
assertEquals(38.2, k.get(0, 1), 0.1);
}
@Test
public void testFourMotorElevator() {
var dt = 0.020;
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);
assertEquals(10.381, regulator.getK().get(0, 0), 1e-2);
assertEquals(0.6929, regulator.getK().get(0, 1), 1e-2);
}
@Test
@SuppressWarnings("LocalVariableName")
public void testLQROnArm() {
var motors = DCMotor.getVex775Pro(2);
var m = 4.0;
var r = 0.4;
var G = 100.0;
var plant = LinearSystemId.createSingleJointedArmSystem(motors, 1d / 3d * m * r * r, G);
var qElms = VecBuilder.fill(0.01745, 0.08726);
var rElms = VecBuilder.fill(12.0);
var dt = 0.00505;
var controller = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt);
var k = controller.getK();
assertEquals(19.16, k.get(0, 0), 0.1);
assertEquals(3.32, k.get(0, 1), 0.1);
}
@Test
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 regulator =
new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt);
regulator.latencyCompensate(plant, dt, 0.01);
assertEquals(8.97115941, regulator.getK().get(0, 0), 1e-3);
assertEquals(0.07904881, regulator.getK().get(0, 1), 1e-3);
}
}

157
wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java Normal file
View File

@@ -0,0 +1,157 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.estimator.KalmanFilter;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.LinearSystemLoop;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
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);
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);
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));
loop.correct(y);
loop.predict(kDt);
}
@Test
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
public void testStateSpaceEnabled() {
m_loop.reset(VecBuilder.fill(0, 0));
Matrix<N2, N1> references = VecBuilder.fill(2.0, 0.0);
var constraints = new TrapezoidProfile.Constraints(4, 3);
m_loop.setNextR(references);
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)));
state = profile.calculate(kDt);
m_loop.setNextR(VecBuilder.fill(state.position, state.velocity));
updateTwoState(m_plant, m_loop, (random.nextGaussian()) * kPositionStddev);
var u = m_loop.getU(0);
assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);
}
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);
var qElms = VecBuilder.fill(9.0);
var rElms = VecBuilder.fill(12.0);
var controller = new LinearQuadraticRegulator<>(plant, qElms, rElms, kDt);
var feedforward = new LinearPlantInversionFeedforward<>(plant, kDt);
var loop = new LinearSystemLoop<>(controller, feedforward, observer, 12);
loop.reset(VecBuilder.fill(0.0));
var references = VecBuilder.fill(3000 / 60d * 2 * Math.PI);
loop.setNextR(references);
List<Double> timeData = new ArrayList<>();
List<Double> xHat = new ArrayList<>();
List<Double> volt = new ArrayList<>();
List<Double> reference = new ArrayList<>();
List<Double> error = new ArrayList<>();
var time = 0.0;
while (time < 10) {
if (time > 10) {
break;
}
loop.setNextR(references);
Matrix<N1, N1> y =
plant
.calculateY(loop.getXHat(), loop.getU())
.plus(VecBuilder.fill(random.nextGaussian() * kPositionStddev));
loop.correct(y);
loop.predict(kDt);
var u = loop.getU(0);
assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);
xHat.add(loop.getXHat(0) / 2d / Math.PI * 60);
volt.add(u);
timeData.add(time);
reference.add(references.get(0, 0) / 2d / Math.PI * 60);
error.add(loop.getError(0) / 2d / Math.PI * 60);
time += kDt;
}
// XYChart bigChart = new XYChartBuilder().build();
// bigChart.addSeries("Xhat, RPM", timeData, xHat);
// bigChart.addSeries("Reference, RPM", timeData, reference);
// bigChart.addSeries("Error, RPM", timeData, error);
// XYChart smolChart = new XYChartBuilder().build();
// smolChart.addSeries("Volts, V", timeData, volt);
// try {
// new SwingWrapper<>(List.of(bigChart, smolChart)).displayChartMatrix();
// Thread.sleep(10000000);
// } catch (InterruptedException e) { e.printStackTrace(); }
assertEquals(0.0, loop.getError(0), 0.1);
}
}

62
wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java Normal file
View File

@@ -0,0 +1,62 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.controller;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Twist2d;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import java.util.ArrayList;
import org.junit.jupiter.api.Test;
class RamseteControllerTest {
private static final double kTolerance = 1 / 12.0;
private static final double kAngularTolerance = Math.toRadians(2);
@Test
@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
void testReachesReference() {
final var controller = new RamseteController(2.0, 0.7);
var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
final var waypoints = new ArrayList<Pose2d>();
waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
var config = new TrajectoryConfig(8.8, 0.1);
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
final double kDt = 0.02;
final var totalTime = trajectory.getTotalTimeSeconds();
for (int i = 0; i < (totalTime / kDt); ++i) {
var state = trajectory.sample(kDt * i);
var output = controller.calculate(robotPose, state);
robotPose =
robotPose.exp(
new Twist2d(output.vxMetersPerSecond * kDt, 0, output.omegaRadiansPerSecond * kDt));
}
final var states = trajectory.getStates();
final var endPose = states.get(states.size() - 1).poseMeters;
// Java lambdas require local variables referenced from a lambda expression
// must be final or effectively final.
final var finalRobotPose = robotPose;
assertAll(
() -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
() -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
() ->
assertEquals(
0.0,
MathUtil.angleModulus(
endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()),
kAngularTolerance));
}
}

44
wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java Normal file
View File

@@ -0,0 +1,44 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
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);
// 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));
}
@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));
}
@Test
public void testAdd() {
var first = VecBuilder.fill(1, Math.toRadians(1), 2);
var second = VecBuilder.fill(1, Math.toRadians(359), 1);
assertTrue(AngleStatistics.angleAdd(first, second, 1).isEqual(VecBuilder.fill(2, 0, 3), 1e-6));
}
}

120
wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java Normal file
View File

@@ -0,0 +1,120 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.MatBuilder;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import java.util.List;
import java.util.Random;
import org.junit.jupiter.api.Test;
public class DifferentialDrivePoseEstimatorTest {
@SuppressWarnings({
"LocalVariableName",
"PMD.ExcessiveMethodLength",
"PMD.AvoidInstantiatingObjectsInLoops"
})
@Test
public void testAccuracy() {
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(
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(10, 5));
var kinematics = new DifferentialDriveKinematics(1);
var rand = new Random(4915);
final double dt = 0.02;
double t = 0.0;
final double visionUpdateRate = 0.1;
Pose2d lastVisionPose = null;
double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
double distanceLeft = 0.0;
double distanceRight = 0.0;
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
Trajectory.State groundtruthState;
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));
if (lastVisionUpdateTime + visionUpdateRate + rand.nextGaussian() * 0.4 < t) {
if (lastVisionPose != null) {
estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
}
var groundPose = groundtruthState.poseMeters;
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()));
lastVisionUpdateTime = t;
}
input.leftMetersPerSecond += rand.nextGaussian() * 0.01;
input.rightMetersPerSecond += rand.nextGaussian() * 0.01;
distanceLeft += input.leftMetersPerSecond * dt;
distanceRight += input.rightMetersPerSecond * dt;
var rotNoise = new Rotation2d(rand.nextGaussian() * 0.001);
var xHat =
estimator.updateWithTime(
t,
groundtruthState.poseMeters.getRotation().plus(rotNoise),
input,
distanceLeft,
distanceRight);
double error =
groundtruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
errorSum += error;
t += dt;
}
assertEquals(0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.035, "Incorrect mean error");
assertEquals(0.0, maxError, 0.055, "Incorrect max error");
}
}

222
wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java Normal file
View File

@@ -0,0 +1,222 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.math.StateSpaceUtil;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N5;
import edu.wpi.first.math.system.NumericalIntegration;
import edu.wpi.first.math.system.NumericalJacobian;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
@SuppressWarnings("ParameterName")
public class ExtendedKalmanFilterTest {
public static Matrix<N5, N1> getDynamics(final Matrix<N5, N1> x, final Matrix<N2, N1> u) {
final var motors = DCMotor.getCIM(2);
final var gr = 7.08; // Gear ratio
final var rb = 0.8382 / 2.0; // Wheelbase radius (track width)
final var r = 0.0746125; // Wheel radius
final var m = 63.503; // Robot mass
final var J = 5.6; // Robot moment of inertia
final var C1 =
-Math.pow(gr, 2) * motors.KtNMPerAmp / (motors.KvRadPerSecPerVolt * motors.rOhms * r * r);
final var C2 = gr * motors.KtNMPerAmp / (motors.rOhms * r);
final var k1 = 1.0 / m + rb * rb / J;
final var k2 = 1.0 / m - rb * rb / J;
final var vl = x.get(3, 0);
final var vr = x.get(4, 0);
final var Vl = u.get(0, 0);
final var Vr = u.get(1, 0);
final Matrix<N5, N1> result = new Matrix<>(Nat.N5(), Nat.N1());
final var v = 0.5 * (vl + vr);
result.set(0, 0, v * Math.cos(x.get(2, 0)));
result.set(1, 0, v * Math.sin(x.get(2, 0)));
result.set(2, 0, (vr - vl) / (2.0 * rb));
result.set(3, 0, k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)));
result.set(4, 0, k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr)));
return result;
}
public static Matrix<N3, N1> getLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
}
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));
}
@SuppressWarnings("LocalVariableName")
@Test
public void testInit() {
double dtSeconds = 0.00505;
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);
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);
});
}
@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);
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());
Matrix<N5, N1> nextR = new Matrix<>(Nat.N5(), Nat.N1());
Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
List<Double> trajXs = new ArrayList<>();
List<Double> trajYs = new ArrayList<>();
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);
observer.setXhat(
VecBuilder.fill(
trajectory.getInitialPose().getTranslation().getX(),
trajectory.getInitialPose().getTranslation().getY(),
trajectory.getInitialPose().getRotation().getRadians(),
0.0,
0.0));
var groundTruthX = observer.getXhat();
double totalTime = trajectory.getTotalTimeSeconds();
for (int i = 0; i < (totalTime / dtSeconds); i++) {
var ref = trajectory.sample(dtSeconds * i);
double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
nextR.set(0, 0, ref.poseMeters.getTranslation().getX());
nextR.set(1, 0, ref.poseMeters.getTranslation().getY());
nextR.set(2, 0, ref.poseMeters.getRotation().getRadians());
nextR.set(3, 0, vl);
nextR.set(4, 0, vr);
var localY = getLocalMeasurementModel(groundTruthX, u);
var whiteNoiseStdDevs = VecBuilder.fill(0.0001, 0.5, 0.5);
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 =
NumericalIntegration.rk4(
ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dtSeconds);
r = nextR;
trajXs.add(ref.poseMeters.getTranslation().getX());
trajYs.add(ref.poseMeters.getTranslation().getY());
observerXs.add(observer.getXhat().get(0, 0));
observerYs.add(observer.getXhat().get(1, 0));
}
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 chartBuilder = new XYChartBuilder();
// chartBuilder.title = "The Magic of Sensor Fusion, now with a "
// + observer.getClass().getSimpleName();
// var xyPosChart = chartBuilder.build();
//
// xyPosChart.setXAxisTitle("X pos, meters");
// xyPosChart.setYAxisTitle("Y pos, meters");
// xyPosChart.addSeries("Trajectory", trajXs, trajYs);
// xyPosChart.addSeries("xHat", observerXs, observerYs);
// new SwingWrapper<>(xyPosChart).displayChart();
// try {
// Thread.sleep(1000000000);
// } catch (InterruptedException ex) {
// ex.printStackTrace();
// }
var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 1.0);
assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 1.0);
assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0);
assertEquals(0.0, observer.getXhat(3), 1.0);
assertEquals(0.0, observer.getXhat(4), 1.0);
}
}

253
wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java Normal file
View File

@@ -0,0 +1,253 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N3;
import edu.wpi.first.math.numbers.N6;
import edu.wpi.first.math.system.LinearSystem;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
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;
private static final double kDt = 0.00505;
static {
createElevator();
}
@SuppressWarnings("LocalVariableName")
public static void createElevator() {
var motors = DCMotor.getVex775Pro(2);
var m = 5.0;
var r = 0.0181864;
var G = 1.0;
elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G);
}
// 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
@Test
@SuppressWarnings("LocalVariableName")
public void testElevatorKalmanFilter() {
var Q = VecBuilder.fill(0.05, 1.0);
var R = VecBuilder.fill(0.0001);
assertDoesNotThrow(() -> new KalmanFilter<>(Nat.N2(), Nat.N1(), elevatorPlant, Q, R, kDt));
}
@Test
@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
public void testSwerveKFStationary() {
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);
List<Double> xhatsX = new ArrayList<>();
List<Double> measurementsX = new ArrayList<>();
List<Double> xhatsY = new ArrayList<>();
List<Double> measurementsY = new ArrayList<>();
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());
filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
// we continue to not accelerate
filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020);
measurementsX.add(measurement.get(0, 0));
measurementsY.add(measurement.get(1, 0));
xhatsX.add(filter.getXhat(0));
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 {
// new SwingWrapper<>(chart).displayChart();
// Thread.sleep(10000000);
// } catch (Exception ign) {
// }
assertEquals(0.0, filter.getXhat(0), 0.3);
assertEquals(0.0, filter.getXhat(0), 0.3);
}
@Test
@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
public void testSwerveKFMovingWithoutAccelerating() {
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);
List<Double> xhatsX = new ArrayList<>();
List<Double> measurementsX = new ArrayList<>();
List<Double> xhatsY = new ArrayList<>();
List<Double> measurementsY = new ArrayList<>();
// we set the velocity of the robot so that it's moving forward slowly
filter.setXhat(0, 0.5);
filter.setXhat(1, 0.5);
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]
);
filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
// we continue to not accelerate
filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020);
measurementsX.add(measurement.get(0, 0));
measurementsY.add(measurement.get(1, 0));
xhatsX.add(filter.getXhat(0));
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 {
// new SwingWrapper<>(chart).displayChart();
// Thread.sleep(10000000);
// } catch (Exception ign) {}
assertEquals(0.0, filter.getXhat(0), 0.2);
assertEquals(0.0, filter.getXhat(1), 0.2);
}
@Test
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
public void testSwerveKFMovingOverTrajectory() {
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);
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 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 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;
filter.correct(u, measurement);
filter.predict(u, 0.020);
measurementsX.add(measurement.get(0, 0));
measurementsY.add(measurement.get(1, 0));
xhatsX.add(filter.getXhat(0));
xhatsY.add(filter.getXhat(1));
time += 0.020;
}
// 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) {
// }
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);
}
}

121
wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java Normal file
View File

@@ -0,0 +1,121 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import java.util.List;
import java.util.Random;
import org.junit.jupiter.api.Test;
public class MecanumDrivePoseEstimatorTest {
@Test
@SuppressWarnings({
"LocalVariableName",
"PMD.AvoidInstantiatingObjectsInLoops",
"PMD.ExcessiveMethodLength"
})
public void testAccuracy() {
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,
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.05),
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 rand = new Random(5190);
final double dt = 0.02;
double t = 0.0;
final double visionUpdateRate = 0.1;
Pose2d lastVisionPose = null;
double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
var groundTruthState = trajectory.sample(t);
if (lastVisionUpdateTime + visionUpdateRate < t) {
if (lastVisionPose != null) {
estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
}
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()));
lastVisionUpdateTime = t;
}
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);
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
errorSum += error;
t += dt;
}
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, "Incorrect mean error");
assertEquals(0.0, maxError, 0.42, "Incorrect max error");
}
}

43
wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java Normal file
View File

@@ -0,0 +1,43 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
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));
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));
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));
}
}

120
wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java Normal file
View File

@@ -0,0 +1,120 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import java.util.List;
import java.util.Random;
import org.junit.jupiter.api.Test;
public class SwerveDrivePoseEstimatorTest {
@Test
@SuppressWarnings({
"LocalVariableName",
"PMD.AvoidInstantiatingObjectsInLoops",
"PMD.ExcessiveMethodLength"
})
public void testAccuracy() {
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,
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.005),
VecBuilder.fill(0.1, 0.1, 0.1));
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));
var rand = new Random(4915);
final double dt = 0.02;
double t = 0.0;
final double visionUpdateRate = 0.1;
Pose2d lastVisionPose = null;
double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
var groundTruthState = trajectory.sample(t);
if (lastVisionUpdateTime + visionUpdateRate < t) {
if (lastVisionPose != null) {
estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
}
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()));
lastVisionUpdateTime = t;
}
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(
t,
groundTruthState
.poseMeters
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05)),
moduleStates);
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
errorSum += error;
t += dt;
}
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, "Incorrect mean error");
assertEquals(0.0, maxError, 0.42, "Incorrect max error");
}
}

447
wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java Normal file
View File

@@ -0,0 +1,447 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.estimator;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.math.Discretization;
import edu.wpi.first.math.math.StateSpaceUtil;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
import edu.wpi.first.math.numbers.N4;
import edu.wpi.first.math.numbers.N6;
import edu.wpi.first.math.system.NumericalIntegration;
import edu.wpi.first.math.system.NumericalJacobian;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import org.junit.jupiter.api.Test;
public class UnscentedKalmanFilterTest {
@SuppressWarnings({"LocalVariableName", "ParameterName"})
public static Matrix<N6, N1> getDynamics(Matrix<N6, N1> x, Matrix<N2, N1> u) {
var motors = DCMotor.getCIM(2);
var gHigh = 7.08;
var rb = 0.8382 / 2.0;
var r = 0.0746125;
var m = 63.503;
var J = 5.6;
var C1 =
-Math.pow(gHigh, 2)
* motors.KtNMPerAmp
/ (motors.KvRadPerSecPerVolt * motors.rOhms * r * r);
var C2 = gHigh * motors.KtNMPerAmp / (motors.rOhms * r);
var c = x.get(2, 0);
var s = x.get(3, 0);
var vl = x.get(4, 0);
var vr = x.get(5, 0);
var Vl = u.get(0, 0);
var Vr = u.get(1, 0);
var k1 = 1.0 / m + rb * rb / J;
var k2 = 1.0 / m - rb * rb / J;
var xvel = (vl + vr) / 2;
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)));
}
@SuppressWarnings("ParameterName")
public static Matrix<N4, N1> getLocalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0), x.get(5, 0));
}
@SuppressWarnings("ParameterName")
public static Matrix<N6, N1> getGlobalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
return x.copy();
}
@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);
var u = VecBuilder.fill(12.0, 12.0);
observer.predict(u, 0.00505);
var localY = getLocalMeasurementModel(observer.getXhat(), u);
observer.correct(u, localY);
});
}
@SuppressWarnings({
"LocalVariableName",
"PMD.AvoidInstantiatingObjectsInLoops",
"PMD.ExcessiveMethodLength"
})
@Test
public void testConvergence() {
double dtSeconds = 0.00505;
double rbMeters = 0.8382 / 2.0; // Robot radius
List<Double> trajXs = new ArrayList<>();
List<Double> trajYs = new ArrayList<>();
List<Double> observerXs = new ArrayList<>();
List<Double> observerYs = new ArrayList<>();
List<Double> observerC = new ArrayList<>();
List<Double> observerS = new ArrayList<>();
List<Double> observervl = new ArrayList<>();
List<Double> observervr = new ArrayList<>();
List<Double> inputVl = new ArrayList<>();
List<Double> inputVr = new ArrayList<>();
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);
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);
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)));
nextR = r.copy();
var trueXhat = observer.getXhat();
double totalTime = trajectory.getTotalTimeSeconds();
for (int i = 0; i < (totalTime / dtSeconds); i++) {
ref = trajectory.sample(dtSeconds * i);
double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
nextR.set(0, 0, ref.poseMeters.getTranslation().getX());
nextR.set(1, 0, ref.poseMeters.getTranslation().getY());
nextR.set(2, 0, ref.poseMeters.getRotation().getCos());
nextR.set(3, 0, ref.poseMeters.getRotation().getSin());
nextR.set(4, 0, vl);
nextR.set(5, 0, vr);
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)));
var rdot = nextR.minus(r).div(dtSeconds);
u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
rdots.add(rdot);
trajXs.add(ref.poseMeters.getTranslation().getX());
trajYs.add(ref.poseMeters.getTranslation().getY());
observerXs.add(observer.getXhat().get(0, 0));
observerYs.add(observer.getXhat().get(1, 0));
observerC.add(observer.getXhat(2));
observerS.add(observer.getXhat(3));
observervl.add(observer.getXhat(4));
observervr.add(observer.getXhat(5));
inputVl.add(u.get(0, 0));
inputVr.add(u.get(1, 0));
timeData.add(i * dtSeconds);
r = nextR;
observer.predict(u, dtSeconds);
trueXhat =
NumericalIntegration.rk4(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,
(sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)),
Matrix::minus,
Matrix::minus,
Matrix::plus);
final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
// var chartBuilder = new XYChartBuilder();
// chartBuilder.title = "The Magic of Sensor Fusion, now with a "
// + observer.getClass().getSimpleName();
// var xyPosChart = chartBuilder.build();
// xyPosChart.setXAxisTitle("X pos, meters");
// xyPosChart.setYAxisTitle("Y pos, meters");
// xyPosChart.addSeries("Trajectory", trajXs, trajYs);
// xyPosChart.addSeries("xHat", observerXs, observerYs);
// var stateChart = new XYChartBuilder()
// .title("States (x-hat)").build();
// stateChart.addSeries("Cos", timeData, observerC);
// stateChart.addSeries("Sin", timeData, observerS);
// stateChart.addSeries("vl, m/s", timeData, observervl);
// stateChart.addSeries("vr, m/s", timeData, observervr);
// var inputChart = new XYChartBuilder().title("Inputs").build();
// inputChart.addSeries("Left voltage", timeData, inputVl);
// inputChart.addSeries("Right voltage", timeData, inputVr);
// var rdotChart = new XYChartBuilder().title("Rdot").build();
// rdotChart.addSeries("xdot, or vx", timeData, rdots.stream().map(it -> it.get(0, 0))
// .collect(Collectors.toList()));
// rdotChart.addSeries("ydot, or vy", timeData, rdots.stream().map(it -> it.get(1, 0))
// .collect(Collectors.toList()));
// rdotChart.addSeries("cos dot", timeData, rdots.stream().map(it -> it.get(2, 0))
// .collect(Collectors.toList()));
// rdotChart.addSeries("sin dot", timeData, rdots.stream().map(it -> it.get(3, 0))
// .collect(Collectors.toList()));
// rdotChart.addSeries("vl dot, or al", timeData, rdots.stream().map(it -> it.get(4, 0))
// .collect(Collectors.toList()));
// rdotChart.addSeries("vr dot, or ar", timeData, rdots.stream().map(it -> it.get(5, 0))
// .collect(Collectors.toList()));
// List<XYChart> charts = new ArrayList<>();
// charts.add(xyPosChart);
// charts.add(stateChart);
// charts.add(inputChart);
// charts.add(rdotChart);
// new SwingWrapper<>(charts).displayChartMatrix();
// try {
// Thread.sleep(1000000000);
// } catch (InterruptedException ex) {
// ex.printStackTrace();
// }
assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.25);
assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.25);
assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0);
assertEquals(0.0, observer.getXhat(3), 1.0);
assertEquals(0.0, observer.getXhat(4), 1.0);
}
@Test
@SuppressWarnings({"LocalVariableName", "ParameterName", "PMD.AvoidInstantiatingObjectsInLoops"})
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 time = new ArrayList<Double>();
var refData = new ArrayList<Double>();
var xhat = new ArrayList<Double>();
var udata = new ArrayList<Double>();
var xdotData = new ArrayList<Double>();
var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dt);
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
Matrix<N1, N1> ref = VecBuilder.fill(100);
Matrix<N1, N1> u = VecBuilder.fill(0);
Matrix<N1, N1> xdot;
for (int i = 0; i < (2.0 / dt); i++) {
observer.predict(u, dt);
u = discB.solve(ref.minus(discA.times(ref)));
xdot = plant.getA().times(observer.getXhat()).plus(plant.getB().times(u));
time.add(i * dt);
refData.add(ref.get(0, 0));
xhat.add(observer.getXhat(0));
udata.add(u.get(0, 0));
xdotData.add(xdot.get(0, 0));
}
// var chartBuilder = new XYChartBuilder();
// chartBuilder.title = "The Magic of Sensor Fusion";
// var chart = chartBuilder.build();
// chart.addSeries("Ref", time, refData);
// chart.addSeries("xHat", time, xhat);
// chart.addSeries("input", time, udata);
//// chart.addSeries("xdot", time, xdotData);
// new SwingWrapper<>(chart).displayChart();
// try {
// Thread.sleep(1000000000);
// } catch (InterruptedException e) {
// }
assertEquals(ref.get(0, 0), observer.getXhat(0), 5);
}
@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);
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));
}
}

111
wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java Normal file
View File

@@ -0,0 +1,111 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.filter;
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;
private static final double kSinglePoleIIRTimeConstant = 0.015915;
private static final double kHighPassTimeConstant = 0.006631;
private static final int kMovAvgTaps = 6;
private static final double kSinglePoleIIRExpectedOutput = -3.2172003;
private static final double kHighPassExpectedOutput = 10.074717;
private static final double kMovAvgExpectedOutput = -10.191644;
private static double getData(double t) {
return 100.0 * Math.sin(2.0 * Math.PI * t) + 20.0 * Math.cos(50.0 * Math.PI * t);
}
private static double getPulseData(double t) {
if (Math.abs(t - 1.0) < 0.001) {
return 1.0;
} else {
return 0.0;
}
}
@Test
void illegalTapNumberTest() {
assertThrows(IllegalArgumentException.class, () -> LinearFilter.movingAverage(0));
}
/** Test if the filter reduces the noise produced by a signal generator. */
@ParameterizedTest
@MethodSource("noiseFilterProvider")
void noiseReduceTest(final LinearFilter filter) {
double noiseGenError = 0.0;
double filterError = 0.0;
final Random gen = new Random();
final double kStdDev = 10.0;
for (double t = 0; t < kFilterTime; t += kFilterStep) {
final double theory = getData(t);
final double noise = gen.nextGaussian() * kStdDev;
filterError += Math.abs(filter.calculate(theory + noise) - theory);
noiseGenError += Math.abs(noise - theory);
}
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));
}
/** 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) {
double filterOutput = 0.0;
for (double t = 0.0; t < kFilterTime; t += kFilterStep) {
filterOutput = filter.calculate(data.apply(t));
}
assertEquals(expectedOutput, filterOutput, 5e-5, "Filter output was incorrect.");
}
static Stream<Arguments> outputFilterProvider() {
return Stream.of(
arguments(
LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
(DoubleFunction<Double>) LinearFilterTest::getData,
kSinglePoleIIRExpectedOutput),
arguments(
LinearFilter.highPass(kHighPassTimeConstant, kFilterStep),
(DoubleFunction<Double>) LinearFilterTest::getData,
kHighPassExpectedOutput),
arguments(
LinearFilter.movingAverage(kMovAvgTaps),
(DoubleFunction<Double>) LinearFilterTest::getData,
kMovAvgExpectedOutput),
arguments(
LinearFilter.movingAverage(kMovAvgTaps),
(DoubleFunction<Double>) LinearFilterTest::getPulseData,
0.0));
}
}

61
wpimath/src/test/java/edu/wpi/first/math/filter/MedianFilterTest.java Normal file
View File

@@ -0,0 +1,61 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.filter;
import static org.junit.jupiter.api.Assertions.assertEquals;
import org.junit.jupiter.api.Test;
public class MedianFilterTest {
@Test
void medianFilterNotFullTestEven() {
MedianFilter filter = new MedianFilter(10);
filter.calculate(3);
filter.calculate(0);
filter.calculate(4);
assertEquals(3.5, filter.calculate(1000));
}
@Test
void medianFilterNotFullTestOdd() {
MedianFilter filter = new MedianFilter(10);
filter.calculate(3);
filter.calculate(0);
filter.calculate(4);
filter.calculate(7);
assertEquals(4, filter.calculate(1000));
}
@Test
void medianFilterFullTestEven() {
MedianFilter filter = new MedianFilter(6);
filter.calculate(3);
filter.calculate(0);
filter.calculate(0);
filter.calculate(5);
filter.calculate(4);
filter.calculate(1000);
assertEquals(4.5, filter.calculate(99));
}
@Test
void medianFilterFullTestOdd() {
MedianFilter filter = new MedianFilter(5);
filter.calculate(3);
filter.calculate(0);
filter.calculate(5);
filter.calculate(4);
filter.calculate(1000);
assertEquals(5, filter.calculate(99));
}
}

67
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java Normal file
View File

@@ -0,0 +1,67 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.geometry;
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 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));
}
@Test
void testRelativeTo() {
var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
var finalRelativeToInitial = last.relativeTo(initial);
assertAll(
() -> assertEquals(finalRelativeToInitial.getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
() -> assertEquals(finalRelativeToInitial.getY(), 0.0, kEpsilon),
() -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon));
}
@Test
void testEquality() {
var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
var two = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
assertEquals(one, two);
}
@Test
void testInequality() {
var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
var two = new Pose2d(0.0, 1.524, Rotation2d.fromDegrees(43.0));
assertNotEquals(one, two);
}
void testMinus() {
var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
final var transform = last.minus(initial);
assertAll(
() -> assertEquals(transform.getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
() -> assertEquals(transform.getY(), 0.0, kEpsilon),
() -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon));
}
}

79
wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java Normal file
View File

@@ -0,0 +1,79 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.geometry;
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;
@Test
void testRadiansToDegrees() {
var rot1 = new Rotation2d(Math.PI / 3);
var rot2 = new Rotation2d(Math.PI / 4);
assertAll(
() -> assertEquals(rot1.getDegrees(), 60.0, kEpsilon),
() -> assertEquals(rot2.getDegrees(), 45.0, kEpsilon));
}
@Test
void testRadiansAndDegrees() {
var rot1 = Rotation2d.fromDegrees(45.0);
var rot2 = Rotation2d.fromDegrees(30.0);
assertAll(
() -> assertEquals(rot1.getRadians(), Math.PI / 4, kEpsilon),
() -> assertEquals(rot2.getRadians(), Math.PI / 6, kEpsilon));
}
@Test
void testRotateByFromZero() {
var zero = new Rotation2d();
var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0));
assertAll(
() -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon),
() -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon));
}
@Test
void testRotateByNonZero() {
var rot = Rotation2d.fromDegrees(90.0);
rot = rot.plus(Rotation2d.fromDegrees(30.0));
assertEquals(rot.getDegrees(), 120.0, kEpsilon);
}
@Test
void testMinus() {
var rot1 = Rotation2d.fromDegrees(70.0);
var rot2 = Rotation2d.fromDegrees(30.0);
assertEquals(rot1.minus(rot2).getDegrees(), 40.0, kEpsilon);
}
@Test
void testEquality() {
var rot1 = Rotation2d.fromDegrees(43.0);
var rot2 = Rotation2d.fromDegrees(43.0);
assertEquals(rot1, rot2);
var rot3 = Rotation2d.fromDegrees(-180.0);
var rot4 = Rotation2d.fromDegrees(180.0);
assertEquals(rot3, rot4);
}
@Test
void testInequality() {
var rot1 = Rotation2d.fromDegrees(43.0);
var rot2 = Rotation2d.fromDegrees(43.5);
assertNotEquals(rot1, rot2);
}
}

32
wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java Normal file
View File

@@ -0,0 +1,32 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.geometry;
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 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));
}
}

117
wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java Normal file
View File

@@ -0,0 +1,117 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.geometry;
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;
@Test
void testSum() {
var one = new Translation2d(1.0, 3.0);
var two = new Translation2d(2.0, 5.0);
var sum = one.plus(two);
assertAll(
() -> assertEquals(sum.getX(), 3.0, kEpsilon),
() -> assertEquals(sum.getY(), 8.0, kEpsilon));
}
@Test
void testDifference() {
var one = new Translation2d(1.0, 3.0);
var two = new Translation2d(2.0, 5.0);
var difference = one.minus(two);
assertAll(
() -> assertEquals(difference.getX(), -1.0, kEpsilon),
() -> assertEquals(difference.getY(), -2.0, kEpsilon));
}
@Test
void testRotateBy() {
var another = new Translation2d(3.0, 0.0);
var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0));
assertAll(
() -> assertEquals(rotated.getX(), 0.0, kEpsilon),
() -> assertEquals(rotated.getY(), 3.0, kEpsilon));
}
@Test
void testMultiplication() {
var original = new Translation2d(3.0, 5.0);
var mult = original.times(3);
assertAll(
() -> assertEquals(mult.getX(), 9.0, kEpsilon),
() -> assertEquals(mult.getY(), 15.0, kEpsilon));
}
@Test
void testDivision() {
var original = new Translation2d(3.0, 5.0);
var div = original.div(2);
assertAll(
() -> assertEquals(div.getX(), 1.5, kEpsilon),
() -> assertEquals(div.getY(), 2.5, kEpsilon));
}
@Test
void testNorm() {
var one = new Translation2d(3.0, 5.0);
assertEquals(one.getNorm(), Math.hypot(3.0, 5.0), kEpsilon);
}
@Test
void testDistance() {
var one = new Translation2d(1, 1);
var two = new Translation2d(6, 6);
assertEquals(one.getDistance(two), 5 * Math.sqrt(2), kEpsilon);
}
@Test
void testUnaryMinus() {
var original = new Translation2d(-4.5, 7);
var inverted = original.unaryMinus();
assertAll(
() -> assertEquals(inverted.getX(), 4.5, kEpsilon),
() -> assertEquals(inverted.getY(), -7, kEpsilon));
}
@Test
void testEquality() {
var one = new Translation2d(9, 5.5);
var two = new Translation2d(9, 5.5);
assertEquals(one, two);
}
@Test
void testInequality() {
var one = new Translation2d(9, 5.5);
var two = new Translation2d(9, 5.7);
assertNotEquals(one, two);
}
@Test
void testPolarConstructor() {
var one = new Translation2d(Math.sqrt(2), Rotation2d.fromDegrees(45.0));
var two = new Translation2d(2, Rotation2d.fromDegrees(60.0));
assertAll(
() -> 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));
}
}

74
wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java Normal file
View File

@@ -0,0 +1,74 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.geometry;
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;
@Test
void testStraightLineTwist() {
var straight = new Twist2d(5.0, 0.0, 0.0);
var straightPose = new Pose2d().exp(straight);
assertAll(
() -> assertEquals(straightPose.getX(), 5.0, kEpsilon),
() -> assertEquals(straightPose.getY(), 0.0, kEpsilon),
() -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon));
}
@Test
void testQuarterCirleTwist() {
var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0);
var quarterCirclePose = new Pose2d().exp(quarterCircle);
assertAll(
() -> assertEquals(quarterCirclePose.getX(), 5.0, kEpsilon),
() -> assertEquals(quarterCirclePose.getY(), 5.0, kEpsilon),
() -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon));
}
@Test
void testDiagonalNoDtheta() {
var diagonal = new Twist2d(2.0, 2.0, 0.0);
var diagonalPose = new Pose2d().exp(diagonal);
assertAll(
() -> assertEquals(diagonalPose.getX(), 2.0, kEpsilon),
() -> assertEquals(diagonalPose.getY(), 2.0, kEpsilon),
() -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon));
}
@Test
void testEquality() {
var one = new Twist2d(5, 1, 3);
var two = new Twist2d(5, 1, 3);
assertEquals(one, two);
}
@Test
void testInequality() {
var one = new Twist2d(5, 1, 3);
var two = new Twist2d(5, 1.2, 3);
assertNotEquals(one, two);
}
void testPose2dLog() {
final var start = new Pose2d();
final var end = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0));
final var twist = start.log(end);
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));
}
}

26
wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java Normal file
View File

@@ -0,0 +1,26 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.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));
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
}

79
wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java Normal file
View File

@@ -0,0 +1,79 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
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);
@Test
void testInverseKinematicsForZeros() {
var chassisSpeeds = new ChassisSpeeds();
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
assertAll(
() -> assertEquals(0.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond, kEpsilon));
}
@Test
void testForwardKinematicsForZeros() {
var wheelSpeeds = new DifferentialDriveWheelSpeeds();
var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
@Test
void testInverseKinematicsForStraightLine() {
var chassisSpeeds = new ChassisSpeeds(3, 0, 0);
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
assertAll(
() -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
() -> assertEquals(3.0, wheelSpeeds.rightMetersPerSecond, kEpsilon));
}
@Test
void testForwardKinematicsForStraightLine() {
var wheelSpeeds = new DifferentialDriveWheelSpeeds(3, 3);
var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
assertAll(
() -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
@Test
void testInverseKinematicsForRotateInPlace() {
var chassisSpeeds = new ChassisSpeeds(0, 0, Math.PI);
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
assertAll(
() -> assertEquals(-0.381 * Math.PI, wheelSpeeds.leftMetersPerSecond, kEpsilon),
() -> assertEquals(+0.381 * Math.PI, wheelSpeeds.rightMetersPerSecond, kEpsilon));
}
@Test
void testForwardKinematicsForRotateInPlace() {
var wheelSpeeds = new DifferentialDriveWheelSpeeds(+0.381 * Math.PI, -0.381 * Math.PI);
var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(-Math.PI, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
}

29
wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java Normal file
View File

@@ -0,0 +1,29 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.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());
@Test
void testOdometryWithEncoderDistances() {
m_odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(45));
var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI);
assertAll(
() -> assertEquals(pose.getX(), 5.0, kEpsilon),
() -> assertEquals(pose.getY(), 5.0, kEpsilon),
() -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon));
}
}

178
wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java Normal file
View File

@@ -0,0 +1,178 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Translation2d;
import org.junit.jupiter.api.Test;
class MecanumDriveKinematicsTest {
private static final double kEpsilon = 1E-9;
private final Translation2d m_fl = new Translation2d(12, 12);
private final Translation2d m_fr = new Translation2d(12, -12);
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);
@Test
void testStraightLineInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
var moduleStates = m_kinematics.toWheelSpeeds(speeds);
assertAll(
() -> assertEquals(5.0, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(5.0, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(5.0, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(5.0, moduleStates.rearRightMetersPerSecond, 0.1));
}
@Test
void testStraightLineForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
assertAll(
() -> assertEquals(3.536, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
void testStrafeInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0);
var moduleStates = m_kinematics.toWheelSpeeds(speeds);
assertAll(
() -> assertEquals(-4.0, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(4.0, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(4.0, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(-4.0, moduleStates.rearRightMetersPerSecond, 0.1));
}
@Test
void testStrafeForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(-2.828427, 2.828427, 2.828427, -2.828427);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
assertAll(
() -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(2.8284, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
void testRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
var moduleStates = m_kinematics.toWheelSpeeds(speeds);
assertAll(
() -> assertEquals(-150.79645, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(150.79645, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(-150.79645, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(150.79645, moduleStates.rearRightMetersPerSecond, 0.1));
}
@Test
void testRotationForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(-150.79645, 150.79645, -150.79645, 150.79645);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
assertAll(
() -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(2 * Math.PI, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
void testMixedTranslationRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1);
var moduleStates = m_kinematics.toWheelSpeeds(speeds);
assertAll(
() -> assertEquals(-25.0, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(29.0, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(-19.0, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(23.0, moduleStates.rearRightMetersPerSecond, 0.1));
}
@Test
void testMixedTranslationRotationForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(-17.677670, 20.51, -13.44, 16.26);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
assertAll(
() -> assertEquals(1.413, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(2.122, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
void testOffCenterRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1);
var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
assertAll(
() -> assertEquals(0, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(24.0, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(-24.0, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(48.0, moduleStates.rearRightMetersPerSecond, 0.1));
}
@Test
void testOffCenterRotationForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(0, 16.971, -16.971, 33.941);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
assertAll(
() -> assertEquals(8.48525, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(-8.48525, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
void testOffCenterTranslationRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1);
var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
assertAll(
() -> assertEquals(3.0, moduleStates.frontLeftMetersPerSecond, 0.1),
() -> assertEquals(31.0, moduleStates.frontRightMetersPerSecond, 0.1),
() -> assertEquals(-17.0, moduleStates.rearLeftMetersPerSecond, 0.1),
() -> assertEquals(51.0, moduleStates.rearRightMetersPerSecond, 0.1));
}
@Test
void testOffCenterRotationTranslationForwardKinematicsKinematics() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(2.12, 21.92, -12.02, 36.06);
var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
assertAll(
() -> assertEquals(12.02, moduleStates.vxMetersPerSecond, 0.1),
() -> assertEquals(-7.07, moduleStates.vyMetersPerSecond, 0.1),
() -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1));
}
@Test
void testNormalize() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);
wheelSpeeds.normalize(5.5);
double factor = 5.5 / 7.0;
assertAll(
() -> 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));
}
}

83
wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java Normal file
View File

@@ -0,0 +1,83 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import org.junit.jupiter.api.Test;
class MecanumDriveOdometryTest {
private final Translation2d m_fl = new Translation2d(12, 12);
private final Translation2d m_fr = new Translation2d(12, -12);
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());
@Test
void testMultipleConsecutiveUpdates() {
var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
var secondPose = m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
assertAll(
() -> assertEquals(secondPose.getX(), 0.0, 0.01),
() -> assertEquals(secondPose.getY(), 0.0, 0.01),
() -> assertEquals(secondPose.getRotation().getDegrees(), 0.0, 0.01));
}
@Test
void testTwoIterations() {
// 5 units/sec in the x axis (forward)
final var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
assertAll(
() -> assertEquals(0.3536, pose.getX(), 0.01),
() -> assertEquals(0.0, pose.getY(), 0.01),
() -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01));
}
@Test
void test90degreeTurn() {
// This is a 90 degree turn about the point between front left and rear left wheels
// fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946
final var wheelSpeeds = new MecanumDriveWheelSpeeds(-13.328, 39.986, -13.329, 39.986);
m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
assertAll(
() -> assertEquals(8.4855, pose.getX(), 0.01),
() -> assertEquals(8.4855, pose.getY(), 0.01),
() -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01));
}
@Test
void testGyroAngleReset() {
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);
m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds());
var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
assertAll(
() -> assertEquals(3.536, pose.getX(), 0.1),
() -> assertEquals(0.0, pose.getY(), 0.1),
() -> assertEquals(0.0, pose.getRotation().getRadians(), 0.1));
}
}

253
wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java Normal file
View File

@@ -0,0 +1,253 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import org.junit.jupiter.api.Test;
class SwerveDriveKinematicsTest {
private static final double kEpsilon = 1E-9;
private final Translation2d m_fl = new Translation2d(12, 12);
private final Translation2d m_fr = new Translation2d(12, -12);
private final Translation2d m_bl = new Translation2d(-12, 12);
private final Translation2d m_br = new Translation2d(-12, -12);
private final SwerveDriveKinematics m_kinematics =
new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
@Test
void testStraightLineInverseKinematics() { // test inverse kinematics going in a straight line
ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
assertAll(
() -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
() -> 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));
}
@Test
void testStraightLineForwardKinematics() { // test forward kinematics going in a straight line
SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
@Test
void testStraightStrafeInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 5, 0);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
assertAll(
() -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
() -> 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));
}
@Test
void testStraightStrafeForwardKinematics() {
SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
}
@Test
void testTurnInPlaceInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
/*
The circumference of the wheels about the COR is pi * diameter, or 2 * pi * radius
the radius is the sqrt(12^2in + 12^2in), or 16.9706in, so the circumference the wheels
trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second,
our wheels must trace out 1 rotation (or 106.63 inches) per second.
*/
assertAll(
() -> assertEquals(106.63, moduleStates[0].speedMetersPerSecond, 0.1),
() -> assertEquals(106.63, moduleStates[1].speedMetersPerSecond, 0.1),
() -> assertEquals(106.63, moduleStates[2].speedMetersPerSecond, 0.1),
() -> assertEquals(106.63, moduleStates[3].speedMetersPerSecond, 0.1),
() -> 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));
}
@Test
void testTurnInPlaceForwardKinematics() {
SwerveModuleState flState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(135));
SwerveModuleState frState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(45));
SwerveModuleState blState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-135));
SwerveModuleState brState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-45));
var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1));
}
@Test
void testOffCenterCORRotationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds, m_fl);
/*
This one is a bit trickier. Because we are rotating about the front-left wheel,
it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel
an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with
radius sqrt(24^2 + 24^2) and circumference 213.2584. As for angles, the front-right wheel
should be pointing straight forward, the back-left wheel should be pointing straight right,
and the back-right wheel should be at a -45 degree angle
*/
assertAll(
() -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, 0.1),
() -> assertEquals(150.796, moduleStates[1].speedMetersPerSecond, 0.1),
() -> assertEquals(150.796, moduleStates[2].speedMetersPerSecond, 0.1),
() -> assertEquals(213.258, moduleStates[3].speedMetersPerSecond, 0.1),
() -> 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));
}
@Test
void testOffCenterCORRotationForwardKinematics() {
SwerveModuleState flState = new SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0));
SwerveModuleState frState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(0.0));
SwerveModuleState blState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(-90));
SwerveModuleState brState = new SwerveModuleState(213.258, Rotation2d.fromDegrees(-45));
var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
/*
We already know that our omega should be 2pi from the previous test. Next, we need to determine
the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center,
we know that vx and vy must be the same. Furthermore, we know that the center of mass makes
a full revolution about the center of revolution once every second. Therefore, the center of
mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triagle are
1:sqrt(2)/2:sqrt(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
*/
assertAll(
() -> assertEquals(75.398, chassisSpeeds.vxMetersPerSecond, 0.1),
() -> assertEquals(-75.398, chassisSpeeds.vyMetersPerSecond, 0.1),
() -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1));
}
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()));
}
/**
* Test the rotation of the robot about a non-central point with both linear and angular
* velocities.
*/
@Test
void testOffCenterCORRotationAndTranslationInverseKinematics() {
ChassisSpeeds speeds = new ChassisSpeeds(0.0, 3.0, 1.5);
var moduleStates = m_kinematics.toSwerveModuleStates(speeds, new Translation2d(24, 0));
// 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))
};
var stateTolerance = new SwerveModuleState(0.1, Rotation2d.fromDegrees(0.1));
for (int i = 0; i < expectedStates.length; i++) {
assertModuleState(expectedStates[i], moduleStates[i], stateTolerance);
}
}
@Test
void testOffCenterCORRotationAndTranslationForwardKinematics() {
SwerveModuleState flState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19));
SwerveModuleState frState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81));
SwerveModuleState blState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44));
SwerveModuleState brState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56));
var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
/*
From equation (13.17), we know that chassis motion is th dot product of the
pseudoinverse of the inverseKinematics matrix (with the center of rotation at
(0,0) -- we don't want the motion of the center of rotation, we want it of
the center of the robot). These above SwerveModuleStates are known to be from
a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been
calculated using Numpy's linalg.pinv function.
*/
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, 0.1),
() -> assertEquals(-33.0, chassisSpeeds.vyMetersPerSecond, 0.1),
() -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond, 0.1));
}
@Test
void testNormalize() {
SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d());
SwerveModuleState bl = new SwerveModuleState(4, new Rotation2d());
SwerveModuleState br = new SwerveModuleState(7, new Rotation2d());
SwerveModuleState[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.normalizeWheelSpeeds(arr, 5.5);
double factor = 5.5 / 7.0;
assertAll(
() -> 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));
}
}

92
wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java Normal file
View File

@@ -0,0 +1,92 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import org.junit.jupiter.api.Test;
class SwerveDriveOdometryTest {
private final Translation2d m_fl = new Translation2d(12, 12);
private final Translation2d m_fr = new Translation2d(12, -12);
private final Translation2d m_bl = new Translation2d(-12, 12);
private final Translation2d m_br = new Translation2d(-12, -12);
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());
@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))
};
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));
}
@Test
void test90degreeTurn() {
// This is a 90 degree turn about the point between front left and rear left wheels
// Module 0: speed 18.84955592153876 angle 90.0
// Module 1: speed 42.14888838624436 angle 26.565051177077986
// Module 2: speed 18.84955592153876 angle -90.0
// 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))
};
final var zero = new SwerveModuleState();
m_odometry.updateWithTime(0.0, new Rotation2d(), zero, zero, zero, zero);
final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
assertAll(
() -> assertEquals(12.0, pose.getX(), 0.01),
() -> assertEquals(12.0, pose.getY(), 0.01),
() -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01));
}
@Test
void testGyroAngleReset() {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
var state = new SwerveModuleState();
m_odometry.updateWithTime(0.0, gyro, state, state, state, state);
state = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0));
var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state);
assertAll(
() -> assertEquals(1.0, pose.getX(), 0.1),
() -> assertEquals(0.00, pose.getY(), 0.1),
() -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1));
}
}

53
wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java Normal file
View File

@@ -0,0 +1,53 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.kinematics;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Rotation2d;
import org.junit.jupiter.api.Test;
class SwerveModuleStateTest {
private static final double kEpsilon = 1E-9;
@Test
void testOptimize() {
var angleA = Rotation2d.fromDegrees(45);
var refA = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(180));
var optimizedA = SwerveModuleState.optimize(refA, angleA);
assertAll(
() -> assertEquals(2.0, optimizedA.speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, optimizedA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.fromDegrees(-50);
var refB = new SwerveModuleState(4.7, Rotation2d.fromDegrees(41));
var optimizedB = SwerveModuleState.optimize(refB, angleB);
assertAll(
() -> assertEquals(-4.7, optimizedB.speedMetersPerSecond, kEpsilon),
() -> assertEquals(-139.0, optimizedB.angle.getDegrees(), kEpsilon));
}
@Test
void testNoOptimize() {
var angleA = Rotation2d.fromDegrees(0);
var refA = new SwerveModuleState(2.0, Rotation2d.fromDegrees(89));
var optimizedA = SwerveModuleState.optimize(refA, angleA);
assertAll(
() -> assertEquals(2.0, optimizedA.speedMetersPerSecond, kEpsilon),
() -> assertEquals(89.0, optimizedA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.fromDegrees(0);
var refB = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(-2));
var optimizedB = SwerveModuleState.optimize(refB, angleB);
assertAll(
() -> assertEquals(-2.0, optimizedB.speedMetersPerSecond, kEpsilon),
() -> assertEquals(-2.0, optimizedB.angle.getDegrees(), kEpsilon));
}
}

194
wpimath/src/test/java/edu/wpi/first/math/math/StateSpaceUtilTest.java Normal file
View File

@@ -0,0 +1,194 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.math;
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.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.SimpleMatrixUtils;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.numbers.N1;
import edu.wpi.first.math.numbers.N2;
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));
assertEquals(1.0, mat.get(0, 0), 1e-3);
assertEquals(0.0, mat.get(0, 1), 1e-3);
assertEquals(0.0, mat.get(0, 2), 1e-3);
assertEquals(0.0, mat.get(1, 0), 1e-3);
assertEquals(1.0 / 4.0, mat.get(1, 1), 1e-3);
assertEquals(0.0, mat.get(1, 2), 1e-3);
assertEquals(0.0, mat.get(0, 2), 1e-3);
assertEquals(0.0, mat.get(1, 2), 1e-3);
assertEquals(1.0 / 9.0, mat.get(2, 2), 1e-3);
}
@Test
public void testCovArray() {
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);
assertEquals(0.0, mat.get(0, 2), 1e-3);
assertEquals(0.0, mat.get(1, 0), 1e-3);
assertEquals(4.0, mat.get(1, 1), 1e-3);
assertEquals(0.0, mat.get(1, 2), 1e-3);
assertEquals(0.0, mat.get(0, 2), 1e-3);
assertEquals(0.0, mat.get(1, 2), 1e-3);
assertEquals(9.0, mat.get(2, 2), 1e-3);
}
@Test
@SuppressWarnings("LocalVariableName")
public void testIsStabilizable() {
Matrix<N2, N2> A;
Matrix<N2, N1> B = VecBuilder.fill(0, 1);
// First eigenvalue is uncontrollable and unstable.
// Second eigenvalue is controllable and stable.
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5);
assertFalse(StateSpaceUtil.isStabilizable(A, B));
// First eigenvalue is uncontrollable and marginally stable.
// Second eigenvalue is controllable and stable.
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5);
assertFalse(StateSpaceUtil.isStabilizable(A, B));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and stable.
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5);
assertTrue(StateSpaceUtil.isStabilizable(A, B));
// First eigenvalue is uncontrollable and stable.
// Second eigenvalue is controllable and unstable.
A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2);
assertTrue(StateSpaceUtil.isStabilizable(A, B));
}
@Test
public void testMakeWhiteNoiseVector() {
var firstData = new ArrayList<Double>();
var secondData = new ArrayList<Double>();
for (int i = 0; i < 1000; i++) {
var noiseVec = StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(1.0, 2.0));
firstData.add(noiseVec.get(0, 0));
secondData.add(noiseVec.get(1, 0));
}
assertEquals(1.0, calculateStandardDeviation(firstData), 0.2);
assertEquals(2.0, calculateStandardDeviation(secondData), 0.2);
}
private double calculateStandardDeviation(List<Double> numArray) {
double sum = 0.0;
double standardDeviation = 0.0;
int length = numArray.size();
for (double num : numArray) {
sum += num;
}
double mean = sum / length;
for (double num : numArray) {
standardDeviation += Math.pow(num - mean, 2);
}
return Math.sqrt(standardDeviation / length);
}
@Test
public void testDiscretizeA() {
var contA = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
var x0 = VecBuilder.fill(1, 1);
var discA = Discretization.discretizeA(contA, 1.0);
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));
assertTrue(x1Truth.isEqual(x1Discrete, 1E-4));
}
@SuppressWarnings("LocalVariableName")
@Test
public void testDiscretizeAB() {
var contA = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
var contB = VecBuilder.fill(0, 1);
var x0 = VecBuilder.fill(1, 1);
var u = VecBuilder.fill(1);
var abPair = Discretization.discretizeAB(contA, contB, 1.0);
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));
assertTrue(x1Truth.isEqual(x1Discrete, 1E-4));
}
@Test
public void testMatrixExp() {
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));
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));
}
@Test
public void testSimpleMatrixExp() {
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));
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));
}
@Test
public void testPoseToVector() {
Pose2d pose = new Pose2d(1, 2, new Rotation2d(3));
var vector = StateSpaceUtil.poseToVector(pose);
assertEquals(pose.getTranslation().getX(), vector.get(0, 0), 1e-6);
assertEquals(pose.getTranslation().getY(), vector.get(1, 0), 1e-6);
assertEquals(pose.getRotation().getRadians(), vector.get(2, 0), 1e-6);
}
}

162
wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java Normal file
View File

@@ -0,0 +1,162 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.spline;
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.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.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;
private static final double kMaxDy = 0.00127;
private static final double kMaxDtheta = 0.0872;
@SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"})
private void run(Pose2d a, List<Translation2d> waypoints, Pose2d b) {
// Start the timer.
// 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]);
var poses = new ArrayList<PoseWithCurvature>();
poses.add(splines[0].getPoint(0.0));
for (var spline : splines) {
poses.addAll(SplineParameterizer.parameterize(spline));
}
// End the timer.
// var end = System.nanoTime();
// Calculate the duration (used when benchmarking)
// var durationMicroseconds = (end - start) / 1000.0;
for (int i = 0; i < poses.size() - 1; i++) {
var p0 = poses.get(i);
var p1 = poses.get(i + 1);
// Make sure the twist is under the tolerance defined by the Spline class.
var twist = p0.poseMeters.log(p1.poseMeters);
assertAll(
() -> assertTrue(Math.abs(twist.dx) < kMaxDx),
() -> assertTrue(Math.abs(twist.dy) < kMaxDy),
() -> 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));
// Check interior waypoints
boolean interiorsGood = true;
for (var waypoint : waypoints) {
boolean found = false;
for (var state : poses) {
if (waypoint.getDistance(state.poseMeters.getTranslation()) == 0) {
found = true;
}
}
interiorsGood &= found;
}
assertTrue(interiorsGood);
// 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));
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testStraightLine() {
run(new Pose2d(), new ArrayList<>(), new Pose2d(3, 0, new Rotation2d()));
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testSCurve() {
var start = new Pose2d(0, 0, Rotation2d.fromDegrees(90.0));
ArrayList<Translation2d> waypoints = new ArrayList<>();
waypoints.add(new Translation2d(1, 1));
waypoints.add(new Translation2d(2, -1));
var end = new Pose2d(3, 0, Rotation2d.fromDegrees(90.0));
run(start, waypoints, end);
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testOneInterior() {
var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
ArrayList<Translation2d> waypoints = new ArrayList<>();
waypoints.add(new Translation2d(2.0, 0.0));
var end = new Pose2d(4, 0, Rotation2d.fromDegrees(0.0));
run(start, waypoints, end);
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testWindyPath() {
final var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
final ArrayList<Translation2d> waypoints = new ArrayList<>();
waypoints.add(new Translation2d(0.5, 0.5));
waypoints.add(new Translation2d(0.5, 0.5));
waypoints.add(new Translation2d(1.0, 0.0));
waypoints.add(new Translation2d(1.5, 0.5));
waypoints.add(new Translation2d(2.0, 0.0));
waypoints.add(new Translation2d(2.5, 0.5));
final var end = new Pose2d(3.0, 0.0, Rotation2d.fromDegrees(0.0));
run(start, waypoints, end);
}
@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))));
}
}

106
wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java Normal file
View File

@@ -0,0 +1,106 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.spline;
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.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.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"})
private void run(Pose2d a, Pose2d b) {
// Start the timer.
// 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();
// Calculate the duration (used when benchmarking)
// var durationMicroseconds = (end - start) / 1000.0;
for (int i = 0; i < poses.size() - 1; i++) {
var p0 = poses.get(i);
var p1 = poses.get(i + 1);
// Make sure the twist is under the tolerance defined by the Spline class.
var twist = p0.poseMeters.log(p1.poseMeters);
assertAll(
() -> assertTrue(Math.abs(twist.dx) < kMaxDx),
() -> assertTrue(Math.abs(twist.dy) < kMaxDy),
() -> 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));
// 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));
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testStraightLine() {
run(new Pose2d(), new Pose2d(3, 0, new Rotation2d()));
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testSimpleSCurve() {
run(new Pose2d(), new Pose2d(1, 1, new Rotation2d()));
}
@SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
@Test
void testSquiggly() {
run(
new Pose2d(0, 0, Rotation2d.fromDegrees(90)),
new Pose2d(-1, 0, Rotation2d.fromDegrees(90)));
}
@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))));
}
}

92
wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java Normal file
View File

@@ -0,0 +1,92 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.system;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.system.plant.DCMotor;
import edu.wpi.first.math.system.plant.LinearSystemId;
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));
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.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.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.getD().isEqual(VecBuilder.fill(0), 0.001));
}
@Test
public void testFlywheelSystem() {
var model = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00032, 1.0);
assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001));
assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001));
assertTrue(model.getC().isEqual(VecBuilder.fill(1), 0.001));
assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
}
@Test
public void testIdentifyPositionSystem() {
// By controls engineering in frc,
// x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u
var kv = 1.0;
var ka = 0.5;
var model = LinearSystemId.identifyPositionSystem(kv, ka);
assertEquals(model.getA(), Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kv / ka));
assertEquals(model.getB(), VecBuilder.fill(0, 1 / ka));
}
@Test
public void testIdentifyVelocitySystem() {
// By controls engineering in frc,
// V = kv * velocity + ka * acceleration
// x-dot = -kv/ka * v + 1/ka \cdot V
var kv = 1.0;
var ka = 0.5;
var model = LinearSystemId.identifyVelocitySystem(kv, ka);
assertEquals(model.getA(), VecBuilder.fill(-kv / ka));
assertEquals(model.getB(), VecBuilder.fill(1 / ka));
}
}

56
wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java Normal file
View File

@@ -0,0 +1,56 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.system;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.Matrix;
import edu.wpi.first.math.Nat;
import edu.wpi.first.math.VecBuilder;
import edu.wpi.first.math.numbers.N1;
import org.junit.jupiter.api.Test;
public class NumericalIntegrationTest {
@Test
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public void testExponential() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
//noinspection SuspiciousNameCombination
var y1 =
NumericalIntegration.rk4(
(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);
}
@Test
@SuppressWarnings({"ParameterName", "LocalVariableName"})
public void testExponentialAdaptive() {
Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
//noinspection SuspiciousNameCombination
var y1 =
NumericalIntegration.rkf45(
(x, u) -> {
var y = new Matrix<>(Nat.N1(), Nat.N1());
y.set(0, 0, Math.exp(x.get(0, 0)));
return y;
},
y0,
VecBuilder.fill(0),
0.1);
assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
}
}

37
wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java Normal file
View File

@@ -0,0 +1,37 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.trajectory;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint;
import edu.wpi.first.math.util.Units;
import java.util.Collections;
import org.junit.jupiter.api.Test;
class CentripetalAccelerationConstraintTest {
@SuppressWarnings("LocalVariableName")
@Test
void testCentripetalAccelerationConstraint() {
double maxCentripetalAcceleration = Units.feetToMeters(7.0); // 7 feet per second squared
var constraint = new CentripetalAccelerationConstraint(maxCentripetalAcceleration);
Trajectory trajectory =
TrajectoryGeneratorTest.getTrajectory(Collections.singletonList(constraint));
var duration = trajectory.getTotalTimeSeconds();
var t = 0.0;
var dt = 0.02;
while (t < duration) {
var point = trajectory.sample(t);
var centripetalAcceleration =
Math.pow(point.velocityMetersPerSecond, 2) * point.curvatureRadPerMeter;
t += dt;
assertTrue(centripetalAcceleration <= maxCentripetalAcceleration + 0.05);
}
}
}

48
wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java Normal file
View File

@@ -0,0 +1,48 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.trajectory;
import static org.junit.jupiter.api.Assertions.assertAll;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.trajectory.constraint.DifferentialDriveKinematicsConstraint;
import edu.wpi.first.math.util.Units;
import java.util.Collections;
import org.junit.jupiter.api.Test;
class DifferentialDriveKinematicsConstraintTest {
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
@Test
void testDifferentialDriveKinematicsConstraint() {
double maxVelocity = Units.feetToMeters(12.0); // 12 feet per second
var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(27));
var constraint = new DifferentialDriveKinematicsConstraint(kinematics, maxVelocity);
Trajectory trajectory =
TrajectoryGeneratorTest.getTrajectory(Collections.singletonList(constraint));
var duration = trajectory.getTotalTimeSeconds();
var t = 0.0;
var dt = 0.02;
while (t < duration) {
var point = trajectory.sample(t);
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));
}
}
}

105
wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java Normal file
View File

@@ -0,0 +1,105 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.trajectory;
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.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
import java.util.ArrayList;
import java.util.Collections;
import org.junit.jupiter.api.Test;
class DifferentialDriveVoltageConstraintTest {
@SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
@Test
void testDifferentialDriveVoltageConstraint() {
// Pick an unreasonably large kA to ensure the constraint has to do some work
var feedforward = new SimpleMotorFeedforward(1, 1, 3);
var kinematics = new DifferentialDriveKinematics(0.5);
double maxVoltage = 10;
var constraint = new DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage);
Trajectory trajectory =
TrajectoryGeneratorTest.getTrajectory(Collections.singletonList(constraint));
var duration = trajectory.getTotalTimeSeconds();
var t = 0.0;
var dt = 0.02;
while (t < duration) {
var point = trajectory.sample(t);
var chassisSpeeds =
new ChassisSpeeds(
point.velocityMetersPerSecond,
0,
point.velocityMetersPerSecond * point.curvatureRadPerMeter);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
t += dt;
// 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));
}
}
@Test
void testEndpointHighCurvature() {
var feedforward = new SimpleMotorFeedforward(1, 1, 3);
// 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 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)));
}
}

80
wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java Normal file
View File

@@ -0,0 +1,80 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.trajectory;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint;
import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint;
import edu.wpi.first.math.util.Units;
import java.util.List;
import org.junit.jupiter.api.Test;
public class EllipticalRegionConstraintTest {
@Test
void testConstraint() {
// 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);
// Get trajectory
var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint));
// Iterate through trajectory and check constraints
boolean exceededConstraintOutsideRegion = false;
for (var point : trajectory.getStates()) {
var translation = point.poseMeters.getTranslation();
if (translation.getX() < Units.feetToMeters(10)
&& translation.getY() < Units.feetToMeters(5)) {
assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05);
} else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) {
exceededConstraintOutsideRegion = true;
}
}
assertTrue(exceededConstraintOutsideRegion);
}
@Test
void testIsPoseWithinRegion() {
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);
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);
assertTrue(
regionConstraintWithRotation.isPoseInRegion(
new Pose2d(Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d())));
}
}

61
wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java Normal file
View File

@@ -0,0 +1,61 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.trajectory;
import static org.junit.jupiter.api.Assertions.assertFalse;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint;
import edu.wpi.first.math.trajectory.constraint.RectangularRegionConstraint;
import edu.wpi.first.math.util.Units;
import java.util.List;
import org.junit.jupiter.api.Test;
public class RectangularRegionConstraintTest {
@Test
void testConstraint() {
// 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);
// Get trajectory
var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint));
// Iterate through trajectory and check constraints
boolean exceededConstraintOutsideRegion = false;
for (var point : trajectory.getStates()) {
if (regionConstraint.isPoseInRegion(point.poseMeters)) {
assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05);
} else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) {
exceededConstraintOutsideRegion = true;
}
}
assertTrue(exceededConstraintOutsideRegion);
}
@Test
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);
assertFalse(regionConstraint.isPoseInRegion(new Pose2d()));
assertTrue(
regionConstraint.isPoseInRegion(
new Pose2d(Units.feetToMeters(3.0), Units.feetToMeters(14.5), new Rotation2d())));
}
}

52
wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryConcatenateTest.java Normal file
View File

@@ -0,0 +1,52 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.trajectory;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import java.util.List;
import org.junit.jupiter.api.Test;
class TrajectoryConcatenateTest {
@Test
void testStates() {
var t1 =
TrajectoryGenerator.generateTrajectory(
new Pose2d(),
List.of(),
new Pose2d(1, 1, new Rotation2d()),
new TrajectoryConfig(2, 2));
var t2 =
TrajectoryGenerator.generateTrajectory(
new Pose2d(1, 1, new Rotation2d()),
List.of(),
new Pose2d(2, 2, Rotation2d.fromDegrees(45)),
new TrajectoryConfig(2, 2));
var t = t1.concatenate(t2);
double time = -1.0;
for (int i = 0; i < t.getStates().size(); ++i) {
var state = t.getStates().get(i);
// Make sure that the timestamps are strictly increasing.
assertTrue(state.timeSeconds > time);
time = state.timeSeconds;
// Ensure that the states in t are the same as those in t1 and t2.
if (i < t1.getStates().size()) {
assertEquals(state, t1.getStates().get(i));
} else {
var st = t2.getStates().get(i - t1.getStates().size() + 1);
st.timeSeconds += t1.getTotalTimeSeconds();
assertEquals(state, st);
}
}
}
}

84
wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java Normal file
View File

@@ -0,0 +1,84 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.trajectory;
import static edu.wpi.first.math.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.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
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) {
final double maxVelocity = feetToMeters(12.0);
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 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(crossScale);
TrajectoryConfig config =
new TrajectoryConfig(maxVelocity, maxAccel).setReversed(true).addConstraints(constraints);
return TrajectoryGenerator.generateTrajectory(waypoints, config);
}
@Test
@SuppressWarnings("LocalVariableName")
void testGenerationAndConstraints() {
Trajectory trajectory = getTrajectory(new ArrayList<>());
double duration = trajectory.getTotalTimeSeconds();
double t = 0.0;
double dt = 0.02;
while (t < duration) {
var point = trajectory.sample(t);
t += dt;
assertAll(
() -> assertTrue(Math.abs(point.velocityMetersPerSecond) < feetToMeters(12.0) + 0.05),
() ->
assertTrue(
Math.abs(point.accelerationMetersPerSecondSq) < feetToMeters(12.0) + 0.05));
}
}
@Test
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)));
assertEquals(traj.getStates().size(), 1);
assertEquals(traj.getTotalTimeSeconds(), 0);
}
}

30
wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryJsonTest.java Normal file
View File

@@ -0,0 +1,30 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.trajectory;
import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.trajectory.constraint.DifferentialDriveKinematicsConstraint;
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 trajectory = TrajectoryGeneratorTest.getTrajectory(config);
var deserialized =
assertDoesNotThrow(
() ->
TrajectoryUtil.deserializeTrajectory(
TrajectoryUtil.serializeTrajectory(trajectory)));
assertEquals(trajectory.getStates(), deserialized.getStates());
}
}

64
wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryTransformTest.java Normal file
View File

@@ -0,0 +1,64 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.trajectory;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.math.geometry.Translation2d;
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 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);
testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
}
@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 transformedTrajectory = trajectory.relativeTo(new Pose2d(1, 2, Rotation2d.fromDegrees(30)));
// Test initial pose.
assertEquals(new Pose2d(), transformedTrajectory.sample(0).poseMeters);
testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
}
void testSameShapedTrajectory(List<Trajectory.State> statesA, List<Trajectory.State> statesB) {
for (int i = 0; i < statesA.size() - 1; i++) {
var a1 = statesA.get(i).poseMeters;
var a2 = statesA.get(i + 1).poseMeters;
var b1 = statesB.get(i).poseMeters;
var b2 = statesB.get(i + 1).poseMeters;
assertEquals(a2.relativeTo(a1), b2.relativeTo(b1));
}
}
}

254
wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java Normal file
View File

@@ -0,0 +1,254 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.trajectory;
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;
/**
* Asserts "val1" is less than or equal to "val2".
*
* @param val1 First operand in comparison.
* @param val2 Second operand in comparison.
*/
private static void assertLessThanOrEquals(double val1, double val2) {
assertTrue(val1 <= val2, val1 + " is greater than " + val2);
}
/**
* Asserts "val1" is within "eps" of "val2".
*
* @param val1 First operand in comparison.
* @param val2 Second operand in comparison.
* @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);
}
/**
* Asserts "val1" is less than or within "eps" of "val2".
*
* @param val1 First operand in comparison.
* @param val2 Second operand in comparison.
* @param eps Tolerance for whether values are near to each other.
*/
private static void assertLessThanOrNear(double val1, double val2, double eps) {
if (val1 <= val2) {
assertLessThanOrEquals(val1, val2);
} else {
assertNear(val1, val2, eps);
}
}
@Test
void reachesGoal() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
for (int i = 0; i < 450; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
}
assertEquals(state, goal);
}
// Tests that decreasing the maximum velocity in the middle when it is already
// moving faster than the new max is handled correctly
@Test
void posContinousUnderVelChange() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
double lastPos = state.position;
for (int i = 0; i < 1600; ++i) {
if (i == 400) {
constraints.maxVelocity = 0.75;
}
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
double estimatedVel = (state.position - lastPos) / kDt;
if (i >= 400) {
// Since estimatedVel can have floating point rounding errors, we check
// whether value is less than or within an error delta of the new
// constraint.
assertLessThanOrNear(estimatedVel, constraints.maxVelocity, 1e-4);
assertLessThanOrEquals(state.velocity, constraints.maxVelocity);
}
lastPos = state.position;
}
assertEquals(state, goal);
}
// There is some somewhat tricky code for dealing with going backwards
@Test
void backwards() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
for (int i = 0; i < 400; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
}
assertEquals(state, goal);
}
@Test
void switchGoalInMiddle() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
for (int i = 0; i < 200; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
}
assertNotEquals(state, goal);
goal = new TrapezoidProfile.State(0.0, 0.0);
for (int i = 0; i < 550; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
}
assertEquals(state, goal);
}
// Checks to make sure that it hits top speed
@Test
void topSpeed() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
for (int i = 0; i < 200; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
}
assertNear(constraints.maxVelocity, state.velocity, 10e-5);
for (int i = 0; i < 2000; ++i) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
}
assertEquals(state, goal);
}
@Test
void timingToCurrent() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
TrapezoidProfile.State state = new TrapezoidProfile.State();
for (int i = 0; i < 400; i++) {
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
}
}
@Test
void timingToGoal() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
double predictedTimeLeft = profile.timeLeftUntil(goal.position);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
if (!reachedGoal && state.equals(goal)) {
// Expected value using for loop index is just an approximation since
// the time left in the profile doesn't increase linearly at the
// endpoints
assertNear(predictedTimeLeft, i / 100.0, 0.25);
reachedGoal = true;
}
}
}
@Test
void timingBeforeGoal() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
double predictedTimeLeft = profile.timeLeftUntil(1);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
if (!reachedGoal && (Math.abs(state.velocity - 1) < 10e-5)) {
assertNear(predictedTimeLeft, i / 100.0, 2e-2);
reachedGoal = true;
}
}
}
@Test
void timingToNegativeGoal() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
double predictedTimeLeft = profile.timeLeftUntil(goal.position);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
if (!reachedGoal && state.equals(goal)) {
// Expected value using for loop index is just an approximation since
// the time left in the profile doesn't increase linearly at the
// endpoints
assertNear(predictedTimeLeft, i / 100.0, 0.25);
reachedGoal = true;
}
}
}
@Test
void timingBeforeNegativeGoal() {
TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
TrapezoidProfile.State state = profile.calculate(kDt);
double predictedTimeLeft = profile.timeLeftUntil(-1);
boolean reachedGoal = false;
for (int i = 0; i < 400; i++) {
profile = new TrapezoidProfile(constraints, goal, state);
state = profile.calculate(kDt);
if (!reachedGoal && (Math.abs(state.velocity + 1) < 10e-5)) {
assertNear(predictedTimeLeft, i / 100.0, 2e-2);
reachedGoal = true;
}
}
}
}

66
wpimath/src/test/java/edu/wpi/first/math/util/UnitsTest.java Normal file
View File

@@ -0,0 +1,66 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.math.util;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.wpilibj.UtilityClassTest;
import org.junit.jupiter.api.Test;
class UnitsTest extends UtilityClassTest<Units> {
UnitsTest() {
super(Units.class);
}
@Test
void metersToFeetTest() {
assertEquals(3.28, Units.metersToFeet(1), 1e-2);
}
@Test
void feetToMetersTest() {
assertEquals(0.30, Units.feetToMeters(1), 1e-2);
}
@Test
void metersToInchesTest() {
assertEquals(39.37, Units.metersToInches(1), 1e-2);
}
@Test
void inchesToMetersTest() {
assertEquals(0.0254, Units.inchesToMeters(1), 1e-3);
}
@Test
void degreesToRadiansTest() {
assertEquals(0.017, Units.degreesToRadians(1), 1e-3);
}
@Test
void radiansToDegreesTest() {
assertEquals(114.59, Units.radiansToDegrees(2), 1e-2);
}
@Test
void rotationsPerMinuteToRadiansPerSecondTest() {
assertEquals(6.28, Units.rotationsPerMinuteToRadiansPerSecond(60), 1e-2);
}
@Test
void radiansPerSecondToRotationsPerMinute() {
assertEquals(76.39, Units.radiansPerSecondToRotationsPerMinute(8), 1e-2);
}
@Test
void kilogramsToLbsTest() {
assertEquals(2.20462, Units.kilogramsToLbs(1), 1e-2);
}
@Test
void lbsToKilogramsTest() {
assertEquals(0.453592, Units.lbsToKilograms(1), 1e-2);
}
}