Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Remove unit suffixes from variable names (#7529)

Browse Source 
* Move units into API docs instead because suffixes make user code verbose and hard to read
* Rename trackWidth to trackwidth
* Make ultrasonic classes use meters instead of a mix of m, cm, mm, ft,
  and inches
This commit is contained in:
Tyler Veness
2025-02-10 07:23:04 -08:00
committed by GitHub
parent 764ada9b66
commit ac1705ae2b
250 changed files with 2953 additions and 3584 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
wpimath/src/test/java/edu/wpi/first/math/controller/DifferentialDriveFeedforwardTest.java
View File

@@ -20,7 +20,7 @@ class DifferentialDriveFeedforwardTest {
private static final double kVAngular = 1.0;
private static final double kAAngular = 1.0;
private static final double trackwidth = 1.0;
private static final double dtSeconds = 0.02;
private static final double dt = 0.02;
@Test
void testCalculateWithTrackwidth() {
@@ -39,12 +39,12 @@ class DifferentialDriveFeedforwardTest {
nextLeftVelocity,
currentRightVelocity,
nextRightVelocity,
dtSeconds);
dt);
Matrix<N2, N1> nextX =
plant.calculateX(
VecBuilder.fill(currentLeftVelocity, currentRightVelocity),
VecBuilder.fill(u.left, u.right),
dtSeconds);
dt);
assertEquals(nextX.get(0, 0), nextLeftVelocity, 1e-6);
assertEquals(nextX.get(1, 0), nextRightVelocity, 1e-6);
}
@@ -69,12 +69,12 @@ class DifferentialDriveFeedforwardTest {
nextLeftVelocity,
currentRightVelocity,
nextRightVelocity,
dtSeconds);
dt);
Matrix<N2, N1> nextX =
plant.calculateX(
VecBuilder.fill(currentLeftVelocity, currentRightVelocity),
VecBuilder.fill(u.left, u.right),
dtSeconds);
dt);
assertEquals(nextX.get(0, 0), nextLeftVelocity, 1e-6);
assertEquals(nextX.get(1, 0), nextRightVelocity, 1e-6);
}

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

@@ -42,22 +42,17 @@ class HolonomicDriveControllerTest {
Trajectory trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
final double kDt = 0.02;
final double kTotalTime = trajectory.getTotalTimeSeconds();
final double kTotalTime = trajectory.getTotalTime();
for (int i = 0; i < (kTotalTime / kDt); i++) {
Trajectory.State state = trajectory.sample(kDt * i);
ChassisSpeeds output = controller.calculate(robotPose, state, Rotation2d.kZero);
robotPose =
robotPose.exp(
new Twist2d(
output.vxMetersPerSecond * kDt,
output.vyMetersPerSecond * kDt,
output.omegaRadiansPerSecond * kDt));
robotPose = robotPose.exp(new Twist2d(output.vx * kDt, output.vy * kDt, output.omega * kDt));
}
final List<Trajectory.State> states = trajectory.getStates();
final Pose2d endPose = states.get(states.size() - 1).poseMeters;
final Pose2d endPose = states.get(states.size() - 1).pose;
// Java lambdas require local variables referenced from a lambda expression
// must be final or effectively final.
@@ -85,6 +80,6 @@ class HolonomicDriveControllerTest {
controller.calculate(
new Pose2d(0, 0, new Rotation2d(1.57)), Pose2d.kZero, 0, new Rotation2d(1.57));
assertEquals(0.0, speeds.omegaRadiansPerSecond);
assertEquals(0.0, speeds.omega);
}
}

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

@@ -96,7 +96,7 @@ class LTVDifferentialDriveControllerTest {
0.0,
0.0);
final var totalTime = trajectory.getTotalTimeSeconds();
final var totalTime = trajectory.getTotalTime();
for (int i = 0; i < (totalTime / kDt); ++i) {
var state = trajectory.sample(kDt * i);
robotPose =
@@ -115,7 +115,7 @@ class LTVDifferentialDriveControllerTest {
}
final var states = trajectory.getStates();
final var endPose = states.get(states.size() - 1).poseMeters;
final var endPose = states.get(states.size() - 1).pose;
// Java lambdas require local variables referenced from a lambda expression
// must be final or effectively final.

8
wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java
View File

@@ -36,18 +36,16 @@ class LTVUnicycleControllerTest {
var config = new TrajectoryConfig(8.8, 0.1);
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
final var totalTime = trajectory.getTotalTimeSeconds();
final var totalTime = trajectory.getTotalTime();
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));
robotPose = robotPose.exp(new Twist2d(output.vx * kDt, 0, output.omega * kDt));
}
final var states = trajectory.getStates();
final var endPose = states.get(states.size() - 1).poseMeters;
final var endPose = states.get(states.size() - 1).pose;
// Java lambdas require local variables referenced from a lambda expression
// must be final or effectively final.

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

