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

[wpimath] Java: add static instantiations of common rotations (#6563)

Browse Source 
C++ doesn't need this because it supports value types, which are much
cheaper to construct. constexpr is also available to make construction
zero-cost.
This commit is contained in:
Tyler Veness
2024-05-03 12:39:35 -07:00
committed by GitHub
parent 9c7120e6bf
commit e172aa66f7
75 changed files with 499 additions and 429 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -32,10 +32,10 @@ class HolonomicDriveControllerTest {
new PIDController(1.0, 0.0, 0.0),
new ProfiledPIDController(
1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2.0 * Math.PI, Math.PI)));
Pose2d robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
Pose2d robotPose = new Pose2d(2.7, 23.0, Rotation2d.kZero);
List<Pose2d> waypoints = new ArrayList<>();
waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
waypoints.add(new Pose2d(2.75, 22.521, Rotation2d.kZero));
waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.8)));
TrajectoryConfig config = new TrajectoryConfig(8.0, 4.0);
@@ -46,7 +46,7 @@ class HolonomicDriveControllerTest {
for (int i = 0; i < (kTotalTime / kDt); i++) {
Trajectory.State state = trajectory.sample(kDt * i);
ChassisSpeeds output = controller.calculate(robotPose, state, new Rotation2d());
ChassisSpeeds output = controller.calculate(robotPose, state, Rotation2d.kZero);
robotPose =
robotPose.exp(
@@ -83,7 +83,7 @@ class HolonomicDriveControllerTest {
ChassisSpeeds speeds =
controller.calculate(
new Pose2d(0, 0, new Rotation2d(1.57)), new Pose2d(), 0, new Rotation2d(1.57));
new Pose2d(0, 0, new Rotation2d(1.57)), Pose2d.kZero, 0, new Rotation2d(1.57));
assertEquals(0.0, speeds.omegaRadiansPerSecond);
}

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

@@ -78,10 +78,10 @@ class LTVDifferentialDriveControllerTest {
VecBuilder.fill(0.0625, 0.125, 2.5, 0.95, 0.95),
VecBuilder.fill(12.0, 12.0),
kDt);
var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
var robotPose = new Pose2d(2.7, 23.0, Rotation2d.kZero);
final var waypoints = new ArrayList<Pose2d>();
waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
waypoints.add(new Pose2d(2.75, 22.521, Rotation2d.kZero));
waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
var config = new TrajectoryConfig(8.8, 0.1);
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);

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

@@ -28,10 +28,10 @@ class LTVUnicycleControllerTest {
final var controller =
new LTVUnicycleController(
VecBuilder.fill(0.0625, 0.125, 2.5), VecBuilder.fill(4.0, 4.0), kDt);
var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
var robotPose = new Pose2d(2.7, 23.0, Rotation2d.kZero);
final var waypoints = new ArrayList<Pose2d>();
waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
waypoints.add(new Pose2d(2.75, 22.521, Rotation2d.kZero));
waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
var config = new TrajectoryConfig(8.8, 0.1);
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);

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

@@ -24,10 +24,10 @@ class RamseteControllerTest {
@Test
void testReachesReference() {
final var controller = new RamseteController(2.0, 0.7);
var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
var robotPose = new Pose2d(2.7, 23.0, Rotation2d.kZero);
final var waypoints = new ArrayList<Pose2d>();
waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
waypoints.add(new Pose2d(2.75, 22.521, Rotation2d.kZero));
waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
var config = new TrajectoryConfig(8.8, 0.1);
final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);

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

@@ -32,19 +32,19 @@ class DifferentialDrivePoseEstimatorTest {
var estimator =
new DifferentialDrivePoseEstimator(
kinematics,
new Rotation2d(),
Rotation2d.kZero,
0,
0,
new Pose2d(),
Pose2d.kZero,
VecBuilder.fill(0.02, 0.02, 0.01),
VecBuilder.fill(0.1, 0.1, 0.1));
var trajectory =
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
new TrajectoryConfig(2, 2));
@@ -73,10 +73,10 @@ class DifferentialDrivePoseEstimatorTest {
var estimator =
new DifferentialDrivePoseEstimator(
kinematics,
new Rotation2d(),
Rotation2d.kZero,
0,
0,
new Pose2d(),
Pose2d.kZero,
VecBuilder.fill(0.02, 0.02, 0.01),
VecBuilder.fill(0.1, 0.1, 0.1));
@@ -84,9 +84,9 @@ class DifferentialDrivePoseEstimatorTest {
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
new TrajectoryConfig(2, 2));
@@ -139,7 +139,7 @@ class DifferentialDrivePoseEstimatorTest {
double rightDistanceMeters = 0;
estimator.resetPosition(
new Rotation2d(), leftDistanceMeters, rightDistanceMeters, startingPose);
Rotation2d.kZero, leftDistanceMeters, rightDistanceMeters, startingPose);
var rand = new Random(3538);
@@ -229,20 +229,20 @@ class DifferentialDrivePoseEstimatorTest {
var estimator =
new DifferentialDrivePoseEstimator(
kinematics,
new Rotation2d(),
Rotation2d.kZero,
0,
0,
new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
new Pose2d(1, 2, Rotation2d.kCW_Pi_2),
VecBuilder.fill(0.02, 0.02, 0.01),
VecBuilder.fill(0.1, 0.1, 0.1));
estimator.updateWithTime(0, new Rotation2d(), 0, 0);
estimator.updateWithTime(0, Rotation2d.kZero, 0, 0);
var visionMeasurements =
new Pose2d[] {
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
new Pose2d(2, 4, Rotation2d.fromRadians(180)),
new Pose2d(0, 0, Rotation2d.kZero),
new Pose2d(3, 1, Rotation2d.kCCW_Pi_2),
new Pose2d(2, 4, Rotation2d.kPi),
};
for (int i = 0; i < 1000; i++) {
@@ -275,10 +275,10 @@ class DifferentialDrivePoseEstimatorTest {
var estimator =
new DifferentialDrivePoseEstimator(
kinematics,
new Rotation2d(),
Rotation2d.kZero,
0,
0,
new Pose2d(),
Pose2d.kZero,
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.9, 0.9, 0.9));
@@ -286,7 +286,7 @@ class DifferentialDrivePoseEstimatorTest {
// Add enough measurements to fill up the buffer
for (; time < 4; time += 0.02) {
estimator.updateWithTime(time, new Rotation2d(), 0, 0);
estimator.updateWithTime(time, Rotation2d.kZero, 0, 0);
}
var odometryPose = estimator.getEstimatedPosition();
@@ -312,10 +312,10 @@ class DifferentialDrivePoseEstimatorTest {
var estimator =
new DifferentialDrivePoseEstimator(
kinematics,
new Rotation2d(),
Rotation2d.kZero,
0,
0,
new Pose2d(),
Pose2d.kZero,
VecBuilder.fill(1, 1, 1),
VecBuilder.fill(1, 1, 1));
@@ -325,34 +325,34 @@ class DifferentialDrivePoseEstimatorTest {
// Add odometry measurements, but don't fill up the buffer
// Add a tiny tolerance for the upper bound because of floating point rounding error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
estimator.updateWithTime(time, new Rotation2d(), time, time);
estimator.updateWithTime(time, Rotation2d.kZero, time, time);
}
// Sample at an added time
assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02));
assertEquals(Optional.of(new Pose2d(1.02, 0, Rotation2d.kZero)), estimator.sampleAt(1.02));
// Sample between updates (test interpolation)
assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01));
assertEquals(Optional.of(new Pose2d(1.01, 0, Rotation2d.kZero)), estimator.sampleAt(1.01));
// Sampling before the oldest value returns the oldest value
assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5));
assertEquals(Optional.of(new Pose2d(1, 0, Rotation2d.kZero)), estimator.sampleAt(0.5));
// Sampling after the newest value returns the newest value
assertEquals(Optional.of(new Pose2d(2, 0, new Rotation2d())), estimator.sampleAt(2.5));
assertEquals(Optional.of(new Pose2d(2, 0, Rotation2d.kZero)), estimator.sampleAt(2.5));
// Add a vision measurement after the odometry measurements (while keeping all of the old
// odometry measurements)
estimator.addVisionMeasurement(new Pose2d(2, 0, new Rotation2d(1)), 2.2);
// Make sure nothing changed (except the newest value)
assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02));
assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01));
assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5));
assertEquals(Optional.of(new Pose2d(1.02, 0, Rotation2d.kZero)), estimator.sampleAt(1.02));
assertEquals(Optional.of(new Pose2d(1.01, 0, Rotation2d.kZero)), estimator.sampleAt(1.01));
assertEquals(Optional.of(new Pose2d(1, 0, Rotation2d.kZero)), estimator.sampleAt(0.5));
// Add a vision measurement before the odometry measurements that's still in the buffer
estimator.addVisionMeasurement(new Pose2d(1, 0.2, new Rotation2d()), 0.9);
estimator.addVisionMeasurement(new Pose2d(1, 0.2, Rotation2d.kZero), 0.9);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
assertEquals(Optional.of(new Pose2d(1.02, 0.1, new Rotation2d())), estimator.sampleAt(1.02));
assertEquals(Optional.of(new Pose2d(1.01, 0.1, new Rotation2d())), estimator.sampleAt(1.01));
assertEquals(Optional.of(new Pose2d(1, 0.1, new Rotation2d())), estimator.sampleAt(0.5));
assertEquals(Optional.of(new Pose2d(2, 0.1, new Rotation2d())), estimator.sampleAt(2.5));
assertEquals(Optional.of(new Pose2d(1.02, 0.1, Rotation2d.kZero)), estimator.sampleAt(1.02));
assertEquals(Optional.of(new Pose2d(1.01, 0.1, Rotation2d.kZero)), estimator.sampleAt(1.01));
assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5));
assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5));
}
}

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

