diff --git a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java index bae5007f54..f09d2626f1 100644 --- a/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java +++ b/apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java @@ -150,7 +150,7 @@ public class AprilTagFieldLayout { public final void setOrigin(OriginPosition origin) { switch (origin) { case kBlueAllianceWallRightSide: - setOrigin(new Pose3d()); + setOrigin(Pose3d.kZero); break; case kRedAllianceWallRightSide: setOrigin( diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java index dc97c1bb7e..2ace7a0ef6 100644 --- a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java +++ b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java @@ -158,7 +158,7 @@ class AprilTagDetectorTest { var estimator = new AprilTagPoseEstimator(new AprilTagPoseEstimator.Config(0.2, 500, 500, 320, 240)); AprilTagPoseEstimate est = estimator.estimateOrthogonalIteration(results[0], 200); - assertEquals(new Transform3d(), est.pose2); + assertEquals(Transform3d.kZero, est.pose2); Transform3d pose = estimator.estimate(results[0]); assertEquals(est.pose1, pose); } finally { diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagPoseSetOriginTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagPoseSetOriginTest.java index 999fe8717d..8f373b363d 100644 --- a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagPoseSetOriginTest.java +++ b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagPoseSetOriginTest.java @@ -19,7 +19,7 @@ class AprilTagPoseSetOriginTest { var layout = new AprilTagFieldLayout( List.of( - new AprilTag(1, new Pose3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0))), + new AprilTag(1, Pose3d.kZero), new AprilTag( 2, new Pose3d( @@ -40,7 +40,7 @@ class AprilTagPoseSetOriginTest { new Pose3d( new Translation3d( Units.feetToMeters(50.0), Units.feetToMeters(23.0), Units.feetToMeters(4)), - new Rotation3d(0.0, 0.0, 0)), + Rotation3d.kZero), layout.getTagPose(2).orElse(null)); } } diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagSerializationTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagSerializationTest.java index 5f5d1fde66..df734f3766 100644 --- a/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagSerializationTest.java +++ b/apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagSerializationTest.java @@ -20,8 +20,8 @@ class AprilTagSerializationTest { var layout = new AprilTagFieldLayout( List.of( - new AprilTag(1, new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0))), - new AprilTag(3, new Pose3d(0, 1, 0, new Rotation3d(0, 0, 0)))), + new AprilTag(1, Pose3d.kZero), + new AprilTag(3, new Pose3d(0, 1, 0, Rotation3d.kZero))), Units.feetToMeters(54.0), Units.feetToMeters(27.0)); diff --git a/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java b/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java index e4e4875a19..66923f4401 100644 --- a/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java +++ b/apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java @@ -37,7 +37,7 @@ class LoadConfigTest { Units.inchesToMeters(127.272), Units.inchesToMeters(216.01), Units.inchesToMeters(67.932), - new Rotation3d(0, 0, 0)); + Rotation3d.kZero); Optional maybePose = layout.getTagPose(1); assertTrue(maybePose.isPresent()); assertEquals(expectedPose, maybePose.get()); diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java index 3ca11010c3..9ac412ce50 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java @@ -41,7 +41,7 @@ class MecanumControllerCommandTest { } private final Timer m_timer = new Timer(); - private Rotation2d m_angle = new Rotation2d(0); + private Rotation2d m_angle = Rotation2d.kZero; private double m_frontLeftSpeed; private double m_frontLeftDistance; @@ -71,10 +71,7 @@ class MecanumControllerCommandTest { private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry( - m_kinematics, - new Rotation2d(0), - new MecanumDriveWheelPositions(), - new Pose2d(0, 0, new Rotation2d(0))); + m_kinematics, Rotation2d.kZero, new MecanumDriveWheelPositions(), Pose2d.kZero); public void setWheelSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) { this.m_frontLeftSpeed = wheelSpeeds.frontLeftMetersPerSecond; @@ -99,7 +96,7 @@ class MecanumControllerCommandTest { final var subsystem = new Subsystem() {}; final var waypoints = new ArrayList(); - waypoints.add(new Pose2d(0, 0, new Rotation2d(0))); + waypoints.add(Pose2d.kZero); waypoints.add(new Pose2d(1, 5, new Rotation2d(3))); var config = new TrajectoryConfig(8.8, 0.1); final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config); diff --git a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java index 9dd03aee33..2f6c696565 100644 --- a/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java +++ b/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java @@ -41,22 +41,22 @@ class SwerveControllerCommandTest { } private final Timer m_timer = new Timer(); - private Rotation2d m_angle = new Rotation2d(0); + private Rotation2d m_angle = Rotation2d.kZero; private SwerveModuleState[] m_moduleStates = new SwerveModuleState[] { - new SwerveModuleState(0, new Rotation2d(0)), - new SwerveModuleState(0, new Rotation2d(0)), - new SwerveModuleState(0, new Rotation2d(0)), - new SwerveModuleState(0, new Rotation2d(0)) + new SwerveModuleState(0, Rotation2d.kZero), + new SwerveModuleState(0, Rotation2d.kZero), + new SwerveModuleState(0, Rotation2d.kZero), + new SwerveModuleState(0, Rotation2d.kZero) }; private SwerveModulePosition[] m_modulePositions = new SwerveModulePosition[] { - new SwerveModulePosition(0, new Rotation2d(0)), - new SwerveModulePosition(0, new Rotation2d(0)), - new SwerveModulePosition(0, new Rotation2d(0)), - new SwerveModulePosition(0, new Rotation2d(0)) + new SwerveModulePosition(0, Rotation2d.kZero), + new SwerveModulePosition(0, Rotation2d.kZero), + new SwerveModulePosition(0, Rotation2d.kZero), + new SwerveModulePosition(0, Rotation2d.kZero) }; private final ProfiledPIDController m_rotController = @@ -77,8 +77,7 @@ class SwerveControllerCommandTest { new Translation2d(-kWheelBase / 2, -kTrackWidth / 2)); private final SwerveDriveOdometry m_odometry = - new SwerveDriveOdometry( - m_kinematics, new Rotation2d(0), m_modulePositions, new Pose2d(0, 0, new Rotation2d(0))); + new SwerveDriveOdometry(m_kinematics, Rotation2d.kZero, m_modulePositions, Pose2d.kZero); @SuppressWarnings("PMD.ArrayIsStoredDirectly") public void setModuleStates(SwerveModuleState[] moduleStates) { @@ -96,7 +95,7 @@ class SwerveControllerCommandTest { final var subsystem = new Subsystem() {}; final var waypoints = new ArrayList(); - waypoints.add(new Pose2d(0, 0, new Rotation2d(0))); + waypoints.add(Pose2d.kZero); waypoints.add(new Pose2d(1, 5, new Rotation2d(3))); var config = new TrajectoryConfig(8.8, 0.1); final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config); diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java index 507e3719c8..0e9731a2d6 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java @@ -181,7 +181,7 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea * positive. */ public void driveCartesian(double xSpeed, double ySpeed, double zRotation) { - driveCartesian(xSpeed, ySpeed, zRotation, new Rotation2d()); + driveCartesian(xSpeed, ySpeed, zRotation, Rotation2d.kZero); } /** @@ -240,7 +240,7 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea } driveCartesian( - magnitude * angle.getCos(), magnitude * angle.getSin(), zRotation, new Rotation2d()); + magnitude * angle.getCos(), magnitude * angle.getSin(), zRotation, Rotation2d.kZero); } /** @@ -256,7 +256,7 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea * @return Wheel speeds [-1.0..1.0]. */ public static WheelSpeeds driveCartesianIK(double xSpeed, double ySpeed, double zRotation) { - return driveCartesianIK(xSpeed, ySpeed, zRotation, new Rotation2d()); + return driveCartesianIK(xSpeed, ySpeed, zRotation, Rotation2d.kZero); } /** diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java index 4b02c4fe02..7ce6af8c3e 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java @@ -34,7 +34,7 @@ public class Field2d implements NTSendable, AutoCloseable { @SuppressWarnings("this-escape") public Field2d() { FieldObject2d obj = new FieldObject2d("Robot"); - obj.setPose(new Pose2d()); + obj.setPose(Pose2d.kZero); m_objects.add(obj); SendableRegistry.add(this, "Field"); } diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java index 0fecbb6691..f3627a6bef 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java @@ -59,7 +59,7 @@ public class FieldObject2d implements AutoCloseable { public synchronized Pose2d getPose() { updateFromEntry(); if (m_poses.isEmpty()) { - return new Pose2d(); + return Pose2d.kZero; } return m_poses.get(0); } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java index 8be2ccb756..8236693446 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java @@ -53,35 +53,35 @@ class MecanumDriveTest { @Test void testCartesianIKGyro90CW() { // Forward in global frame; left in robot frame - var speeds = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.0, Rotation2d.fromDegrees(90.0)); + var speeds = MecanumDrive.driveCartesianIK(1.0, 0.0, 0.0, Rotation2d.kCCW_Pi_2); assertEquals(-1.0, speeds.frontLeft, 1e-9); assertEquals(1.0, speeds.frontRight, 1e-9); assertEquals(1.0, speeds.rearLeft, 1e-9); assertEquals(-1.0, speeds.rearRight, 1e-9); // Left in global frame; backward in robot frame - speeds = MecanumDrive.driveCartesianIK(0.0, -1.0, 0.0, Rotation2d.fromDegrees(90.0)); + speeds = MecanumDrive.driveCartesianIK(0.0, -1.0, 0.0, Rotation2d.kCCW_Pi_2); assertEquals(-1.0, speeds.frontLeft, 1e-9); assertEquals(-1.0, speeds.frontRight, 1e-9); assertEquals(-1.0, speeds.rearLeft, 1e-9); assertEquals(-1.0, speeds.rearRight, 1e-9); // Right in global frame; forward in robot frame - speeds = MecanumDrive.driveCartesianIK(0.0, 1.0, 0.0, Rotation2d.fromDegrees(90.0)); + speeds = MecanumDrive.driveCartesianIK(0.0, 1.0, 0.0, Rotation2d.kCCW_Pi_2); assertEquals(1.0, speeds.frontLeft, 1e-9); assertEquals(1.0, speeds.frontRight, 1e-9); assertEquals(1.0, speeds.rearLeft, 1e-9); assertEquals(1.0, speeds.rearRight, 1e-9); // Rotate CCW - speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, -1.0, Rotation2d.fromDegrees(90.0)); + speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, -1.0, Rotation2d.kCCW_Pi_2); assertEquals(-1.0, speeds.frontLeft, 1e-9); assertEquals(1.0, speeds.frontRight, 1e-9); assertEquals(-1.0, speeds.rearLeft, 1e-9); assertEquals(1.0, speeds.rearRight, 1e-9); // Rotate CW - speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, 1.0, Rotation2d.fromDegrees(90.0)); + speeds = MecanumDrive.driveCartesianIK(0.0, 0.0, 1.0, Rotation2d.kCCW_Pi_2); assertEquals(1.0, speeds.frontLeft, 1e-9); assertEquals(-1.0, speeds.frontRight, 1e-9); assertEquals(1.0, speeds.rearLeft, 1e-9); @@ -143,35 +143,35 @@ class MecanumDriveTest { drive.setDeadband(0.0); // Forward in global frame; left in robot frame - drive.driveCartesian(1.0, 0.0, 0.0, Rotation2d.fromDegrees(90.0)); + drive.driveCartesian(1.0, 0.0, 0.0, Rotation2d.kCCW_Pi_2); assertEquals(-1.0, fl.get(), 1e-9); assertEquals(1.0, fr.get(), 1e-9); assertEquals(1.0, rl.get(), 1e-9); assertEquals(-1.0, rr.get(), 1e-9); // Left in global frame; backward in robot frame - drive.driveCartesian(0.0, -1.0, 0.0, Rotation2d.fromDegrees(90.0)); + drive.driveCartesian(0.0, -1.0, 0.0, Rotation2d.kCCW_Pi_2); assertEquals(-1.0, fl.get(), 1e-9); assertEquals(-1.0, fr.get(), 1e-9); assertEquals(-1.0, rl.get(), 1e-9); assertEquals(-1.0, rr.get(), 1e-9); // Right in global frame; forward in robot frame - drive.driveCartesian(0.0, 1.0, 0.0, Rotation2d.fromDegrees(90.0)); + drive.driveCartesian(0.0, 1.0, 0.0, Rotation2d.kCCW_Pi_2); assertEquals(1.0, fl.get(), 1e-9); assertEquals(1.0, fr.get(), 1e-9); assertEquals(1.0, rl.get(), 1e-9); assertEquals(1.0, rr.get(), 1e-9); // Rotate CCW - drive.driveCartesian(0.0, 0.0, -1.0, Rotation2d.fromDegrees(90.0)); + drive.driveCartesian(0.0, 0.0, -1.0, Rotation2d.kCCW_Pi_2); assertEquals(-1.0, fl.get(), 1e-9); assertEquals(1.0, fr.get(), 1e-9); assertEquals(-1.0, rl.get(), 1e-9); assertEquals(1.0, rr.get(), 1e-9); // Rotate CW - drive.driveCartesian(0.0, 0.0, 1.0, Rotation2d.fromDegrees(90.0)); + drive.driveCartesian(0.0, 0.0, 1.0, Rotation2d.kCCW_Pi_2); assertEquals(1.0, fl.get(), 1e-9); assertEquals(-1.0, fr.get(), 1e-9); assertEquals(1.0, rl.get(), 1e-9); @@ -188,35 +188,35 @@ class MecanumDriveTest { drive.setDeadband(0.0); // Forward - drive.drivePolar(1.0, Rotation2d.fromDegrees(0.0), 0.0); + drive.drivePolar(1.0, Rotation2d.kZero, 0.0); assertEquals(1.0, fl.get(), 1e-9); assertEquals(1.0, fr.get(), 1e-9); assertEquals(1.0, rl.get(), 1e-9); assertEquals(1.0, rr.get(), 1e-9); // Left - drive.drivePolar(1.0, Rotation2d.fromDegrees(-90.0), 0.0); + drive.drivePolar(1.0, Rotation2d.kCW_Pi_2, 0.0); assertEquals(-1.0, fl.get(), 1e-9); assertEquals(1.0, fr.get(), 1e-9); assertEquals(1.0, rl.get(), 1e-9); assertEquals(-1.0, rr.get(), 1e-9); // Right - drive.drivePolar(1.0, Rotation2d.fromDegrees(90.0), 0.0); + drive.drivePolar(1.0, Rotation2d.kCCW_Pi_2, 0.0); assertEquals(1.0, fl.get(), 1e-9); assertEquals(-1.0, fr.get(), 1e-9); assertEquals(-1.0, rl.get(), 1e-9); assertEquals(1.0, rr.get(), 1e-9); // Rotate CCW - drive.drivePolar(0.0, Rotation2d.fromDegrees(0.0), -1.0); + drive.drivePolar(0.0, Rotation2d.kZero, -1.0); assertEquals(-1.0, fl.get(), 1e-9); assertEquals(1.0, fr.get(), 1e-9); assertEquals(-1.0, rl.get(), 1e-9); assertEquals(1.0, rr.get(), 1e-9); // Rotate CW - drive.drivePolar(0.0, Rotation2d.fromDegrees(0.0), 1.0); + drive.drivePolar(0.0, Rotation2d.kZero, 1.0); assertEquals(1.0, fl.get(), 1e-9); assertEquals(-1.0, fr.get(), 1e-9); assertEquals(1.0, rl.get(), 1e-9); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java index a04ce10bb0..765420dac3 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java @@ -18,7 +18,7 @@ class AnalogEncoderSimTest { var analogEncoder = new AnalogEncoder(analogInput)) { var encoderSim = new AnalogEncoderSim(analogEncoder); - encoderSim.setPosition(Rotation2d.fromDegrees(180)); + encoderSim.setPosition(Rotation2d.kPi); assertEquals(analogEncoder.get(), 0.5, 1E-8); assertEquals(encoderSim.getTurns(), 0.5, 1E-8); assertEquals(encoderSim.getPosition().getRadians(), Math.PI, 1E-8); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java index 554d967ab1..25bbdb5983 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java @@ -55,9 +55,9 @@ class DifferentialDrivetrainSimTest { var traj = TrajectoryGenerator.generateTrajectory( - new Pose2d(), + Pose2d.kZero, List.of(), - new Pose2d(2, 2, new Rotation2d()), + new Pose2d(2, 2, Rotation2d.kZero), new TrajectoryConfig(1, 1) .addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, 1))); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java index d0c200250b..4fc38ca525 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java @@ -83,7 +83,7 @@ public class Drivetrain { m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance(), - new Pose2d(), + Pose2d.kZero, VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java index d278f054c4..5ceb363806 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java @@ -90,11 +90,11 @@ public class RobotContainer { Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( // Start at the origin facing the +X direction - new Pose2d(0, 0, new Rotation2d(0)), + Pose2d.kZero, // Pass through these two interior waypoints, making an 's' curve path List.of(new Translation2d(1, 1), new Translation2d(2, -1)), // End 3 meters straight ahead of where we started, facing forward - new Pose2d(3, 0, new Rotation2d(0)), + new Pose2d(3, 0, Rotation2d.kZero), config); MecanumControllerCommand mecanumControllerCommand = diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java index 0625b3fc37..a534ae31b0 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java @@ -58,7 +58,7 @@ public class Drivetrain { m_kinematics, m_gyro.getRotation2d(), getCurrentDistances(), - new Pose2d(), + Pose2d.kZero, VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))); diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java index 553ac9cd19..0227c3bfc4 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java @@ -34,9 +34,9 @@ public class Robot extends TimedRobot { public void robotInit() { m_trajectory = TrajectoryGenerator.generateTrajectory( - new Pose2d(2, 2, new Rotation2d()), + new Pose2d(2, 2, Rotation2d.kZero), List.of(), - new Pose2d(6, 4, new Rotation2d()), + new Pose2d(6, 4, Rotation2d.kZero), new TrajectoryConfig(2, 2)); } diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java index 1f8e3e57ea..f2641890a6 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java @@ -88,11 +88,11 @@ public class RobotContainer { Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory( // Start at the origin facing the +X direction - new Pose2d(0, 0, new Rotation2d(0)), + Pose2d.kZero, // Pass through these two interior waypoints, making an 's' curve path List.of(new Translation2d(1, 1), new Translation2d(2, -1)), // End 3 meters straight ahead of where we started, facing forward - new Pose2d(3, 0, new Rotation2d(0)), + new Pose2d(3, 0, Rotation2d.kZero), config); var thetaController = diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java index 6eb80e0fe5..9d68c0e3c3 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java @@ -48,7 +48,7 @@ public class Drivetrain { m_backLeft.getPosition(), m_backRight.getPosition() }, - new Pose2d(), + Pose2d.kZero, VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)), VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30))); diff --git a/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java b/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java index 609cdf5e5a..c288d7fb2d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java +++ b/wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java @@ -27,7 +27,7 @@ public final class ComputerVisionUtil { * either be a constant for a rigidly mounted camera, or variable if the camera is mounted to * a turret. If the camera was mounted 3 inches in front of the "origin" (usually physical * center) of the robot, this would be new Transform3d(Units.inchesToMeters(3.0), 0.0, 0.0, - * new Rotation3d()). + * Rotation3d.kZero). * @return The robot's field-relative pose. */ public static Pose3d objectToRobotPose( diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java index 87148a45db..7775a30de7 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java @@ -22,9 +22,9 @@ import edu.wpi.first.math.util.Units; * point toward. This heading reference is profiled for smoothness. */ public class HolonomicDriveController { - private Pose2d m_poseError = new Pose2d(); - private Rotation2d m_rotationError = new Rotation2d(); - private Pose2d m_poseTolerance = new Pose2d(); + private Pose2d m_poseError = Pose2d.kZero; + private Rotation2d m_rotationError = Rotation2d.kZero; + private Pose2d m_poseTolerance = Pose2d.kZero; private boolean m_enabled = true; private final PIDController m_xController; diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java index 3853776e6b..d936f9efc8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java @@ -37,8 +37,8 @@ public class RamseteController { private final double m_zeta; - private Pose2d m_poseError = new Pose2d(); - private Pose2d m_poseTolerance = new Pose2d(); + private Pose2d m_poseError = Pose2d.kZero; + private Pose2d m_poseTolerance = Pose2d.kZero; private boolean m_enabled = true; /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java index 97e8307999..d00490f450 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java @@ -26,13 +26,20 @@ import java.util.Objects; @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Pose2d implements Interpolatable, ProtobufSerializable, StructSerializable { + /** + * A preallocated Pose2d representing the origin. + * + *

This exists to avoid allocations for common poses. + */ + public static final Pose2d kZero = new Pose2d(); + private final Translation2d m_translation; private final Rotation2d m_rotation; /** Constructs a pose at the origin facing toward the positive X axis. */ public Pose2d() { - m_translation = new Translation2d(); - m_rotation = new Rotation2d(); + m_translation = Translation2d.kZero; + m_rotation = Rotation2d.kZero; } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java index 282420fefb..9cebf0882d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java @@ -20,13 +20,20 @@ import java.util.Objects; @JsonIgnoreProperties(ignoreUnknown = true) @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Pose3d implements Interpolatable, ProtobufSerializable, StructSerializable { + /** + * A preallocated Pose3d representing the origin. + * + *

This exists to avoid allocations for common poses. + */ + public static final Pose3d kZero = new Pose3d(); + private final Translation3d m_translation; private final Rotation3d m_rotation; /** Constructs a pose at the origin facing toward the positive X axis. */ public Pose3d() { - m_translation = new Translation3d(); - m_rotation = new Rotation3d(); + m_translation = Translation3d.kZero; + m_rotation = Rotation3d.kZero; } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java index 435ceed7ab..126a6df9d3 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java @@ -32,6 +32,55 @@ import java.util.Objects; @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Rotation2d implements Interpolatable, ProtobufSerializable, StructSerializable { + /** + * A preallocated Rotation2d representing no rotation. + * + *

This exists to avoid allocations for common rotations. + */ + public static final Rotation2d kZero = new Rotation2d(); + + /** + * A preallocated Rotation2d representing a clockwise rotation by π/2 rad (90°). + * + *

This exists to avoid allocations for common rotations. + */ + public static final Rotation2d kCW_Pi_2 = new Rotation2d(-Math.PI / 2); + + /** + * A preallocated Rotation2d representing a clockwise rotation by 90° (π/2 rad). + * + *

This exists to avoid allocations for common rotations. + */ + public static final Rotation2d kCW_90deg = kCW_Pi_2; + + /** + * A preallocated Rotation2d representing a counterclockwise rotation by π/2 rad (90°). + * + *

This exists to avoid allocations for common rotations. + */ + public static final Rotation2d kCCW_Pi_2 = new Rotation2d(Math.PI / 2); + + /** + * A preallocated Rotation2d representing a counterclockwise rotation by 90° (π/2 rad). + * + *

This exists to avoid allocations for common rotations. + */ + public static final Rotation2d kCCW_90deg = kCCW_Pi_2; + + /** + * A preallocated Rotation2d representing a counterclockwise rotation by π rad (180°). + * + *

This exists to avoid allocations for common rotations. + */ + public static final Rotation2d kPi = new Rotation2d(Math.PI); + + /** + * A preallocated Rotation2d representing a counterclockwise rotation by 180° (π rad). + * + *

This exists to avoid allocations for common rotations. + */ + public static final Rotation2d k180deg = kPi; + private final double m_value; private final double m_cos; private final double m_sin; diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java index d5031508a2..001c648cf0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java @@ -28,6 +28,13 @@ import org.ejml.dense.row.factory.DecompositionFactory_DDRM; @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Rotation3d implements Interpolatable, ProtobufSerializable, StructSerializable { + /** + * A preallocated Rotation3d representing no rotation. + * + *

This exists to avoid allocations for common rotations. + */ + public static final Rotation3d kZero = new Rotation3d(); + private final Quaternion m_q; /** Constructs a Rotation3d representing no rotation. */ diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java index 68747af423..8667f6d6e6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java @@ -16,6 +16,13 @@ import java.util.Objects; /** Represents a transformation for a Pose2d in the pose's frame. */ public class Transform2d implements ProtobufSerializable, StructSerializable { + /** + * A preallocated Transform2d representing no transformation. + * + *

This exists to avoid allocations for common transformations. + */ + public static final Transform2d kZero = new Transform2d(); + private final Translation2d m_translation; private final Rotation2d m_rotation; @@ -74,8 +81,8 @@ public class Transform2d implements ProtobufSerializable, StructSerializable { /** Constructs the identity transform -- maps an initial pose to itself. */ public Transform2d() { - m_translation = new Translation2d(); - m_rotation = new Rotation2d(); + m_translation = Translation2d.kZero; + m_rotation = Rotation2d.kZero; } /** @@ -106,7 +113,7 @@ public class Transform2d implements ProtobufSerializable, StructSerializable { * @return The composition of the two transformations. */ public Transform2d plus(Transform2d other) { - return new Transform2d(new Pose2d(), new Pose2d().transformBy(this).transformBy(other)); + return new Transform2d(Pose2d.kZero, Pose2d.kZero.transformBy(this).transformBy(other)); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java index cdd021f835..2f986ff29d 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java @@ -12,6 +12,13 @@ import java.util.Objects; /** Represents a transformation for a Pose3d in the pose's frame. */ public class Transform3d implements ProtobufSerializable, StructSerializable { + /** + * A preallocated Transform3d representing no transformation. + * + *

This exists to avoid allocations for common transformations. + */ + public static final Transform3d kZero = new Transform3d(); + private final Translation3d m_translation; private final Rotation3d m_rotation; @@ -59,8 +66,8 @@ public class Transform3d implements ProtobufSerializable, StructSerializable { /** Constructs the identity transform -- maps an initial pose to itself. */ public Transform3d() { - m_translation = new Translation3d(); - m_rotation = new Rotation3d(); + m_translation = Translation3d.kZero; + m_rotation = Rotation3d.kZero; } /** @@ -91,7 +98,7 @@ public class Transform3d implements ProtobufSerializable, StructSerializable { * @return The composition of the two transformations. */ public Transform3d plus(Transform3d other) { - return new Transform3d(new Pose3d(), new Pose3d().transformBy(this).transformBy(other)); + return new Transform3d(Pose3d.kZero, Pose3d.kZero.transformBy(this).transformBy(other)); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java index 4d04df354e..d0357e80ae 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java @@ -36,6 +36,13 @@ import java.util.Objects; @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Translation2d implements Interpolatable, ProtobufSerializable, StructSerializable { + /** + * A preallocated Translation2d representing the origin. + * + *

This exists to avoid allocations for common translations. + */ + public static final Translation2d kZero = new Translation2d(); + private final double m_x; private final double m_y; diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java index 6068979fc3..ba8c699dc4 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java @@ -34,6 +34,13 @@ import java.util.Objects; @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE) public class Translation3d implements Interpolatable, ProtobufSerializable, StructSerializable { + /** + * A preallocated Translation3d representing the origin. + * + *

This exists to avoid allocations for common translations. + */ + public static final Translation3d kZero = new Translation3d(); + private final double m_x; private final double m_y; private final double m_z; diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java index 4e1a8c9355..252ab1b594 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java @@ -109,7 +109,7 @@ public class ChassisSpeeds implements ProtobufSerializable, StructSerializable { // Find the chassis translation/rotation deltas in the robot frame that move the robot from its // current pose to the desired pose - var twist = new Pose2d().log(desiredDeltaPose); + var twist = Pose2d.kZero.log(desiredDeltaPose); // Turn the chassis translation/rotation deltas into average velocities return new ChassisSpeeds(twist.dx / dtSeconds, twist.dy / dtSeconds, twist.dtheta / dtSeconds); diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java index 87d7bbf77f..ad2199e4b9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java @@ -70,7 +70,7 @@ public class DifferentialDriveOdometry extends Odometry leftDistance, Measure rightDistance) { - this(gyroAngle, leftDistance, rightDistance, new Pose2d()); + this(gyroAngle, leftDistance, rightDistance, Pose2d.kZero); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java index 44dbb6b25f..f638b14b7c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java @@ -46,7 +46,7 @@ public class MecanumDriveKinematics private final Translation2d m_rearLeftWheelMeters; private final Translation2d m_rearRightWheelMeters; - private Translation2d m_prevCoR = new Translation2d(); + private Translation2d m_prevCoR = Translation2d.kZero; /** MecanumDriveKinematics protobuf for serialization. */ public static final MecanumDriveKinematicsProto proto = new MecanumDriveKinematicsProto(); @@ -140,7 +140,7 @@ public class MecanumDriveKinematics */ @Override public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) { - return toWheelSpeeds(chassisSpeeds, new Translation2d()); + return toWheelSpeeds(chassisSpeeds, Translation2d.kZero); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java index 32bc9bff05..0fd4b37416 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java @@ -45,6 +45,6 @@ public class MecanumDriveOdometry extends Odometry { MecanumDriveKinematics kinematics, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) { - this(kinematics, gyroAngle, wheelPositions, new Pose2d()); + this(kinematics, gyroAngle, wheelPositions, Pose2d.kZero); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index 6648343f89..9e78094268 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -65,7 +65,7 @@ public class SwerveDriveKinematics private final int m_numModules; private final Translation2d[] m_modules; private Rotation2d[] m_moduleHeadings; - private Translation2d m_prevCoR = new Translation2d(); + private Translation2d m_prevCoR = Translation2d.kZero; /** * Constructs a swerve drive kinematics object. This takes in a variable number of module @@ -84,7 +84,7 @@ public class SwerveDriveKinematics m_numModules = moduleTranslationsMeters.length; m_modules = Arrays.copyOf(moduleTranslationsMeters, m_numModules); m_moduleHeadings = new Rotation2d[m_numModules]; - Arrays.fill(m_moduleHeadings, new Rotation2d()); + Arrays.fill(m_moduleHeadings, Rotation2d.kZero); m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3); for (int i = 0; i < m_numModules; i++) { @@ -196,7 +196,7 @@ public class SwerveDriveKinematics * @return An array containing the module states. */ public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) { - return toSwerveModuleStates(chassisSpeeds, new Translation2d()); + return toSwerveModuleStates(chassisSpeeds, Translation2d.kZero); } @Override diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java index 4f954e930e..14f62b2f20 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java @@ -51,7 +51,7 @@ public class SwerveDriveOdometry extends Odometry { SwerveDriveKinematics kinematics, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { - this(kinematics, gyroAngle, modulePositions, new Pose2d()); + this(kinematics, gyroAngle, modulePositions, Pose2d.kZero); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java index 3041d42cd0..ec430331e9 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java @@ -27,7 +27,7 @@ public class SwerveModulePosition public double distanceMeters; /** Angle of the module. */ - public Rotation2d angle = Rotation2d.fromDegrees(0); + public Rotation2d angle = Rotation2d.kZero; /** SwerveModulePosition protobuf for serialization. */ public static final SwerveModulePositionProto proto = new SwerveModulePositionProto(); diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java index 9f1604a0d6..a5db397260 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java @@ -23,7 +23,7 @@ public class SwerveModuleState public double speedMetersPerSecond; /** Angle of the module. */ - public Rotation2d angle = Rotation2d.fromDegrees(0); + public Rotation2d angle = Rotation2d.kZero; /** SwerveModuleState protobuf for serialization. */ public static final SwerveModuleStateProto proto = new SwerveModuleStateProto(); @@ -102,8 +102,7 @@ public class SwerveModuleState var delta = desiredState.angle.minus(currentAngle); if (Math.abs(delta.getDegrees()) > 90.0) { return new SwerveModuleState( - -desiredState.speedMetersPerSecond, - desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0))); + -desiredState.speedMetersPerSecond, desiredState.angle.rotateBy(Rotation2d.kPi)); } else { return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle); } diff --git a/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java b/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java index fbeeb35dc8..94ab7616d0 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java +++ b/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java @@ -27,6 +27,6 @@ public class PoseWithCurvature { /** Constructs a PoseWithCurvature with default values. */ public PoseWithCurvature() { - poseMeters = new Pose2d(); + poseMeters = Pose2d.kZero; } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java index c0dc037a75..cdcac9ef48 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java @@ -295,7 +295,7 @@ public class Trajectory implements ProtobufSerializable { /** Default constructor. */ public State() { - poseMeters = new Pose2d(); + poseMeters = Pose2d.kZero; } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java index fcfef4508c..3de3effd82 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java @@ -21,6 +21,8 @@ import java.util.function.BiConsumer; /** Helper class used to generate trajectories with various constraints. */ public final class TrajectoryGenerator { + private static final Transform2d kFlip = new Transform2d(Translation2d.kZero, Rotation2d.kPi); + private static final Trajectory kDoNothingTrajectory = new Trajectory(List.of(new Trajectory.State())); private static BiConsumer errorFunc; @@ -62,8 +64,6 @@ public final class TrajectoryGenerator { List interiorWaypoints, Spline.ControlVector end, TrajectoryConfig config) { - final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0)); - // Clone the control vectors. var newInitial = new Spline.ControlVector(initial.x, initial.y); var newEnd = new Spline.ControlVector(end.x, end.y); @@ -91,7 +91,7 @@ public final class TrajectoryGenerator { // Change the points back to their original orientation. if (config.isReversed()) { for (var point : points) { - point.poseMeters = point.poseMeters.plus(flip); + point.poseMeters = point.poseMeters.plus(kFlip); point.curvatureRadPerMeter *= -1; } } @@ -140,7 +140,6 @@ public final class TrajectoryGenerator { */ public static Trajectory generateTrajectory( ControlVectorList controlVectors, TrajectoryConfig config) { - final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0)); final var newControlVectors = new ArrayList(controlVectors.size()); // Create a new control vector list, flipping the orientation if reversed. @@ -168,7 +167,7 @@ public final class TrajectoryGenerator { // Change the points back to their original orientation. if (config.isReversed()) { for (var point : points) { - point.poseMeters = point.poseMeters.plus(flip); + point.poseMeters = point.poseMeters.plus(kFlip); point.curvatureRadPerMeter *= -1; } } @@ -194,12 +193,10 @@ public final class TrajectoryGenerator { * @return The generated trajectory. */ public static Trajectory generateTrajectory(List waypoints, TrajectoryConfig config) { - final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0)); - List newWaypoints = new ArrayList<>(); if (config.isReversed()) { for (Pose2d originalWaypoint : waypoints) { - newWaypoints.add(originalWaypoint.plus(flip)); + newWaypoints.add(originalWaypoint.plus(kFlip)); } } else { newWaypoints.addAll(waypoints); @@ -220,7 +217,7 @@ public final class TrajectoryGenerator { // Change the points back to their original orientation. if (config.isReversed()) { for (var point : points) { - point.poseMeters = point.poseMeters.plus(flip); + point.poseMeters = point.poseMeters.plus(kFlip); point.curvatureRadPerMeter *= -1; } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java index db3f6dcf2a..4617fb76ae 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java @@ -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 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); } diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java index e977de8792..99bc544bd0 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVDifferentialDriveControllerTest.java @@ -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(); - 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); diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java index 463e1ce754..23316854e2 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/LTVUnicycleControllerTest.java @@ -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(); - 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); diff --git a/wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java b/wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java index c68272d881..de76e528a0 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java @@ -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(); - 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); diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java index b019c60811..0fdcfc3114 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java @@ -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)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java index 8204fec084..a1b5b3a1cf 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java @@ -114,7 +114,7 @@ class ExtendedKalmanFilterTest { List 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)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java index f5f0925568..d0609b8b13 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java @@ -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); diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java index c48012f7f9..d65cf25675 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java @@ -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)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java index 7101fa554a..0abdd5de6a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java @@ -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)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java index bcc33aec67..c5a6cd11a5 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java @@ -137,7 +137,7 @@ class UnscentedKalmanFilterTest { List 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)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java index 4dc18263b4..5d25562da7 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java @@ -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()); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java index 2c683c23ba..0844e1321e 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java @@ -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( diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java index 3ef8a48356..b333d24592 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java @@ -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), diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java index 8a5dee60ea..b75182edb7 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java @@ -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); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java index 896afbfa4f..43b7e9b4c5 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java @@ -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)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java index 39e2073b63..cd7d021afa 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java @@ -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); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java index 4108a628ed..c055f9bd31 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java @@ -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); diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist3dTest.java index bb3bbb7e87..c7ef787724 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist3dTest.java @@ -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))); diff --git a/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java b/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java index a66f71457b..67742c7546 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/interpolation/TimeInterpolatableBufferTest.java @@ -16,7 +16,7 @@ class TimeInterpolatableBufferTest { TimeInterpolatableBuffer 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 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 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); diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java index 53af407759..d5d81858cb 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java @@ -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), diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java index 6d15f64251..aaf7f4106a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java @@ -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( diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java index 405749f26b..86b2b40595 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java @@ -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); diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java index f429a68d8c..546db37314 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java @@ -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); diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java index 716da0398e..411ba705f4 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java @@ -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)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java index 01815be00d..15ca831f80 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java @@ -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); diff --git a/wpimath/src/test/java/edu/wpi/first/math/path/TravelingSalesmanTest.java b/wpimath/src/test/java/edu/wpi/first/math/path/TravelingSalesmanTest.java index cf4317721d..d44ea8b87b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/path/TravelingSalesmanTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/path/TravelingSalesmanTest.java @@ -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(); diff --git a/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java b/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java index ecff333919..4552c20a2b 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java @@ -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 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 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 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))); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java b/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java index 6435dd5ed4..9ae887e4f4 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java @@ -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))); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java index 58ee8305b1..1f9d59b795 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java @@ -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))); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java index 039b3f7983..96f83b3ece 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java @@ -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))); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java index 390a6c2841..3c8da41bcc 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java @@ -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))); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryConcatenateTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryConcatenateTest.java index 2e80d7b2ef..6635dfb86a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryConcatenateTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryConcatenateTest.java @@ -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)); diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java index 4b0f3fc158..9cc23a07ec 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java @@ -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) { diff --git a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryTransformTest.java b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryTransformTest.java index 6268768d84..7f1a9d6622 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryTransformTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryTransformTest.java @@ -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()); }