@@ -85,7 +85,7 @@ class LinearQuadraticRegulatorTest {
* @param Q State cost matrix.
* @param R Input cost matrix.
* @param Aref Desired state matrix.
* @param dtSeconds Discretization timestep in seconds.
* @param dt Discretization timestep in seconds.
*/
<States extends Num, Inputs extends Num> Matrix<Inputs, States> getImplicitModelFollowingK(
Matrix<States, States> A,
@@ -93,21 +93,21 @@ class LinearQuadraticRegulatorTest {
Matrix<States, States> Q,
Matrix<Inputs, Inputs> R,
Matrix<States, States> Aref,
double dtSeconds) {
double dt) {
// Discretize real dynamics
var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
var discABPair = Discretization.discretizeAB(A, B, dt);
var discA = discABPair.getFirst();
var discB = discABPair.getSecond();
// Discretize desired dynamics
var discAref = Discretization.discretizeA(Aref, dtSeconds);
var discAref = Discretization.discretizeA(Aref, dt);
Matrix<States, States> Qimf =
(discA.minus(discAref)).transpose().times(Q).times(discA.minus(discAref));
Matrix<Inputs, Inputs> Rimf = discB.transpose().times(Q).times(discB).plus(R);
Matrix<States, Inputs> Nimf = (discA.minus(discAref)).transpose().times(Q).times(discB);
return new LinearQuadraticRegulator<>(A, B, Qimf, Rimf, Nimf, dtSeconds).getK();
return new LinearQuadraticRegulator<>(A, B, Qimf, Rimf, Nimf, dt).getK();
}
@Test

39
wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3dTest.java
View File

@@ -58,12 +58,8 @@ class DifferentialDrivePoseEstimator3dTest {
kinematics,
estimator,
trajectory,
state ->
new ChassisSpeeds(
state.velocityMetersPerSecond,
0,
state.velocityMetersPerSecond * state.curvatureRadPerMeter),
state -> state.poseMeters,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
trajectory.getInitialPose(),
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
0.02,
@@ -113,12 +109,8 @@ class DifferentialDrivePoseEstimator3dTest {
kinematics,
estimator,
trajectory,
state ->
new ChassisSpeeds(
state.velocityMetersPerSecond,
0,
state.velocityMetersPerSecond * state.curvatureRadPerMeter),
state -> state.poseMeters,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
initial_pose,
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
0.02,
@@ -141,11 +133,11 @@ class DifferentialDrivePoseEstimator3dTest {
final double visionUpdateRate,
final double visionUpdateDelay,
final boolean checkError) {
double leftDistanceMeters = 0;
double rightDistanceMeters = 0;
double leftDistance = 0;
double rightDistance = 0;
estimator.resetPosition(
Rotation3d.kZero, leftDistanceMeters, rightDistanceMeters, new Pose3d(startingPose));
Rotation3d.kZero, leftDistance, rightDistance, new Pose3d(startingPose));
var rand = new Random(3538);
@@ -155,7 +147,7 @@ class DifferentialDrivePoseEstimator3dTest {
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
// We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
@@ -183,24 +175,24 @@ class DifferentialDrivePoseEstimator3dTest {
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
leftDistanceMeters += wheelSpeeds.leftMetersPerSecond * dt;
rightDistanceMeters += wheelSpeeds.rightMetersPerSecond * dt;
leftDistance += wheelSpeeds.left * dt;
rightDistance += wheelSpeeds.right * dt;
var xHat =
estimator.updateWithTime(
t,
new Rotation3d(
groundTruthState
.poseMeters
.pose
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05))
.minus(trajectory.getInitialPose().getRotation())),
leftDistanceMeters,
rightDistanceMeters);
leftDistance,
rightDistance);
double error =
groundTruthState
.poseMeters
.pose
.getTranslation()
.getDistance(xHat.getTranslation().toTranslation2d());
if (error > maxError) {
@@ -222,8 +214,7 @@ class DifferentialDrivePoseEstimator3dTest {
"Incorrect Final Theta");
if (checkError) {
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error");
assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error");
assertEquals(0.0, maxError, 0.2, "Incorrect max error");
}
}

41
wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
View File

@@ -55,12 +55,8 @@ class DifferentialDrivePoseEstimatorTest {
kinematics,
estimator,
trajectory,
state ->
new ChassisSpeeds(
state.velocityMetersPerSecond,
0,
state.velocityMetersPerSecond * state.curvatureRadPerMeter),
state -> state.poseMeters,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
trajectory.getInitialPose(),
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
0.02,
@@ -110,12 +106,8 @@ class DifferentialDrivePoseEstimatorTest {
kinematics,
estimator,
trajectory,
state ->
new ChassisSpeeds(
state.velocityMetersPerSecond,
0,
state.velocityMetersPerSecond * state.curvatureRadPerMeter),
state -> state.poseMeters,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
initial_pose,
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
0.02,
@@ -138,11 +130,10 @@ class DifferentialDrivePoseEstimatorTest {
final double visionUpdateRate,
final double visionUpdateDelay,
final boolean checkError) {
double leftDistanceMeters = 0;
double rightDistanceMeters = 0;
double leftDistance = 0;
double rightDistance = 0;
estimator.resetPosition(
Rotation2d.kZero, leftDistanceMeters, rightDistanceMeters, startingPose);
estimator.resetPosition(Rotation2d.kZero, leftDistance, rightDistance, startingPose);
var rand = new Random(3538);
@@ -152,7 +143,7 @@ class DifferentialDrivePoseEstimatorTest {
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
// We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
@@ -180,22 +171,21 @@ class DifferentialDrivePoseEstimatorTest {
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
leftDistanceMeters += wheelSpeeds.leftMetersPerSecond * dt;
rightDistanceMeters += wheelSpeeds.rightMetersPerSecond * dt;
leftDistance += wheelSpeeds.left * dt;
rightDistance += wheelSpeeds.right * dt;
var xHat =
estimator.updateWithTime(
t,
groundTruthState
.poseMeters
.pose
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05))
.minus(trajectory.getInitialPose().getRotation()),
leftDistanceMeters,
rightDistanceMeters);
leftDistance,
rightDistance);
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
@@ -215,8 +205,7 @@ class DifferentialDrivePoseEstimatorTest {
"Incorrect Final Theta");
if (checkError) {
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error");
assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error");
assertEquals(0.0, maxError, 0.2, "Incorrect max error");
}
}

50
wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
View File

@@ -30,14 +30,13 @@ class ExtendedKalmanFilterTest {
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 rb = 0.8382 / 2.0; // Wheelbase radius (trackwidth)
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 C1 = -Math.pow(gr, 2) * motors.Kt / (motors.Kv * motors.R * r * r);
final var C2 = gr * motors.Kt / (motors.R * r);
final var k1 = 1.0 / m + rb * rb / J;
final var k2 = 1.0 / m - rb * rb / J;
@@ -68,7 +67,7 @@ class ExtendedKalmanFilterTest {
@Test
void testInit() {
double dtSeconds = 0.00505;
double dt = 0.00505;
assertDoesNotThrow(
() -> {
@@ -81,10 +80,10 @@ class ExtendedKalmanFilterTest {
ExtendedKalmanFilterTest::getLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
VecBuilder.fill(0.0001, 0.01, 0.01),
dtSeconds);
dt);
Matrix<N2, N1> u = VecBuilder.fill(12.0, 12.0);
observer.predict(u, dtSeconds);
observer.predict(u, dt);
var localY = getLocalMeasurementModel(observer.getXhat(), u);
observer.correct(u, localY);
@@ -98,8 +97,8 @@ class ExtendedKalmanFilterTest {
@Test
void testConvergence() {
double dtSeconds = 0.00505;
double rbMeters = 0.8382 / 2.0; // Robot radius
double dt = 0.00505;
double rb = 0.8382 / 2.0; // Robot radius
ExtendedKalmanFilter<N5, N2, N3> observer =
new ExtendedKalmanFilter<>(
@@ -110,7 +109,7 @@ class ExtendedKalmanFilterTest {
ExtendedKalmanFilterTest::getLocalMeasurementModel,
VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
VecBuilder.fill(0.001, 0.01, 0.01),
dtSeconds);
dt);
List<Pose2d> waypoints =
List.of(
@@ -142,15 +141,15 @@ class ExtendedKalmanFilterTest {
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));
double totalTime = trajectory.getTotalTime();
for (int i = 0; i < (totalTime / dt); i++) {
var ref = trajectory.sample(dt * i);
double vl = ref.velocity * (1 - (ref.curvature * rb));
double vr = ref.velocity * (1 + (ref.curvature * rb));
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(0, 0, ref.pose.getTranslation().getX());
nextR.set(1, 0, ref.pose.getTranslation().getY());
nextR.set(2, 0, ref.pose.getRotation().getRadians());
nextR.set(3, 0, vl);
nextR.set(4, 0, vr);
@@ -158,14 +157,13 @@ class ExtendedKalmanFilterTest {
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);
Matrix<N5, N1> rdot = nextR.minus(r).div(dt);
u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
observer.predict(u, dtSeconds);
observer.predict(u, dt);
groundTruthX =
NumericalIntegration.rk4(
ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dtSeconds);
NumericalIntegration.rk4(ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dt);
r = nextR;
}
@@ -177,10 +175,10 @@ class ExtendedKalmanFilterTest {
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 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);
var finalPosition = trajectory.sample(trajectory.getTotalTime());
assertEquals(finalPosition.pose.getTranslation().getX(), observer.getXhat(0), 1.0);
assertEquals(finalPosition.pose.getTranslation().getY(), observer.getXhat(1), 1.0);
assertEquals(finalPosition.pose.getRotation().getRadians(), observer.getXhat(2), 1.0);
assertEquals(0.0, observer.getXhat(3), 1.0);
assertEquals(0.0, observer.getXhat(4), 1.0);
}

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

@@ -159,19 +159,19 @@ class KalmanFilterTest {
var time = 0.0;
var lastVelocity = VecBuilder.fill(0.0, 0.0, 0.0);
while (time <= trajectory.getTotalTimeSeconds()) {
while (time <= trajectory.getTotalTime()) {
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);
sample.pose.getTranslation().getX() + random.nextGaussian() / 5d,
sample.pose.getTranslation().getY() + random.nextGaussian() / 5d,
sample.pose.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);
sample.velocity * sample.pose.getRotation().getCos(),
sample.velocity * sample.pose.getRotation().getSin(),
sample.curvature * sample.velocity);
var u = (velocity.minus(lastVelocity)).div(0.020);
lastVelocity = velocity;
@@ -182,11 +182,11 @@ class KalmanFilterTest {
}
assertEquals(
trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters.getTranslation().getX(),
trajectory.sample(trajectory.getTotalTime()).pose.getTranslation().getX(),
filter.getXhat(0),
0.2);
assertEquals(
trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters.getTranslation().getY(),
trajectory.sample(trajectory.getTotalTime()).pose.getTranslation().getY(),
filter.getXhat(1),
0.2);
}

33
wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3dTest.java
View File

@@ -64,12 +64,8 @@ class MecanumDrivePoseEstimator3dTest {
kinematics,
estimator,
trajectory,
state ->
new ChassisSpeeds(
state.velocityMetersPerSecond,
0,
state.velocityMetersPerSecond * state.curvatureRadPerMeter),
state -> state.poseMeters,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
trajectory.getInitialPose(),
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
0.02,
@@ -123,12 +119,8 @@ class MecanumDrivePoseEstimator3dTest {
kinematics,
estimator,
trajectory,
state ->
new ChassisSpeeds(
state.velocityMetersPerSecond,
0,
state.velocityMetersPerSecond * state.curvatureRadPerMeter),
state -> state.poseMeters,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
initial_pose,
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
0.02,
@@ -163,7 +155,7 @@ class MecanumDrivePoseEstimator3dTest {
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
// We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
@@ -191,17 +183,17 @@ class MecanumDrivePoseEstimator3dTest {
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
var xHat =
estimator.updateWithTime(
t,
new Rotation3d(
groundTruthState
.poseMeters
.pose
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05))
.minus(trajectory.getInitialPose().getRotation())),
@@ -209,7 +201,7 @@ class MecanumDrivePoseEstimator3dTest {
double error =
groundTruthState
.poseMeters
.pose
.getTranslation()
.getDistance(xHat.getTranslation().toTranslation2d());
if (error > maxError) {
@@ -231,8 +223,7 @@ class MecanumDrivePoseEstimator3dTest {
"Incorrect Final Theta");
if (checkError) {
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error");
assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error");
assertEquals(0.0, maxError, 0.2, "Incorrect max error");
}
}

34
wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
View File

@@ -61,12 +61,8 @@ class MecanumDrivePoseEstimatorTest {
kinematics,
estimator,
trajectory,
state ->
new ChassisSpeeds(
state.velocityMetersPerSecond,
0,
state.velocityMetersPerSecond * state.curvatureRadPerMeter),
state -> state.poseMeters,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
trajectory.getInitialPose(),
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
0.02,
@@ -120,12 +116,8 @@ class MecanumDrivePoseEstimatorTest {
kinematics,
estimator,
trajectory,
state ->
new ChassisSpeeds(
state.velocityMetersPerSecond,
0,
state.velocityMetersPerSecond * state.curvatureRadPerMeter),
state -> state.poseMeters,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
initial_pose,
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
0.02,
@@ -160,7 +152,7 @@ class MecanumDrivePoseEstimatorTest {
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
// We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
@@ -188,23 +180,22 @@ class MecanumDrivePoseEstimatorTest {
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
var xHat =
estimator.updateWithTime(
t,
groundTruthState
.poseMeters
.pose
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05))
.minus(trajectory.getInitialPose().getRotation()),
wheelPositions);
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
@@ -224,8 +215,7 @@ class MecanumDrivePoseEstimatorTest {
"Incorrect Final Theta");
if (checkError) {
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error");
assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error");
assertEquals(0.0, maxError, 0.2, "Incorrect max error");
}
}

28
wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java
View File

@@ -69,12 +69,8 @@ class SwerveDrivePoseEstimator3dTest {
kinematics,
estimator,
trajectory,
state ->
new ChassisSpeeds(
state.velocityMetersPerSecond,
0,
state.velocityMetersPerSecond * state.curvatureRadPerMeter),
state -> state.poseMeters,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
trajectory.getInitialPose(),
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
0.02,
@@ -132,12 +128,8 @@ class SwerveDrivePoseEstimator3dTest {
kinematics,
estimator,
trajectory,
state ->
new ChassisSpeeds(
state.velocityMetersPerSecond,
0,
state.velocityMetersPerSecond * state.curvatureRadPerMeter),
state -> state.poseMeters,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
initial_pose,
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
0.02,
@@ -177,7 +169,7 @@ class SwerveDrivePoseEstimator3dTest {
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
// We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
@@ -206,8 +198,7 @@ class SwerveDrivePoseEstimator3dTest {
var moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds);
for (int i = 0; i < moduleStates.length; i++) {
positions[i].distanceMeters +=
moduleStates[i].speedMetersPerSecond * (1 - rand.nextGaussian() * 0.05) * dt;
positions[i].distance += moduleStates[i].speed * (1 - rand.nextGaussian() * 0.05) * dt;
positions[i].angle =
moduleStates[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
}
@@ -217,7 +208,7 @@ class SwerveDrivePoseEstimator3dTest {
t,
new Rotation3d(
groundTruthState
.poseMeters
.pose
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05))
.minus(trajectory.getInitialPose().getRotation())),
@@ -225,7 +216,7 @@ class SwerveDrivePoseEstimator3dTest {
double error =
groundTruthState
.poseMeters
.pose
.getTranslation()
.getDistance(xHat.getTranslation().toTranslation2d());
if (error > maxError) {
@@ -247,8 +238,7 @@ class SwerveDrivePoseEstimator3dTest {
"Incorrect Final Theta");
if (checkError) {
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error");
assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error");
assertEquals(0.0, maxError, 0.2, "Incorrect max error");
}
}

29
wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
View File

@@ -66,12 +66,8 @@ class SwerveDrivePoseEstimatorTest {
kinematics,
estimator,
trajectory,
state ->
new ChassisSpeeds(
state.velocityMetersPerSecond,
0,
state.velocityMetersPerSecond * state.curvatureRadPerMeter),
state -> state.poseMeters,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
trajectory.getInitialPose(),
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
0.02,
@@ -129,12 +125,8 @@ class SwerveDrivePoseEstimatorTest {
kinematics,
estimator,
trajectory,
state ->
new ChassisSpeeds(
state.velocityMetersPerSecond,
0,
state.velocityMetersPerSecond * state.curvatureRadPerMeter),
state -> state.poseMeters,
state -> new ChassisSpeeds(state.velocity, 0, state.velocity * state.curvature),
state -> state.pose,
initial_pose,
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
0.02,
@@ -174,7 +166,7 @@ class SwerveDrivePoseEstimatorTest {
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
// We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the
@@ -203,8 +195,7 @@ class SwerveDrivePoseEstimatorTest {
var moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds);
for (int i = 0; i < moduleStates.length; i++) {
positions[i].distanceMeters +=
moduleStates[i].speedMetersPerSecond * (1 - rand.nextGaussian() * 0.05) * dt;
positions[i].distance += moduleStates[i].speed * (1 - rand.nextGaussian() * 0.05) * dt;
positions[i].angle =
moduleStates[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
}
@@ -213,14 +204,13 @@ class SwerveDrivePoseEstimatorTest {
estimator.updateWithTime(
t,
groundTruthState
.poseMeters
.pose
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05))
.minus(trajectory.getInitialPose().getRotation()),
positions);
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
@@ -240,8 +230,7 @@ class SwerveDrivePoseEstimatorTest {
"Incorrect Final Theta");
if (checkError) {
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error");
assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.07, "Incorrect mean error");
assertEquals(0.0, maxError, 0.2, "Incorrect max error");
}
}

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

@@ -40,11 +40,8 @@ class UnscentedKalmanFilterTest {
var m = 63.503; // Robot mass
var J = 5.6; // Robot moment of inertia
var C1 =
-Math.pow(gHigh, 2)
* motors.KtNMPerAmp
/ (motors.KvRadPerSecPerVolt * motors.rOhms * r * r);
var C2 = gHigh * motors.KtNMPerAmp / (motors.rOhms * r);
var C1 = -Math.pow(gHigh, 2) * motors.Kt / (motors.Kv * motors.R * r * r);
var C2 = gHigh * motors.Kt / (motors.R * r);
var k1 = 1.0 / m + Math.pow(rb, 2) / J;
var k2 = 1.0 / m - Math.pow(rb, 2) / J;
@@ -74,7 +71,7 @@ class UnscentedKalmanFilterTest {
@Test
void testInit() {
var dtSeconds = 0.005;
var dt = 0.005;
assertDoesNotThrow(
() -> {
UnscentedKalmanFilter<N5, N2, N3> observer =
@@ -90,10 +87,10 @@ class UnscentedKalmanFilterTest {
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(0),
AngleStatistics.angleAdd(2),
dtSeconds);
dt);
var u = VecBuilder.fill(12.0, 12.0);
observer.predict(u, dtSeconds);
observer.predict(u, dt);
var localY = getLocalMeasurementModel(observer.getXhat(), u);
observer.correct(u, localY);
@@ -117,8 +114,8 @@ class UnscentedKalmanFilterTest {
@Test
void testConvergence() {
double dtSeconds = 0.005;
double rbMeters = 0.8382 / 2.0; // Robot radius
double dt = 0.005;
double rb = 0.8382 / 2.0; // Robot radius
UnscentedKalmanFilter<N5, N2, N3> observer =
new UnscentedKalmanFilter<>(
@@ -133,7 +130,7 @@ class UnscentedKalmanFilterTest {
AngleStatistics.angleResidual(2),
AngleStatistics.angleResidual(0),
AngleStatistics.angleAdd(2),
dtSeconds);
dt);
List<Pose2d> waypoints =
List.of(
@@ -163,17 +160,17 @@ class UnscentedKalmanFilterTest {
var trueXhat = 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));
double totalTime = trajectory.getTotalTime();
for (int i = 0; i < (totalTime / dt); i++) {
var ref = trajectory.sample(dt * i);
double vl = ref.velocity * (1 - (ref.curvature * rb));
double vr = ref.velocity * (1 + (ref.curvature * rb));
var nextR =
VecBuilder.fill(
ref.poseMeters.getTranslation().getX(),
ref.poseMeters.getTranslation().getY(),
ref.poseMeters.getRotation().getRadians(),
ref.pose.getTranslation().getX(),
ref.pose.getTranslation().getY(),
ref.pose.getRotation().getRadians(),
vl,
vr);
@@ -182,14 +179,13 @@ class UnscentedKalmanFilterTest {
observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));
var rdot = nextR.minus(r).div(dtSeconds);
var rdot = nextR.minus(r).div(dt);
u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
observer.predict(u, dtSeconds);
observer.predict(u, dt);
r = nextR;
trueXhat =
NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds);
trueXhat = NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dt);
}
var localY = getLocalMeasurementModel(trueXhat, u);
@@ -210,12 +206,11 @@ class UnscentedKalmanFilterTest {
AngleStatistics.angleResidual(2),
AngleStatistics.angleAdd(2));
final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
final var finalPosition = trajectory.sample(trajectory.getTotalTime());
assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.055);
assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.15);
assertEquals(
finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 0.000005);
assertEquals(finalPosition.pose.getTranslation().getX(), observer.getXhat(0), 0.055);
assertEquals(finalPosition.pose.getTranslation().getY(), observer.getXhat(1), 0.15);
assertEquals(finalPosition.pose.getRotation().getRadians(), observer.getXhat(2), 0.000005);
assertEquals(0.0, observer.getXhat(3), 0.1);
assertEquals(0.0, observer.getXhat(4), 0.1);
}
@@ -252,7 +247,7 @@ class UnscentedKalmanFilterTest {
@Test
void testRoundTripP() {
var dtSeconds = 0.005;
var dt = 0.005;
var observer =
new UnscentedKalmanFilter<>(
@@ -262,7 +257,7 @@ class UnscentedKalmanFilterTest {
(x, u) -> x,
VecBuilder.fill(0.0, 0.0),
VecBuilder.fill(0.0, 0.0),
dtSeconds);
dt);
var P = MatBuilder.fill(Nat.N2(), Nat.N2(), 2.0, 1.0, 1.0, 2.0);
observer.setP(P);

64
wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
View File

@@ -24,11 +24,7 @@ class ChassisSpeedsTest {
final var dt = 0.01;
final var speeds = target.discretize(duration);
final var twist =
new Twist2d(
speeds.vxMetersPerSecond * dt,
speeds.vyMetersPerSecond * dt,
speeds.omegaRadiansPerSecond * dt);
final var twist = new Twist2d(speeds.vx * dt, speeds.vy * dt, speeds.omega * dt);
var pose = Pose2d.kZero;
for (double time = 0; time < duration; time += dt) {
@@ -37,13 +33,9 @@ class ChassisSpeedsTest {
final var result = pose; // For lambda capture
assertAll(
() -> assertEquals(target.vxMetersPerSecond * duration, result.getX(), kEpsilon),
() -> assertEquals(target.vyMetersPerSecond * duration, result.getY(), kEpsilon),
() ->
assertEquals(
target.omegaRadiansPerSecond * duration,
result.getRotation().getRadians(),
kEpsilon));
() -> assertEquals(target.vx * duration, result.getX(), kEpsilon),
() -> assertEquals(target.vy * duration, result.getY(), kEpsilon),
() -> assertEquals(target.omega * duration, result.getRotation().getRadians(), kEpsilon));
}
@Test
@@ -54,9 +46,9 @@ class ChassisSpeedsTest {
var speeds = new ChassisSpeeds(vx, vy, omega);
assertAll(
() -> assertEquals(0.368808, speeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(0, speeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.002094395102, speeds.omegaRadiansPerSecond, kEpsilon));
() -> assertEquals(0.368808, speeds.vx, kEpsilon),
() -> assertEquals(0, speeds.vy, kEpsilon),
() -> assertEquals(0.002094395102, speeds.omega, kEpsilon));
}
@Test
@@ -64,9 +56,9 @@ class ChassisSpeedsTest {
final var chassisSpeeds = new ChassisSpeeds(1.0, 0.0, 0.5).toRobotRelative(Rotation2d.kCW_Pi_2);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
() -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon),
() -> assertEquals(1.0, chassisSpeeds.vy, kEpsilon),
() -> assertEquals(0.5, chassisSpeeds.omega, kEpsilon));
}
@Test
@@ -75,9 +67,9 @@ class ChassisSpeedsTest {
new ChassisSpeeds(1.0, 0.0, 0.5).toFieldRelative(Rotation2d.fromDegrees(45.0));
assertAll(
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vx, kEpsilon),
() -> assertEquals(1.0 / Math.sqrt(2.0), chassisSpeeds.vy, kEpsilon),
() -> assertEquals(0.5, chassisSpeeds.omega, kEpsilon));
}
@Test
@@ -88,9 +80,9 @@ class ChassisSpeedsTest {
final var chassisSpeeds = left.plus(right);
assertAll(
() -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond),
() -> assertEquals(2.0, chassisSpeeds.vyMetersPerSecond),
() -> assertEquals(1.0, chassisSpeeds.omegaRadiansPerSecond));
() -> assertEquals(3.0, chassisSpeeds.vx),
() -> assertEquals(2.0, chassisSpeeds.vy),
() -> assertEquals(1.0, chassisSpeeds.omega));
}
@Test
@@ -101,9 +93,9 @@ class ChassisSpeedsTest {
final var chassisSpeeds = left.minus(right);
assertAll(
() -> assertEquals(-1.0, chassisSpeeds.vxMetersPerSecond),
() -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond),
() -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond));
() -> assertEquals(-1.0, chassisSpeeds.vx),
() -> assertEquals(0.0, chassisSpeeds.vy),
() -> assertEquals(0.5, chassisSpeeds.omega));
}
@Test
@@ -111,9 +103,9 @@ class ChassisSpeedsTest {
final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).unaryMinus();
assertAll(
() -> assertEquals(-1.0, chassisSpeeds.vxMetersPerSecond),
() -> assertEquals(-0.5, chassisSpeeds.vyMetersPerSecond),
() -> assertEquals(-0.75, chassisSpeeds.omegaRadiansPerSecond));
() -> assertEquals(-1.0, chassisSpeeds.vx),
() -> assertEquals(-0.5, chassisSpeeds.vy),
() -> assertEquals(-0.75, chassisSpeeds.omega));
}
@Test
@@ -121,9 +113,9 @@ class ChassisSpeedsTest {
final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).times(2.0);
assertAll(
() -> assertEquals(2.0, chassisSpeeds.vxMetersPerSecond),
() -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond),
() -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond));
() -> assertEquals(2.0, chassisSpeeds.vx),
() -> assertEquals(1.0, chassisSpeeds.vy),
() -> assertEquals(1.5, chassisSpeeds.omega));
}
@Test
@@ -131,8 +123,8 @@ class ChassisSpeedsTest {
final var chassisSpeeds = (new ChassisSpeeds(1.0, 0.5, 0.75)).div(2.0);
assertAll(
() -> assertEquals(0.5, chassisSpeeds.vxMetersPerSecond),
() -> assertEquals(0.25, chassisSpeeds.vyMetersPerSecond),
() -> assertEquals(0.375, chassisSpeeds.omegaRadiansPerSecond));
() -> assertEquals(0.5, chassisSpeeds.vx),
() -> assertEquals(0.25, chassisSpeeds.vy),
() -> assertEquals(0.375, chassisSpeeds.omega));
}
}

30
wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java
View File

@@ -20,8 +20,8 @@ class DifferentialDriveKinematicsTest {
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
assertAll(
() -> assertEquals(0.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond, kEpsilon));
() -> assertEquals(0.0, wheelSpeeds.left, kEpsilon),
() -> assertEquals(0.0, wheelSpeeds.right, kEpsilon));
}
@Test
@@ -30,9 +30,9 @@ class DifferentialDriveKinematicsTest {
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));
() -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omega, kEpsilon));
}
@Test
@@ -41,8 +41,8 @@ class DifferentialDriveKinematicsTest {
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
assertAll(
() -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
() -> assertEquals(3.0, wheelSpeeds.rightMetersPerSecond, kEpsilon));
() -> assertEquals(3.0, wheelSpeeds.left, kEpsilon),
() -> assertEquals(3.0, wheelSpeeds.right, kEpsilon));
}
@Test
@@ -51,9 +51,9 @@ class DifferentialDriveKinematicsTest {
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));
() -> assertEquals(3.0, chassisSpeeds.vx, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omega, kEpsilon));
}
@Test
@@ -62,8 +62,8 @@ class DifferentialDriveKinematicsTest {
var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
assertAll(
() -> assertEquals(-0.381 * Math.PI, wheelSpeeds.leftMetersPerSecond, kEpsilon),
() -> assertEquals(+0.381 * Math.PI, wheelSpeeds.rightMetersPerSecond, kEpsilon));
() -> assertEquals(-0.381 * Math.PI, wheelSpeeds.left, kEpsilon),
() -> assertEquals(+0.381 * Math.PI, wheelSpeeds.right, kEpsilon));
}
@Test
@@ -72,8 +72,8 @@ class DifferentialDriveKinematicsTest {
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));
() -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon),
() -> assertEquals(-Math.PI, chassisSpeeds.omega, kEpsilon));
}
}

