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

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

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

2
apriltag/src/main/java/edu/wpi/first/apriltag/AprilTagFieldLayout.java
View File

@@ -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(

2
apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagDetectorTest.java
View File

@@ -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 {

4
apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagPoseSetOriginTest.java
View File

@@ -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));
}
}

4
apriltag/src/test/java/edu/wpi/first/apriltag/AprilTagSerializationTest.java
View File

@@ -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));

2
apriltag/src/test/java/edu/wpi/first/apriltag/LoadConfigTest.java
View File

@@ -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<Pose3d> maybePose = layout.getTagPose(1);
assertTrue(maybePose.isPresent());
assertEquals(expectedPose, maybePose.get());

9
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
View File

@@ -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<Pose2d>();
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);

23
wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java
View File

@@ -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<Pose2d>();
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);

6
wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
View File

@@ -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);
}
/**

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java
View File

@@ -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");
}

2
wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java
View File

@@ -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);
}

30
wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java
View File

@@ -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);

2
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java
View File

@@ -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);

4
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
View File

@@ -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)));

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
View File

@@ -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)));

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
View File

@@ -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 =

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
View File

@@ -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)));

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java
View File

@@ -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));
}

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
View File

@@ -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 =

2
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
View File

@@ -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)));

2
wpimath/src/main/java/edu/wpi/first/math/ComputerVisionUtil.java
View File

@@ -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(

6
wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
View File

@@ -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;

4
wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java
View File

@@ -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;
/**

11
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
View File

@@ -26,13 +26,20 @@ import java.util.Objects;
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Pose2d implements Interpolatable<Pose2d>, ProtobufSerializable, StructSerializable {
/**
* A preallocated Pose2d representing the origin.
*
* <p>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;
}
/**

11
wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
View File

@@ -20,13 +20,20 @@ import java.util.Objects;
@JsonIgnoreProperties(ignoreUnknown = true)
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Pose3d implements Interpolatable<Pose3d>, ProtobufSerializable, StructSerializable {
/**
* A preallocated Pose3d representing the origin.
*
* <p>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;
}
/**

49
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
View File

@@ -32,6 +32,55 @@ import java.util.Objects;
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Rotation2d
implements Interpolatable<Rotation2d>, ProtobufSerializable, StructSerializable {
/**
* A preallocated Rotation2d representing no rotation.
*
* <p>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°).
*
* <p>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).
*
* <p>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°).
*
* <p>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).
*
* <p>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°).
*
* <p>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).
*
* <p>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;

7
wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
View File

@@ -28,6 +28,13 @@ import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Rotation3d
implements Interpolatable<Rotation3d>, ProtobufSerializable, StructSerializable {
/**
* A preallocated Rotation3d representing no rotation.
*
* <p>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. */

13
wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
View File

@@ -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.
*
* <p>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));
}
/**

13
wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
View File

@@ -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.
*
* <p>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));
}
/**

7
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
View File

@@ -36,6 +36,13 @@ import java.util.Objects;
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Translation2d
implements Interpolatable<Translation2d>, ProtobufSerializable, StructSerializable {
/**
* A preallocated Translation2d representing the origin.
*
* <p>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;

7
wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
View File

@@ -34,6 +34,13 @@ import java.util.Objects;
@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
public class Translation3d
implements Interpolatable<Translation3d>, ProtobufSerializable, StructSerializable {
/**
* A preallocated Translation3d representing the origin.
*
* <p>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;

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
View File

@@ -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);

4
wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
View File

@@ -70,7 +70,7 @@ public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPo
*/
public DifferentialDriveOdometry(
Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
this(gyroAngle, leftDistanceMeters, rightDistanceMeters, new Pose2d());
this(gyroAngle, leftDistanceMeters, rightDistanceMeters, Pose2d.kZero);
}
/**
@@ -82,7 +82,7 @@ public class DifferentialDriveOdometry extends Odometry<DifferentialDriveWheelPo
*/
public DifferentialDriveOdometry(
Rotation2d gyroAngle, Measure<Distance> leftDistance, Measure<Distance> rightDistance) {
this(gyroAngle, leftDistance, rightDistance, new Pose2d());
this(gyroAngle, leftDistance, rightDistance, Pose2d.kZero);
}
/**

4
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
View File

@@ -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);
}
/**

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
View File

@@ -45,6 +45,6 @@ public class MecanumDriveOdometry extends Odometry<MecanumDriveWheelPositions> {
MecanumDriveKinematics kinematics,
Rotation2d gyroAngle,
MecanumDriveWheelPositions wheelPositions) {
this(kinematics, gyroAngle, wheelPositions, new Pose2d());
this(kinematics, gyroAngle, wheelPositions, Pose2d.kZero);
}
}

6
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
View File

@@ -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

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
View File

@@ -51,7 +51,7 @@ public class SwerveDriveOdometry extends Odometry<SwerveDriveWheelPositions> {
SwerveDriveKinematics kinematics,
Rotation2d gyroAngle,
SwerveModulePosition[] modulePositions) {
this(kinematics, gyroAngle, modulePositions, new Pose2d());
this(kinematics, gyroAngle, modulePositions, Pose2d.kZero);
}
/**

2
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java
View File

@@ -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();

5
wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
View File

@@ -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);
}

2
wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java
View File

@@ -27,6 +27,6 @@ public class PoseWithCurvature {
/** Constructs a PoseWithCurvature with default values. */
public PoseWithCurvature() {
poseMeters = new Pose2d();
poseMeters = Pose2d.kZero;
}
}

2
wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
View File

@@ -295,7 +295,7 @@ public class Trajectory implements ProtobufSerializable {
/** Default constructor. */
public State() {
poseMeters = new Pose2d();
poseMeters = Pose2d.kZero;
}
/**

15
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
View File

@@ -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<String, StackTraceElement[]> errorFunc;
@@ -62,8 +64,6 @@ public final class TrajectoryGenerator {
List<Translation2d> 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<Spline.ControlVector>(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<Pose2d> waypoints, TrajectoryConfig config) {
final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
List<Pose2d> 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;
}
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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