@@ -114,7 +114,7 @@ class ExtendedKalmanFilterTest {
List<Pose2d> waypoints =
List.of(
new Pose2d(2.75, 22.521, new Rotation2d()),
new Pose2d(2.75, 22.521, Rotation2d.kZero),
new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
var trajectory =
TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));

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

@@ -150,7 +150,7 @@ class KalmanFilterTest {
var trajectory =
TrajectoryGenerator.generateTrajectory(
List.of(new Pose2d(0, 0, new Rotation2d()), new Pose2d(5, 5, new Rotation2d())),
List.of(new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(5, 5, Rotation2d.kZero)),
new TrajectoryConfig(2, 2));
var time = 0.0;
var lastVelocity = VecBuilder.fill(0.0, 0.0, 0.0);

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

@@ -38,9 +38,9 @@ class MecanumDrivePoseEstimatorTest {
var estimator =
new MecanumDrivePoseEstimator(
kinematics,
new Rotation2d(),
Rotation2d.kZero,
wheelPositions,
new Pose2d(),
Pose2d.kZero,
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.45, 0.45, 0.1));
@@ -48,9 +48,9 @@ class MecanumDrivePoseEstimatorTest {
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
new TrajectoryConfig(2, 2));
@@ -84,9 +84,9 @@ class MecanumDrivePoseEstimatorTest {
var estimator =
new MecanumDrivePoseEstimator(
kinematics,
new Rotation2d(),
Rotation2d.kZero,
wheelPositions,
new Pose2d(),
Pose2d.kZero,
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.45, 0.45, 0.1));
@@ -94,9 +94,9 @@ class MecanumDrivePoseEstimatorTest {
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
new TrajectoryConfig(2, 2));
@@ -147,7 +147,7 @@ class MecanumDrivePoseEstimatorTest {
final boolean checkError) {
var wheelPositions = new MecanumDriveWheelPositions();
estimator.resetPosition(new Rotation2d(), wheelPositions, startingPose);
estimator.resetPosition(Rotation2d.kZero, wheelPositions, startingPose);
var rand = new Random(3538);
@@ -243,19 +243,19 @@ class MecanumDrivePoseEstimatorTest {
var estimator =
new MecanumDrivePoseEstimator(
kinematics,
new Rotation2d(),
Rotation2d.kZero,
wheelPositions,
new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
new Pose2d(1, 2, Rotation2d.kCW_Pi_2),
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.45, 0.45, 0.1));
estimator.updateWithTime(0, new Rotation2d(), wheelPositions);
estimator.updateWithTime(0, Rotation2d.kZero, wheelPositions);
var visionMeasurements =
new Pose2d[] {
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
new Pose2d(2, 4, Rotation2d.fromRadians(180)),
new Pose2d(0, 0, Rotation2d.kZero),
new Pose2d(3, 1, Rotation2d.kCCW_Pi_2),
new Pose2d(2, 4, Rotation2d.kPi),
};
for (int i = 0; i < 1000; i++) {
@@ -293,9 +293,9 @@ class MecanumDrivePoseEstimatorTest {
var estimator =
new MecanumDrivePoseEstimator(
kinematics,
new Rotation2d(),
Rotation2d.kZero,
new MecanumDriveWheelPositions(),
new Pose2d(),
Pose2d.kZero,
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.9, 0.9, 0.9));
@@ -303,7 +303,7 @@ class MecanumDrivePoseEstimatorTest {
// Add enough measurements to fill up the buffer
for (; time < 4; time += 0.02) {
estimator.updateWithTime(time, new Rotation2d(), new MecanumDriveWheelPositions());
estimator.updateWithTime(time, Rotation2d.kZero, new MecanumDriveWheelPositions());
}
var odometryPose = estimator.getEstimatedPosition();
@@ -334,9 +334,9 @@ class MecanumDrivePoseEstimatorTest {
var estimator =
new MecanumDrivePoseEstimator(
kinematics,
new Rotation2d(),
Rotation2d.kZero,
new MecanumDriveWheelPositions(),
new Pose2d(),
Pose2d.kZero,
VecBuilder.fill(1, 1, 1),
VecBuilder.fill(1, 1, 1));
@@ -347,34 +347,34 @@ class MecanumDrivePoseEstimatorTest {
// Add a tiny tolerance for the upper bound because of floating point rounding error
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
var wheelPositions = new MecanumDriveWheelPositions(time, time, time, time);
estimator.updateWithTime(time, new Rotation2d(), wheelPositions);
estimator.updateWithTime(time, Rotation2d.kZero, wheelPositions);
}
// Sample at an added time
assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02));
assertEquals(Optional.of(new Pose2d(1.02, 0, Rotation2d.kZero)), estimator.sampleAt(1.02));
// Sample between updates (test interpolation)
assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01));
assertEquals(Optional.of(new Pose2d(1.01, 0, Rotation2d.kZero)), estimator.sampleAt(1.01));
// Sampling before the oldest value returns the oldest value
assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5));
assertEquals(Optional.of(new Pose2d(1, 0, Rotation2d.kZero)), estimator.sampleAt(0.5));
// Sampling after the newest value returns the newest value
assertEquals(Optional.of(new Pose2d(2, 0, new Rotation2d())), estimator.sampleAt(2.5));
assertEquals(Optional.of(new Pose2d(2, 0, Rotation2d.kZero)), estimator.sampleAt(2.5));
// Add a vision measurement after the odometry measurements (while keeping all of the old
// odometry measurements)
estimator.addVisionMeasurement(new Pose2d(2, 0, new Rotation2d(1)), 2.2);
// Make sure nothing changed (except the newest value)
assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02));
assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01));
assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5));
assertEquals(Optional.of(new Pose2d(1.02, 0, Rotation2d.kZero)), estimator.sampleAt(1.02));
assertEquals(Optional.of(new Pose2d(1.01, 0, Rotation2d.kZero)), estimator.sampleAt(1.01));
assertEquals(Optional.of(new Pose2d(1, 0, Rotation2d.kZero)), estimator.sampleAt(0.5));
// Add a vision measurement before the odometry measurements that's still in the buffer
estimator.addVisionMeasurement(new Pose2d(1, 0.2, new Rotation2d()), 0.9);
estimator.addVisionMeasurement(new Pose2d(1, 0.2, Rotation2d.kZero), 0.9);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
assertEquals(Optional.of(new Pose2d(1.02, 0.1, new Rotation2d())), estimator.sampleAt(1.02));
assertEquals(Optional.of(new Pose2d(1.01, 0.1, new Rotation2d())), estimator.sampleAt(1.01));
assertEquals(Optional.of(new Pose2d(1, 0.1, new Rotation2d())), estimator.sampleAt(0.5));
assertEquals(Optional.of(new Pose2d(2, 0.1, new Rotation2d())), estimator.sampleAt(2.5));
assertEquals(Optional.of(new Pose2d(1.02, 0.1, Rotation2d.kZero)), estimator.sampleAt(1.02));
assertEquals(Optional.of(new Pose2d(1.01, 0.1, Rotation2d.kZero)), estimator.sampleAt(1.01));
assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5));
assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5));
}
}

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