2
wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java
View File

@@ -25,7 +25,7 @@ class DifferentialDriveOdometry3dTest {
0,
0,
new Pose3d(1, 2, 0, new Rotation3d(0, 0, Units.degreesToRadians(45))));
var pose = odometry.getPoseMeters();
var pose = odometry.getPose();
assertAll(
() -> assertEquals(pose.getX(), 1.0, kEpsilon),
() -> assertEquals(pose.getY(), 2.0, kEpsilon),

15
wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeedsTest.java
View File

@@ -18,8 +18,7 @@ class DifferentialDriveWheelSpeedsTest {
final var wheelSpeeds = left.plus(right);
assertAll(
() -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond),
() -> assertEquals(2.0, wheelSpeeds.rightMetersPerSecond));
() -> assertEquals(3.0, wheelSpeeds.left), () -> assertEquals(2.0, wheelSpeeds.right));
}
@Test
@@ -30,8 +29,7 @@ class DifferentialDriveWheelSpeedsTest {
final var wheelSpeeds = left.minus(right);
assertAll(
() -> assertEquals(-1.0, wheelSpeeds.leftMetersPerSecond),
() -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond));
() -> assertEquals(-1.0, wheelSpeeds.left), () -> assertEquals(0.0, wheelSpeeds.right));
}
@Test
@@ -39,8 +37,7 @@ class DifferentialDriveWheelSpeedsTest {
final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).unaryMinus();
assertAll(
() -> assertEquals(-1.0, wheelSpeeds.leftMetersPerSecond),
() -> assertEquals(-0.5, wheelSpeeds.rightMetersPerSecond));
() -> assertEquals(-1.0, wheelSpeeds.left), () -> assertEquals(-0.5, wheelSpeeds.right));
}
@Test
@@ -48,8 +45,7 @@ class DifferentialDriveWheelSpeedsTest {
final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).times(2.0);
assertAll(
() -> assertEquals(2.0, wheelSpeeds.leftMetersPerSecond),
() -> assertEquals(1.0, wheelSpeeds.rightMetersPerSecond));
() -> assertEquals(2.0, wheelSpeeds.left), () -> assertEquals(1.0, wheelSpeeds.right));
}
@Test
@@ -57,7 +53,6 @@ class DifferentialDriveWheelSpeedsTest {
final var wheelSpeeds = new DifferentialDriveWheelSpeeds(1.0, 0.5).div(2.0);
assertAll(
() -> assertEquals(0.5, wheelSpeeds.leftMetersPerSecond),
() -> assertEquals(0.25, wheelSpeeds.rightMetersPerSecond));
() -> assertEquals(0.5, wheelSpeeds.left), () -> assertEquals(0.25, wheelSpeeds.right));
}
}

100
wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
View File

@@ -27,10 +27,10 @@ class MecanumDriveKinematicsTest {
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));
() -> assertEquals(5.0, moduleStates.frontLeft, 0.1),
() -> assertEquals(5.0, moduleStates.frontRight, 0.1),
() -> assertEquals(5.0, moduleStates.rearLeft, 0.1),
() -> assertEquals(5.0, moduleStates.rearRight, 0.1));
}
@Test
@@ -39,9 +39,9 @@ class MecanumDriveKinematicsTest {
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));
() -> assertEquals(3.536, moduleStates.vx, 0.1),
() -> assertEquals(0, moduleStates.vy, 0.1),
() -> assertEquals(0, moduleStates.omega, 0.1));
}
@Test
@@ -61,10 +61,10 @@ class MecanumDriveKinematicsTest {
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));
() -> assertEquals(-4.0, moduleStates.frontLeft, 0.1),
() -> assertEquals(4.0, moduleStates.frontRight, 0.1),
() -> assertEquals(4.0, moduleStates.rearLeft, 0.1),
() -> assertEquals(-4.0, moduleStates.rearRight, 0.1));
}
@Test
@@ -73,9 +73,9 @@ class MecanumDriveKinematicsTest {
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));
() -> assertEquals(0, moduleStates.vx, 0.1),
() -> assertEquals(2.8284, moduleStates.vy, 0.1),
() -> assertEquals(0, moduleStates.omega, 0.1));
}
@Test
@@ -95,10 +95,10 @@ class MecanumDriveKinematicsTest {
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));
() -> assertEquals(-150.79645, moduleStates.frontLeft, 0.1),
() -> assertEquals(150.79645, moduleStates.frontRight, 0.1),
() -> assertEquals(-150.79645, moduleStates.rearLeft, 0.1),
() -> assertEquals(150.79645, moduleStates.rearRight, 0.1));
}
@Test
@@ -107,9 +107,9 @@ class MecanumDriveKinematicsTest {
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));
() -> assertEquals(0, moduleStates.vx, 0.1),
() -> assertEquals(0, moduleStates.vy, 0.1),
() -> assertEquals(2 * Math.PI, moduleStates.omega, 0.1));
}
@Test
@@ -129,10 +129,10 @@ class MecanumDriveKinematicsTest {
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));
() -> assertEquals(-25.0, moduleStates.frontLeft, 0.1),
() -> assertEquals(29.0, moduleStates.frontRight, 0.1),
() -> assertEquals(-19.0, moduleStates.rearLeft, 0.1),
() -> assertEquals(23.0, moduleStates.rearRight, 0.1));
}
@Test
@@ -141,9 +141,9 @@ class MecanumDriveKinematicsTest {
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));
() -> assertEquals(1.413, moduleStates.vx, 0.1),
() -> assertEquals(2.122, moduleStates.vy, 0.1),
() -> assertEquals(0.707, moduleStates.omega, 0.1));
}
@Test
@@ -163,10 +163,10 @@ class MecanumDriveKinematicsTest {
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));
() -> assertEquals(0, moduleStates.frontLeft, 0.1),
() -> assertEquals(24.0, moduleStates.frontRight, 0.1),
() -> assertEquals(-24.0, moduleStates.rearLeft, 0.1),
() -> assertEquals(48.0, moduleStates.rearRight, 0.1));
}
@Test
@@ -175,9 +175,9 @@ class MecanumDriveKinematicsTest {
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));
() -> assertEquals(8.48525, moduleStates.vx, 0.1),
() -> assertEquals(-8.48525, moduleStates.vy, 0.1),
() -> assertEquals(0.707, moduleStates.omega, 0.1));
}
@Test
@@ -197,10 +197,10 @@ class MecanumDriveKinematicsTest {
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));
() -> assertEquals(3.0, moduleStates.frontLeft, 0.1),
() -> assertEquals(31.0, moduleStates.frontRight, 0.1),
() -> assertEquals(-17.0, moduleStates.rearLeft, 0.1),
() -> assertEquals(51.0, moduleStates.rearRight, 0.1));
}
@Test
@@ -209,9 +209,9 @@ class MecanumDriveKinematicsTest {
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));
() -> assertEquals(12.02, moduleStates.vx, 0.1),
() -> assertEquals(-7.07, moduleStates.vy, 0.1),
() -> assertEquals(0.707, moduleStates.omega, 0.1));
}
@Test
@@ -232,10 +232,10 @@ class MecanumDriveKinematicsTest {
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));
() -> assertEquals(5.0 * factor, wheelSpeeds.frontLeft, kEpsilon),
() -> assertEquals(6.0 * factor, wheelSpeeds.frontRight, kEpsilon),
() -> assertEquals(4.0 * factor, wheelSpeeds.rearLeft, kEpsilon),
() -> assertEquals(7.0 * factor, wheelSpeeds.rearRight, kEpsilon));
}
@Test
@@ -245,9 +245,9 @@ class MecanumDriveKinematicsTest {
final double kFactor = 5.5 / 7.0;
assertAll(
() -> assertEquals(-5.0 * kFactor, wheelSpeeds.frontLeftMetersPerSecond, kEpsilon),
() -> assertEquals(6.0 * kFactor, wheelSpeeds.frontRightMetersPerSecond, kEpsilon),
() -> assertEquals(4.0 * kFactor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon),
() -> assertEquals(-7.0 * kFactor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon));
() -> assertEquals(-5.0 * kFactor, wheelSpeeds.frontLeft, kEpsilon),
() -> assertEquals(6.0 * kFactor, wheelSpeeds.frontRight, kEpsilon),
() -> assertEquals(4.0 * kFactor, wheelSpeeds.rearLeft, kEpsilon),
() -> assertEquals(-7.0 * kFactor, wheelSpeeds.rearRight, kEpsilon));
}
}

71
wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java
View File

@@ -42,7 +42,7 @@ class MecanumDriveOdometry3dTest {
Rotation3d.kZero,
zero,
new Pose3d(1, 2, 0, new Rotation3d(0, 0, Units.degreesToRadians(45))));
var pose = odometry.getPoseMeters();
var pose = odometry.getPose();
assertAll(
() -> assertEquals(pose.getX(), 1.0, 1e-9),
() -> assertEquals(pose.getY(), 2.0, 1e-9),
@@ -148,38 +148,36 @@ class MecanumDriveOdometry3dTest {
double errorSum = 0;
double odometryDistanceTravelled = 0;
double trajectoryDistanceTravelled = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
trajectoryDistanceTravelled +=
groundTruthState.velocityMetersPerSecond * dt
+ 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
var wheelSpeeds =
kinematics.toWheelSpeeds(
new ChassisSpeeds(
groundTruthState.velocityMetersPerSecond,
groundTruthState.velocity,
0,
groundTruthState.velocityMetersPerSecond
* groundTruthState.curvatureRadPerMeter));
groundTruthState.velocity * groundTruthState.curvature));
wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.frontLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRight += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRight += rand.nextGaussian() * 0.1;
wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
var lastPose = odometry.getPoseMeters();
var lastPose = odometry.getPose();
var xHat =
odometry.update(
new Rotation3d(
groundTruthState
.poseMeters
.pose
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05))),
wheelPositions);
@@ -188,7 +186,7 @@ class MecanumDriveOdometry3dTest {
double error =
groundTruthState
.poseMeters
.pose
.getTranslation()
.getDistance(xHat.getTranslation().toTranslation2d());
if (error > maxError) {
@@ -199,8 +197,7 @@ class MecanumDriveOdometry3dTest {
t += dt;
}
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.35, "Incorrect mean error");
assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.35, "Incorrect mean error");
assertEquals(0.0, maxError, 0.35, "Incorrect max error");
assertEquals(
1.0,
@@ -241,33 +238,30 @@ class MecanumDriveOdometry3dTest {
double errorSum = 0;
double odometryDistanceTravelled = 0;
double trajectoryDistanceTravelled = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
trajectoryDistanceTravelled +=
groundTruthState.velocityMetersPerSecond * dt
+ 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
var wheelSpeeds =
kinematics.toWheelSpeeds(
new ChassisSpeeds(
groundTruthState.velocityMetersPerSecond
* groundTruthState.poseMeters.getRotation().getCos(),
groundTruthState.velocityMetersPerSecond
* groundTruthState.poseMeters.getRotation().getSin(),
groundTruthState.velocity * groundTruthState.pose.getRotation().getCos(),
groundTruthState.velocity * groundTruthState.pose.getRotation().getSin(),
0));
wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.frontLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRight += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRight += rand.nextGaussian() * 0.1;
wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
var lastPose = odometry.getPoseMeters();
var lastPose = odometry.getPose();
var xHat = odometry.update(new Rotation3d(0, 0, rand.nextGaussian() * 0.05), wheelPositions);
@@ -275,7 +269,7 @@ class MecanumDriveOdometry3dTest {
double error =
groundTruthState
.poseMeters
.pose
.getTranslation()
.getDistance(xHat.getTranslation().toTranslation2d());
if (error > maxError) {
@@ -286,8 +280,7 @@ class MecanumDriveOdometry3dTest {
t += dt;
}
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error");
assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.15, "Incorrect mean error");
assertEquals(0.0, maxError, 0.3, "Incorrect max error");
assertEquals(
1.0,

74
wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
View File

@@ -124,45 +124,39 @@ class MecanumDriveOdometryTest {
double errorSum = 0;
double odometryDistanceTravelled = 0;
double trajectoryDistanceTravelled = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
trajectoryDistanceTravelled +=
groundTruthState.velocityMetersPerSecond * dt
+ 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
var wheelSpeeds =
kinematics.toWheelSpeeds(
new ChassisSpeeds(
groundTruthState.velocityMetersPerSecond,
groundTruthState.velocity,
0,
groundTruthState.velocityMetersPerSecond
* groundTruthState.curvatureRadPerMeter));
groundTruthState.velocity * groundTruthState.curvature));
wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.frontLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRight += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRight += rand.nextGaussian() * 0.1;
wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
var lastPose = odometry.getPoseMeters();
var lastPose = odometry.getPose();
var xHat =
odometry.update(
groundTruthState
.poseMeters
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05)),
groundTruthState.pose.getRotation().plus(new Rotation2d(rand.nextGaussian() * 0.05)),
wheelPositions);
odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation());
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
@@ -171,8 +165,7 @@ class MecanumDriveOdometryTest {
t += dt;
}
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.35, "Incorrect mean error");
assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.35, "Incorrect mean error");
assertEquals(0.0, maxError, 0.35, "Incorrect max error");
assertEquals(
1.0,
@@ -213,40 +206,36 @@ class MecanumDriveOdometryTest {
double errorSum = 0;
double odometryDistanceTravelled = 0;
double trajectoryDistanceTravelled = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
trajectoryDistanceTravelled +=
groundTruthState.velocityMetersPerSecond * dt
+ 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
var wheelSpeeds =
kinematics.toWheelSpeeds(
new ChassisSpeeds(
groundTruthState.velocityMetersPerSecond
* groundTruthState.poseMeters.getRotation().getCos(),
groundTruthState.velocityMetersPerSecond
* groundTruthState.poseMeters.getRotation().getSin(),
groundTruthState.velocity * groundTruthState.pose.getRotation().getCos(),
groundTruthState.velocity * groundTruthState.pose.getRotation().getSin(),
0));
wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
wheelSpeeds.frontLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.frontRight += rand.nextGaussian() * 0.1;
wheelSpeeds.rearLeft += rand.nextGaussian() * 0.1;
wheelSpeeds.rearRight += rand.nextGaussian() * 0.1;
wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt;
wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt;
wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt;
wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt;
wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt;
wheelPositions.frontRight += wheelSpeeds.frontRight * dt;
wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt;
wheelPositions.rearRight += wheelSpeeds.rearRight * dt;
var lastPose = odometry.getPoseMeters();
var lastPose = odometry.getPose();
var xHat = odometry.update(new Rotation2d(rand.nextGaussian() * 0.05), wheelPositions);
odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation());
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
@@ -255,8 +244,7 @@ class MecanumDriveOdometryTest {
t += dt;
}
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error");
assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.15, "Incorrect mean error");
assertEquals(0.0, maxError, 0.3, "Incorrect max error");
assertEquals(
1.0,

40
wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeedsTest.java
View File

@@ -18,10 +18,10 @@ class MecanumDriveWheelSpeedsTest {
final var wheelSpeeds = left.plus(right);
assertAll(
() -> assertEquals(3.0, wheelSpeeds.frontLeftMetersPerSecond),
() -> assertEquals(2.0, wheelSpeeds.frontRightMetersPerSecond),
() -> assertEquals(2.5, wheelSpeeds.rearLeftMetersPerSecond),
() -> assertEquals(2.5, wheelSpeeds.rearRightMetersPerSecond));
() -> assertEquals(3.0, wheelSpeeds.frontLeft),
() -> assertEquals(2.0, wheelSpeeds.frontRight),
() -> assertEquals(2.5, wheelSpeeds.rearLeft),
() -> assertEquals(2.5, wheelSpeeds.rearRight));
}
@Test
@@ -32,10 +32,10 @@ class MecanumDriveWheelSpeedsTest {
final var wheelSpeeds = left.minus(right);
assertAll(
() -> assertEquals(-1.0, wheelSpeeds.frontLeftMetersPerSecond),
() -> assertEquals(0.0, wheelSpeeds.frontRightMetersPerSecond),
() -> assertEquals(1.5, wheelSpeeds.rearLeftMetersPerSecond),
() -> assertEquals(0.5, wheelSpeeds.rearRightMetersPerSecond));
() -> assertEquals(-1.0, wheelSpeeds.frontLeft),
() -> assertEquals(0.0, wheelSpeeds.frontRight),
() -> assertEquals(1.5, wheelSpeeds.rearLeft),
() -> assertEquals(0.5, wheelSpeeds.rearRight));
}
@Test
@@ -43,10 +43,10 @@ class MecanumDriveWheelSpeedsTest {
final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).unaryMinus();
assertAll(
() -> assertEquals(-1.0, wheelSpeeds.frontLeftMetersPerSecond),
() -> assertEquals(-0.5, wheelSpeeds.frontRightMetersPerSecond),
() -> assertEquals(-2.0, wheelSpeeds.rearLeftMetersPerSecond),
() -> assertEquals(-1.5, wheelSpeeds.rearRightMetersPerSecond));
() -> assertEquals(-1.0, wheelSpeeds.frontLeft),
() -> assertEquals(-0.5, wheelSpeeds.frontRight),
() -> assertEquals(-2.0, wheelSpeeds.rearLeft),
() -> assertEquals(-1.5, wheelSpeeds.rearRight));
}
@Test
@@ -54,10 +54,10 @@ class MecanumDriveWheelSpeedsTest {
final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).times(2.0);
assertAll(
() -> assertEquals(2.0, wheelSpeeds.frontLeftMetersPerSecond),
() -> assertEquals(1.0, wheelSpeeds.frontRightMetersPerSecond),
() -> assertEquals(4.0, wheelSpeeds.rearLeftMetersPerSecond),
() -> assertEquals(3.0, wheelSpeeds.rearRightMetersPerSecond));
() -> assertEquals(2.0, wheelSpeeds.frontLeft),
() -> assertEquals(1.0, wheelSpeeds.frontRight),
() -> assertEquals(4.0, wheelSpeeds.rearLeft),
() -> assertEquals(3.0, wheelSpeeds.rearRight));
}
@Test
@@ -65,9 +65,9 @@ class MecanumDriveWheelSpeedsTest {
final var wheelSpeeds = new MecanumDriveWheelSpeeds(1.0, 0.5, 2.0, 1.5).div(2.0);
assertAll(
() -> assertEquals(0.5, wheelSpeeds.frontLeftMetersPerSecond),
() -> assertEquals(0.25, wheelSpeeds.frontRightMetersPerSecond),
() -> assertEquals(1.0, wheelSpeeds.rearLeftMetersPerSecond),
() -> assertEquals(0.75, wheelSpeeds.rearRightMetersPerSecond));
() -> assertEquals(0.5, wheelSpeeds.frontLeft),
() -> assertEquals(0.25, wheelSpeeds.frontRight),
() -> assertEquals(1.0, wheelSpeeds.rearLeft),
() -> assertEquals(0.75, wheelSpeeds.rearRight));
}
}