@@ -43,9 +43,9 @@ class SwerveDrivePoseEstimatorTest {
var estimator =
new SwerveDrivePoseEstimator(
kinematics,
new Rotation2d(),
Rotation2d.kZero,
new SwerveModulePosition[] {fl, fr, bl, br},
new Pose2d(),
Pose2d.kZero,
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.5, 0.5, 0.5));
@@ -53,9 +53,9 @@ class SwerveDrivePoseEstimatorTest {
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
new TrajectoryConfig(2, 2));
@@ -94,7 +94,7 @@ class SwerveDrivePoseEstimatorTest {
var estimator =
new SwerveDrivePoseEstimator(
kinematics,
new Rotation2d(),
Rotation2d.kZero,
new SwerveModulePosition[] {fl, fr, bl, br},
new Pose2d(-1, -1, Rotation2d.fromRadians(-1)),
VecBuilder.fill(0.1, 0.1, 0.1),
@@ -103,9 +103,9 @@ class SwerveDrivePoseEstimatorTest {
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
new TrajectoryConfig(2, 2));
@@ -161,7 +161,7 @@ class SwerveDrivePoseEstimatorTest {
new SwerveModulePosition()
};
estimator.resetPosition(new Rotation2d(), positions, startingPose);
estimator.resetPosition(Rotation2d.kZero, positions, startingPose);
var rand = new Random(3538);
@@ -264,19 +264,19 @@ class SwerveDrivePoseEstimatorTest {
var estimator =
new SwerveDrivePoseEstimator(
kinematics,
new Rotation2d(),
Rotation2d.kZero,
new SwerveModulePosition[] {fl, fr, bl, br},
new Pose2d(1, 2, Rotation2d.fromDegrees(270)),
new Pose2d(1, 2, Rotation2d.kCW_Pi_2),
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.9, 0.9, 0.9));
estimator.updateWithTime(0, new Rotation2d(), new SwerveModulePosition[] {fl, fr, bl, br});
estimator.updateWithTime(0, Rotation2d.kZero, new SwerveModulePosition[] {fl, fr, bl, br});
var visionMeasurements =
new Pose2d[] {
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(3, 1, Rotation2d.fromDegrees(90)),
new Pose2d(2, 4, Rotation2d.fromRadians(180)),
new Pose2d(0, 0, Rotation2d.kZero),
new Pose2d(3, 1, Rotation2d.kCCW_Pi_2),
new Pose2d(2, 4, Rotation2d.kPi),
};
for (int i = 0; i < 1000; i++) {
@@ -314,14 +314,14 @@ class SwerveDrivePoseEstimatorTest {
var estimator =
new SwerveDrivePoseEstimator(
kinematics,
new Rotation2d(),
Rotation2d.kZero,
new SwerveModulePosition[] {
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition()
},
new Pose2d(),
Pose2d.kZero,
VecBuilder.fill(0.1, 0.1, 0.1),
VecBuilder.fill(0.9, 0.9, 0.9));
@@ -331,7 +331,7 @@ class SwerveDrivePoseEstimatorTest {
for (; time < 4; time += 0.02) {
estimator.updateWithTime(
time,
new Rotation2d(),
Rotation2d.kZero,
new SwerveModulePosition[] {
new SwerveModulePosition(),
new SwerveModulePosition(),
@@ -368,14 +368,14 @@ class SwerveDrivePoseEstimatorTest {
var estimator =
new SwerveDrivePoseEstimator(
kinematics,
new Rotation2d(),
Rotation2d.kZero,
new SwerveModulePosition[] {
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition()
},
new Pose2d(),
Pose2d.kZero,
VecBuilder.fill(1, 1, 1),
VecBuilder.fill(1, 1, 1));
@@ -387,39 +387,39 @@ class SwerveDrivePoseEstimatorTest {
for (double time = 1; time <= 2 + 1e-9; time += 0.02) {
var wheelPositions =
new SwerveModulePosition[] {
new SwerveModulePosition(time, new Rotation2d()),
new SwerveModulePosition(time, new Rotation2d()),
new SwerveModulePosition(time, new Rotation2d()),
new SwerveModulePosition(time, new Rotation2d())
new SwerveModulePosition(time, Rotation2d.kZero),
new SwerveModulePosition(time, Rotation2d.kZero),
new SwerveModulePosition(time, Rotation2d.kZero),
new SwerveModulePosition(time, Rotation2d.kZero)
};
estimator.updateWithTime(time, new Rotation2d(), wheelPositions);
estimator.updateWithTime(time, Rotation2d.kZero, wheelPositions);
}
// Sample at an added time
assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02));
assertEquals(Optional.of(new Pose2d(1.02, 0, Rotation2d.kZero)), estimator.sampleAt(1.02));
// Sample between updates (test interpolation)
assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01));
assertEquals(Optional.of(new Pose2d(1.01, 0, Rotation2d.kZero)), estimator.sampleAt(1.01));
// Sampling before the oldest value returns the oldest value
assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5));
assertEquals(Optional.of(new Pose2d(1, 0, Rotation2d.kZero)), estimator.sampleAt(0.5));
// Sampling after the newest value returns the newest value
assertEquals(Optional.of(new Pose2d(2, 0, new Rotation2d())), estimator.sampleAt(2.5));
assertEquals(Optional.of(new Pose2d(2, 0, Rotation2d.kZero)), estimator.sampleAt(2.5));
// Add a vision measurement after the odometry measurements (while keeping all of the old
// odometry measurements)
estimator.addVisionMeasurement(new Pose2d(2, 0, new Rotation2d(1)), 2.2);
// Make sure nothing changed (except the newest value)
assertEquals(Optional.of(new Pose2d(1.02, 0, new Rotation2d())), estimator.sampleAt(1.02));
assertEquals(Optional.of(new Pose2d(1.01, 0, new Rotation2d())), estimator.sampleAt(1.01));
assertEquals(Optional.of(new Pose2d(1, 0, new Rotation2d())), estimator.sampleAt(0.5));
assertEquals(Optional.of(new Pose2d(1.02, 0, Rotation2d.kZero)), estimator.sampleAt(1.02));
assertEquals(Optional.of(new Pose2d(1.01, 0, Rotation2d.kZero)), estimator.sampleAt(1.01));
assertEquals(Optional.of(new Pose2d(1, 0, Rotation2d.kZero)), estimator.sampleAt(0.5));
// Add a vision measurement before the odometry measurements that's still in the buffer
estimator.addVisionMeasurement(new Pose2d(1, 0.2, new Rotation2d()), 0.9);
estimator.addVisionMeasurement(new Pose2d(1, 0.2, Rotation2d.kZero), 0.9);
// Everything should be the same except Y is 0.1 (halfway between 0 and 0.2)
assertEquals(Optional.of(new Pose2d(1.02, 0.1, new Rotation2d())), estimator.sampleAt(1.02));
assertEquals(Optional.of(new Pose2d(1.01, 0.1, new Rotation2d())), estimator.sampleAt(1.01));
assertEquals(Optional.of(new Pose2d(1, 0.1, new Rotation2d())), estimator.sampleAt(0.5));
assertEquals(Optional.of(new Pose2d(2, 0.1, new Rotation2d())), estimator.sampleAt(2.5));
assertEquals(Optional.of(new Pose2d(1.02, 0.1, Rotation2d.kZero)), estimator.sampleAt(1.02));
assertEquals(Optional.of(new Pose2d(1.01, 0.1, Rotation2d.kZero)), estimator.sampleAt(1.01));
assertEquals(Optional.of(new Pose2d(1, 0.1, Rotation2d.kZero)), estimator.sampleAt(0.5));
assertEquals(Optional.of(new Pose2d(2, 0.1, Rotation2d.kZero)), estimator.sampleAt(2.5));
}
}

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

@@ -137,7 +137,7 @@ class UnscentedKalmanFilterTest {
List<Pose2d> waypoints =
List.of(
new Pose2d(2.75, 22.521, new Rotation2d()),
new Pose2d(2.75, 22.521, Rotation2d.kZero),
new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
var trajectory =
TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));

12
wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java
View File

@@ -51,7 +51,7 @@ class CoordinateSystemTest {
void testPose3dEDNtoNWU() {
// No rotation from EDN to NWU
checkPose3dConvert(
new Pose3d(1.0, 2.0, 3.0, new Rotation3d()),
new Pose3d(1.0, 2.0, 3.0, Rotation3d.kZero),
new Pose3d(
3.0,
-1.0,
@@ -101,7 +101,7 @@ class CoordinateSystemTest {
void testPose3dEDNtoNED() {
// No rotation from EDN to NED
checkPose3dConvert(
new Pose3d(1.0, 2.0, 3.0, new Rotation3d()),
new Pose3d(1.0, 2.0, 3.0, Rotation3d.kZero),
new Pose3d(
3.0,
1.0,
@@ -151,8 +151,8 @@ class CoordinateSystemTest {
void testTransform3dEDNtoNWU() {
// No rotation from EDN to NWU
checkTransform3dConvert(
new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()),
new Transform3d(new Translation3d(3.0, -1.0, -2.0), new Rotation3d()),
new Transform3d(new Translation3d(1.0, 2.0, 3.0), Rotation3d.kZero),
new Transform3d(new Translation3d(3.0, -1.0, -2.0), Rotation3d.kZero),
CoordinateSystem.EDN(),
CoordinateSystem.NWU());
@@ -194,8 +194,8 @@ class CoordinateSystemTest {
void testTransform3dEDNtoNED() {
// No rotation from EDN to NED
checkTransform3dConvert(
new Transform3d(new Translation3d(1.0, 2.0, 3.0), new Rotation3d()),
new Transform3d(new Translation3d(3.0, 1.0, 2.0), new Rotation3d()),
new Transform3d(new Translation3d(1.0, 2.0, 3.0), Rotation3d.kZero),
new Transform3d(new Translation3d(3.0, 1.0, 2.0), Rotation3d.kZero),
CoordinateSystem.EDN(),
CoordinateSystem.NED());

21
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
View File

@@ -101,20 +101,17 @@ class Pose2dTest {
@Test
void testNearest() {
var origin = new Pose2d();
var origin = Pose2d.kZero;
// Distance sort
// each poseX is X units away from the origin at a random angle.
final var pose1 =
new Pose2d(new Translation2d(1, Rotation2d.fromDegrees(45)), new Rotation2d());
final var pose2 =
new Pose2d(new Translation2d(2, Rotation2d.fromDegrees(90)), new Rotation2d());
new Pose2d(new Translation2d(1, Rotation2d.fromDegrees(45)), Rotation2d.kZero);
final var pose2 = new Pose2d(new Translation2d(2, Rotation2d.kCCW_Pi_2), Rotation2d.kZero);
final var pose3 =
new Pose2d(new Translation2d(3, Rotation2d.fromDegrees(135)), new Rotation2d());
final var pose4 =
new Pose2d(new Translation2d(4, Rotation2d.fromDegrees(180)), new Rotation2d());
final var pose5 =
new Pose2d(new Translation2d(5, Rotation2d.fromDegrees(270)), new Rotation2d());
new Pose2d(new Translation2d(3, Rotation2d.fromDegrees(135)), Rotation2d.kZero);
final var pose4 = new Pose2d(new Translation2d(4, Rotation2d.kPi), Rotation2d.kZero);
final var pose5 = new Pose2d(new Translation2d(5, Rotation2d.kCW_Pi_2), Rotation2d.kZero);
assertEquals(pose3, origin.nearest(List.of(pose5, pose3, pose4)));
assertEquals(pose1, origin.nearest(List.of(pose1, pose2, pose3)));
@@ -123,12 +120,12 @@ class Pose2dTest {
// Rotation component sort (when distance is the same)
// Use the same translation because using different angles at the same distance can cause
// rounding error.
final var translation = new Translation2d(1, new Rotation2d());
final var translation = new Translation2d(1, Rotation2d.kZero);
final var poseA = new Pose2d(translation, Rotation2d.fromDegrees(0));
final var poseA = new Pose2d(translation, Rotation2d.kZero);
final var poseB = new Pose2d(translation, Rotation2d.fromDegrees(30));
final var poseC = new Pose2d(translation, Rotation2d.fromDegrees(120));
final var poseD = new Pose2d(translation, Rotation2d.fromDegrees(90));
final var poseD = new Pose2d(translation, Rotation2d.kCCW_Pi_2);
final var poseE = new Pose2d(translation, Rotation2d.fromDegrees(-180));
assertEquals(

14
wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
View File

@@ -51,17 +51,11 @@ class Pose3dTest {
@Test
void testTransformByRotations() {
var initialPose =
new Pose3d(
new Translation3d(0.0, 0.0, 0.0),
new Rotation3d(
Units.degreesToRadians(0.0),
Units.degreesToRadians(0.0),
Units.degreesToRadians(0.0)));
var initialPose = Pose3d.kZero;
var transform1 =
new Transform3d(
new Translation3d(0.0, 0.0, 0.0),
Translation3d.kZero,
new Rotation3d(
Units.degreesToRadians(90.0),
Units.degreesToRadians(45.0),
@@ -69,7 +63,7 @@ class Pose3dTest {
var transform2 =
new Transform3d(
new Translation3d(0.0, 0.0, 0.0),
Translation3d.kZero,
new Rotation3d(
Units.degreesToRadians(-90.0),
Units.degreesToRadians(0.0),
@@ -77,7 +71,7 @@ class Pose3dTest {
var transform3 =
new Transform3d(
new Translation3d(0.0, 0.0, 0.0),
Translation3d.kZero,
new Rotation3d(
Units.degreesToRadians(0.0),
Units.degreesToRadians(-45.0),

6
wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
View File

@@ -43,8 +43,8 @@ class Rotation2dTest {
@Test
void testRotateByFromZero() {
var zero = new Rotation2d();
var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0));
var zero = Rotation2d.kZero;
var rotated = zero.rotateBy(Rotation2d.kCCW_Pi_2);
assertAll(
() -> assertEquals(Math.PI / 2.0, rotated.getRadians(), kEpsilon),
@@ -53,7 +53,7 @@ class Rotation2dTest {
@Test
void testRotateByNonZero() {
var rot = Rotation2d.fromDegrees(90.0);
var rot = Rotation2d.kCCW_Pi_2;
rot = rot.plus(Rotation2d.fromDegrees(30.0));
assertEquals(120.0, rot.getDegrees(), kEpsilon);

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

@@ -81,7 +81,7 @@ class Rotation3dTest {
// No rotation
final var R1 = Matrix.eye(Nat.N3());
final var rot1 = new Rotation3d(R1);
assertEquals(new Rotation3d(), rot1);
assertEquals(Rotation3d.kZero, rot1);
// 90 degree CCW rotation around z-axis
final var R2 = new Matrix<>(Nat.N3(), Nat.N3());
@@ -119,15 +119,15 @@ class Rotation3dTest {
// 0 degree rotation of x-axes
final var rot3 = new Rotation3d(xAxis, xAxis);
assertEquals(new Rotation3d(), rot3);
assertEquals(Rotation3d.kZero, rot3);
// 0 degree rotation of y-axes
final var rot4 = new Rotation3d(yAxis, yAxis);
assertEquals(new Rotation3d(), rot4);
assertEquals(Rotation3d.kZero, rot4);
// 0 degree rotation of z-axes
final var rot5 = new Rotation3d(zAxis, zAxis);
assertEquals(new Rotation3d(), rot5);
assertEquals(Rotation3d.kZero, rot5);
// 180 degree rotation tests. For 180 degree rotations, any quaternion with
// an orthogonal rotation axis is acceptable. The rotation axis and initial
@@ -194,7 +194,7 @@ class Rotation3dTest {
@Test
void testRotationLoop() {
var rot = new Rotation3d();
var rot = Rotation3d.kZero;
rot = rot.plus(new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0));
var expected = new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0);
@@ -212,14 +212,14 @@ class Rotation3dTest {
assertEquals(expected, rot);
rot = rot.plus(new Rotation3d(0.0, Units.degreesToRadians(-90.0), 0.0));
assertEquals(new Rotation3d(), rot);
assertEquals(Rotation3d.kZero, rot);
}
@Test
void testRotateByFromZeroX() {
final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
final var zero = new Rotation3d();
final var zero = Rotation3d.kZero;
var rotated = zero.rotateBy(new Rotation3d(xAxis, Units.degreesToRadians(90.0)));
var expected = new Rotation3d(xAxis, Units.degreesToRadians(90.0));
@@ -230,7 +230,7 @@ class Rotation3dTest {
void testRotateByFromZeroY() {
final var yAxis = VecBuilder.fill(0.0, 1.0, 0.0);
final var zero = new Rotation3d();
final var zero = Rotation3d.kZero;
var rotated = zero.rotateBy(new Rotation3d(yAxis, Units.degreesToRadians(90.0)));
var expected = new Rotation3d(yAxis, Units.degreesToRadians(90.0));
@@ -241,7 +241,7 @@ class Rotation3dTest {
void testRotateByFromZeroZ() {
final var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
final var zero = new Rotation3d();
final var zero = Rotation3d.kZero;
var rotated = zero.rotateBy(new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
var expected = new Rotation3d(zAxis, Units.degreesToRadians(90.0));

10
wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
View File

@@ -42,7 +42,7 @@ class Translation2dTest {
@Test
void testRotateBy() {
var another = new Translation2d(3.0, 0.0);
var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0));
var rotated = another.rotateBy(Rotation2d.kCCW_Pi_2);
assertAll(
() -> assertEquals(0.0, rotated.getX(), kEpsilon),
@@ -119,14 +119,14 @@ class Translation2dTest {
@Test
void testNearest() {
var origin = new Translation2d();
var origin = Translation2d.kZero;
// each translationX is X units away from the origin at a random angle.
var translation1 = new Translation2d(1, Rotation2d.fromDegrees(45));
var translation2 = new Translation2d(2, Rotation2d.fromDegrees(90));
var translation2 = new Translation2d(2, Rotation2d.kCCW_Pi_2);
var translation3 = new Translation2d(3, Rotation2d.fromDegrees(135));
var translation4 = new Translation2d(4, Rotation2d.fromDegrees(180));
var translation5 = new Translation2d(5, Rotation2d.fromDegrees(270));
var translation4 = new Translation2d(4, Rotation2d.kPi);
var translation5 = new Translation2d(5, Rotation2d.kCW_Pi_2);
assertEquals(origin.nearest(List.of(translation5, translation3, translation4)), translation3);
assertEquals(origin.nearest(List.of(translation1, translation2, translation3)), translation1);

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

@@ -13,27 +13,27 @@ class Twist2dTest {
@Test
void testStraight() {
var straight = new Twist2d(5.0, 0.0, 0.0);
var straightPose = new Pose2d().exp(straight);
var straightPose = Pose2d.kZero.exp(straight);
var expected = new Pose2d(5.0, 0.0, new Rotation2d());
var expected = new Pose2d(5.0, 0.0, Rotation2d.kZero);
assertEquals(expected, straightPose);
}
@Test
void testQuarterCirle() {
var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0);
var quarterCirclePose = new Pose2d().exp(quarterCircle);
var quarterCirclePose = Pose2d.kZero.exp(quarterCircle);
var expected = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0));
var expected = new Pose2d(5.0, 5.0, Rotation2d.kCCW_Pi_2);
assertEquals(expected, quarterCirclePose);
}
@Test
void testDiagonalNoDtheta() {
var diagonal = new Twist2d(2.0, 2.0, 0.0);
var diagonalPose = new Pose2d().exp(diagonal);
var diagonalPose = Pose2d.kZero.exp(diagonal);
var expected = new Pose2d(2.0, 2.0, new Rotation2d());
var expected = new Pose2d(2.0, 2.0, Rotation2d.kZero);
assertEquals(expected, diagonalPose);
}
@@ -53,8 +53,8 @@ class Twist2dTest {
@Test
void testPose2dLog() {
final var start = new Pose2d();
final var end = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0));
final var start = Pose2d.kZero;
final var end = new Pose2d(5.0, 5.0, Rotation2d.kCCW_Pi_2);
final var twist = start.log(end);

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

@@ -15,27 +15,27 @@ class Twist3dTest {
@Test
void testStraightX() {
var straight = new Twist3d(5.0, 0.0, 0.0, 0.0, 0.0, 0.0);
var straightPose = new Pose3d().exp(straight);
var straightPose = Pose3d.kZero.exp(straight);
var expected = new Pose3d(5.0, 0.0, 0.0, new Rotation3d());
var expected = new Pose3d(5.0, 0.0, 0.0, Rotation3d.kZero);
assertEquals(expected, straightPose);
}
@Test
void testStraightY() {
var straight = new Twist3d(0.0, 5.0, 0.0, 0.0, 0.0, 0.0);
var straightPose = new Pose3d().exp(straight);
var straightPose = Pose3d.kZero.exp(straight);
var expected = new Pose3d(0.0, 5.0, 0.0, new Rotation3d());
var expected = new Pose3d(0.0, 5.0, 0.0, Rotation3d.kZero);
assertEquals(expected, straightPose);
}
@Test
void testStraightZ() {
var straight = new Twist3d(0.0, 0.0, 5.0, 0.0, 0.0, 0.0);
var straightPose = new Pose3d().exp(straight);
var straightPose = Pose3d.kZero.exp(straight);
var expected = new Pose3d(0.0, 0.0, 5.0, new Rotation3d());
var expected = new Pose3d(0.0, 0.0, 5.0, Rotation3d.kZero);
assertEquals(expected, straightPose);
}
@@ -44,7 +44,7 @@ class Twist3dTest {
var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
var quarterCircle = new Twist3d(5.0 / 2.0 * Math.PI, 0.0, 0.0, 0.0, 0.0, Math.PI / 2.0);
var quarterCirclePose = new Pose3d().exp(quarterCircle);
var quarterCirclePose = Pose3d.kZero.exp(quarterCircle);
var expected = new Pose3d(5.0, 5.0, 0.0, new Rotation3d(zAxis, Units.degreesToRadians(90.0)));
assertEquals(expected, quarterCirclePose);
@@ -53,9 +53,9 @@ class Twist3dTest {
@Test
void testDiagonalNoDtheta() {
var diagonal = new Twist3d(2.0, 2.0, 0.0, 0.0, 0.0, 0.0);
var diagonalPose = new Pose3d().exp(diagonal);
var diagonalPose = Pose3d.kZero.exp(diagonal);
var expected = new Pose3d(2.0, 2.0, 0.0, new Rotation3d());
var expected = new Pose3d(2.0, 2.0, 0.0, Rotation3d.kZero);
assertEquals(expected, diagonalPose);
}
@@ -75,7 +75,7 @@ class Twist3dTest {
@Test
void testPose3dLogX() {
final var start = new Pose3d();
final var start = Pose3d.kZero;
final var end =
new Pose3d(0.0, 5.0, 5.0, new Rotation3d(Units.degreesToRadians(90.0), 0.0, 0.0));
@@ -92,7 +92,7 @@ class Twist3dTest {
@Test
void testPose3dLogY() {
final var start = new Pose3d();
final var start = Pose3d.kZero;
final var end =
new Pose3d(5.0, 0.0, 5.0, new Rotation3d(0.0, Units.degreesToRadians(90.0), 0.0));
@@ -108,7 +108,7 @@ class Twist3dTest {
@Test
void testPose3dLogZ() {
final var start = new Pose3d();
final var start = Pose3d.kZero;
final var end =
new Pose3d(5.0, 5.0, 0.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(90.0)));

8
wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java
View File

@@ -16,7 +16,7 @@ class TimeInterpolatableBufferTest {
TimeInterpolatableBuffer<Rotation2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
// No entries
buffer.addSample(1.0, new Rotation2d());
buffer.addSample(1.0, Rotation2d.kZero);
assertEquals(0.0, buffer.getSample(1.0).get().getRadians(), 0.001);
// New entry at start of container
@@ -36,7 +36,7 @@ class TimeInterpolatableBufferTest {
void testInterpolation() {
TimeInterpolatableBuffer<Rotation2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
buffer.addSample(0.0, new Rotation2d());
buffer.addSample(0.0, Rotation2d.kZero);
assertEquals(0.0, buffer.getSample(0.0).get().getRadians(), 0.001);
buffer.addSample(1.0, new Rotation2d(1.0));
assertEquals(0.5, buffer.getSample(0.5).get().getRadians(), 0.001);
@@ -53,8 +53,8 @@ class TimeInterpolatableBufferTest {
TimeInterpolatableBuffer<Pose2d> buffer = TimeInterpolatableBuffer.createBuffer(10);
// We expect to be at (1 - 1/Math.sqrt(2), 1/Math.sqrt(2), 45deg) at t=0.5
buffer.addSample(0.0, new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(90.0)));
buffer.addSample(1.0, new Pose2d(1.0, 1.0, Rotation2d.fromDegrees(0.0)));
buffer.addSample(0.0, new Pose2d(0.0, 0.0, Rotation2d.kCCW_Pi_2));
buffer.addSample(1.0, new Pose2d(1.0, 1.0, Rotation2d.kZero));
Pose2d sample = buffer.getSample(0.5).get();
assertEquals(1.0 - 1.0 / Math.sqrt(2.0), sample.getTranslation().getX(), 0.01);

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

@@ -30,7 +30,7 @@ class ChassisSpeedsTest {
speeds.vyMetersPerSecond * dt,
speeds.omegaRadiansPerSecond * dt);
var pose = new Pose2d();
var pose = Pose2d.kZero;
for (double time = 0; time < duration; time += dt) {
pose = pose.exp(twist);
}
@@ -62,7 +62,7 @@ class ChassisSpeedsTest {
@Test
void testFromFieldRelativeSpeeds() {
final var chassisSpeeds =
ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0));
ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.kCW_Pi_2);
assertAll(
() -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),

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

@@ -14,11 +14,11 @@ import org.junit.jupiter.api.Test;
class DifferentialDriveOdometryTest {
private static final double kEpsilon = 1E-9;
private final DifferentialDriveOdometry m_odometry =
new DifferentialDriveOdometry(new Rotation2d(), 0, 0);
new DifferentialDriveOdometry(Rotation2d.kZero, 0, 0);
@Test
void testOdometryWithEncoderDistances() {
m_odometry.resetPosition(Rotation2d.fromDegrees(45), 0, 0, new Pose2d());
m_odometry.resetPosition(Rotation2d.fromDegrees(45), 0, 0, Pose2d.kZero);
var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI);
assertAll(

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

@@ -28,16 +28,16 @@ class MecanumDriveOdometryTest {
private final MecanumDriveWheelPositions zero = new MecanumDriveWheelPositions();
private final MecanumDriveOdometry m_odometry =
new MecanumDriveOdometry(m_kinematics, new Rotation2d(), zero);
new MecanumDriveOdometry(m_kinematics, Rotation2d.kZero, zero);
@Test
void testMultipleConsecutiveUpdates() {
var wheelPositions = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
m_odometry.resetPosition(new Rotation2d(), wheelPositions, new Pose2d());
m_odometry.resetPosition(Rotation2d.kZero, wheelPositions, Pose2d.kZero);
m_odometry.update(new Rotation2d(), wheelPositions);
var secondPose = m_odometry.update(new Rotation2d(), wheelPositions);
m_odometry.update(Rotation2d.kZero, wheelPositions);
var secondPose = m_odometry.update(Rotation2d.kZero, wheelPositions);
assertAll(
() -> assertEquals(secondPose.getX(), 0.0, 0.01),
@@ -49,10 +49,10 @@ class MecanumDriveOdometryTest {
void testTwoIterations() {
// 5 units/sec in the x-axis (forward)
final var wheelPositions = new MecanumDriveWheelPositions(0.3536, 0.3536, 0.3536, 0.3536);
m_odometry.resetPosition(new Rotation2d(), new MecanumDriveWheelPositions(), new Pose2d());
m_odometry.resetPosition(Rotation2d.kZero, new MecanumDriveWheelPositions(), Pose2d.kZero);
m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions());
var pose = m_odometry.update(new Rotation2d(), wheelPositions);
m_odometry.update(Rotation2d.kZero, new MecanumDriveWheelPositions());
var pose = m_odometry.update(Rotation2d.kZero, wheelPositions);
assertAll(
() -> assertEquals(0.3536, pose.getX(), 0.01),
@@ -65,10 +65,10 @@ class MecanumDriveOdometryTest {
// This is a 90 degree turn about the point between front left and rear left wheels
// fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946
final var wheelPositions = new MecanumDriveWheelPositions(-13.328, 39.986, -13.329, 39.986);
m_odometry.resetPosition(new Rotation2d(), new MecanumDriveWheelPositions(), new Pose2d());
m_odometry.resetPosition(Rotation2d.kZero, new MecanumDriveWheelPositions(), Pose2d.kZero);
m_odometry.update(new Rotation2d(), new MecanumDriveWheelPositions());
final var pose = m_odometry.update(Rotation2d.fromDegrees(90.0), wheelPositions);
m_odometry.update(Rotation2d.kZero, new MecanumDriveWheelPositions());
final var pose = m_odometry.update(Rotation2d.kCCW_Pi_2, wheelPositions);
assertAll(
() -> assertEquals(8.4855, pose.getX(), 0.01),
@@ -78,10 +78,10 @@ class MecanumDriveOdometryTest {
@Test
void testGyroAngleReset() {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
var gyro = Rotation2d.kCCW_Pi_2;
var fieldAngle = Rotation2d.kZero;
m_odometry.resetPosition(
gyro, new MecanumDriveWheelPositions(), new Pose2d(new Translation2d(), fieldAngle));
gyro, new MecanumDriveWheelPositions(), new Pose2d(Translation2d.kZero, fieldAngle));
var speeds = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536);
m_odometry.update(gyro, new MecanumDriveWheelPositions());
var pose = m_odometry.update(gyro, speeds);
@@ -102,17 +102,17 @@ class MecanumDriveOdometryTest {
var wheelPositions = new MecanumDriveWheelPositions();
var odometry =
new MecanumDriveOdometry(kinematics, new Rotation2d(), wheelPositions, new Pose2d());
new MecanumDriveOdometry(kinematics, Rotation2d.kZero, wheelPositions, Pose2d.kZero);
var trajectory =
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(),
new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
Pose2d.kZero,
new Pose2d(20, 20, Rotation2d.kZero),
new Pose2d(10, 10, Rotation2d.kPi),
new Pose2d(30, 30, Rotation2d.kZero),
new Pose2d(20, 20, Rotation2d.kPi),
new Pose2d(10, 10, Rotation2d.kZero)),
new TrajectoryConfig(0.5, 2));
var rand = new Random(5190);
@@ -191,17 +191,17 @@ class MecanumDriveOdometryTest {
var wheelPositions = new MecanumDriveWheelPositions();
var odometry =
new MecanumDriveOdometry(kinematics, new Rotation2d(), wheelPositions, new Pose2d());
new MecanumDriveOdometry(kinematics, Rotation2d.kZero, wheelPositions, Pose2d.kZero);
var trajectory =
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(),
new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
Pose2d.kZero,
new Pose2d(20, 20, Rotation2d.kZero),
new Pose2d(10, 10, Rotation2d.kPi),
new Pose2d(30, 30, Rotation2d.kZero),
new Pose2d(20, 20, Rotation2d.kPi),
new Pose2d(10, 10, Rotation2d.kZero)),
new TrajectoryConfig(0.5, 2));
var rand = new Random(5190);

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

@@ -41,7 +41,7 @@ class SwerveDriveKinematicsTest {
@Test
void testStraightLineForwardKinematics() { // test forward kinematics going in a straight line
SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(0.0));
SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.kZero);
var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
assertAll(
@@ -53,7 +53,7 @@ class SwerveDriveKinematicsTest {
@Test
void testStraightLineForwardKinematicsWithDeltas() {
// test forward kinematics going in a straight line
SwerveModulePosition delta = new SwerveModulePosition(5.0, Rotation2d.fromDegrees(0.0));
SwerveModulePosition delta = new SwerveModulePosition(5.0, Rotation2d.kZero);
var twist = m_kinematics.toTwist2d(delta, delta, delta, delta);
assertAll(
@@ -80,7 +80,7 @@ class SwerveDriveKinematicsTest {
@Test
void testStraightStrafeForwardKinematics() {
SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.kCCW_Pi_2);
var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
assertAll(
@@ -91,7 +91,7 @@ class SwerveDriveKinematicsTest {
@Test
void testStraightStrafeForwardKinematicsWithDeltas() {
SwerveModulePosition delta = new SwerveModulePosition(5.0, Rotation2d.fromDegrees(90.0));
SwerveModulePosition delta = new SwerveModulePosition(5.0, Rotation2d.kCCW_Pi_2);
var twist = m_kinematics.toTwist2d(delta, delta, delta, delta);
assertAll(
@@ -121,9 +121,9 @@ class SwerveDriveKinematicsTest {
@Test
void testResetWheelAngle() {
Rotation2d fl = new Rotation2d(0);
Rotation2d fr = new Rotation2d(Math.PI / 2);
Rotation2d bl = new Rotation2d(Math.PI);
Rotation2d fl = Rotation2d.kZero;
Rotation2d fr = Rotation2d.kCCW_Pi_2;
Rotation2d bl = Rotation2d.kPi;
Rotation2d br = new Rotation2d(3 * Math.PI / 2);
m_kinematics.resetHeadings(fl, fr, bl, br);
var moduleStates = m_kinematics.toSwerveModuleStates(new ChassisSpeeds());
@@ -221,9 +221,9 @@ class SwerveDriveKinematicsTest {
@Test
void testOffCenterCORRotationForwardKinematics() {
SwerveModuleState flState = new SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0));
SwerveModuleState frState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(0.0));
SwerveModuleState blState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(-90));
SwerveModuleState flState = new SwerveModuleState(0.0, Rotation2d.kZero);
SwerveModuleState frState = new SwerveModuleState(150.796, Rotation2d.kZero);
SwerveModuleState blState = new SwerveModuleState(150.796, Rotation2d.kCW_Pi_2);
SwerveModuleState brState = new SwerveModuleState(213.258, Rotation2d.fromDegrees(-45));
var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
@@ -245,8 +245,8 @@ class SwerveDriveKinematicsTest {
@Test
void testOffCenterCORRotationForwardKinematicsWithDeltas() {
SwerveModulePosition flDelta = new SwerveModulePosition(0.0, Rotation2d.fromDegrees(0.0));
SwerveModulePosition frDelta = new SwerveModulePosition(150.796, Rotation2d.fromDegrees(0.0));
SwerveModulePosition flDelta = new SwerveModulePosition(0.0, Rotation2d.kZero);
SwerveModulePosition frDelta = new SwerveModulePosition(150.796, Rotation2d.kZero);
SwerveModulePosition blDelta = new SwerveModulePosition(150.796, Rotation2d.fromDegrees(-90));
SwerveModulePosition brDelta = new SwerveModulePosition(213.258, Rotation2d.fromDegrees(-45));
@@ -357,10 +357,10 @@ class SwerveDriveKinematicsTest {
@Test
void testDesaturate() {
SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d());
SwerveModuleState bl = new SwerveModuleState(4, new Rotation2d());
SwerveModuleState br = new SwerveModuleState(7, new Rotation2d());
SwerveModuleState fl = new SwerveModuleState(5, Rotation2d.kZero);
SwerveModuleState fr = new SwerveModuleState(6, Rotation2d.kZero);
SwerveModuleState bl = new SwerveModuleState(4, Rotation2d.kZero);
SwerveModuleState br = new SwerveModuleState(7, Rotation2d.kZero);
SwerveModuleState[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.desaturateWheelSpeeds(arr, 5.5);
@@ -376,10 +376,10 @@ class SwerveDriveKinematicsTest {
@Test
void testDesaturateSmooth() {
SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d());
SwerveModuleState bl = new SwerveModuleState(4, new Rotation2d());
SwerveModuleState br = new SwerveModuleState(7, new Rotation2d());
SwerveModuleState fl = new SwerveModuleState(5, Rotation2d.kZero);
SwerveModuleState fr = new SwerveModuleState(6, Rotation2d.kZero);
SwerveModuleState bl = new SwerveModuleState(4, Rotation2d.kZero);
SwerveModuleState br = new SwerveModuleState(7, Rotation2d.kZero);
SwerveModuleState[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.desaturateWheelSpeeds(
@@ -396,10 +396,10 @@ class SwerveDriveKinematicsTest {
@Test
void testDesaturateNegativeSpeed() {
SwerveModuleState fl = new SwerveModuleState(1, new Rotation2d());
SwerveModuleState fr = new SwerveModuleState(1, new Rotation2d());
SwerveModuleState bl = new SwerveModuleState(-2, new Rotation2d());
SwerveModuleState br = new SwerveModuleState(-2, new Rotation2d());
SwerveModuleState fl = new SwerveModuleState(1, Rotation2d.kZero);
SwerveModuleState fr = new SwerveModuleState(1, Rotation2d.kZero);
SwerveModuleState bl = new SwerveModuleState(-2, Rotation2d.kZero);
SwerveModuleState br = new SwerveModuleState(-2, Rotation2d.kZero);
SwerveModuleState[] arr = {fl, fr, bl, br};
SwerveDriveKinematics.desaturateWheelSpeeds(arr, 1);

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

@@ -29,27 +29,27 @@ class SwerveDriveOdometryTest {
private final SwerveDriveOdometry m_odometry =
new SwerveDriveOdometry(
m_kinematics, new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
m_kinematics, Rotation2d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero});
@Test
void testTwoIterations() {
// 5 units/sec in the x-axis (forward)
final SwerveModulePosition[] wheelDeltas = {
new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)),
new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)),
new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0)),
new SwerveModulePosition(0.5, Rotation2d.fromDegrees(0))
new SwerveModulePosition(0.5, Rotation2d.kZero),
new SwerveModulePosition(0.5, Rotation2d.kZero),
new SwerveModulePosition(0.5, Rotation2d.kZero),
new SwerveModulePosition(0.5, Rotation2d.kZero)
};
m_odometry.update(
new Rotation2d(),
Rotation2d.kZero,
new SwerveModulePosition[] {
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition(),
new SwerveModulePosition()
});
var pose = m_odometry.update(new Rotation2d(), wheelDeltas);
var pose = m_odometry.update(Rotation2d.kZero, wheelDeltas);
assertAll(
() -> assertEquals(5.0 / 10.0, pose.getX(), 0.01),
@@ -66,15 +66,15 @@ class SwerveDriveOdometryTest {
// Module 3: speed 42.14888838624436 angle -26.565051177077986
final SwerveModulePosition[] wheelDeltas = {
new SwerveModulePosition(18.85, Rotation2d.fromDegrees(90.0)),
new SwerveModulePosition(18.85, Rotation2d.kCCW_Pi_2),
new SwerveModulePosition(42.15, Rotation2d.fromDegrees(26.565)),
new SwerveModulePosition(18.85, Rotation2d.fromDegrees(-90)),
new SwerveModulePosition(18.85, Rotation2d.kCW_Pi_2),
new SwerveModulePosition(42.15, Rotation2d.fromDegrees(-26.565))
};
final var zero = new SwerveModulePosition();
m_odometry.update(new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
final var pose = m_odometry.update(Rotation2d.fromDegrees(90.0), wheelDeltas);
m_odometry.update(Rotation2d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero});
final var pose = m_odometry.update(Rotation2d.kCCW_Pi_2, wheelDeltas);
assertAll(
() -> assertEquals(12.0, pose.getX(), 0.01),
@@ -84,15 +84,15 @@ class SwerveDriveOdometryTest {
@Test
void testGyroAngleReset() {
var gyro = Rotation2d.fromDegrees(90.0);
var fieldAngle = Rotation2d.fromDegrees(0.0);
var gyro = Rotation2d.kCCW_Pi_2;
var fieldAngle = Rotation2d.kZero;
m_odometry.resetPosition(
gyro,
new SwerveModulePosition[] {zero, zero, zero, zero},
new Pose2d(new Translation2d(), fieldAngle));
new Pose2d(Translation2d.kZero, fieldAngle));
var delta = new SwerveModulePosition();
m_odometry.update(gyro, new SwerveModulePosition[] {delta, delta, delta, delta});
delta = new SwerveModulePosition(1.0, Rotation2d.fromDegrees(0));
delta = new SwerveModulePosition(1.0, Rotation2d.kZero);
var pose = m_odometry.update(gyro, new SwerveModulePosition[] {delta, delta, delta, delta});
assertAll(
@@ -111,7 +111,7 @@ class SwerveDriveOdometryTest {
new Translation2d(-1, 1));
var odometry =
new SwerveDriveOdometry(
kinematics, new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
kinematics, Rotation2d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero});
SwerveModulePosition fl = new SwerveModulePosition();
SwerveModulePosition fr = new SwerveModulePosition();
@@ -122,9 +122,9 @@ class SwerveDriveOdometryTest {
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
new TrajectoryConfig(0.5, 2));
@@ -201,7 +201,7 @@ class SwerveDriveOdometryTest {
new Translation2d(-1, 1));
var odometry =
new SwerveDriveOdometry(
kinematics, new Rotation2d(), new SwerveModulePosition[] {zero, zero, zero, zero});
kinematics, Rotation2d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero});
SwerveModulePosition fl = new SwerveModulePosition();
SwerveModulePosition fr = new SwerveModulePosition();
@@ -212,9 +212,9 @@ class SwerveDriveOdometryTest {
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
new Pose2d(-3, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
new TrajectoryConfig(0.5, 2));

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

@@ -16,7 +16,7 @@ class SwerveModuleStateTest {
@Test
void testOptimize() {
var angleA = Rotation2d.fromDegrees(45);
var refA = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(180));
var refA = new SwerveModuleState(-2.0, Rotation2d.kPi);
var optimizedA = SwerveModuleState.optimize(refA, angleA);
assertAll(
@@ -34,7 +34,7 @@ class SwerveModuleStateTest {
@Test
void testNoOptimize() {
var angleA = Rotation2d.fromDegrees(0);
var angleA = Rotation2d.kZero;
var refA = new SwerveModuleState(2.0, Rotation2d.fromDegrees(89));
var optimizedA = SwerveModuleState.optimize(refA, angleA);
@@ -42,7 +42,7 @@ class SwerveModuleStateTest {
() -> assertEquals(2.0, optimizedA.speedMetersPerSecond, kEpsilon),
() -> assertEquals(89.0, optimizedA.angle.getDegrees(), kEpsilon));
var angleB = Rotation2d.fromDegrees(0);
var angleB = Rotation2d.kZero;
var refB = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(-2));
var optimizedB = SwerveModuleState.optimize(refB, angleB);

30
wpimath/src/test/java/edu/wpi/first/math/path/TravelingSalesmanTest.java
View File

@@ -57,11 +57,11 @@ class TravelingSalesmanTest {
// ...................
var poses =
new Pose2d[] {
new Pose2d(3, 3, new Rotation2d(0)),
new Pose2d(11, 5, new Rotation2d(0)),
new Pose2d(9, 2, new Rotation2d(0)),
new Pose2d(5, 5, new Rotation2d(0)),
new Pose2d(14, 3, new Rotation2d(0))
new Pose2d(3, 3, Rotation2d.kZero),
new Pose2d(11, 5, Rotation2d.kZero),
new Pose2d(9, 2, Rotation2d.kZero),
new Pose2d(5, 5, Rotation2d.kZero),
new Pose2d(14, 3, Rotation2d.kZero)
};
var traveler = new TravelingSalesman();
@@ -82,16 +82,16 @@ class TravelingSalesmanTest {
// ...................
var poses =
new Pose2d[] {
new Pose2d(2, 4, new Rotation2d(0)),
new Pose2d(10, 1, new Rotation2d(0)),
new Pose2d(12, 1, new Rotation2d(0)),
new Pose2d(7, 1, new Rotation2d(0)),
new Pose2d(3, 2, new Rotation2d(0)),
new Pose2d(9, 5, new Rotation2d(0)),
new Pose2d(5, 1, new Rotation2d(0)),
new Pose2d(6, 5, new Rotation2d(0)),
new Pose2d(13, 5, new Rotation2d(0)),
new Pose2d(14, 3, new Rotation2d(0))
new Pose2d(2, 4, Rotation2d.kZero),
new Pose2d(10, 1, Rotation2d.kZero),
new Pose2d(12, 1, Rotation2d.kZero),
new Pose2d(7, 1, Rotation2d.kZero),
new Pose2d(3, 2, Rotation2d.kZero),
new Pose2d(9, 5, Rotation2d.kZero),
new Pose2d(5, 1, Rotation2d.kZero),
new Pose2d(6, 5, Rotation2d.kZero),
new Pose2d(13, 5, Rotation2d.kZero),
new Pose2d(14, 3, Rotation2d.kZero)
};
var traveler = new TravelingSalesman();

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

@@ -97,33 +97,33 @@ class CubicHermiteSplineTest {
@Test
void testStraightLine() {
run(new Pose2d(), new ArrayList<>(), new Pose2d(3, 0, new Rotation2d()));
run(Pose2d.kZero, new ArrayList<>(), new Pose2d(3, 0, Rotation2d.kZero));
}
@Test
void testSCurve() {
var start = new Pose2d(0, 0, Rotation2d.fromDegrees(90.0));
var start = new Pose2d(0, 0, Rotation2d.kCCW_Pi_2);
ArrayList<Translation2d> waypoints = new ArrayList<>();
waypoints.add(new Translation2d(1, 1));
waypoints.add(new Translation2d(2, -1));
var end = new Pose2d(3, 0, Rotation2d.fromDegrees(90.0));
var end = new Pose2d(3, 0, Rotation2d.kCCW_Pi_2);
run(start, waypoints, end);
}
@Test
void testOneInterior() {
var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
var start = Pose2d.kZero;
ArrayList<Translation2d> waypoints = new ArrayList<>();
waypoints.add(new Translation2d(2.0, 0.0));
var end = new Pose2d(4, 0, Rotation2d.fromDegrees(0.0));
var end = new Pose2d(4, 0, Rotation2d.kZero);
run(start, waypoints, end);
}
@Test
void testWindyPath() {
final var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
final var start = Pose2d.kZero;
final ArrayList<Translation2d> waypoints = new ArrayList<>();
waypoints.add(new Translation2d(0.5, 0.5));
waypoints.add(new Translation2d(0.5, 0.5));
@@ -131,7 +131,7 @@ class CubicHermiteSplineTest {
waypoints.add(new Translation2d(1.5, 0.5));
waypoints.add(new Translation2d(2.0, 0.0));
waypoints.add(new Translation2d(2.5, 0.5));
final var end = new Pose2d(3.0, 0.0, Rotation2d.fromDegrees(0.0));
final var end = new Pose2d(3.0, 0.0, Rotation2d.kZero);
run(start, waypoints, end);
}
@@ -142,15 +142,15 @@ class CubicHermiteSplineTest {
MalformedSplineException.class,
() ->
run(
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(0, 0, Rotation2d.kZero),
new ArrayList<>(),
new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
new Pose2d(1, 0, Rotation2d.kPi)));
assertThrows(
MalformedSplineException.class,
() ->
run(
new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
new Pose2d(10, 10, Rotation2d.kCCW_Pi_2),
List.of(new Translation2d(10, 10.5)),
new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
new Pose2d(10, 11, Rotation2d.kCW_Pi_2)));
}
}

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

@@ -69,34 +69,27 @@ class QuinticHermiteSplineTest {
@Test
void testStraightLine() {
run(new Pose2d(), new Pose2d(3, 0, new Rotation2d()));
run(Pose2d.kZero, new Pose2d(3, 0, Rotation2d.kZero));
}
@Test
void testSimpleSCurve() {
run(new Pose2d(), new Pose2d(1, 1, new Rotation2d()));
run(Pose2d.kZero, new Pose2d(1, 1, Rotation2d.kZero));
}
@Test
void testSquiggly() {
run(
new Pose2d(0, 0, Rotation2d.fromDegrees(90)),
new Pose2d(-1, 0, Rotation2d.fromDegrees(90)));
run(new Pose2d(0, 0, Rotation2d.kCCW_Pi_2), new Pose2d(-1, 0, Rotation2d.kCCW_Pi_2));
}
@Test
void testMalformed() {
assertThrows(
MalformedSplineException.class,
() ->
run(
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
() -> run(new Pose2d(0, 0, Rotation2d.kZero), new Pose2d(1, 0, Rotation2d.kPi)));
assertThrows(
MalformedSplineException.class,
() ->
run(
new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
run(new Pose2d(10, 10, Rotation2d.kCCW_Pi_2), new Pose2d(10, 11, Rotation2d.kCW_Pi_2)));
}
}

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

@@ -87,17 +87,17 @@ class DifferentialDriveVoltageConstraintTest {
assertDoesNotThrow(
() ->
TrajectoryGenerator.generateTrajectory(
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
new Pose2d(1, 0, Rotation2d.kCCW_Pi_2),
new ArrayList<>(),
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
new Pose2d(0, 1, Rotation2d.kPi),
config));
assertDoesNotThrow(
() ->
TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
new Pose2d(0, 1, Rotation2d.kPi),
new ArrayList<>(),
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
new Pose2d(1, 0, Rotation2d.kCCW_Pi_2),
config.setReversed(true)));
}
}

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

@@ -27,7 +27,7 @@ class EllipticalRegionConstraintTest {
new Translation2d(Units.feetToMeters(5.0), Units.feetToMeters(5.0)),
Units.feetToMeters(10.0),
Units.feetToMeters(5.0),
Rotation2d.fromDegrees(180.0),
Rotation2d.kPi,
maxVelocityConstraint);
// Get trajectory
@@ -58,23 +58,23 @@ class EllipticalRegionConstraintTest {
new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
Units.feetToMeters(2.0),
Units.feetToMeters(4.0),
new Rotation2d(),
Rotation2d.kZero,
maxVelocityConstraint);
assertFalse(
regionConstraintNoRotation.isPoseInRegion(
new Pose2d(Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d())));
new Pose2d(Units.feetToMeters(2.1), Units.feetToMeters(1.0), Rotation2d.kZero)));
var regionConstraintWithRotation =
new EllipticalRegionConstraint(
new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
Units.feetToMeters(2.0),
Units.feetToMeters(4.0),
Rotation2d.fromDegrees(90.0),
Rotation2d.kCCW_Pi_2,
maxVelocityConstraint);
assertTrue(
regionConstraintWithRotation.isPoseInRegion(
new Pose2d(Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d())));
new Pose2d(Units.feetToMeters(2.1), Units.feetToMeters(1.0), Rotation2d.kZero)));
}
}

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

@@ -53,9 +53,9 @@ class RectangularRegionConstraintTest {
new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)),
maxVelocityConstraint);
assertFalse(regionConstraint.isPoseInRegion(new Pose2d()));
assertFalse(regionConstraint.isPoseInRegion(Pose2d.kZero));
assertTrue(
regionConstraint.isPoseInRegion(
new Pose2d(Units.feetToMeters(3.0), Units.feetToMeters(14.5), new Rotation2d())));
new Pose2d(Units.feetToMeters(3.0), Units.feetToMeters(14.5), Rotation2d.kZero)));
}
}

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

@@ -17,14 +17,14 @@ class TrajectoryConcatenateTest {
void testStates() {
var t1 =
TrajectoryGenerator.generateTrajectory(
new Pose2d(),
Pose2d.kZero,
List.of(),
new Pose2d(1, 1, new Rotation2d()),
new Pose2d(1, 1, Rotation2d.kZero),
new TrajectoryConfig(2, 2));
var t2 =
TrajectoryGenerator.generateTrajectory(
new Pose2d(1, 1, new Rotation2d()),
new Pose2d(1, 1, Rotation2d.kZero),
List.of(),
new Pose2d(2, 2, Rotation2d.fromDegrees(45)),
new TrajectoryConfig(2, 2));

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

@@ -25,8 +25,7 @@ class TrajectoryGeneratorTest {
final double maxAccel = feetToMeters(12);
// 2018 cross scale auto waypoints.
var sideStart =
new Pose2d(feetToMeters(1.54), feetToMeters(23.23), Rotation2d.fromDegrees(-180));
var sideStart = new Pose2d(feetToMeters(1.54), feetToMeters(23.23), Rotation2d.kPi);
var crossScale =
new Pose2d(feetToMeters(23.7), feetToMeters(6.8), Rotation2d.fromDegrees(-160));
@@ -35,12 +34,11 @@ class TrajectoryGeneratorTest {
waypoints.add(
sideStart.plus(
new Transform2d(
new Translation2d(feetToMeters(-13), feetToMeters(0)), new Rotation2d())));
new Translation2d(feetToMeters(-13), feetToMeters(0)), Rotation2d.kZero)));
waypoints.add(
sideStart.plus(
new Transform2d(
new Translation2d(feetToMeters(-19.5), feetToMeters(5)),
Rotation2d.fromDegrees(-90))));
new Translation2d(feetToMeters(-19.5), feetToMeters(5)), Rotation2d.kCW_Pi_2)));
waypoints.add(crossScale);
TrajectoryConfig config =
@@ -72,9 +70,7 @@ class TrajectoryGeneratorTest {
void testMalformedTrajectory() {
var traj =
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
new Pose2d(1, 0, Rotation2d.fromDegrees(180))),
List.of(Pose2d.kZero, new Pose2d(1, 0, Rotation2d.kPi)),
new TrajectoryConfig(feetToMeters(12), feetToMeters(12)));
assertEquals(traj.getStates().size(), 1);
@@ -86,11 +82,11 @@ class TrajectoryGeneratorTest {
Trajectory t =
TrajectoryGenerator.generateTrajectory(
List.of(
new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
new Pose2d(-1, 0, Rotation2d.fromDegrees(270)),
new Pose2d(0, -1, Rotation2d.fromDegrees(360)),
new Pose2d(1, 0, Rotation2d.fromDegrees(90))),
new Pose2d(1, 0, Rotation2d.kCCW_Pi_2),
new Pose2d(0, 1, Rotation2d.kPi),
new Pose2d(-1, 0, Rotation2d.kCW_Pi_2),
new Pose2d(0, -1, Rotation2d.kZero),
new Pose2d(1, 0, Rotation2d.kCCW_Pi_2)),
new TrajectoryConfig(2, 2));
for (int i = 1; i < t.getStates().size() - 1; ++i) {

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

@@ -19,7 +19,7 @@ class TrajectoryTransformTest {
var config = new TrajectoryConfig(3, 3);
var trajectory =
TrajectoryGenerator.generateTrajectory(
new Pose2d(), List.of(), new Pose2d(1, 1, Rotation2d.fromDegrees(90)), config);
Pose2d.kZero, List.of(), new Pose2d(1, 1, Rotation2d.kCCW_Pi_2), config);
var transformedTrajectory =
trajectory.transformBy(
@@ -39,13 +39,13 @@ class TrajectoryTransformTest {
TrajectoryGenerator.generateTrajectory(
new Pose2d(1, 2, Rotation2d.fromDegrees(30.0)),
List.of(),
new Pose2d(5, 7, Rotation2d.fromDegrees(90)),
new Pose2d(5, 7, Rotation2d.kCCW_Pi_2),
config);
var transformedTrajectory = trajectory.relativeTo(new Pose2d(1, 2, Rotation2d.fromDegrees(30)));
// Test initial pose.
assertEquals(new Pose2d(), transformedTrajectory.sample(0).poseMeters);
assertEquals(Pose2d.kZero, transformedTrajectory.sample(0).poseMeters);
testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
}