108
wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
View File

@@ -29,10 +29,10 @@ class SwerveDriveKinematicsTest {
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(5.0, moduleStates[0].speed, kEpsilon),
() -> assertEquals(5.0, moduleStates[1].speed, kEpsilon),
() -> assertEquals(5.0, moduleStates[2].speed, kEpsilon),
() -> assertEquals(5.0, moduleStates[3].speed, 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),
@@ -45,9 +45,9 @@ class SwerveDriveKinematicsTest {
var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
assertAll(
() -> assertEquals(5.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
() -> assertEquals(5.0, chassisSpeeds.vx, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omega, kEpsilon));
}
@Test
@@ -68,10 +68,10 @@ class SwerveDriveKinematicsTest {
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(5.0, moduleStates[0].speed, kEpsilon),
() -> assertEquals(5.0, moduleStates[1].speed, kEpsilon),
() -> assertEquals(5.0, moduleStates[2].speed, kEpsilon),
() -> assertEquals(5.0, moduleStates[3].speed, 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),
@@ -84,9 +84,9 @@ class SwerveDriveKinematicsTest {
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));
() -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon),
() -> assertEquals(5.0, chassisSpeeds.vy, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.omega, kEpsilon));
}
@Test
@@ -109,10 +109,10 @@ class SwerveDriveKinematicsTest {
// Robot is stationary, but module angles are preserved.
assertAll(
() -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, moduleStates[0].speed, kEpsilon),
() -> assertEquals(0.0, moduleStates[1].speed, kEpsilon),
() -> assertEquals(0.0, moduleStates[2].speed, kEpsilon),
() -> assertEquals(0.0, moduleStates[3].speed, kEpsilon),
() -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon),
@@ -131,10 +131,10 @@ class SwerveDriveKinematicsTest {
// Robot is stationary, but module angles are preserved.
assertAll(
() -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, moduleStates[0].speed, kEpsilon),
() -> assertEquals(0.0, moduleStates[1].speed, kEpsilon),
() -> assertEquals(0.0, moduleStates[2].speed, kEpsilon),
() -> assertEquals(0.0, moduleStates[3].speed, kEpsilon),
() -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon),
() -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon),
() -> assertEquals(180.0, moduleStates[2].angle.getDegrees(), kEpsilon),
@@ -154,10 +154,10 @@ class SwerveDriveKinematicsTest {
*/
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(106.63, moduleStates[0].speed, 0.1),
() -> assertEquals(106.63, moduleStates[1].speed, 0.1),
() -> assertEquals(106.63, moduleStates[2].speed, 0.1),
() -> assertEquals(106.63, moduleStates[3].speed, 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),
@@ -174,9 +174,9 @@ class SwerveDriveKinematicsTest {
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));
() -> assertEquals(0.0, chassisSpeeds.vx, kEpsilon),
() -> assertEquals(0.0, chassisSpeeds.vy, kEpsilon),
() -> assertEquals(2 * Math.PI, chassisSpeeds.omega, 0.1));
}
@Test
@@ -209,10 +209,10 @@ class SwerveDriveKinematicsTest {
*/
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].speed, 0.1),
() -> assertEquals(150.796, moduleStates[1].speed, 0.1),
() -> assertEquals(150.796, moduleStates[2].speed, 0.1),
() -> assertEquals(213.258, moduleStates[3].speed, 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),
@@ -238,9 +238,9 @@ class SwerveDriveKinematicsTest {
*/
assertAll(
() -> assertEquals(75.398, chassisSpeeds.vxMetersPerSecond, 0.1),
() -> assertEquals(-75.398, chassisSpeeds.vyMetersPerSecond, 0.1),
() -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1));
() -> assertEquals(75.398, chassisSpeeds.vx, 0.1),
() -> assertEquals(-75.398, chassisSpeeds.vy, 0.1),
() -> assertEquals(2 * Math.PI, chassisSpeeds.omega, 0.1));
}
@Test
@@ -270,11 +270,7 @@ class SwerveDriveKinematicsTest {
private void assertModuleState(
SwerveModuleState expected, SwerveModuleState actual, SwerveModuleState tolerance) {
assertAll(
() ->
assertEquals(
expected.speedMetersPerSecond,
actual.speedMetersPerSecond,
tolerance.speedMetersPerSecond),
() -> assertEquals(expected.speed, actual.speed, tolerance.speed),
() ->
assertEquals(
expected.angle.getDegrees(),
@@ -326,9 +322,9 @@ class SwerveDriveKinematicsTest {
*/
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, 0.1),
() -> assertEquals(-33.0, chassisSpeeds.vyMetersPerSecond, 0.1),
() -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond, 0.1));
() -> assertEquals(0.0, chassisSpeeds.vx, 0.1),
() -> assertEquals(-33.0, chassisSpeeds.vy, 0.1),
() -> assertEquals(1.5, chassisSpeeds.omega, 0.1));
}
@Test
@@ -368,10 +364,10 @@ class SwerveDriveKinematicsTest {
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));
() -> assertEquals(5.0 * factor, arr[0].speed, kEpsilon),
() -> assertEquals(6.0 * factor, arr[1].speed, kEpsilon),
() -> assertEquals(4.0 * factor, arr[2].speed, kEpsilon),
() -> assertEquals(7.0 * factor, arr[3].speed, kEpsilon));
}
@Test
@@ -388,10 +384,10 @@ class SwerveDriveKinematicsTest {
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));
() -> assertEquals(5.0 * factor, arr[0].speed, kEpsilon),
() -> assertEquals(6.0 * factor, arr[1].speed, kEpsilon),
() -> assertEquals(4.0 * factor, arr[2].speed, kEpsilon),
() -> assertEquals(7.0 * factor, arr[3].speed, kEpsilon));
}
@Test
@@ -405,9 +401,9 @@ class SwerveDriveKinematicsTest {
SwerveDriveKinematics.desaturateWheelSpeeds(arr, 1);
assertAll(
() -> assertEquals(0.5, arr[0].speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.5, arr[1].speedMetersPerSecond, kEpsilon),
() -> assertEquals(-1.0, arr[2].speedMetersPerSecond, kEpsilon),
() -> assertEquals(-1.0, arr[3].speedMetersPerSecond, kEpsilon));
() -> assertEquals(0.5, arr[0].speed, kEpsilon),
() -> assertEquals(0.5, arr[1].speed, kEpsilon),
() -> assertEquals(-1.0, arr[2].speed, kEpsilon),
() -> assertEquals(-1.0, arr[3].speed, kEpsilon));
}
}

73
wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java
View File

@@ -43,7 +43,7 @@ class SwerveDriveOdometry3dTest {
Rotation3d.kZero,
new SwerveModulePosition[] {zero, zero, zero, zero},
new Pose3d(1, 2, 0, new Rotation3d(0, 0, Units.degreesToRadians(45))));
var pose = odometry.getPoseMeters();
var pose = odometry.getPose();
assertAll(
() -> assertEquals(pose.getX(), 1.0, 1e-9),
() -> assertEquals(pose.getY(), 2.0, 1e-9),
@@ -158,25 +158,24 @@ class SwerveDriveOdometry3dTest {
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
var moduleStates =
kinematics.toSwerveModuleStates(
new ChassisSpeeds(
groundTruthState.velocityMetersPerSecond,
groundTruthState.velocity,
0.0,
groundTruthState.velocityMetersPerSecond
* groundTruthState.curvatureRadPerMeter));
groundTruthState.velocity * groundTruthState.curvature));
for (var moduleState : moduleStates) {
moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1;
moduleState.speed += rand.nextGaussian() * 0.1;
}
fl.distanceMeters += moduleStates[0].speedMetersPerSecond * dt;
fr.distanceMeters += moduleStates[1].speedMetersPerSecond * dt;
bl.distanceMeters += moduleStates[2].speedMetersPerSecond * dt;
br.distanceMeters += moduleStates[3].speedMetersPerSecond * dt;
fl.distance += moduleStates[0].speed * dt;
fr.distance += moduleStates[1].speed * dt;
bl.distance += moduleStates[2].speed * dt;
br.distance += moduleStates[3].speed * dt;
fl.angle = moduleStates[0].angle;
fr.angle = moduleStates[1].angle;
@@ -187,14 +186,14 @@ class SwerveDriveOdometry3dTest {
odometry.update(
new Rotation3d(
groundTruthState
.poseMeters
.pose
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05))),
new SwerveModulePosition[] {fl, fr, bl, br});
double error =
groundTruthState
.poseMeters
.pose
.getTranslation()
.getDistance(xHat.getTranslation().toTranslation2d());
if (error > maxError) {
@@ -205,17 +204,16 @@ class SwerveDriveOdometry3dTest {
t += dt;
}
assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X");
assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y");
assertEquals(0.0, odometry.getPoseMeters().getZ(), 1e-1, "Incorrect Final Y");
assertEquals(0.0, odometry.getPose().getX(), 1e-1, "Incorrect Final X");
assertEquals(0.0, odometry.getPose().getY(), 1e-1, "Incorrect Final Y");
assertEquals(0.0, odometry.getPose().getZ(), 1e-1, "Incorrect Final Y");
assertEquals(
Math.PI / 4,
odometry.getPoseMeters().getRotation().toRotation2d().getRadians(),
odometry.getPose().getRotation().toRotation2d().getRadians(),
10 * Math.PI / 180,
"Incorrect Final Theta");
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.05, "Incorrect mean error");
assertEquals(0.0, maxError, 0.125, "Incorrect max error");
}
@@ -253,26 +251,18 @@ class SwerveDriveOdometry3dTest {
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
fl.distanceMeters +=
groundTruthState.velocityMetersPerSecond * dt
+ 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
fr.distanceMeters +=
groundTruthState.velocityMetersPerSecond * dt
+ 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
bl.distanceMeters +=
groundTruthState.velocityMetersPerSecond * dt
+ 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
br.distanceMeters +=
groundTruthState.velocityMetersPerSecond * dt
+ 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
fl.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
fr.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
bl.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
br.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
fl.angle = groundTruthState.poseMeters.getRotation();
fr.angle = groundTruthState.poseMeters.getRotation();
bl.angle = groundTruthState.poseMeters.getRotation();
br.angle = groundTruthState.poseMeters.getRotation();
fl.angle = groundTruthState.pose.getRotation();
fr.angle = groundTruthState.pose.getRotation();
bl.angle = groundTruthState.pose.getRotation();
br.angle = groundTruthState.pose.getRotation();
var xHat =
odometry.update(
@@ -281,7 +271,7 @@ class SwerveDriveOdometry3dTest {
double error =
groundTruthState
.poseMeters
.pose
.getTranslation()
.getDistance(xHat.getTranslation().toTranslation2d());
if (error > maxError) {
@@ -292,17 +282,16 @@ class SwerveDriveOdometry3dTest {
t += dt;
}
assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X");
assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y");
assertEquals(0.0, odometry.getPoseMeters().getZ(), 1e-1, "Incorrect Final Y");
assertEquals(0.0, odometry.getPose().getX(), 1e-1, "Incorrect Final X");
assertEquals(0.0, odometry.getPose().getY(), 1e-1, "Incorrect Final Y");
assertEquals(0.0, odometry.getPose().getZ(), 1e-1, "Incorrect Final Y");
assertEquals(
0.0,
odometry.getPoseMeters().getRotation().toRotation2d().getRadians(),
odometry.getPose().getRotation().toRotation2d().getRadians(),
10 * Math.PI / 180,
"Incorrect Final Theta");
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.06, "Incorrect mean error");
assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.06, "Incorrect mean error");
assertEquals(0.0, maxError, 0.125, "Incorrect max error");
}
}

72
wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
View File

@@ -135,25 +135,24 @@ class SwerveDriveOdometryTest {
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
var moduleStates =
kinematics.toSwerveModuleStates(
new ChassisSpeeds(
groundTruthState.velocityMetersPerSecond,
groundTruthState.velocity,
0.0,
groundTruthState.velocityMetersPerSecond
* groundTruthState.curvatureRadPerMeter));
groundTruthState.velocity * groundTruthState.curvature));
for (var moduleState : moduleStates) {
moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1;
moduleState.speed += rand.nextGaussian() * 0.1;
}
fl.distanceMeters += moduleStates[0].speedMetersPerSecond * dt;
fr.distanceMeters += moduleStates[1].speedMetersPerSecond * dt;
bl.distanceMeters += moduleStates[2].speedMetersPerSecond * dt;
br.distanceMeters += moduleStates[3].speedMetersPerSecond * dt;
fl.distance += moduleStates[0].speed * dt;
fr.distance += moduleStates[1].speed * dt;
bl.distance += moduleStates[2].speed * dt;
br.distance += moduleStates[3].speed * dt;
fl.angle = moduleStates[0].angle;
fr.angle = moduleStates[1].angle;
@@ -162,14 +161,10 @@ class SwerveDriveOdometryTest {
var xHat =
odometry.update(
groundTruthState
.poseMeters
.getRotation()
.plus(new Rotation2d(rand.nextGaussian() * 0.05)),
groundTruthState.pose.getRotation().plus(new Rotation2d(rand.nextGaussian() * 0.05)),
new SwerveModulePosition[] {fl, fr, bl, br});
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
@@ -178,16 +173,15 @@ class SwerveDriveOdometryTest {
t += dt;
}
assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X");
assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y");
assertEquals(0.0, odometry.getPose().getX(), 1e-1, "Incorrect Final X");
assertEquals(0.0, odometry.getPose().getY(), 1e-1, "Incorrect Final Y");
assertEquals(
Math.PI / 4,
odometry.getPoseMeters().getRotation().getRadians(),
odometry.getPose().getRotation().getRadians(),
10 * Math.PI / 180,
"Incorrect Final Theta");
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error");
assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.05, "Incorrect mean error");
assertEquals(0.0, maxError, 0.125, "Incorrect max error");
}
@@ -225,34 +219,25 @@ class SwerveDriveOdometryTest {
double maxError = Double.NEGATIVE_INFINITY;
double errorSum = 0;
while (t <= trajectory.getTotalTimeSeconds()) {
while (t <= trajectory.getTotalTime()) {
var groundTruthState = trajectory.sample(t);
fl.distanceMeters +=
groundTruthState.velocityMetersPerSecond * dt
+ 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
fr.distanceMeters +=
groundTruthState.velocityMetersPerSecond * dt
+ 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
bl.distanceMeters +=
groundTruthState.velocityMetersPerSecond * dt
+ 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
br.distanceMeters +=
groundTruthState.velocityMetersPerSecond * dt
+ 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt;
fl.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
fr.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
bl.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
br.distance += groundTruthState.velocity * dt + 0.5 * groundTruthState.acceleration * dt * dt;
fl.angle = groundTruthState.poseMeters.getRotation();
fr.angle = groundTruthState.poseMeters.getRotation();
bl.angle = groundTruthState.poseMeters.getRotation();
br.angle = groundTruthState.poseMeters.getRotation();
fl.angle = groundTruthState.pose.getRotation();
fr.angle = groundTruthState.pose.getRotation();
bl.angle = groundTruthState.pose.getRotation();
br.angle = groundTruthState.pose.getRotation();
var xHat =
odometry.update(
new Rotation2d(rand.nextGaussian() * 0.05),
new SwerveModulePosition[] {fl, fr, bl, br});
double error =
groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
double error = groundTruthState.pose.getTranslation().getDistance(xHat.getTranslation());
if (error > maxError) {
maxError = error;
}
@@ -261,16 +246,15 @@ class SwerveDriveOdometryTest {
t += dt;
}
assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X");
assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y");
assertEquals(0.0, odometry.getPose().getX(), 1e-1, "Incorrect Final X");
assertEquals(0.0, odometry.getPose().getY(), 1e-1, "Incorrect Final Y");
assertEquals(
0.0,
odometry.getPoseMeters().getRotation().getRadians(),
odometry.getPose().getRotation().getRadians(),
10 * Math.PI / 180,
"Incorrect Final Theta");
assertEquals(
0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.06, "Incorrect mean error");
assertEquals(0.0, errorSum / (trajectory.getTotalTime() / dt), 0.06, "Incorrect mean error");
assertEquals(0.0, maxError, 0.125, "Incorrect max error");
}
}

32
wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java
View File

@@ -20,7 +20,7 @@ class SwerveModuleStateTest {
refA.optimize(angleA);
assertAll(
() -> assertEquals(2.0, refA.speedMetersPerSecond, kEpsilon),
() -> assertEquals(2.0, refA.speed, kEpsilon),
() -> assertEquals(0.0, refA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.fromDegrees(-50);
@@ -28,7 +28,7 @@ class SwerveModuleStateTest {
refB.optimize(angleB);
assertAll(
() -> assertEquals(-4.7, refB.speedMetersPerSecond, kEpsilon),
() -> assertEquals(-4.7, refB.speed, kEpsilon),
() -> assertEquals(-139.0, refB.angle.getDegrees(), kEpsilon));
}
@@ -39,7 +39,7 @@ class SwerveModuleStateTest {
refA.optimize(angleA);
assertAll(
() -> assertEquals(2.0, refA.speedMetersPerSecond, kEpsilon),
() -> assertEquals(2.0, refA.speed, kEpsilon),
() -> assertEquals(89.0, refA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.kZero;
@@ -47,7 +47,7 @@ class SwerveModuleStateTest {
refB.optimize(angleB);
assertAll(
() -> assertEquals(-2.0, refB.speedMetersPerSecond, kEpsilon),
() -> assertEquals(-2.0, refB.speed, kEpsilon),
() -> assertEquals(-2.0, refB.angle.getDegrees(), kEpsilon));
}
@@ -58,7 +58,7 @@ class SwerveModuleStateTest {
refA.cosineScale(angleA);
assertAll(
() -> assertEquals(Math.sqrt(2.0), refA.speedMetersPerSecond, kEpsilon),
() -> assertEquals(Math.sqrt(2.0), refA.speed, kEpsilon),
() -> assertEquals(45.0, refA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.fromDegrees(45.0);
@@ -66,7 +66,7 @@ class SwerveModuleStateTest {
refB.cosineScale(angleB);
assertAll(
() -> assertEquals(2.0, refB.speedMetersPerSecond, kEpsilon),
() -> assertEquals(2.0, refB.speed, kEpsilon),
() -> assertEquals(45.0, refB.angle.getDegrees(), kEpsilon));
var angleC = Rotation2d.fromDegrees(-45.0);
@@ -74,7 +74,7 @@ class SwerveModuleStateTest {
refC.cosineScale(angleC);
assertAll(
() -> assertEquals(0.0, refC.speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, refC.speed, kEpsilon),
() -> assertEquals(45.0, refC.angle.getDegrees(), kEpsilon));
var angleD = Rotation2d.fromDegrees(135.0);
@@ -82,7 +82,7 @@ class SwerveModuleStateTest {
refD.cosineScale(angleD);
assertAll(
() -> assertEquals(0.0, refD.speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, refD.speed, kEpsilon),
() -> assertEquals(45.0, refD.angle.getDegrees(), kEpsilon));
var angleE = Rotation2d.fromDegrees(-135.0);
@@ -90,7 +90,7 @@ class SwerveModuleStateTest {
refE.cosineScale(angleE);
assertAll(
() -> assertEquals(-2.0, refE.speedMetersPerSecond, kEpsilon),
() -> assertEquals(-2.0, refE.speed, kEpsilon),
() -> assertEquals(45.0, refE.angle.getDegrees(), kEpsilon));
var angleF = Rotation2d.fromDegrees(180.0);
@@ -98,7 +98,7 @@ class SwerveModuleStateTest {
refF.cosineScale(angleF);
assertAll(
() -> assertEquals(-Math.sqrt(2.0), refF.speedMetersPerSecond, kEpsilon),
() -> assertEquals(-Math.sqrt(2.0), refF.speed, kEpsilon),
() -> assertEquals(45.0, refF.angle.getDegrees(), kEpsilon));
var angleG = Rotation2d.fromDegrees(0.0);
@@ -106,7 +106,7 @@ class SwerveModuleStateTest {
refG.cosineScale(angleG);
assertAll(
() -> assertEquals(-Math.sqrt(2.0), refG.speedMetersPerSecond, kEpsilon),
() -> assertEquals(-Math.sqrt(2.0), refG.speed, kEpsilon),
() -> assertEquals(45.0, refG.angle.getDegrees(), kEpsilon));
var angleH = Rotation2d.fromDegrees(45.0);
@@ -114,7 +114,7 @@ class SwerveModuleStateTest {
refH.cosineScale(angleH);
assertAll(
() -> assertEquals(-2.0, refH.speedMetersPerSecond, kEpsilon),
() -> assertEquals(-2.0, refH.speed, kEpsilon),
() -> assertEquals(45.0, refH.angle.getDegrees(), kEpsilon));
var angleI = Rotation2d.fromDegrees(-45.0);
@@ -122,7 +122,7 @@ class SwerveModuleStateTest {
refI.cosineScale(angleI);
assertAll(
() -> assertEquals(0.0, refI.speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, refI.speed, kEpsilon),
() -> assertEquals(45.0, refI.angle.getDegrees(), kEpsilon));
var angleJ = Rotation2d.fromDegrees(135.0);
@@ -130,7 +130,7 @@ class SwerveModuleStateTest {
refJ.cosineScale(angleJ);
assertAll(
() -> assertEquals(0.0, refJ.speedMetersPerSecond, kEpsilon),
() -> assertEquals(0.0, refJ.speed, kEpsilon),
() -> assertEquals(45.0, refJ.angle.getDegrees(), kEpsilon));
var angleK = Rotation2d.fromDegrees(-135.0);
@@ -138,7 +138,7 @@ class SwerveModuleStateTest {
refK.cosineScale(angleK);
assertAll(
() -> assertEquals(2.0, refK.speedMetersPerSecond, kEpsilon),
() -> assertEquals(2.0, refK.speed, kEpsilon),
() -> assertEquals(45.0, refK.angle.getDegrees(), kEpsilon));
var angleL = Rotation2d.fromDegrees(180.0);
@@ -146,7 +146,7 @@ class SwerveModuleStateTest {
refL.cosineScale(angleL);
assertAll(
() -> assertEquals(Math.sqrt(2.0), refL.speedMetersPerSecond, kEpsilon),
() -> assertEquals(Math.sqrt(2.0), refL.speed, kEpsilon),
() -> assertEquals(45.0, refL.angle.getDegrees(), kEpsilon));
}
}

6
wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/ChassisSpeedsProtoTest.java
View File

@@ -19,8 +19,8 @@ class ChassisSpeedsProtoTest {
ChassisSpeeds.proto.pack(proto, DATA);
ChassisSpeeds data = ChassisSpeeds.proto.unpack(proto);
assertEquals(DATA.vxMetersPerSecond, data.vxMetersPerSecond);
assertEquals(DATA.vyMetersPerSecond, data.vyMetersPerSecond);
assertEquals(DATA.omegaRadiansPerSecond, data.omegaRadiansPerSecond);
assertEquals(DATA.vx, data.vx);
assertEquals(DATA.vy, data.vy);
assertEquals(DATA.omega, data.omega);
}
}

2
wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveKinematicsProtoTest.java
View File

@@ -19,6 +19,6 @@ class DifferentialDriveKinematicsProtoTest {
DifferentialDriveKinematics.proto.pack(proto, DATA);
DifferentialDriveKinematics data = DifferentialDriveKinematics.proto.unpack(proto);
assertEquals(DATA.trackWidthMeters, data.trackWidthMeters);
assertEquals(DATA.trackwidth, data.trackwidth);
}
}

4
wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/DifferentialDriveWheelSpeedsProtoTest.java
View File

@@ -20,7 +20,7 @@ class DifferentialDriveWheelSpeedsProtoTest {
DifferentialDriveWheelSpeeds.proto.pack(proto, DATA);
DifferentialDriveWheelSpeeds data = DifferentialDriveWheelSpeeds.proto.unpack(proto);
assertEquals(DATA.leftMetersPerSecond, data.leftMetersPerSecond);
assertEquals(DATA.rightMetersPerSecond, data.rightMetersPerSecond);
assertEquals(DATA.left, data.left);
assertEquals(DATA.right, data.right);
}
}

8
wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelPositionsProtoTest.java
View File

@@ -20,9 +20,9 @@ class MecanumDriveWheelPositionsProtoTest {
MecanumDriveWheelPositions.proto.pack(proto, DATA);
MecanumDriveWheelPositions data = MecanumDriveWheelPositions.proto.unpack(proto);
assertEquals(DATA.frontLeftMeters, data.frontLeftMeters);
assertEquals(DATA.frontRightMeters, data.frontRightMeters);
assertEquals(DATA.rearLeftMeters, data.rearLeftMeters);
assertEquals(DATA.rearRightMeters, data.rearRightMeters);
assertEquals(DATA.frontLeft, data.frontLeft);
assertEquals(DATA.frontRight, data.frontRight);
assertEquals(DATA.rearLeft, data.rearLeft);
assertEquals(DATA.rearRight, data.rearRight);
}
}

8
wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/MecanumDriveWheelSpeedsProtoTest.java
View File

@@ -20,9 +20,9 @@ class MecanumDriveWheelSpeedsProtoTest {
MecanumDriveWheelSpeeds.proto.pack(proto, DATA);
MecanumDriveWheelSpeeds data = MecanumDriveWheelSpeeds.proto.unpack(proto);
assertEquals(DATA.frontLeftMetersPerSecond, data.frontLeftMetersPerSecond);
assertEquals(DATA.frontRightMetersPerSecond, data.frontRightMetersPerSecond);
assertEquals(DATA.rearLeftMetersPerSecond, data.rearLeftMetersPerSecond);
assertEquals(DATA.rearRightMetersPerSecond, data.rearRightMetersPerSecond);
assertEquals(DATA.frontLeft, data.frontLeft);
assertEquals(DATA.frontRight, data.frontRight);
assertEquals(DATA.rearLeft, data.rearLeft);
assertEquals(DATA.rearRight, data.rearRight);
}
}

2
wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModulePositionProtoTest.java
View File

@@ -21,7 +21,7 @@ class SwerveModulePositionProtoTest {
SwerveModulePosition.proto.pack(proto, DATA);
SwerveModulePosition data = SwerveModulePosition.proto.unpack(proto);
assertEquals(DATA.distanceMeters, data.distanceMeters);
assertEquals(DATA.distance, data.distance);
assertEquals(DATA.angle, data.angle);
}
}

2
wpimath/src/test/java/edu/wpi/first/math/kinematics/proto/SwerveModuleStateProtoTest.java
View File

@@ -20,7 +20,7 @@ class SwerveModuleStateProtoTest {
SwerveModuleState.proto.pack(proto, DATA);
SwerveModuleState data = SwerveModuleState.proto.unpack(proto);
assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond);
assertEquals(DATA.speed, data.speed);
assertEquals(DATA.angle, data.angle);
}
}

6
wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/ChassisSpeedsStructTest.java
View File

@@ -22,8 +22,8 @@ class ChassisSpeedsStructTest {
buffer.rewind();
ChassisSpeeds data = ChassisSpeeds.struct.unpack(buffer);
assertEquals(DATA.vxMetersPerSecond, data.vxMetersPerSecond);
assertEquals(DATA.vyMetersPerSecond, data.vyMetersPerSecond);
assertEquals(DATA.omegaRadiansPerSecond, data.omegaRadiansPerSecond);
assertEquals(DATA.vx, data.vx);
assertEquals(DATA.vy, data.vy);
assertEquals(DATA.omega, data.omega);
}
}

2
wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveKinematicsStructTest.java
View File

@@ -22,6 +22,6 @@ class DifferentialDriveKinematicsStructTest {
buffer.rewind();
DifferentialDriveKinematics data = DifferentialDriveKinematics.struct.unpack(buffer);
assertEquals(DATA.trackWidthMeters, data.trackWidthMeters);
assertEquals(DATA.trackwidth, data.trackwidth);
}
}

4
wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelPositionsStructTest.java
View File

@@ -23,7 +23,7 @@ class DifferentialDriveWheelPositionsStructTest {
buffer.rewind();
DifferentialDriveWheelPositions data = DifferentialDriveWheelPositions.struct.unpack(buffer);
assertEquals(DATA.leftMeters, data.leftMeters);
assertEquals(DATA.rightMeters, data.rightMeters);
assertEquals(DATA.left, data.left);
assertEquals(DATA.right, data.right);
}
}

4
wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/DifferentialDriveWheelSpeedsStructTest.java
View File

@@ -23,7 +23,7 @@ class DifferentialDriveWheelSpeedsStructTest {
buffer.rewind();
DifferentialDriveWheelSpeeds data = DifferentialDriveWheelSpeeds.struct.unpack(buffer);
assertEquals(DATA.leftMetersPerSecond, data.leftMetersPerSecond);
assertEquals(DATA.rightMetersPerSecond, data.rightMetersPerSecond);
assertEquals(DATA.left, data.left);
assertEquals(DATA.right, data.right);
}
}

8
wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelPositionsStructTest.java
View File

@@ -23,9 +23,9 @@ class MecanumDriveWheelPositionsStructTest {
buffer.rewind();
MecanumDriveWheelPositions data = MecanumDriveWheelPositions.struct.unpack(buffer);
assertEquals(DATA.frontLeftMeters, data.frontLeftMeters);
assertEquals(DATA.frontRightMeters, data.frontRightMeters);
assertEquals(DATA.rearLeftMeters, data.rearLeftMeters);
assertEquals(DATA.rearRightMeters, data.rearRightMeters);
assertEquals(DATA.frontLeft, data.frontLeft);
assertEquals(DATA.frontRight, data.frontRight);
assertEquals(DATA.rearLeft, data.rearLeft);
assertEquals(DATA.rearRight, data.rearRight);
}
}

8
wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/MecanumDriveWheelSpeedsStructTest.java
View File

@@ -23,9 +23,9 @@ class MecanumDriveWheelSpeedsStructTest {
buffer.rewind();
MecanumDriveWheelSpeeds data = MecanumDriveWheelSpeeds.struct.unpack(buffer);
assertEquals(DATA.frontLeftMetersPerSecond, data.frontLeftMetersPerSecond);
assertEquals(DATA.frontRightMetersPerSecond, data.frontRightMetersPerSecond);
assertEquals(DATA.rearLeftMetersPerSecond, data.rearLeftMetersPerSecond);
assertEquals(DATA.rearRightMetersPerSecond, data.rearRightMetersPerSecond);
assertEquals(DATA.frontLeft, data.frontLeft);
assertEquals(DATA.frontRight, data.frontRight);
assertEquals(DATA.rearLeft, data.rearLeft);
assertEquals(DATA.rearRight, data.rearRight);
}
}

2
wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModulePositionStructTest.java
View File

@@ -24,7 +24,7 @@ class SwerveModulePositionStructTest {
buffer.rewind();
SwerveModulePosition data = SwerveModulePosition.struct.unpack(buffer);
assertEquals(DATA.distanceMeters, data.distanceMeters);
assertEquals(DATA.distance, data.distance);
assertEquals(DATA.angle, data.angle);
}
}

2
wpimath/src/test/java/edu/wpi/first/math/kinematics/struct/SwerveModuleStateStructTest.java
View File

@@ -23,7 +23,7 @@ class SwerveModuleStateStructTest {
buffer.rewind();
SwerveModuleState data = SwerveModuleState.struct.unpack(buffer);
assertEquals(DATA.speedMetersPerSecond, data.speedMetersPerSecond);
assertEquals(DATA.speed, data.speed);
assertEquals(DATA.angle, data.angle);
}
}

18
wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java
View File

@@ -53,7 +53,7 @@ class CubicHermiteSplineTest {
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);
var twist = p0.pose.log(p1.pose);
assertAll(
() -> assertTrue(Math.abs(twist.dx) < kMaxDx),
() -> assertTrue(Math.abs(twist.dy) < kMaxDy),
@@ -62,20 +62,18 @@ class CubicHermiteSplineTest {
// 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.getX(), poses.get(0).pose.getX(), 1E-9),
() -> assertEquals(a.getY(), poses.get(0).pose.getY(), 1E-9),
() ->
assertEquals(
a.getRotation().getRadians(),
poses.get(0).poseMeters.getRotation().getRadians(),
1E-9));
a.getRotation().getRadians(), poses.get(0).pose.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) {
if (waypoint.getDistance(state.pose.getTranslation()) == 0) {
found = true;
}
}
@@ -86,12 +84,12 @@ class CubicHermiteSplineTest {
// Check last point
assertAll(
() -> assertEquals(b.getX(), poses.get(poses.size() - 1).poseMeters.getX(), 1E-9),
() -> assertEquals(b.getY(), poses.get(poses.size() - 1).poseMeters.getY(), 1E-9),
() -> assertEquals(b.getX(), poses.get(poses.size() - 1).pose.getX(), 1E-9),
() -> assertEquals(b.getY(), poses.get(poses.size() - 1).pose.getY(), 1E-9),
() ->
assertEquals(
b.getRotation().getRadians(),
poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(),
poses.get(poses.size() - 1).pose.getRotation().getRadians(),
1E-9));
}

16
wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java
View File

@@ -39,7 +39,7 @@ class QuinticHermiteSplineTest {
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);
var twist = p0.pose.log(p1.pose);
assertAll(
() -> assertTrue(Math.abs(twist.dx) < kMaxDx),
() -> assertTrue(Math.abs(twist.dy) < kMaxDy),
@@ -48,22 +48,20 @@ class QuinticHermiteSplineTest {
// Check first point
assertAll(
() -> assertEquals(a.getX(), poses.get(0).poseMeters.getX(), 1E-9),
() -> assertEquals(a.getY(), poses.get(0).poseMeters.getY(), 1E-9),
() -> assertEquals(a.getX(), poses.get(0).pose.getX(), 1E-9),
() -> assertEquals(a.getY(), poses.get(0).pose.getY(), 1E-9),
() ->
assertEquals(
a.getRotation().getRadians(),
poses.get(0).poseMeters.getRotation().getRadians(),
1E-9));
a.getRotation().getRadians(), poses.get(0).pose.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.getX(), poses.get(poses.size() - 1).pose.getX(), 1E-9),
() -> assertEquals(b.getY(), poses.get(poses.size() - 1).pose.getY(), 1E-9),
() ->
assertEquals(
b.getRotation().getRadians(),
poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(),
poses.get(poses.size() - 1).pose.getRotation().getRadians(),
1E-9));
}

16
wpimath/src/test/java/edu/wpi/first/math/system/plant/proto/DCMotorProtoTest.java
View File

@@ -19,13 +19,13 @@ class DCMotorProtoTest {
DCMotor.proto.pack(proto, DATA);
DCMotor data = DCMotor.proto.unpack(proto);
assertEquals(DATA.nominalVoltageVolts, data.nominalVoltageVolts);
assertEquals(DATA.stallTorqueNewtonMeters, data.stallTorqueNewtonMeters);
assertEquals(DATA.stallCurrentAmps, data.stallCurrentAmps);
assertEquals(DATA.freeCurrentAmps, data.freeCurrentAmps);
assertEquals(DATA.freeSpeedRadPerSec, data.freeSpeedRadPerSec);
assertEquals(DATA.rOhms, data.rOhms);
assertEquals(DATA.KvRadPerSecPerVolt, data.KvRadPerSecPerVolt);
assertEquals(DATA.KtNMPerAmp, data.KtNMPerAmp);
assertEquals(DATA.nominalVoltage, data.nominalVoltage);
assertEquals(DATA.stallTorque, data.stallTorque);
assertEquals(DATA.stallCurrent, data.stallCurrent);
assertEquals(DATA.freeCurrent, data.freeCurrent);
assertEquals(DATA.freeSpeed, data.freeSpeed);
assertEquals(DATA.R, data.R);
assertEquals(DATA.Kv, data.Kv);
assertEquals(DATA.Kt, data.Kt);
}
}

16
wpimath/src/test/java/edu/wpi/first/math/system/plant/struct/DCMotorStructTest.java
View File

@@ -22,13 +22,13 @@ class DCMotorStructTest {
buffer.rewind();
DCMotor data = DCMotor.struct.unpack(buffer);
assertEquals(DATA.nominalVoltageVolts, data.nominalVoltageVolts);
assertEquals(DATA.stallTorqueNewtonMeters, data.stallTorqueNewtonMeters);
assertEquals(DATA.stallCurrentAmps, data.stallCurrentAmps);
assertEquals(DATA.freeCurrentAmps, data.freeCurrentAmps);
assertEquals(DATA.freeSpeedRadPerSec, data.freeSpeedRadPerSec);
assertEquals(DATA.rOhms, data.rOhms);
assertEquals(DATA.KvRadPerSecPerVolt, data.KvRadPerSecPerVolt);
assertEquals(DATA.KtNMPerAmp, data.KtNMPerAmp);
assertEquals(DATA.nominalVoltage, data.nominalVoltage);
assertEquals(DATA.stallTorque, data.stallTorque);
assertEquals(DATA.stallCurrent, data.stallCurrent);
assertEquals(DATA.freeCurrent, data.freeCurrent);
assertEquals(DATA.freeSpeed, data.freeSpeed);
assertEquals(DATA.R, data.R);
assertEquals(DATA.Kv, data.Kv);
assertEquals(DATA.Kt, data.Kt);
}
}

5
wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java
View File

@@ -20,14 +20,13 @@ class CentripetalAccelerationConstraintTest {
Trajectory trajectory =
TrajectoryGeneratorTest.getTrajectory(Collections.singletonList(constraint));
var duration = trajectory.getTotalTimeSeconds();
var duration = trajectory.getTotalTime();
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;
var centripetalAcceleration = Math.pow(point.velocity, 2) * point.curvature;
t += dt;
assertTrue(centripetalAcceleration <= maxCentripetalAcceleration + 0.05);

12
wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java
View File

@@ -24,24 +24,20 @@ class DifferentialDriveKinematicsConstraintTest {
Trajectory trajectory =
TrajectoryGeneratorTest.getTrajectory(Collections.singletonList(constraint));
var duration = trajectory.getTotalTimeSeconds();
var duration = trajectory.getTotalTime();
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 chassisSpeeds = new ChassisSpeeds(point.velocity, 0, point.velocity * point.curvature);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
t += dt;
assertAll(
() -> assertTrue(wheelSpeeds.leftMetersPerSecond <= maxVelocity + 0.05),
() -> assertTrue(wheelSpeeds.rightMetersPerSecond <= maxVelocity + 0.05));
() -> assertTrue(wheelSpeeds.left <= maxVelocity + 0.05),
() -> assertTrue(wheelSpeeds.right <= maxVelocity + 0.05));
}
}
}

26
wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
View File

@@ -30,48 +30,36 @@ class DifferentialDriveVoltageConstraintTest {
Trajectory trajectory =
TrajectoryGeneratorTest.getTrajectory(Collections.singletonList(constraint));
var duration = trajectory.getTotalTimeSeconds();
var duration = trajectory.getTotalTime();
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 chassisSpeeds = new ChassisSpeeds(point.velocity, 0, point.velocity * point.curvature);
var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
t += dt;
var acceleration = point.accelerationMetersPerSecondSq;
var acceleration = point.acceleration;
// 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,
wheelSpeeds.leftMetersPerSecond + dt * acceleration)
feedforward.calculate(wheelSpeeds.left, wheelSpeeds.left + dt * acceleration)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.leftMetersPerSecond,
wheelSpeeds.leftMetersPerSecond + dt * acceleration)
feedforward.calculate(wheelSpeeds.left, wheelSpeeds.left + dt * acceleration)
>= -maxVoltage - 0.05),
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.rightMetersPerSecond,
wheelSpeeds.rightMetersPerSecond + dt * acceleration)
feedforward.calculate(wheelSpeeds.right, wheelSpeeds.right + dt * acceleration)
<= maxVoltage + 0.05),
() ->
assertTrue(
feedforward.calculate(
wheelSpeeds.rightMetersPerSecond,
wheelSpeeds.rightMetersPerSecond + dt * acceleration)
feedforward.calculate(wheelSpeeds.right, wheelSpeeds.right + dt * acceleration)
>= -maxVoltage - 0.05));
}
}

6
wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java
View File

@@ -32,9 +32,9 @@ class EllipticalRegionConstraintTest {
boolean exceededConstraintOutsideRegion = false;
for (var point : trajectory.getStates()) {
if (ellipse.contains(point.poseMeters.getTranslation())) {
assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05);
} else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) {
if (ellipse.contains(point.pose.getTranslation())) {
assertTrue(Math.abs(point.velocity) < maxVelocity + 0.05);
} else if (Math.abs(point.velocity) >= maxVelocity + 0.05) {
exceededConstraintOutsideRegion = true;
}
}

6
wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java
View File

@@ -31,9 +31,9 @@ class RectangularRegionConstraintTest {
boolean exceededConstraintOutsideRegion = false;
for (var point : trajectory.getStates()) {
if (rectangle.contains(point.poseMeters.getTranslation())) {
assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05);
} else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) {
if (rectangle.contains(point.pose.getTranslation())) {
assertTrue(Math.abs(point.velocity) < maxVelocity + 0.05);
} else if (Math.abs(point.velocity) >= maxVelocity + 0.05) {
exceededConstraintOutsideRegion = true;
}
}

6
wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryConcatenateTest.java
View File

@@ -36,15 +36,15 @@ class TrajectoryConcatenateTest {
var state = t.getStates().get(i);
// Make sure that the timestamps are strictly increasing.
assertTrue(state.timeSeconds > time);
time = state.timeSeconds;
assertTrue(state.time > time);
time = state.time;
// 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();
st.time += t1.getTotalTime();
assertEquals(state, st);
}
}

12
wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java
View File

@@ -51,7 +51,7 @@ class TrajectoryGeneratorTest {
void testGenerationAndConstraints() {
Trajectory trajectory = getTrajectory(new ArrayList<>());
double duration = trajectory.getTotalTimeSeconds();
double duration = trajectory.getTotalTime();
double t = 0.0;
double dt = 0.02;
@@ -59,10 +59,8 @@ class TrajectoryGeneratorTest {
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));
() -> assertTrue(Math.abs(point.velocity) < feetToMeters(12.0) + 0.05),
() -> assertTrue(Math.abs(point.acceleration) < feetToMeters(12.0) + 0.05));
}
}
@@ -74,7 +72,7 @@ class TrajectoryGeneratorTest {
new TrajectoryConfig(feetToMeters(12), feetToMeters(12)));
assertEquals(traj.getStates().size(), 1);
assertEquals(traj.getTotalTimeSeconds(), 0);
assertEquals(traj.getTotalTime(), 0);
}
@Test
@@ -90,7 +88,7 @@ class TrajectoryGeneratorTest {
new TrajectoryConfig(2, 2));
for (int i = 1; i < t.getStates().size() - 1; ++i) {
assertNotEquals(0, t.getStates().get(i).curvatureRadPerMeter);
assertNotEquals(0, t.getStates().get(i).curvature);
}
}
}

12
wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryTransformTest.java
View File

@@ -27,7 +27,7 @@ class TrajectoryTransformTest {
// Test initial pose.
assertEquals(
new Pose2d(1, 2, Rotation2d.fromDegrees(30)), transformedTrajectory.sample(0).poseMeters);
new Pose2d(1, 2, Rotation2d.fromDegrees(30)), transformedTrajectory.sample(0).pose);
testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
}
@@ -45,18 +45,18 @@ class TrajectoryTransformTest {
var transformedTrajectory = trajectory.relativeTo(new Pose2d(1, 2, Rotation2d.fromDegrees(30)));
// Test initial pose.
assertEquals(Pose2d.kZero, transformedTrajectory.sample(0).poseMeters);
assertEquals(Pose2d.kZero, transformedTrajectory.sample(0).pose);
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 a1 = statesA.get(i).pose;
var a2 = statesA.get(i + 1).pose;
var b1 = statesB.get(i).poseMeters;
var b2 = statesB.get(i + 1).poseMeters;
var b1 = statesB.get(i).pose;
var b2 = statesB.get(i + 1).pose;
assertEquals(a2.relativeTo(a1), b2.relativeTo(b1));
}

10
wpimath/src/test/java/edu/wpi/first/math/trajectory/proto/TrajectoryStateProtoTest.java
View File

@@ -24,10 +24,10 @@ class TrajectoryStateProtoTest {
Trajectory.State.proto.pack(proto, DATA);
Trajectory.State data = Trajectory.State.proto.unpack(proto);
assertEquals(DATA.timeSeconds, data.timeSeconds);
assertEquals(DATA.velocityMetersPerSecond, data.velocityMetersPerSecond);
assertEquals(DATA.accelerationMetersPerSecondSq, data.accelerationMetersPerSecondSq);
assertEquals(DATA.poseMeters, data.poseMeters);
assertEquals(DATA.curvatureRadPerMeter, data.curvatureRadPerMeter);
assertEquals(DATA.time, data.time);
assertEquals(DATA.velocity, data.velocity);
assertEquals(DATA.acceleration, data.acceleration);
assertEquals(DATA.pose, data.pose);
assertEquals(DATA.curvature, data.curvature);
}
}

4
wpimath/src/test/native/cpp/kinematics/proto/DifferentialDriveKinematicsProtoTest.cpp
View File

@@ -24,6 +24,6 @@ TEST(DifferentialDriveKinematicsProtoTest, Roundtrip) {
auto unpacked_data = message.Unpack(buf);
ASSERT_TRUE(unpacked_data.has_value());
EXPECT_EQ(kExpectedData.trackWidth.value(),
unpacked_data->trackWidth.value());
EXPECT_EQ(kExpectedData.trackwidth.value(),
unpacked_data->trackwidth.value());
}

2
wpimath/src/test/native/cpp/kinematics/struct/DifferentialDriveKinematicsStructTest.cpp
View File

@@ -22,5 +22,5 @@ TEST(DifferentialDriveKinematicsStructTest, Roundtrip) {
DifferentialDriveKinematics unpacked_data = StructType::Unpack(buffer);
EXPECT_EQ(kExpectedData.trackWidth.value(), unpacked_data.trackWidth.value());
EXPECT_EQ(kExpectedData.trackwidth.value(), unpacked_data.trackwidth.value());
}