diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java index 8434e3eedff..470a05aa373 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java @@ -10,11 +10,12 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.kinematics.DifferentialDriveOdometry3d; import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions; import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.numbers.N4; /** @@ -23,7 +24,11 @@ * intended to be a drop-in replacement for {@link DifferentialDriveOdometry3d}; in fact, if you * never call {@link DifferentialDrivePoseEstimator3d#addVisionMeasurement} and only call {@link * DifferentialDrivePoseEstimator3d#update} then this will behave exactly the same as - * DifferentialDriveOdometry3d. + * DifferentialDriveOdometry3d. It is also intended to be an easy replacement for {@link + * DifferentialDrivePoseEstimator}, only requiring the addition of a standard deviation for Z and + * appropriate conversions between 2D and 3D versions of geometry classes. (See {@link + * Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link + * Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.) * *

{@link DifferentialDrivePoseEstimator3d#update} should be called every robot loop. * @@ -32,67 +37,6 @@ */ public class DifferentialDrivePoseEstimator3d extends PoseEstimator3d { - /** - * Constructs a DifferentialDrivePoseEstimator3d with default standard deviations for the model - * and vision measurements. - * - *

The default standard deviations of the model states are 0.02 meters for x, 0.02 meters for - * y, and 0.01 radians for heading. The default standard deviations of the vision measurements are - * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading. - * - * @param kinematics A correctly-configured kinematics object for your drivetrain. - * @param gyroAngle The current gyro angle. - * @param leftDistanceMeters The distance traveled by the left encoder. - * @param rightDistanceMeters The distance traveled by the right encoder. - * @param initialPoseMeters The starting pose estimate. - */ - public DifferentialDrivePoseEstimator3d( - DifferentialDriveKinematics kinematics, - Rotation2d gyroAngle, - double leftDistanceMeters, - double rightDistanceMeters, - Pose2d initialPoseMeters) { - this( - kinematics, - gyroAngle, - leftDistanceMeters, - rightDistanceMeters, - initialPoseMeters, - VecBuilder.fill(0.02, 0.02, 0.01), - VecBuilder.fill(0.1, 0.1, 0.1)); - } - - /** - * Constructs a DifferentialDrivePoseEstimator3d. - * - * @param kinematics A correctly-configured kinematics object for your drivetrain. - * @param gyroAngle The gyro angle of the robot. - * @param leftDistanceMeters The distance traveled by the left encoder. - * @param rightDistanceMeters The distance traveled by the right encoder. - * @param initialPoseMeters The estimated initial pose. - * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position - * in meters, and heading in radians). Increase these numbers to trust your state estimate - * less. - * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position - * in meters, y position in meters, and heading in radians). Increase these numbers to trust - * the vision pose measurement less. - */ - public DifferentialDrivePoseEstimator3d( - DifferentialDriveKinematics kinematics, - Rotation2d gyroAngle, - double leftDistanceMeters, - double rightDistanceMeters, - Pose2d initialPoseMeters, - Matrix stateStdDevs, - Matrix visionMeasurementStdDevs) { - super( - kinematics, - new DifferentialDriveOdometry3d( - gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters), - stateStdDevs, - visionMeasurementStdDevs); - } - /** * Constructs a DifferentialDrivePoseEstimator3d with default standard deviations for the model * and vision measurements. @@ -152,30 +96,7 @@ public DifferentialDrivePoseEstimator3d( new DifferentialDriveOdometry3d( gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters), stateStdDevs, - visionMeasurementStdDevs, - N3.instance); - } - - /** - * Resets the robot's position on the field. - * - *

The gyroscope angle does not need to be reset here on the user's robot code. The library - * automatically takes care of offsetting the gyro angle. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param leftPositionMeters The distance traveled by the left encoder. - * @param rightPositionMeters The distance traveled by the right encoder. - * @param poseMeters The position on the field that your robot is at. - */ - public void resetPosition( - Rotation2d gyroAngle, - double leftPositionMeters, - double rightPositionMeters, - Pose2d poseMeters) { - resetPosition( - gyroAngle, - new DifferentialDriveWheelPositions(leftPositionMeters, rightPositionMeters), - poseMeters); + visionMeasurementStdDevs); } /** @@ -200,21 +121,6 @@ public void resetPosition( poseMeters); } - /** - * Updates the pose estimator with wheel encoder and gyro information. This should be called every - * loop. - * - * @param gyroAngle The current gyro angle. - * @param distanceLeftMeters The total distance travelled by the left wheel in meters. - * @param distanceRightMeters The total distance travelled by the right wheel in meters. - * @return The estimated pose of the robot in meters. - */ - public Pose2d update( - Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters) { - return update( - gyroAngle, new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters)); - } - /** * Updates the pose estimator with wheel encoder and gyro information. This should be called every * loop. @@ -230,27 +136,6 @@ public Pose3d update( gyroAngle, new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters)); } - /** - * Updates the pose estimator with wheel encoder and gyro information. This should be called every - * loop. - * - * @param currentTimeSeconds Time at which this method was called, in seconds. - * @param gyroAngle The current gyro angle. - * @param distanceLeftMeters The total distance travelled by the left wheel in meters. - * @param distanceRightMeters The total distance travelled by the right wheel in meters. - * @return The estimated pose of the robot in meters. - */ - public Pose2d updateWithTime( - double currentTimeSeconds, - Rotation2d gyroAngle, - double distanceLeftMeters, - double distanceRightMeters) { - return updateWithTime( - currentTimeSeconds, - gyroAngle, - new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters)); - } - /** * Updates the pose estimator with wheel encoder and gyro information. This should be called every * loop. diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java index b165246c5ef..e2f7acfdb0c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java @@ -10,18 +10,23 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.MecanumDriveKinematics; import edu.wpi.first.math.kinematics.MecanumDriveOdometry3d; import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.numbers.N4; /** * This class wraps {@link MecanumDriveOdometry3d Mecanum Drive Odometry} to fuse * latency-compensated vision measurements with mecanum drive encoder distance measurements. It will * correct for noisy measurements and encoder drift. It is intended to be a drop-in replacement for - * {@link MecanumDriveOdometry3d}. + * {@link MecanumDriveOdometry3d}. It is also intended to be an easy replacement for {@link + * MecanumDrivePoseEstimator}, only requiring the addition of a standard deviation for Z and + * appropriate conversions between 2D and 3D versions of geometry classes. (See {@link + * Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link + * Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.) * *

{@link MecanumDrivePoseEstimator3d#update} should be called every robot loop. * @@ -29,61 +34,6 @@ * want; if you never call it, then this class will behave mostly like regular encoder odometry. */ public class MecanumDrivePoseEstimator3d extends PoseEstimator3d { - /** - * Constructs a MecanumDrivePoseEstimator3d with default standard deviations for the model and - * vision measurements. - * - *

The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y, - * and 0.1 radians for heading. The default standard deviations of the vision measurements are - * 0.45 meters for x, 0.45 meters for y, and 0.45 radians for heading. - * - * @param kinematics A correctly-configured kinematics object for your drivetrain. - * @param gyroAngle The current gyro angle. - * @param wheelPositions The distances driven by each wheel. - * @param initialPoseMeters The starting pose estimate. - */ - public MecanumDrivePoseEstimator3d( - MecanumDriveKinematics kinematics, - Rotation2d gyroAngle, - MecanumDriveWheelPositions wheelPositions, - Pose2d initialPoseMeters) { - this( - kinematics, - gyroAngle, - wheelPositions, - initialPoseMeters, - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.45, 0.45, 0.45)); - } - - /** - * Constructs a MecanumDrivePoseEstimator3d. - * - * @param kinematics A correctly-configured kinematics object for your drivetrain. - * @param gyroAngle The current gyro angle. - * @param wheelPositions The distance measured by each wheel. - * @param initialPoseMeters The starting pose estimate. - * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position - * in meters, and heading in radians). Increase these numbers to trust your state estimate - * less. - * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position - * in meters, y position in meters, and heading in radians). Increase these numbers to trust - * the vision pose measurement less. - */ - public MecanumDrivePoseEstimator3d( - MecanumDriveKinematics kinematics, - Rotation2d gyroAngle, - MecanumDriveWheelPositions wheelPositions, - Pose2d initialPoseMeters, - Matrix stateStdDevs, - Matrix visionMeasurementStdDevs) { - super( - kinematics, - new MecanumDriveOdometry3d(kinematics, gyroAngle, wheelPositions, initialPoseMeters), - stateStdDevs, - visionMeasurementStdDevs); - } - /** * Constructs a MecanumDrivePoseEstimator3d with default standard deviations for the model and * vision measurements. @@ -137,7 +87,6 @@ public MecanumDrivePoseEstimator3d( kinematics, new MecanumDriveOdometry3d(kinematics, gyroAngle, wheelPositions, initialPoseMeters), stateStdDevs, - visionMeasurementStdDevs, - N3.instance); + visionMeasurementStdDevs); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java index cb62cfbf3b4..9ce96f0243b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java @@ -20,7 +20,6 @@ import edu.wpi.first.math.kinematics.Kinematics; import edu.wpi.first.math.kinematics.Odometry3d; import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.numbers.N4; import edu.wpi.first.math.numbers.N6; import java.util.NavigableMap; @@ -33,7 +32,11 @@ * drivetrain (e.g., {@link DifferentialDrivePoseEstimator3d}). It is intended to be a drop-in * replacement for {@link Odometry3d}; in fact, if you never call {@link * PoseEstimator3d#addVisionMeasurement} and only call {@link PoseEstimator3d#update} then this will - * behave exactly the same as Odometry3d. + * behave exactly the same as Odometry3d. It is also intended to be an easy replacement for {@link + * PoseEstimator}, only requiring the addition of a standard deviation for Z and appropriate + * conversions between 2D and 3D versions of geometry classes. (See {@link Pose3d#Pose3d(Pose2d)}, + * {@link Rotation3d#Rotation3d(Rotation2d)}, {@link Translation3d#Translation3d(Translation2d)}, + * and {@link Pose3d#toPose2d()}.) * *

{@link PoseEstimator3d#update} should be called every robot loop. * @@ -58,34 +61,6 @@ public class PoseEstimator3d { private Pose3d m_poseEstimate; - /** - * Constructs a PoseEstimator3d. - * - * @param kinematics A correctly-configured kinematics object for your drivetrain. - * @param odometry A correctly-configured odometry object for your drivetrain. - * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position - * in meters, and angle in radians). Increase these numbers to trust your state estimate less. - * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position - * in meters, y position in meters, and heading in radians). Increase these numbers to trust - * the vision pose measurement less. - */ - public PoseEstimator3d( - Kinematics kinematics, - Odometry3d odometry, - Matrix stateStdDevs, - Matrix visionMeasurementStdDevs) { - this( - kinematics, - odometry, - VecBuilder.fill(stateStdDevs.get(0, 0), stateStdDevs.get(1, 0), 0, stateStdDevs.get(2, 0)), - VecBuilder.fill( - visionMeasurementStdDevs.get(0, 0), - visionMeasurementStdDevs.get(1, 0), - 0, - visionMeasurementStdDevs.get(2, 0)), - N3.instance); - } - /** * Constructs a PoseEstimator3d. * @@ -97,42 +72,21 @@ public PoseEstimator3d( * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position * in meters, y position in meters, z position in meters, and angle in radians). Increase * these numbers to trust the vision pose measurement less. - * @param dimension Marker parameter to differentiate the 2D and 3D cases. Necessary because type - * erasure would otherwise make this look identical to the 2D version. */ @SuppressWarnings("PMD.UnusedFormalParameter") public PoseEstimator3d( Kinematics kinematics, Odometry3d odometry, Matrix stateStdDevs, - Matrix visionMeasurementStdDevs, - N3 dimension) { + Matrix visionMeasurementStdDevs) { m_odometry = odometry; - m_poseEstimate = m_odometry.getPose3dMeters(); + m_poseEstimate = m_odometry.getPoseMeters(); for (int i = 0; i < 4; ++i) { m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); } - setVisionMeasurementStdDevs3d(visionMeasurementStdDevs); - } - - /** - * Sets the pose estimator's trust of global measurements. This might be used to change trust in - * vision measurements after the autonomous period, or to change trust as distance to a vision - * target increases. - * - * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these - * numbers to trust global measurements from vision less. This matrix is in the form [x, y, - * theta]ᵀ, with units in meters and radians. - */ - public final void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) { - setVisionMeasurementStdDevs3d( - VecBuilder.fill( - visionMeasurementStdDevs.get(0, 0), - visionMeasurementStdDevs.get(1, 0), - 0, - visionMeasurementStdDevs.get(2, 0))); + setVisionMeasurementStdDevs(visionMeasurementStdDevs); } /** @@ -144,7 +98,7 @@ public final void setVisionMeasurementStdDevs(Matrix visionMeasurementSt * numbers to trust global measurements from vision less. This matrix is in the form [x, y, z, * theta]ᵀ, with units in meters and radians. */ - public final void setVisionMeasurementStdDevs3d(Matrix visionMeasurementStdDevs) { + public final void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) { var r = new double[4]; for (int i = 0; i < 4; ++i) { r[i] = visionMeasurementStdDevs.get(i, 0) * visionMeasurementStdDevs.get(i, 0); @@ -166,21 +120,6 @@ public final void setVisionMeasurementStdDevs3d(Matrix visionMeasurement m_visionK.set(5, 5, angle_gain); } - /** - * Resets the robot's position on the field. - * - *

The gyroscope angle does not need to be reset here on the user's robot code. The library - * automatically takes care of offsetting the gyro angle. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param wheelPositions The current encoder readings. - * @param poseMeters The position on the field that your robot is at. - */ - public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) { - resetPosition( - new Rotation3d(0, 0, gyroAngle.getRadians()), wheelPositions, new Pose3d(poseMeters)); - } - /** * Resets the robot's position on the field. * @@ -196,16 +135,7 @@ public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMet m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPose3dMeters(); - } - - /** - * Resets the robot's pose. - * - * @param pose The pose to reset to. - */ - public void resetPose(Pose2d pose) { - resetPose(new Pose3d(pose)); + m_poseEstimate = m_odometry.getPoseMeters(); } /** @@ -217,16 +147,7 @@ public void resetPose(Pose3d pose) { m_odometry.resetPose(pose); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPose3dMeters(); - } - - /** - * Resets the robot's translation. - * - * @param translation The pose to translation to. - */ - public void resetTranslation(Translation2d translation) { - resetTranslation(new Translation3d(translation.getX(), translation.getY(), 0)); + m_poseEstimate = m_odometry.getPoseMeters(); } /** @@ -238,16 +159,7 @@ public void resetTranslation(Translation3d translation) { m_odometry.resetTranslation(translation); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPose3dMeters(); - } - - /** - * Resets the robot's rotation. - * - * @param rotation The rotation to reset to. - */ - public void resetRotation(Rotation2d rotation) { - resetRotation(new Rotation3d(0, 0, rotation.getRadians())); + m_poseEstimate = m_odometry.getPoseMeters(); } /** @@ -259,16 +171,7 @@ public void resetRotation(Rotation3d rotation) { m_odometry.resetRotation(rotation); m_odometryPoseBuffer.clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.getPose3dMeters(); - } - - /** - * Gets the estimated robot pose. - * - * @return The estimated robot pose in meters. - */ - public Pose2d getEstimatedPosition() { - return m_poseEstimate.toPose2d(); + m_poseEstimate = m_odometry.getPoseMeters(); } /** @@ -276,7 +179,7 @@ public Pose2d getEstimatedPosition() { * * @return The estimated robot pose in meters. */ - public Pose3d getEstimatedPosition3d() { + public Pose3d getEstimatedPosition() { return m_poseEstimate; } @@ -286,17 +189,7 @@ public Pose3d getEstimatedPosition3d() { * @param timestampSeconds The pose's timestamp in seconds. * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty). */ - public Optional sampleAt(double timestampSeconds) { - return sampleAt3d(timestampSeconds).map(pose3d -> pose3d.toPose2d()); - } - - /** - * Return the pose at a given timestamp, if the buffer is not empty. - * - * @param timestampSeconds The pose's timestamp in seconds. - * @return The pose at the given timestamp (or Optional.empty() if the buffer is empty). - */ - public Optional sampleAt3d(double timestampSeconds) { + public Optional sampleAt(double timestampSeconds) { // Step 0: If there are no odometry updates to sample, skip. if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) { return Optional.empty(); @@ -361,65 +254,7 @@ private void cleanUpVisionUpdates() { * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you * don't use your own time source by calling {@link - * PoseEstimator3d#updateWithTime(double,Rotation2d,Object)} then you must use a timestamp - * with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as - * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you should use - * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or sync the - * epochs. - */ - public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { - addVisionMeasurement(new Pose3d(visionRobotPoseMeters), timestampSeconds); - } - - /** - * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate - * while still accounting for measurement noise. - * - *

This method can be called as infrequently as you want, as long as you are calling {@link - * PoseEstimator3d#update} every loop. - * - *

To promote stability of the pose estimate and make it robust to bad vision data, we - * recommend only adding vision measurements that are already within one meter or so of the - * current pose estimate. - * - *

Note that the vision measurement standard deviations passed into this method will continue - * to apply to future measurements until a subsequent call to {@link - * PoseEstimator3d#setVisionMeasurementStdDevs(Matrix)} or this method. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you - * don't use your own time source by calling {@link #updateWithTime}, then you must use a - * timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same - * epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you - * should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in - * this case. - * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position - * in meters, y position in meters, and heading in radians). Increase these numbers to trust - * the vision pose measurement less. - */ - public void addVisionMeasurement( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs) { - setVisionMeasurementStdDevs(visionMeasurementStdDevs); - addVisionMeasurement(visionRobotPoseMeters, timestampSeconds); - } - - /** - * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate - * while still accounting for measurement noise. - * - *

This method can be called as infrequently as you want, as long as you are calling {@link - * PoseEstimator3d#update} every loop. - * - *

To promote stability of the pose estimate and make it robust to bad vision data, we - * recommend only adding vision measurements that are already within one meter or so of the - * current pose estimate. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you - * don't use your own time source by calling {@link - * PoseEstimator3d#updateWithTime(double,Rotation2d,Object)} then you must use a timestamp + * PoseEstimator3d#updateWithTime(double,Rotation3d,Object)} then you must use a timestamp * with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same epoch as * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you should use * {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or sync the @@ -445,7 +280,7 @@ public void addVisionMeasurement(Pose3d visionRobotPoseMeters, double timestampS // Step 3: Get the vision-compensated pose estimate at the moment the vision measurement was // made. - var visionSample = sampleAt3d(timestampSeconds); + var visionSample = sampleAt(timestampSeconds); if (visionSample.isEmpty()) { return; @@ -479,7 +314,7 @@ public void addVisionMeasurement(Pose3d visionRobotPoseMeters, double timestampS // Step 9: Update latest pose estimate. Since we cleared all updates after this vision update, // it's guaranteed to be the latest vision update. - m_poseEstimate = visionUpdate.compensate(m_odometry.getPose3dMeters()); + m_poseEstimate = visionUpdate.compensate(m_odometry.getPoseMeters()); } /** @@ -508,26 +343,14 @@ public void addVisionMeasurement(Pose3d visionRobotPoseMeters, double timestampS * in meters, y position in meters, z position in meters, and angle in radians). Increase * these numbers to trust the vision pose measurement less. */ - public void addVisionMeasurement3d( + public void addVisionMeasurement( Pose3d visionRobotPoseMeters, double timestampSeconds, Matrix visionMeasurementStdDevs) { - setVisionMeasurementStdDevs3d(visionMeasurementStdDevs); + setVisionMeasurementStdDevs(visionMeasurementStdDevs); addVisionMeasurement(visionRobotPoseMeters, timestampSeconds); } - /** - * Updates the pose estimator with wheel encoder and gyro information. This should be called every - * loop. - * - * @param gyroAngle The current gyro angle. - * @param wheelPositions The current encoder readings. - * @return The estimated pose of the robot in meters. - */ - public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { - return update(new Rotation3d(0, 0, gyroAngle.getRadians()), wheelPositions).toPose2d(); - } - /** * Updates the pose estimator with wheel encoder and gyro information. This should be called every * loop. @@ -540,21 +363,6 @@ public Pose3d update(Rotation3d gyroAngle, T wheelPositions) { return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, wheelPositions); } - /** - * Updates the pose estimator with wheel encoder and gyro information. This should be called every - * loop. - * - * @param currentTimeSeconds Time at which this method was called, in seconds. - * @param gyroAngle The current gyro angle. - * @param wheelPositions The current encoder readings. - * @return The estimated pose of the robot in meters. - */ - public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, T wheelPositions) { - return updateWithTime( - currentTimeSeconds, new Rotation3d(0, 0, gyroAngle.getRadians()), wheelPositions) - .toPose2d(); - } - /** * Updates the pose estimator with wheel encoder and gyro information. This should be called every * loop. @@ -576,7 +384,7 @@ public Pose3d updateWithTime(double currentTimeSeconds, Rotation3d gyroAngle, T m_poseEstimate = visionUpdate.compensate(odometryEstimate); } - return getEstimatedPosition3d(); + return getEstimatedPosition(); } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java index 2b26b6c9907..668f45c410e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java @@ -10,17 +10,22 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry3d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.numbers.N4; /** * This class wraps {@link SwerveDriveOdometry3d Swerve Drive Odometry} to fuse latency-compensated * vision measurements with swerve drive encoder distance measurements. It is intended to be a - * drop-in replacement for {@link SwerveDriveOdometry3d}. + * drop-in replacement for {@link SwerveDriveOdometry3d}. It is also intended to be an easy + * replacement for {@link SwerveDrivePoseEstimator}, only requiring the addition of a standard + * deviation for Z and appropriate conversions between 2D and 3D versions of geometry classes. (See + * {@link Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link + * Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.) * *

{@link SwerveDrivePoseEstimator3d#update} should be called every robot loop. * @@ -30,63 +35,6 @@ public class SwerveDrivePoseEstimator3d extends PoseEstimator3d { private final int m_numModules; - /** - * Constructs a SwerveDrivePoseEstimator3d with default standard deviations for the model and - * vision measurements. - * - *

The default standard deviations of the model states are 0.1 meters for x, 0.1 meters for y, - * and 0.1 radians for heading. The default standard deviations of the vision measurements are 0.9 - * meters for x, 0.9 meters for y, and 0.9 radians for heading. - * - * @param kinematics A correctly-configured kinematics object for your drivetrain. - * @param gyroAngle The current gyro angle. - * @param modulePositions The current distance measurements and rotations of the swerve modules. - * @param initialPoseMeters The starting pose estimate. - */ - public SwerveDrivePoseEstimator3d( - SwerveDriveKinematics kinematics, - Rotation2d gyroAngle, - SwerveModulePosition[] modulePositions, - Pose2d initialPoseMeters) { - this( - kinematics, - gyroAngle, - modulePositions, - initialPoseMeters, - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.9, 0.9, 0.9)); - } - - /** - * Constructs a SwerveDrivePoseEstimator3d. - * - * @param kinematics A correctly-configured kinematics object for your drivetrain. - * @param gyroAngle The current gyro angle. - * @param modulePositions The current distance and rotation measurements of the swerve modules. - * @param initialPoseMeters The starting pose estimate. - * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position - * in meters, and heading in radians). Increase these numbers to trust your state estimate - * less. - * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position - * in meters, y position in meters, and heading in radians). Increase these numbers to trust - * the vision pose measurement less. - */ - public SwerveDrivePoseEstimator3d( - SwerveDriveKinematics kinematics, - Rotation2d gyroAngle, - SwerveModulePosition[] modulePositions, - Pose2d initialPoseMeters, - Matrix stateStdDevs, - Matrix visionMeasurementStdDevs) { - super( - kinematics, - new SwerveDriveOdometry3d(kinematics, gyroAngle, modulePositions, initialPoseMeters), - stateStdDevs, - visionMeasurementStdDevs); - - m_numModules = modulePositions.length; - } - /** * Constructs a SwerveDrivePoseEstimator3d with default standard deviations for the model and * vision measurements. @@ -140,24 +88,11 @@ public SwerveDrivePoseEstimator3d( kinematics, new SwerveDriveOdometry3d(kinematics, gyroAngle, modulePositions, initialPoseMeters), stateStdDevs, - visionMeasurementStdDevs, - N3.instance); + visionMeasurementStdDevs); m_numModules = modulePositions.length; } - @Override - public Pose2d updateWithTime( - double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] wheelPositions) { - if (wheelPositions.length != m_numModules) { - throw new IllegalArgumentException( - "Number of modules is not consistent with number of wheel locations provided in " - + "constructor"); - } - - return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions); - } - @Override public Pose3d updateWithTime( double currentTimeSeconds, Rotation3d gyroAngle, SwerveModulePosition[] wheelPositions) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java index 78220aeb96b..7a5769a5777 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java @@ -12,12 +12,19 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.measure.Distance; /** * Class for differential drive odometry. Odometry allows you to track the robot's position on the * field over the course of a match using readings from 2 encoders and a gyroscope. * + *

This class is meant to be an easy replacement for {@link DifferentialDriveOdometry}, only + * requiring the addition of appropriate conversions between 2D and 3D versions of geometry classes. + * (See {@link Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link + * Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.) + * *

Teams can use odometry during the autonomous period for complex tasks like path following. * Furthermore, odometry can be used for latency compensation when using computer-vision systems. * @@ -25,67 +32,6 @@ * pose resets also require the encoders to be reset to zero. */ public class DifferentialDriveOdometry3d extends Odometry3d { - /** - * Constructs a DifferentialDriveOdometry3d object. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param leftDistanceMeters The distance traveled by the left encoder. - * @param rightDistanceMeters The distance traveled by the right encoder. - * @param initialPoseMeters The starting position of the robot on the field. - */ - public DifferentialDriveOdometry3d( - Rotation2d gyroAngle, - double leftDistanceMeters, - double rightDistanceMeters, - Pose2d initialPoseMeters) { - super( - new DifferentialDriveKinematics(1), - gyroAngle, - new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters), - initialPoseMeters); - MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1); - } - - /** - * Constructs a DifferentialDriveOdometry3d object. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param leftDistance The distance traveled by the left encoder. - * @param rightDistance The distance traveled by the right encoder. - * @param initialPoseMeters The starting position of the robot on the field. - */ - public DifferentialDriveOdometry3d( - Rotation2d gyroAngle, - Distance leftDistance, - Distance rightDistance, - Pose2d initialPoseMeters) { - this(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), initialPoseMeters); - } - - /** - * Constructs a DifferentialDriveOdometry3d object. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param leftDistanceMeters The distance traveled by the left encoder. - * @param rightDistanceMeters The distance traveled by the right encoder. - */ - public DifferentialDriveOdometry3d( - Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) { - this(gyroAngle, leftDistanceMeters, rightDistanceMeters, Pose2d.kZero); - } - - /** - * Constructs a DifferentialDriveOdometry3d object. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param leftDistance The distance traveled by the left encoder. - * @param rightDistance The distance traveled by the right encoder. - */ - public DifferentialDriveOdometry3d( - Rotation2d gyroAngle, Distance leftDistance, Distance rightDistance) { - this(gyroAngle, leftDistance, rightDistance, Pose2d.kZero); - } - /** * Constructs a DifferentialDriveOdometry3d object. * @@ -147,44 +93,6 @@ public DifferentialDriveOdometry3d( this(gyroAngle, leftDistance, rightDistance, Pose3d.kZero); } - /** - * Resets the robot's position on the field. - * - *

The gyroscope angle does not need to be reset here on the user's robot code. The library - * automatically takes care of offsetting the gyro angle. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param leftDistanceMeters The distance traveled by the left encoder. - * @param rightDistanceMeters The distance traveled by the right encoder. - * @param poseMeters The position on the field that your robot is at. - */ - public void resetPosition( - Rotation2d gyroAngle, - double leftDistanceMeters, - double rightDistanceMeters, - Pose2d poseMeters) { - super.resetPosition( - gyroAngle, - new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters), - poseMeters); - } - - /** - * Resets the robot's position on the field. - * - *

The gyroscope angle does not need to be reset here on the user's robot code. The library - * automatically takes care of offsetting the gyro angle. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param leftDistance The distance traveled by the left encoder. - * @param rightDistance The distance traveled by the right encoder. - * @param poseMeters The position on the field that your robot is at. - */ - public void resetPosition( - Rotation2d gyroAngle, Distance leftDistance, Distance rightDistance, Pose2d poseMeters) { - resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters); - } - /** * Resets the robot's position on the field. * @@ -223,22 +131,6 @@ public void resetPosition( resetPosition(gyroAngle, leftDistance.in(Meters), rightDistance.in(Meters), poseMeters); } - /** - * Updates the robot position on the field using distance measurements from encoders. This method - * is more numerically accurate than using velocities to integrate the pose and is also - * advantageous for teams that are using lower CPR encoders. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param leftDistanceMeters The distance traveled by the left encoder. - * @param rightDistanceMeters The distance traveled by the right encoder. - * @return The new pose of the robot. - */ - public Pose2d update( - Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) { - return super.update( - gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters)); - } - /** * Updates the robot position on the field using distance measurements from encoders. This method * is more numerically accurate than using velocities to integrate the pose and is also diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3d.java index 0c305d694cc..00d66c0ab1b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3d.java @@ -10,46 +10,22 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; /** * Class for mecanum drive odometry. Odometry allows you to track the robot's position on the field * over a course of a match using readings from your mecanum wheel encoders. * + *

This class is meant to be an easy replacement for {@link MecanumDriveOdometry}, only requiring + * the addition of appropriate conversions between 2D and 3D versions of geometry classes. (See + * {@link Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link + * Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.) + * *

Teams can use odometry during the autonomous period for complex tasks like path following. * Furthermore, odometry can be used for latency compensation when using computer-vision systems. */ public class MecanumDriveOdometry3d extends Odometry3d { - /** - * Constructs a MecanumDriveOdometry3d object. - * - * @param kinematics The mecanum drive kinematics for your drivetrain. - * @param gyroAngle The angle reported by the gyroscope. - * @param wheelPositions The distances driven by each wheel. - * @param initialPoseMeters The starting position of the robot on the field. - */ - public MecanumDriveOdometry3d( - MecanumDriveKinematics kinematics, - Rotation2d gyroAngle, - MecanumDriveWheelPositions wheelPositions, - Pose2d initialPoseMeters) { - super(kinematics, gyroAngle, wheelPositions, initialPoseMeters); - MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1); - } - - /** - * Constructs a MecanumDriveOdometry3d object with the default pose at the origin. - * - * @param kinematics The mecanum drive kinematics for your drivetrain. - * @param gyroAngle The angle reported by the gyroscope. - * @param wheelPositions The distances driven by each wheel. - */ - public MecanumDriveOdometry3d( - MecanumDriveKinematics kinematics, - Rotation2d gyroAngle, - MecanumDriveWheelPositions wheelPositions) { - this(kinematics, gyroAngle, wheelPositions, Pose2d.kZero); - } - /** * Constructs a MecanumDriveOdometry3d object. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java index d4284e520f1..c1ccd3c9a16 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java @@ -18,6 +18,11 @@ * robot's position on the field over the course of a match using readings from encoders and a * gyroscope. * + *

This class is meant to be an easy replacement for {@link Odometry}, only requiring the + * addition of appropriate conversions between 2D and 3D versions of geometry classes. (See {@link + * Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link + * Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.) + * *

Teams can use odometry during the autonomous period for complex tasks like path following. * Furthermore, odometry can be used for latency compensation when using computer-vision systems. * @@ -25,33 +30,12 @@ */ public class Odometry3d { private final Kinematics m_kinematics; - private Pose2d m_pose2dMeters; // Only for getPoseMeters() private Pose3d m_poseMeters; private Rotation3d m_gyroOffset; private Rotation3d m_previousAngle; private final T m_previousWheelPositions; - /** - * Constructs an Odometry3d object. - * - * @param kinematics The kinematics of the drivebase. - * @param gyroAngle The angle reported by the gyroscope. - * @param wheelPositions The current encoder readings. - * @param initialPoseMeters The starting position of the robot on the field. - */ - public Odometry3d( - Kinematics kinematics, - Rotation2d gyroAngle, - T wheelPositions, - Pose2d initialPoseMeters) { - this( - kinematics, - new Rotation3d(0, 0, gyroAngle.getRadians()), - wheelPositions, - new Pose3d(initialPoseMeters)); - } - /** * Constructs an Odometry3d object. * @@ -67,27 +51,11 @@ public Odometry3d( Pose3d initialPoseMeters) { m_kinematics = kinematics; m_poseMeters = initialPoseMeters; - m_pose2dMeters = m_poseMeters.toPose2d(); m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); m_previousAngle = m_poseMeters.getRotation(); m_previousWheelPositions = m_kinematics.copy(wheelPositions); } - /** - * Resets the robot's position on the field. - * - *

The gyroscope angle does not need to be reset here on the user's robot code. The library - * automatically takes care of offsetting the gyro angle. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param wheelPositions The current encoder readings. - * @param poseMeters The position on the field that your robot is at. - */ - public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) { - resetPosition( - new Rotation3d(0, 0, gyroAngle.getRadians()), wheelPositions, new Pose3d(poseMeters)); - } - /** * Resets the robot's position on the field. * @@ -100,21 +68,11 @@ public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMet */ public void resetPosition(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters) { m_poseMeters = poseMeters; - m_pose2dMeters = m_poseMeters.toPose2d(); m_previousAngle = m_poseMeters.getRotation(); m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); } - /** - * Resets the pose. - * - * @param poseMeters The pose to reset to. - */ - public void resetPose(Pose2d poseMeters) { - resetPose(new Pose3d(poseMeters)); - } - /** * Resets the pose. * @@ -123,19 +81,9 @@ public void resetPose(Pose2d poseMeters) { public void resetPose(Pose3d poseMeters) { m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation())); m_poseMeters = poseMeters; - m_pose2dMeters = m_poseMeters.toPose2d(); m_previousAngle = m_poseMeters.getRotation(); } - /** - * Resets the translation of the pose. - * - * @param translation The translation to reset to. - */ - public void resetTranslation(Translation2d translation) { - resetTranslation(new Translation3d(translation.getX(), translation.getY(), 0)); - } - /** * Resets the translation of the pose. * @@ -143,16 +91,6 @@ public void resetTranslation(Translation2d translation) { */ public void resetTranslation(Translation3d translation) { m_poseMeters = new Pose3d(translation, m_poseMeters.getRotation()); - m_pose2dMeters = m_poseMeters.toPose2d(); - } - - /** - * Resets the rotation of the pose. - * - * @param rotation The rotation to reset to. - */ - public void resetRotation(Rotation2d rotation) { - resetRotation(new Rotation3d(0, 0, rotation.getRadians())); } /** @@ -163,43 +101,18 @@ public void resetRotation(Rotation2d rotation) { public void resetRotation(Rotation3d rotation) { m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation())); m_poseMeters = new Pose3d(m_poseMeters.getTranslation(), rotation); - m_pose2dMeters = m_poseMeters.toPose2d(); m_previousAngle = m_poseMeters.getRotation(); } - /** - * Returns the position of the robot on the field. - * - * @return The pose of the robot (x and y are in meters). - */ - public Pose2d getPoseMeters() { - return m_pose2dMeters; - } - /** * Returns the position of the robot on the field. * * @return The pose of the robot (x, y, and z are in meters). */ - public Pose3d getPose3dMeters() { + public Pose3d getPoseMeters() { return m_poseMeters; } - /** - * Updates the robot's position on the field using forward kinematics and integration of the pose - * over time. This method takes in an angle parameter which is used instead of the angular rate - * that is calculated from forward kinematics, in addition to the current distance measurement at - * each wheel. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param wheelPositions The current encoder readings. - * @return The new pose of the robot. - */ - public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { - update(new Rotation3d(0, 0, gyroAngle.getRadians()), wheelPositions); - return getPoseMeters(); - } - /** * Updates the robot's position on the field using forward kinematics and integration of the pose * over time. This method takes in an angle parameter which is used instead of the angular rate @@ -229,7 +142,6 @@ public Pose3d update(Rotation3d gyroAngle, T wheelPositions) { m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); m_previousAngle = angle; m_poseMeters = new Pose3d(newPose.getTranslation(), angle); - m_pose2dMeters = m_poseMeters.toPose2d(); return m_poseMeters; } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3d.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3d.java index 878b198dd90..35fc6fbc3a8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3d.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3d.java @@ -10,52 +10,25 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; /** * Class for swerve drive odometry. Odometry allows you to track the robot's position on the field * over a course of a match using readings from your swerve drive encoders and swerve azimuth * encoders. * + *

This class is meant to be an easy replacement for {@link SwerveDriveOdometry}, only requiring + * the addition of appropriate conversions between 2D and 3D versions of geometry classes. (See + * {@link Pose3d#Pose3d(Pose2d)}, {@link Rotation3d#Rotation3d(Rotation2d)}, {@link + * Translation3d#Translation3d(Translation2d)}, and {@link Pose3d#toPose2d()}.) + * *

Teams can use odometry during the autonomous period for complex tasks like path following. * Furthermore, odometry can be used for latency compensation when using computer-vision systems. */ public class SwerveDriveOdometry3d extends Odometry3d { private final int m_numModules; - /** - * Constructs a SwerveDriveOdometry3d object. - * - * @param kinematics The swerve drive kinematics for your drivetrain. - * @param gyroAngle The angle reported by the gyroscope. - * @param modulePositions The wheel positions reported by each module. - * @param initialPose The starting position of the robot on the field. - */ - public SwerveDriveOdometry3d( - SwerveDriveKinematics kinematics, - Rotation2d gyroAngle, - SwerveModulePosition[] modulePositions, - Pose2d initialPose) { - super(kinematics, gyroAngle, modulePositions, initialPose); - - m_numModules = modulePositions.length; - - MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1); - } - - /** - * Constructs a SwerveDriveOdometry3d object with the default pose at the origin. - * - * @param kinematics The swerve drive kinematics for your drivetrain. - * @param gyroAngle The angle reported by the gyroscope. - * @param modulePositions The wheel positions reported by each module. - */ - public SwerveDriveOdometry3d( - SwerveDriveKinematics kinematics, - Rotation2d gyroAngle, - SwerveModulePosition[] modulePositions) { - this(kinematics, gyroAngle, modulePositions, Pose2d.kZero); - } - /** * Constructs a SwerveDriveOdometry3d object. * @@ -90,17 +63,6 @@ public SwerveDriveOdometry3d( this(kinematics, gyroAngle, modulePositions, Pose3d.kZero); } - @Override - public void resetPosition( - Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) { - if (modulePositions.length != m_numModules) { - throw new IllegalArgumentException( - "Number of modules is not consistent with number of wheel locations provided in " - + "constructor"); - } - super.resetPosition(gyroAngle, modulePositions, pose); - } - @Override public void resetPosition( Rotation3d gyroAngle, SwerveModulePosition[] modulePositions, Pose3d pose) { @@ -112,16 +74,6 @@ public void resetPosition( super.resetPosition(gyroAngle, modulePositions, pose); } - @Override - public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { - if (modulePositions.length != m_numModules) { - throw new IllegalArgumentException( - "Number of modules is not consistent with number of wheel locations provided in " - + "constructor"); - } - return super.update(gyroAngle, modulePositions); - } - @Override public Pose3d update(Rotation3d gyroAngle, SwerveModulePosition[] modulePositions) { if (modulePositions.length != m_numModules) { diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp index 2dbe0984ca3..baa53b3eb44 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp @@ -6,25 +6,6 @@ using namespace frc; -DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d( - DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle, - units::meter_t leftDistance, units::meter_t rightDistance, - const Pose2d& initialPose) - : DifferentialDrivePoseEstimator3d{ - kinematics, gyroAngle, leftDistance, rightDistance, - initialPose, {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}} {} - -DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d( - DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle, - units::meter_t leftDistance, units::meter_t rightDistance, - const Pose2d& initialPose, const wpi::array& stateStdDevs, - const wpi::array& visionMeasurementStdDevs) - : PoseEstimator3d(kinematics, m_odometryImpl, stateStdDevs, - visionMeasurementStdDevs), - m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} { - ResetPose(initialPose); -} - DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d( DifferentialDriveKinematics& kinematics, const Rotation3d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp index 5700f5f705b..644b3da8dfc 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp @@ -12,24 +12,6 @@ using namespace frc; -frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d( - MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle, - const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose) - : MecanumDrivePoseEstimator3d{kinematics, gyroAngle, - wheelPositions, initialPose, - {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}} {} - -frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d( - MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle, - const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose, - const wpi::array& stateStdDevs, - const wpi::array& visionMeasurementStdDevs) - : PoseEstimator3d(kinematics, m_odometryImpl, stateStdDevs, - visionMeasurementStdDevs), - m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) { - ResetPose(initialPose); -} - frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d( MecanumDriveKinematics& kinematics, const Rotation3d& gyroAngle, const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose) diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp index deea7b90a0f..93fcddbdf00 100644 --- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp +++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp @@ -8,15 +8,6 @@ using namespace frc; -DifferentialDriveOdometry3d::DifferentialDriveOdometry3d( - const Rotation2d& gyroAngle, units::meter_t leftDistance, - units::meter_t rightDistance, const Pose2d& initialPose) - : Odometry3d(m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance}, - initialPose) { - wpi::math::MathSharedStore::ReportUsage( - wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1); -} - DifferentialDriveOdometry3d::DifferentialDriveOdometry3d( const Rotation3d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose3d& initialPose) diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp index 0e86217d088..bd6954006fe 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp @@ -8,15 +8,6 @@ using namespace frc; -MecanumDriveOdometry3d::MecanumDriveOdometry3d( - MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle, - const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose) - : Odometry3d(m_kinematicsImpl, gyroAngle, wheelPositions, initialPose), - m_kinematicsImpl(kinematics) { - wpi::math::MathSharedStore::ReportUsage( - wpi::math::MathUsageId::kOdometry_MecanumDrive, 1); -} - MecanumDriveOdometry3d::MecanumDriveOdometry3d( MecanumDriveKinematics kinematics, const Rotation3d& gyroAngle, const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose) diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h index 7531de41158..371536b2980 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h @@ -21,7 +21,11 @@ namespace frc { * correct for noisy vision measurements and encoder drift. It is intended to be * an easy drop-in for DifferentialDriveOdometry3d. In fact, if you never call * AddVisionMeasurement(), and only call Update(), this will behave exactly the - * same as DifferentialDriveOdometry3d. + * same as DifferentialDriveOdometry3d. It is also intended to be an easy + * replacement for PoseEstimator, only requiring the addition of a standard + * deviation for Z and appropriate conversions between 2D and 3D versions of + * geometry classes. (See Pose3d(Pose2d), Rotation3d(Rotation2d), + * Translation3d(Translation2d), and Pose3d.ToPose2d().) * * Update() should be called every robot loop (if your robot loops are faster or * slower than the default of 20 ms, then you should change the nominal delta @@ -34,51 +38,6 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d : public PoseEstimator3d { public: - /** - * Constructs a DifferentialDrivePoseEstimator3d with default standard - * deviations for the model and vision measurements. - * - * The default standard deviations of the model states are - * 0.02 meters for x, 0.02 meters for y, and 0.01 radians for heading. - * The default standard deviations of the vision measurements are - * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading. - * - * @param kinematics A correctly-configured kinematics object for your - * drivetrain. - * @param gyroAngle The gyro angle of the robot. - * @param leftDistance The distance traveled by the left encoder. - * @param rightDistance The distance traveled by the right encoder. - * @param initialPose The estimated initial pose. - */ - DifferentialDrivePoseEstimator3d(DifferentialDriveKinematics& kinematics, - const Rotation2d& gyroAngle, - units::meter_t leftDistance, - units::meter_t rightDistance, - const Pose2d& initialPose); - - /** - * Constructs a DifferentialDrivePoseEstimator3d. - * - * @param kinematics A correctly-configured kinematics object for your - * drivetrain. - * @param gyroAngle The gyro angle of the robot. - * @param leftDistance The distance traveled by the left encoder. - * @param rightDistance The distance traveled by the right encoder. - * @param initialPose The estimated initial pose. - * @param stateStdDevs Standard deviations of the pose estimate (x position in - * meters, y position in meters, and heading in radians). Increase these - * numbers to trust your state estimate less. - * @param visionMeasurementStdDevs Standard deviations of the vision pose - * measurement (x position in meters, y position in meters, and heading in - * radians). Increase these numbers to trust the vision pose measurement - * less. - */ - DifferentialDrivePoseEstimator3d( - DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle, - units::meter_t leftDistance, units::meter_t rightDistance, - const Pose2d& initialPose, const wpi::array& stateStdDevs, - const wpi::array& visionMeasurementStdDevs); - /** * Constructs a DifferentialDrivePoseEstimator3d with default standard * deviations for the model and vision measurements. @@ -125,20 +84,6 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d const Pose3d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs); - /** - * Resets the robot's position on the field. - * - * @param gyroAngle The current gyro angle. - * @param leftDistance The distance traveled by the left encoder. - * @param rightDistance The distance traveled by the right encoder. - * @param pose The estimated pose of the robot on the field. - */ - void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance, - units::meter_t rightDistance, const Pose2d& pose) { - PoseEstimator3d::ResetPosition(gyroAngle, {leftDistance, rightDistance}, - pose); - } - /** * Resets the robot's position on the field. * @@ -153,21 +98,6 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d pose); } - /** - * Updates the pose estimator with wheel encoder and gyro information. This - * should be called every loop. - * - * @param gyroAngle The current gyro angle. - * @param leftDistance The distance traveled by the left encoder. - * @param rightDistance The distance traveled by the right encoder. - * - * @return The estimated pose of the robot. - */ - Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance, - units::meter_t rightDistance) { - return PoseEstimator3d::Update(gyroAngle, {leftDistance, rightDistance}); - } - /** * Updates the pose estimator with wheel encoder and gyro information. This * should be called every loop. @@ -183,25 +113,6 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator3d return PoseEstimator3d::Update(gyroAngle, {leftDistance, rightDistance}); } - /** - * Updates the pose estimator with wheel encoder and gyro information. This - * should be called every loop. - * - * @param currentTime The time at which this method was called. - * @param gyroAngle The current gyro angle. - * @param leftDistance The distance traveled by the left encoder. - * @param rightDistance The distance traveled by the right encoder. - * - * @return The estimated pose of the robot. - */ - Pose2d UpdateWithTime(units::second_t currentTime, - const Rotation2d& gyroAngle, - units::meter_t leftDistance, - units::meter_t rightDistance) { - return PoseEstimator3d::UpdateWithTime(currentTime, gyroAngle, - {leftDistance, rightDistance}); - } - /** * Updates the pose estimator with wheel encoder and gyro information. This * should be called every loop. diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator3d.h index 0b26507301f..bb1c6ea6936 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator3d.h @@ -22,7 +22,11 @@ namespace frc { * This class wraps Mecanum Drive Odometry to fuse latency-compensated * vision measurements with mecanum drive encoder velocity measurements. It will * correct for noisy measurements and encoder drift. It is intended to be an - * easy drop-in for MecanumDriveOdometry3d. + * easy drop-in for MecanumDriveOdometry3d. It is also intended to be an easy + * replacement for PoseEstimator, only requiring the addition of a standard + * deviation for Z and appropriate conversions between 2D and 3D versions of + * geometry classes. (See Pose3d(Pose2d), Rotation3d(Rotation2d), + * Translation3d(Translation2d), and Pose3d.ToPose2d().) * * Update() should be called every robot loop. If your loops are faster or * slower than the default of 20 ms, then you should change the nominal delta @@ -36,48 +40,6 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator3d : public PoseEstimator3d { public: - /** - * Constructs a MecanumDrivePoseEstimator3d with default standard deviations - * for the model and vision measurements. - * - * The default standard deviations of the model states are - * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading. - * The default standard deviations of the vision measurements are - * 0.45 meters for x, 0.45 meters for y, and 0.45 radians for heading. - * - * @param kinematics A correctly-configured kinematics object for your - * drivetrain. - * @param gyroAngle The current gyro angle. - * @param wheelPositions The distance measured by each wheel. - * @param initialPose The starting pose estimate. - */ - MecanumDrivePoseEstimator3d(MecanumDriveKinematics& kinematics, - const Rotation2d& gyroAngle, - const MecanumDriveWheelPositions& wheelPositions, - const Pose2d& initialPose); - - /** - * Constructs a MecanumDrivePoseEstimator3d. - * - * @param kinematics A correctly-configured kinematics object for your - * drivetrain. - * @param gyroAngle The current gyro angle. - * @param wheelPositions The distance measured by each wheel. - * @param initialPose The starting pose estimate. - * @param stateStdDevs Standard deviations of the pose estimate (x position in - * meters, y position in meters, and heading in radians). Increase these - * numbers to trust your state estimate less. - * @param visionMeasurementStdDevs Standard deviations of the vision pose - * measurement (x position in meters, y position in meters, and heading in - * radians). Increase these numbers to trust the vision pose measurement - * less. - */ - MecanumDrivePoseEstimator3d( - MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle, - const MecanumDriveWheelPositions& wheelPositions, - const Pose2d& initialPose, const wpi::array& stateStdDevs, - const wpi::array& visionMeasurementStdDevs); - /** * Constructs a MecanumDrivePoseEstimator3d with default standard deviations * for the model and vision measurements. diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h index 95d2e5bc407..bc8bdb08a66 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h @@ -31,7 +31,11 @@ namespace frc { * directly- Instead, use the particular type for your drivetrain (e.g., * DifferentialDrivePoseEstimator3d). It will correct for noisy vision * measurements and encoder drift. It is intended to be an easy drop-in for - * Odometry3d. + * Odometry3d. It is also intended to be an easy replacement for PoseEstimator, + * only requiring the addition of a standard deviation for Z and appropriate + * conversions between 2D and 3D versions of geometry classes. (See + * Pose3d(Pose2d), Rotation3d(Rotation2d), Translation3d(Translation2d), and + * Pose3d.ToPose2d().) * * Update() should be called every robot loop. * @@ -44,34 +48,6 @@ namespace frc { template class WPILIB_DLLEXPORT PoseEstimator3d { public: - /** - * Constructs a PoseEstimator3d. - * - * @warning The initial pose estimate will always be the default pose, - * regardless of the odometry's current pose. - * - * @param kinematics A correctly-configured kinematics object for your - * drivetrain. - * @param odometry A correctly-configured odometry object for your drivetrain. - * @param stateStdDevs Standard deviations of the pose estimate (x position in - * meters, y position in meters, and heading in radians). Increase these - * numbers to trust your state estimate less. - * @param visionMeasurementStdDevs Standard deviations of the vision pose - * measurement (x position in meters, y position in meters, and heading in - * radians). Increase these numbers to trust the vision pose measurement - * less. - */ - PoseEstimator3d(Kinematics& kinematics, - Odometry3d& odometry, - const wpi::array& stateStdDevs, - const wpi::array& visionMeasurementStdDevs) - : PoseEstimator3d{ - kinematics, - odometry, - {stateStdDevs[0], stateStdDevs[1], 0.0, stateStdDevs[2]}, - {visionMeasurementStdDevs[0], visionMeasurementStdDevs[1], 0.0, - visionMeasurementStdDevs[2]}} {} - /** * Constructs a PoseEstimator3d. * @@ -101,23 +77,6 @@ class WPILIB_DLLEXPORT PoseEstimator3d { SetVisionMeasurementStdDevs(visionMeasurementStdDevs); } - /** - * Sets the pose estimator's trust in vision measurements. This might be used - * to change trust in vision measurements after the autonomous period, or to - * change trust as distance to a vision target increases. - * - * @param visionMeasurementStdDevs Standard deviations of the vision pose - * measurement (x position in meters, y position in meters, and heading in - * radians). Increase these numbers to trust the vision pose measurement - * less. - */ - void SetVisionMeasurementStdDevs( - const wpi::array& visionMeasurementStdDevs) { - SetVisionMeasurementStdDevs({visionMeasurementStdDevs[0], - visionMeasurementStdDevs[1], 0.0, - visionMeasurementStdDevs[2]}); - } - /** * Sets the pose estimator's trust in vision measurements. This might be used * to change trust in vision measurements after the autonomous period, or to @@ -150,22 +109,6 @@ class WPILIB_DLLEXPORT PoseEstimator3d { m_visionK(5, 5) = angle_gain; } - /** - * Resets the robot's position on the field. - * - * The gyroscope angle does not need to be reset in the user's robot code. - * The library automatically takes care of offsetting the gyro angle. - * - * @param gyroAngle The current gyro angle. - * @param wheelPositions The distances traveled by the encoders. - * @param pose The estimated pose of the robot on the field. - */ - void ResetPosition(const Rotation2d& gyroAngle, - const WheelPositions& wheelPositions, const Pose2d& pose) { - ResetPosition(Rotation3d{0_rad, 0_rad, gyroAngle.Radians()}, wheelPositions, - Pose3d{pose}); - } - /** * Resets the robot's position on the field. * @@ -182,16 +125,9 @@ class WPILIB_DLLEXPORT PoseEstimator3d { m_odometry.ResetPosition(gyroAngle, wheelPositions, pose); m_odometryPoseBuffer.Clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.GetPose3d(); + m_poseEstimate = m_odometry.GetPose(); } - /** - * Resets the robot's pose. - * - * @param pose The pose to reset to. - */ - void ResetPose(const Pose2d& pose) { ResetPose(Pose3d{pose}); } - /** * Resets the robot's pose. * @@ -201,16 +137,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d { m_odometry.ResetPose(pose); m_odometryPoseBuffer.Clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.GetPose3d(); - } - - /** - * Resets the robot's translation. - * - * @param translation The pose to translation to. - */ - void ResetTranslation(const Translation2d& translation) { - ResetTranslation(Translation3d{translation.X(), translation.Y(), 0_m}); + m_poseEstimate = m_odometry.GetPose(); } /** @@ -222,16 +149,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d { m_odometry.ResetTranslation(translation); m_odometryPoseBuffer.Clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.GetPose3d(); - } - - /** - * Resets the robot's rotation. - * - * @param rotation The rotation to reset to. - */ - void ResetRotation(const Rotation2d& rotation) { - ResetRotation(Rotation3d{0_rad, 0_rad, rotation.Radians()}); + m_poseEstimate = m_odometry.GetPose(); } /** @@ -243,16 +161,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d { m_odometry.ResetRotation(rotation); m_odometryPoseBuffer.Clear(); m_visionUpdates.clear(); - m_poseEstimate = m_odometry.GetPose3d(); - } - - /** - * Gets the estimated robot pose. - * - * @return The estimated robot pose in meters. - */ - Pose2d GetEstimatedPosition() const { - return GetEstimatedPosition3d().ToPose2d(); + m_poseEstimate = m_odometry.GetPose(); } /** @@ -260,24 +169,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d { * * @return The estimated robot pose in meters. */ - Pose3d GetEstimatedPosition3d() const { return m_poseEstimate; } - - /** - * Return the pose at a given timestamp, if the buffer is not empty. - * - * @param timestamp The pose's timestamp. - * @return The pose at the given timestamp (or std::nullopt if the buffer is - * empty). - */ - std::optional SampleAt(units::second_t timestamp) const { - auto sample = SampleAt3d(timestamp); - // TODO Replace with std::optional::transform() in C++23 - if (sample) { - return sample->ToPose2d(); - } else { - return std::nullopt; - } - } + Pose3d GetEstimatedPosition() const { return m_poseEstimate; } /** * Return the pose at a given timestamp, if the buffer is not empty. @@ -286,7 +178,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d { * @return The pose at the given timestamp (or std::nullopt if the buffer is * empty). */ - std::optional SampleAt3d(units::second_t timestamp) const { + std::optional SampleAt(units::second_t timestamp) const { // Step 0: If there are no odometry updates to sample, skip. if (m_odometryPoseBuffer.GetInternalBuffer().empty()) { return std::nullopt; @@ -328,66 +220,6 @@ class WPILIB_DLLEXPORT PoseEstimator3d { return std::nullopt; } - /** - * Adds a vision measurement to the Kalman Filter. This will correct - * the odometry pose estimate while still accounting for measurement noise. - * - * This method can be called as infrequently as you want, as long as you are - * calling Update() every loop. - * - * To promote stability of the pose estimate and make it robust to bad vision - * data, we recommend only adding vision measurements that are already within - * one meter or so of the current pose estimate. - * - * @param visionRobotPose The pose of the robot as measured by the vision - * camera. - * @param timestamp The timestamp of the vision measurement in seconds. Note - * that if you don't use your own time source by calling UpdateWithTime(), - * then you must use a timestamp with an epoch since FPGA startup (i.e., - * the epoch of this timestamp is the same epoch as - * frc::Timer::GetFPGATimestamp(). This means that you should use - * frc::Timer::GetFPGATimestamp() as your time source in this case. - */ - void AddVisionMeasurement(const Pose2d& visionRobotPose, - units::second_t timestamp) { - AddVisionMeasurement(Pose3d{visionRobotPose}, timestamp); - } - - /** - * Adds a vision measurement to the Kalman Filter. This will correct - * the odometry pose estimate while still accounting for measurement noise. - * - * This method can be called as infrequently as you want, as long as you are - * calling Update() every loop. - * - * To promote stability of the pose estimate and make it robust to bad vision - * data, we recommend only adding vision measurements that are already within - * one meter or so of the current pose estimate. - * - * Note that the vision measurement standard deviations passed into this - * method will continue to apply to future measurements until a subsequent - * call to SetVisionMeasurementStdDevs() or this method. - * - * @param visionRobotPose The pose of the robot as measured by the vision - * camera. - * @param timestamp The timestamp of the vision measurement in seconds. Note - * that if you don't use your own time source by calling UpdateWithTime(), - * then you must use a timestamp with an epoch since FPGA startup (i.e., - * the epoch of this timestamp is the same epoch as - * frc::Timer::GetFPGATimestamp(). This means that you should use - * frc::Timer::GetFPGATimestamp() as your time source in this case. - * @param visionMeasurementStdDevs Standard deviations of the vision pose - * measurement (x position in meters, y position in meters, and heading in - * radians). Increase these numbers to trust the vision pose measurement - * less. - */ - void AddVisionMeasurement( - const Pose2d& visionRobotPose, units::second_t timestamp, - const wpi::array& visionMeasurementStdDevs) { - SetVisionMeasurementStdDevs(visionMeasurementStdDevs); - AddVisionMeasurement(visionRobotPose, timestamp); - } - /** * Adds a vision measurement to the Kalman Filter. This will correct * the odometry pose estimate while still accounting for measurement noise. @@ -432,7 +264,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d { // Step 3: Get the vision-compensated pose estimate at the moment the vision // measurement was made. - auto visionSample = SampleAt3d(timestamp); + auto visionSample = SampleAt(timestamp); if (!visionSample) { return; @@ -466,7 +298,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d { // Step 9: Update latest pose estimate. Since we cleared all updates after // this vision update, it's guaranteed to be the latest vision update. - m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose3d()); + m_poseEstimate = visionUpdate.Compensate(m_odometry.GetPose()); } /** @@ -504,21 +336,6 @@ class WPILIB_DLLEXPORT PoseEstimator3d { AddVisionMeasurement(visionRobotPose, timestamp); } - /** - * Updates the pose estimator with wheel encoder and gyro information. This - * should be called every loop. - * - * @param gyroAngle The current gyro angle. - * @param wheelPositions The distances traveled by the encoders. - * - * @return The estimated pose of the robot in meters. - */ - Pose2d Update(const Rotation2d& gyroAngle, - const WheelPositions& wheelPositions) { - return Update(Rotation3d{0_rad, 0_rad, gyroAngle.Radians()}, wheelPositions) - .ToPose2d(); - } - /** * Updates the pose estimator with wheel encoder and gyro information. This * should be called every loop. @@ -534,25 +351,6 @@ class WPILIB_DLLEXPORT PoseEstimator3d { wheelPositions); } - /** - * Updates the pose estimator with wheel encoder and gyro information. This - * should be called every loop. - * - * @param currentTime The time at which this method was called. - * @param gyroAngle The current gyro angle. - * @param wheelPositions The distances traveled by the encoders. - * - * @return The estimated pose of the robot in meters. - */ - Pose2d UpdateWithTime(units::second_t currentTime, - const Rotation2d& gyroAngle, - const WheelPositions& wheelPositions) { - return UpdateWithTime(currentTime, - Rotation3d{0_rad, 0_rad, gyroAngle.Radians()}, - wheelPositions) - .ToPose2d(); - } - /** * Updates the pose estimator with wheel encoder and gyro information. This * should be called every loop. @@ -577,7 +375,7 @@ class WPILIB_DLLEXPORT PoseEstimator3d { m_poseEstimate = visionUpdate.Compensate(odometryEstimate); } - return GetEstimatedPosition3d(); + return GetEstimatedPosition(); } private: diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h index 10299e15975..7efcbb573cd 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h @@ -21,7 +21,11 @@ namespace frc { /** * This class wraps Swerve Drive Odometry to fuse latency-compensated * vision measurements with swerve drive encoder distance measurements. It is - * intended to be a drop-in for SwerveDriveOdometry3d. + * intended to be a drop-in for SwerveDriveOdometry3d. It is also intended to be + * an easy replacement for PoseEstimator, only requiring the addition of a + * standard deviation for Z and appropriate conversions between 2D and 3D + * versions of geometry classes. (See Pose3d(Pose2d), Rotation3d(Rotation2d), + * Translation3d(Translation2d), and Pose3d.ToPose2d().) * * Update() should be called every robot loop. * @@ -34,60 +38,6 @@ class SwerveDrivePoseEstimator3d : public PoseEstimator3d, wpi::array> { public: - /** - * Constructs a SwerveDrivePoseEstimator3d with default standard deviations - * for the model and vision measurements. - * - * The default standard deviations of the model states are - * 0.1 meters for x, 0.1 meters for y, and 0.1 radians for heading. - * The default standard deviations of the vision measurements are - * 0.9 meters for x, 0.9 meters for y, and 0.9 radians for heading. - * - * @param kinematics A correctly-configured kinematics object for your - * drivetrain. - * @param gyroAngle The current gyro angle. - * @param modulePositions The current distance and rotation measurements of - * the swerve modules. - * @param initialPose The starting pose estimate. - */ - SwerveDrivePoseEstimator3d( - SwerveDriveKinematics& kinematics, - const Rotation2d& gyroAngle, - const wpi::array& modulePositions, - const Pose2d& initialPose) - : SwerveDrivePoseEstimator3d{kinematics, gyroAngle, - modulePositions, initialPose, - {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}} {} - - /** - * Constructs a SwerveDrivePoseEstimator3d. - * - * @param kinematics A correctly-configured kinematics object for your - * drivetrain. - * @param gyroAngle The current gyro angle. - * @param modulePositions The current distance and rotation measurements of - * the swerve modules. - * @param initialPose The starting pose estimate. - * @param stateStdDevs Standard deviations of the pose estimate (x position in - * meters, y position in meters, and heading in radians). Increase these - * numbers to trust your state estimate less. - * @param visionMeasurementStdDevs Standard deviations of the vision pose - * measurement (x position in meters, y position in meters, and heading in - * radians). Increase these numbers to trust the vision pose measurement - * less. - */ - SwerveDrivePoseEstimator3d( - SwerveDriveKinematics& kinematics, - const Rotation2d& gyroAngle, - const wpi::array& modulePositions, - const Pose2d& initialPose, const wpi::array& stateStdDevs, - const wpi::array& visionMeasurementStdDevs) - : SwerveDrivePoseEstimator3d::PoseEstimator3d( - kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), - m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} { - this->ResetPose(initialPose); - } - /** * Constructs a SwerveDrivePoseEstimator3d with default standard deviations * for the model and vision measurements. diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h index 6784ec0070c..d6470442f27 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h @@ -30,22 +30,6 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry3d : public Odometry3d { public: - /** - * Constructs a DifferentialDriveOdometry3d object. - * - * IF leftDistance and rightDistance are unspecified, - * You NEED to reset your encoders (to zero). - * - * @param gyroAngle The angle reported by the gyroscope. - * @param leftDistance The distance traveled by the left encoder. - * @param rightDistance The distance traveled by the right encoder. - * @param initialPose The starting position of the robot on the field. - */ - explicit DifferentialDriveOdometry3d(const Rotation2d& gyroAngle, - units::meter_t leftDistance, - units::meter_t rightDistance, - const Pose2d& initialPose = Pose2d{}); - /** * Constructs a DifferentialDriveOdometry3d object. * @@ -62,25 +46,6 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry3d units::meter_t rightDistance, const Pose3d& initialPose = Pose3d{}); - /** - * Resets the robot's position on the field. - * - * IF leftDistance and rightDistance are unspecified, - * You NEED to reset your encoders (to zero). - * - * The gyroscope angle does not need to be reset here on the user's robot - * code. The library automatically takes care of offsetting the gyro angle. - * - * @param pose The position on the field that your robot is at. - * @param gyroAngle The angle reported by the gyroscope. - * @param leftDistance The distance traveled by the left encoder. - * @param rightDistance The distance traveled by the right encoder. - */ - void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance, - units::meter_t rightDistance, const Pose2d& pose) { - Odometry3d::ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose); - } - /** * Resets the robot's position on the field. * @@ -100,22 +65,6 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry3d Odometry3d::ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose); } - /** - * Updates the robot position on the field using distance measurements from - * encoders. This method is more numerically accurate than using velocities to - * integrate the pose and is also advantageous for teams that are using lower - * CPR encoders. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param leftDistance The distance traveled by the left encoder. - * @param rightDistance The distance traveled by the right encoder. - * @return The new pose of the robot. - */ - const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance, - units::meter_t rightDistance) { - return Odometry3d::Update(gyroAngle, {leftDistance, rightDistance}); - } - /** * Updates the robot position on the field using distance measurements from * encoders. This method is more numerically accurate than using velocities to diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry3d.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry3d.h index 7a961476db4..2f3726e45de 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry3d.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry3d.h @@ -28,19 +28,6 @@ namespace frc { class WPILIB_DLLEXPORT MecanumDriveOdometry3d : public Odometry3d { public: - /** - * Constructs a MecanumDriveOdometry3d object. - * - * @param kinematics The mecanum drive kinematics for your drivetrain. - * @param gyroAngle The angle reported by the gyroscope. - * @param wheelPositions The current distances measured by each wheel. - * @param initialPose The starting position of the robot on the field. - */ - explicit MecanumDriveOdometry3d( - MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle, - const MecanumDriveWheelPositions& wheelPositions, - const Pose2d& initialPose = Pose2d{}); - /** * Constructs a MecanumDriveOdometry3d object. * diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h b/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h index 876d3cfc90d..ffdfb05bef1 100644 --- a/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h +++ b/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h @@ -32,21 +32,6 @@ namespace frc { template class WPILIB_DLLEXPORT Odometry3d { public: - /** - * Constructs an Odometry3d object. - * - * @param kinematics The kinematics for your drivetrain. - * @param gyroAngle The angle reported by the gyroscope. - * @param wheelPositions The current distances measured by each wheel. - * @param initialPose The starting position of the robot on the field. - */ - explicit Odometry3d(const Kinematics& kinematics, - const Rotation2d& gyroAngle, - const WheelPositions& wheelPositions, - const Pose2d& initialPose = Pose2d{}) - : Odometry3d{kinematics, Rotation3d{0_rad, 0_rad, gyroAngle.Radians()}, - wheelPositions, Pose3d{initialPose}} {} - /** * Constructs an Odometry3d object. * @@ -62,27 +47,10 @@ class WPILIB_DLLEXPORT Odometry3d { : m_kinematics(kinematics), m_pose(initialPose), m_previousWheelPositions(wheelPositions) { - m_pose2d = m_pose.ToPose2d(); m_previousAngle = m_pose.Rotation(); m_gyroOffset = m_pose.Rotation() - gyroAngle; } - /** - * Resets the robot's position on the field. - * - * The gyroscope angle does not need to be reset here on the user's robot - * code. The library automatically takes care of offsetting the gyro angle. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param wheelPositions The current distances measured by each wheel. - * @param pose The position on the field that your robot is at. - */ - void ResetPosition(const Rotation2d& gyroAngle, - const WheelPositions& wheelPositions, const Pose2d& pose) { - ResetPosition(Rotation3d{0_rad, 0_rad, gyroAngle.Radians()}, wheelPositions, - Pose3d{pose}); - } - /** * Resets the robot's position on the field. * @@ -96,19 +64,11 @@ class WPILIB_DLLEXPORT Odometry3d { void ResetPosition(const Rotation3d& gyroAngle, const WheelPositions& wheelPositions, const Pose3d& pose) { m_pose = pose; - m_pose2d = m_pose.ToPose2d(); m_previousAngle = pose.Rotation(); m_gyroOffset = m_pose.Rotation() - gyroAngle; m_previousWheelPositions = wheelPositions; } - /** - * Resets the pose. - * - * @param pose The pose to reset to. - */ - void ResetPose(const Pose2d& pose) { ResetPose(Pose3d{pose}); } - /** * Resets the pose. * @@ -117,19 +77,9 @@ class WPILIB_DLLEXPORT Odometry3d { void ResetPose(const Pose3d& pose) { m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation()); m_pose = pose; - m_pose2d = m_pose.ToPose2d(); m_previousAngle = pose.Rotation(); } - /** - * Resets the translation of the pose. - * - * @param translation The translation to reset to. - */ - void ResetTranslation(const Translation2d& translation) { - ResetTranslation(Translation3d{translation.X(), translation.Y(), 0_m}); - } - /** * Resets the translation of the pose. * @@ -137,16 +87,6 @@ class WPILIB_DLLEXPORT Odometry3d { */ void ResetTranslation(const Translation3d& translation) { m_pose = Pose3d{translation, m_pose.Rotation()}; - m_pose2d = m_pose.ToPose2d(); - } - - /** - * Resets the rotation of the pose. - * - * @param rotation The rotation to reset to. - */ - void ResetRotation(const Rotation2d& rotation) { - ResetRotation(Rotation3d{0_rad, 0_rad, rotation.Radians()}); } /** @@ -157,7 +97,6 @@ class WPILIB_DLLEXPORT Odometry3d { void ResetRotation(const Rotation3d& rotation) { m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation()); m_pose = Pose3d{m_pose.Translation(), rotation}; - m_pose2d = m_pose.ToPose2d(); m_previousAngle = rotation; } @@ -165,30 +104,7 @@ class WPILIB_DLLEXPORT Odometry3d { * Returns the position of the robot on the field. * @return The pose of the robot. */ - const Pose2d& GetPose() const { return m_pose2d; } - - /** - * Returns the position of the robot on the field. - * @return The pose of the robot. - */ - const Pose3d& GetPose3d() const { return m_pose; } - - /** - * Updates the robot's position on the field using forward kinematics and - * integration of the pose over time. This method takes in an angle parameter - * which is used instead of the angular rate that is calculated from forward - * kinematics, in addition to the current distance measurement at each wheel. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param wheelPositions The current distances measured by each wheel. - * - * @return The new pose of the robot. - */ - const Pose2d& Update(const Rotation2d& gyroAngle, - const WheelPositions& wheelPositions) { - Update(Rotation3d{0_rad, 0_rad, gyroAngle.Radians()}, wheelPositions); - return GetPose(); - } + const Pose3d& GetPose() const { return m_pose; } /** * Updates the robot's position on the field using forward kinematics and @@ -221,14 +137,12 @@ class WPILIB_DLLEXPORT Odometry3d { m_previousWheelPositions = wheelPositions; m_previousAngle = angle; m_pose = {newPose.Translation(), angle}; - m_pose2d = m_pose.ToPose2d(); return m_pose; } private: const Kinematics& m_kinematics; - Pose2d m_pose2d; // Only for GetPose() Pose3d m_pose; WheelPositions m_previousWheelPositions; diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h index 31a3447f419..0215a2c5dc8 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h @@ -45,32 +45,6 @@ class SwerveDriveOdometry3d #if defined(__GNUC__) && !defined(__clang__) #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wmaybe-uninitialized" -#endif // defined(__GNUC__) && !defined(__clang__) - SwerveDriveOdometry3d( - SwerveDriveKinematics kinematics, const Rotation2d& gyroAngle, - const wpi::array& modulePositions, - const Pose2d& initialPose = Pose2d{}) - : SwerveDriveOdometry3d::Odometry3d(m_kinematicsImpl, gyroAngle, - modulePositions, initialPose), - m_kinematicsImpl(kinematics) { - wpi::math::MathSharedStore::ReportUsage( - wpi::math::MathUsageId::kOdometry_SwerveDrive, 1); - } -#if defined(__GNUC__) && !defined(__clang__) -#pragma GCC diagnostic pop -#endif // defined(__GNUC__) && !defined(__clang__) - - /** - * Constructs a SwerveDriveOdometry3d object. - * - * @param kinematics The swerve drive kinematics for your drivetrain. - * @param gyroAngle The angle reported by the gyroscope. - * @param modulePositions The wheel positions reported by each module. - * @param initialPose The starting position of the robot on the field. - */ -#if defined(__GNUC__) && !defined(__clang__) -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" #endif // defined(__GNUC__) && !defined(__clang__) SwerveDriveOdometry3d( SwerveDriveKinematics kinematics, const Rotation3d& gyroAngle, diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3dTest.java index 7aba316b086..7912d3f9f54 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3dTest.java @@ -10,9 +10,12 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.trajectory.Trajectory; @@ -35,12 +38,12 @@ void testAccuracy() { var estimator = new DifferentialDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, 0, 0, - Pose2d.kZero, - VecBuilder.fill(0.02, 0.02, 0.01), - VecBuilder.fill(0.1, 0.1, 0.1)); + Pose3d.kZero, + VecBuilder.fill(0.02, 0.02, 0.02, 0.01), + VecBuilder.fill(0.1, 0.1, 0.1, 0.1)); var trajectory = TrajectoryGenerator.generateTrajectory( List.of( @@ -76,12 +79,12 @@ void testBadInitialPose() { var estimator = new DifferentialDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, 0, 0, - Pose2d.kZero, - VecBuilder.fill(0.02, 0.02, 0.01), - VecBuilder.fill(0.1, 0.1, 0.1)); + Pose3d.kZero, + VecBuilder.fill(0.02, 0.02, 0.02, 0.01), + VecBuilder.fill(0.1, 0.1, 0.1, 0.1)); var trajectory = TrajectoryGenerator.generateTrajectory( @@ -142,7 +145,7 @@ void testFollowTrajectory( double rightDistanceMeters = 0; estimator.resetPosition( - Rotation2d.kZero, leftDistanceMeters, rightDistanceMeters, startingPose); + Rotation3d.kZero, leftDistanceMeters, rightDistanceMeters, new Pose3d(startingPose)); var rand = new Random(3538); @@ -173,7 +176,7 @@ void testFollowTrajectory( // since it was measured if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) { var visionEntry = visionUpdateQueue.pollFirstEntry(); - estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey()); + estimator.addVisionMeasurement(new Pose3d(visionEntry.getValue()), visionEntry.getKey()); } var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState); @@ -186,16 +189,20 @@ void testFollowTrajectory( var xHat = estimator.updateWithTime( t, - groundTruthState - .poseMeters - .getRotation() - .plus(new Rotation2d(rand.nextGaussian() * 0.05)) - .minus(trajectory.getInitialPose().getRotation()), + new Rotation3d( + groundTruthState + .poseMeters + .getRotation() + .plus(new Rotation2d(rand.nextGaussian() * 0.05)) + .minus(trajectory.getInitialPose().getRotation())), leftDistanceMeters, rightDistanceMeters); double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + groundTruthState + .poseMeters + .getTranslation() + .getDistance(xHat.getTranslation().toTranslation2d()); if (error > maxError) { maxError = error; } @@ -210,7 +217,7 @@ void testFollowTrajectory( endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y"); assertEquals( endingPose.getRotation().getRadians(), - estimator.getEstimatedPosition().getRotation().getRadians(), + estimator.getEstimatedPosition().getRotation().toRotation2d().getRadians(), 0.15, "Incorrect Final Theta"); @@ -232,14 +239,14 @@ void testSimultaneousVisionMeasurements() { var estimator = new DifferentialDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, 0, 0, - new Pose2d(1, 2, Rotation2d.kCW_Pi_2), - VecBuilder.fill(0.02, 0.02, 0.01), - VecBuilder.fill(0.1, 0.1, 0.1)); + new Pose3d(1, 2, 0, new Rotation3d(0, 0, -Math.PI / 2)), + VecBuilder.fill(0.02, 0.02, 0.02, 0.01), + VecBuilder.fill(0.1, 0.1, 0.1, 0.1)); - estimator.updateWithTime(0, Rotation2d.kZero, 0, 0); + estimator.updateWithTime(0, Rotation3d.kZero, 0, 0); var visionMeasurements = new Pose2d[] { @@ -250,7 +257,7 @@ void testSimultaneousVisionMeasurements() { for (int i = 0; i < 1000; i++) { for (var measurement : visionMeasurements) { - estimator.addVisionMeasurement(measurement, 0); + estimator.addVisionMeasurement(new Pose3d(measurement), 0); } } @@ -266,7 +273,7 @@ void testSimultaneousVisionMeasurements() { var dtheta = Math.abs( measurement.getRotation().getDegrees() - - estimator.getEstimatedPosition().getRotation().getDegrees()); + - estimator.getEstimatedPosition().getRotation().toRotation2d().getDegrees()); assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog); } @@ -278,18 +285,18 @@ void testDiscardsStaleVisionMeasurements() { var estimator = new DifferentialDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, 0, 0, - Pose2d.kZero, - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.9, 0.9, 0.9)); + Pose3d.kZero, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.9, 0.9, 0.9, 0.9)); double time = 0; // Add enough measurements to fill up the buffer for (; time < 4; time += 0.02) { - estimator.updateWithTime(time, Rotation2d.kZero, 0, 0); + estimator.updateWithTime(time, Rotation3d.kZero, 0, 0); } var odometryPose = estimator.getEstimatedPosition(); @@ -297,16 +304,33 @@ void testDiscardsStaleVisionMeasurements() { // Apply a vision measurement made 3 seconds ago // This test passes if this does not cause a ConcurrentModificationException. estimator.addVisionMeasurement( - new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)), - 1, - VecBuilder.fill(0.1, 0.1, 0.1)); + new Pose3d(10, 10, 0, new Rotation3d(0, 0, 0.1)), 1, VecBuilder.fill(0.1, 0.1, 0.1, 0.1)); - assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X"); - assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y"); - assertEquals( - odometryPose.getRotation().getRadians(), - estimator.getEstimatedPosition().getRotation().getRadians(), - "Incorrect Final Theta"); + assertAll( + () -> + assertEquals( + odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X"), + () -> + assertEquals( + odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y"), + () -> + assertEquals( + odometryPose.getZ(), estimator.getEstimatedPosition().getZ(), "Incorrect Final Y"), + () -> + assertEquals( + odometryPose.getRotation().getX(), + estimator.getEstimatedPosition().getRotation().getX(), + "Incorrect Final Roll"), + () -> + assertEquals( + odometryPose.getRotation().getY(), + estimator.getEstimatedPosition().getRotation().getY(), + "Incorrect Final Pitch"), + () -> + assertEquals( + odometryPose.getRotation().getZ(), + estimator.getEstimatedPosition().getRotation().getZ(), + "Incorrect Final Yaw")); } @Test @@ -315,12 +339,12 @@ void testSampleAt() { var estimator = new DifferentialDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, 0, 0, - Pose2d.kZero, - VecBuilder.fill(1, 1, 1), - VecBuilder.fill(1, 1, 1)); + Pose3d.kZero, + VecBuilder.fill(1, 1, 1, 1), + VecBuilder.fill(1, 1, 1, 1)); // Returns empty when null assertEquals(Optional.empty(), estimator.sampleAt(1)); @@ -328,35 +352,35 @@ void testSampleAt() { // 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, Rotation2d.kZero, time, time); + estimator.updateWithTime(time, Rotation3d.kZero, time, time); } // Sample at an added time - assertEquals(Optional.of(new Pose2d(1.02, 0, Rotation2d.kZero)), estimator.sampleAt(1.02)); + assertEquals(Optional.of(new Pose3d(1.02, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.02)); // Sample between updates (test interpolation) - assertEquals(Optional.of(new Pose2d(1.01, 0, Rotation2d.kZero)), estimator.sampleAt(1.01)); + assertEquals(Optional.of(new Pose3d(1.01, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.01)); // Sampling before the oldest value returns the oldest value - assertEquals(Optional.of(new Pose2d(1, 0, Rotation2d.kZero)), estimator.sampleAt(0.5)); + assertEquals(Optional.of(new Pose3d(1, 0, 0, Rotation3d.kZero)), estimator.sampleAt(0.5)); // Sampling after the newest value returns the newest value - assertEquals(Optional.of(new Pose2d(2, 0, Rotation2d.kZero)), estimator.sampleAt(2.5)); + assertEquals(Optional.of(new Pose3d(2, 0, 0, Rotation3d.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); + estimator.addVisionMeasurement(new Pose3d(2, 0, 0, new Rotation3d(0, 0, 1)), 2.2); // Make sure nothing changed (except the newest value) - 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)); + assertEquals(Optional.of(new Pose3d(1.02, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.02)); + assertEquals(Optional.of(new Pose3d(1.01, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.01)); + assertEquals(Optional.of(new Pose3d(1, 0, 0, Rotation3d.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, Rotation2d.kZero), 0.9); + estimator.addVisionMeasurement(new Pose3d(1, 0.2, 0, Rotation3d.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, 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)); + assertEquals(Optional.of(new Pose3d(1.02, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(1.02)); + assertEquals(Optional.of(new Pose3d(1.01, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(1.01)); + assertEquals(Optional.of(new Pose3d(1, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(0.5)); + assertEquals(Optional.of(new Pose3d(2, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(2.5)); } @Test @@ -365,74 +389,83 @@ void testReset() { var estimator = new DifferentialDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, 0, 0, - Pose2d.kZero, - VecBuilder.fill(1, 1, 1), - VecBuilder.fill(1, 1, 1)); + Pose3d.kZero, + VecBuilder.fill(1, 1, 1, 1), + VecBuilder.fill(1, 1, 1, 1)); // Test reset position - estimator.resetPosition(Rotation2d.kZero, 1, 1, new Pose2d(1, 0, Rotation2d.kZero)); + estimator.resetPosition(Rotation3d.kZero, 1, 1, new Pose3d(1, 0, 0, Rotation3d.kZero)); assertAll( () -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), - () -> - assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); // Test orientation and wheel positions - estimator.update(Rotation2d.kZero, 2, 2); + estimator.update(Rotation3d.kZero, 2, 2); assertAll( () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), - () -> - assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); // Test reset rotation - estimator.resetRotation(Rotation2d.kCCW_Pi_2); + estimator.resetRotation(new Rotation3d(Rotation2d.kCCW_Pi_2)); assertAll( () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), () -> assertEquals( - Math.PI / 2, - estimator.getEstimatedPosition().getRotation().getRadians(), - kEpsilon)); + Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); // Test orientation - estimator.update(Rotation2d.kZero, 3, 3); + estimator.update(Rotation3d.kZero, 3, 3); assertAll( () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), () -> assertEquals( - Math.PI / 2, - estimator.getEstimatedPosition().getRotation().getRadians(), - kEpsilon)); + Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); // Test reset translation - estimator.resetTranslation(new Translation2d(-1, -1)); + estimator.resetTranslation(new Translation3d(-1, -1, -1)); assertAll( () -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> assertEquals(-1, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), () -> assertEquals( - Math.PI / 2, - estimator.getEstimatedPosition().getRotation().getRadians(), - kEpsilon)); + Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); // Test reset pose - estimator.resetPose(Pose2d.kZero); + estimator.resetPose(Pose3d.kZero); assertAll( () -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), - () -> - assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3dTest.java index 74d6afc765f..2ec6a200e7c 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3dTest.java @@ -10,9 +10,12 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.MecanumDriveKinematics; import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; @@ -41,11 +44,11 @@ void testAccuracyFacingTrajectory() { var estimator = new MecanumDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, wheelPositions, - Pose2d.kZero, - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.45, 0.45, 0.1)); + Pose3d.kZero, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.45, 0.45, 0.45, 0.1)); var trajectory = TrajectoryGenerator.generateTrajectory( @@ -87,11 +90,11 @@ void testBadInitialPose() { var estimator = new MecanumDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, wheelPositions, - Pose2d.kZero, - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.45, 0.45, 0.1)); + Pose3d.kZero, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.45, 0.45, 0.45, 0.1)); var trajectory = TrajectoryGenerator.generateTrajectory( @@ -150,7 +153,7 @@ void testFollowTrajectory( final boolean checkError) { var wheelPositions = new MecanumDriveWheelPositions(); - estimator.resetPosition(Rotation2d.kZero, wheelPositions, startingPose); + estimator.resetPosition(Rotation3d.kZero, wheelPositions, new Pose3d(startingPose)); var rand = new Random(3538); @@ -181,7 +184,7 @@ void testFollowTrajectory( // since it was measured if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) { var visionEntry = visionUpdateQueue.pollFirstEntry(); - estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey()); + estimator.addVisionMeasurement(new Pose3d(visionEntry.getValue()), visionEntry.getKey()); } var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState); @@ -196,15 +199,19 @@ void testFollowTrajectory( var xHat = estimator.updateWithTime( t, - groundTruthState - .poseMeters - .getRotation() - .plus(new Rotation2d(rand.nextGaussian() * 0.05)) - .minus(trajectory.getInitialPose().getRotation()), + new Rotation3d( + groundTruthState + .poseMeters + .getRotation() + .plus(new Rotation2d(rand.nextGaussian() * 0.05)) + .minus(trajectory.getInitialPose().getRotation())), wheelPositions); double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + groundTruthState + .poseMeters + .getTranslation() + .getDistance(xHat.getTranslation().toTranslation2d()); if (error > maxError) { maxError = error; } @@ -219,7 +226,7 @@ void testFollowTrajectory( endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y"); assertEquals( endingPose.getRotation().getRadians(), - estimator.getEstimatedPosition().getRotation().getRadians(), + estimator.getEstimatedPosition().getRotation().toRotation2d().getRadians(), 0.15, "Incorrect Final Theta"); @@ -246,13 +253,13 @@ void testSimultaneousVisionMeasurements() { var estimator = new MecanumDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, wheelPositions, - new Pose2d(1, 2, Rotation2d.kCW_Pi_2), - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.45, 0.45, 0.1)); + new Pose3d(1, 2, 0, new Rotation3d(0, 0, -Math.PI / 2)), + VecBuilder.fill(0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.45, 0.45, 0.45, 0.1)); - estimator.updateWithTime(0, Rotation2d.kZero, wheelPositions); + estimator.updateWithTime(0, Rotation3d.kZero, wheelPositions); var visionMeasurements = new Pose2d[] { @@ -263,7 +270,7 @@ void testSimultaneousVisionMeasurements() { for (int i = 0; i < 1000; i++) { for (var measurement : visionMeasurements) { - estimator.addVisionMeasurement(measurement, 0); + estimator.addVisionMeasurement(new Pose3d(measurement), 0); } } @@ -279,7 +286,7 @@ void testSimultaneousVisionMeasurements() { var dtheta = Math.abs( measurement.getRotation().getDegrees() - - estimator.getEstimatedPosition().getRotation().getDegrees()); + - estimator.getEstimatedPosition().getRotation().toRotation2d().getDegrees()); assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog); } @@ -296,17 +303,17 @@ void testDiscardsOldVisionMeasurements() { var estimator = new MecanumDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, new MecanumDriveWheelPositions(), - Pose2d.kZero, - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.9, 0.9, 0.9)); + Pose3d.kZero, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.9, 0.9, 0.9, 0.9)); double time = 0; // Add enough measurements to fill up the buffer for (; time < 4; time += 0.02) { - estimator.updateWithTime(time, Rotation2d.kZero, new MecanumDriveWheelPositions()); + estimator.updateWithTime(time, Rotation3d.kZero, new MecanumDriveWheelPositions()); } var odometryPose = estimator.getEstimatedPosition(); @@ -314,16 +321,33 @@ void testDiscardsOldVisionMeasurements() { // Apply a vision measurement made 3 seconds ago // This test passes if this does not cause a ConcurrentModificationException. estimator.addVisionMeasurement( - new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)), - 1, - VecBuilder.fill(0.1, 0.1, 0.1)); + new Pose3d(10, 10, 0, new Rotation3d(0, 0, 0.1)), 1, VecBuilder.fill(0.1, 0.1, 0.1, 0.1)); - assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X"); - assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y"); - assertEquals( - odometryPose.getRotation().getRadians(), - estimator.getEstimatedPosition().getRotation().getRadians(), - "Incorrect Final Theta"); + assertAll( + () -> + assertEquals( + odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X"), + () -> + assertEquals( + odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y"), + () -> + assertEquals( + odometryPose.getZ(), estimator.getEstimatedPosition().getZ(), "Incorrect Final Y"), + () -> + assertEquals( + odometryPose.getRotation().getX(), + estimator.getEstimatedPosition().getRotation().getX(), + "Incorrect Final Roll"), + () -> + assertEquals( + odometryPose.getRotation().getY(), + estimator.getEstimatedPosition().getRotation().getY(), + "Incorrect Final Pitch"), + () -> + assertEquals( + odometryPose.getRotation().getZ(), + estimator.getEstimatedPosition().getRotation().getZ(), + "Incorrect Final Yaw")); } @Test @@ -337,11 +361,11 @@ void testSampleAt() { var estimator = new MecanumDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, new MecanumDriveWheelPositions(), - Pose2d.kZero, - VecBuilder.fill(1, 1, 1), - VecBuilder.fill(1, 1, 1)); + Pose3d.kZero, + VecBuilder.fill(1, 1, 1, 1), + VecBuilder.fill(1, 1, 1, 1)); // Returns empty when null assertEquals(Optional.empty(), estimator.sampleAt(1)); @@ -350,35 +374,35 @@ void testSampleAt() { // 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, Rotation2d.kZero, wheelPositions); + estimator.updateWithTime(time, Rotation3d.kZero, wheelPositions); } // Sample at an added time - assertEquals(Optional.of(new Pose2d(1.02, 0, Rotation2d.kZero)), estimator.sampleAt(1.02)); + assertEquals(Optional.of(new Pose3d(1.02, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.02)); // Sample between updates (test interpolation) - assertEquals(Optional.of(new Pose2d(1.01, 0, Rotation2d.kZero)), estimator.sampleAt(1.01)); + assertEquals(Optional.of(new Pose3d(1.01, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.01)); // Sampling before the oldest value returns the oldest value - assertEquals(Optional.of(new Pose2d(1, 0, Rotation2d.kZero)), estimator.sampleAt(0.5)); + assertEquals(Optional.of(new Pose3d(1, 0, 0, Rotation3d.kZero)), estimator.sampleAt(0.5)); // Sampling after the newest value returns the newest value - assertEquals(Optional.of(new Pose2d(2, 0, Rotation2d.kZero)), estimator.sampleAt(2.5)); + assertEquals(Optional.of(new Pose3d(2, 0, 0, Rotation3d.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); + estimator.addVisionMeasurement(new Pose3d(2, 0, 0, new Rotation3d(0, 0, 1)), 2.2); // Make sure nothing changed (except the newest value) - 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)); + assertEquals(Optional.of(new Pose3d(1.02, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.02)); + assertEquals(Optional.of(new Pose3d(1.01, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.01)); + assertEquals(Optional.of(new Pose3d(1, 0, 0, Rotation3d.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, Rotation2d.kZero), 0.9); + estimator.addVisionMeasurement(new Pose3d(1, 0.2, 0, Rotation3d.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, 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)); + assertEquals(Optional.of(new Pose3d(1.02, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(1.02)); + assertEquals(Optional.of(new Pose3d(1.01, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(1.01)); + assertEquals(Optional.of(new Pose3d(1, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(0.5)); + assertEquals(Optional.of(new Pose3d(2, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(2.5)); } @Test @@ -392,76 +416,85 @@ void testReset() { var estimator = new MecanumDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, new MecanumDriveWheelPositions(), - Pose2d.kZero, - VecBuilder.fill(1, 1, 1), - VecBuilder.fill(1, 1, 1)); + Pose3d.kZero, + VecBuilder.fill(1, 1, 1, 1), + VecBuilder.fill(1, 1, 1, 1)); // Test reset position estimator.resetPosition( - Rotation2d.kZero, + Rotation3d.kZero, new MecanumDriveWheelPositions(1, 1, 1, 1), - new Pose2d(1, 0, Rotation2d.kZero)); + new Pose3d(1, 0, 0, Rotation3d.kZero)); assertAll( () -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), - () -> - assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); // Test orientation and wheel positions - estimator.update(Rotation2d.kZero, new MecanumDriveWheelPositions(2, 2, 2, 2)); + estimator.update(Rotation3d.kZero, new MecanumDriveWheelPositions(2, 2, 2, 2)); assertAll( () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), - () -> - assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); // Test reset rotation - estimator.resetRotation(Rotation2d.kCCW_Pi_2); + estimator.resetRotation(new Rotation3d(Rotation2d.kCCW_Pi_2)); assertAll( () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), () -> assertEquals( - Math.PI / 2, - estimator.getEstimatedPosition().getRotation().getRadians(), - kEpsilon)); + Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); // Test orientation - estimator.update(Rotation2d.kZero, new MecanumDriveWheelPositions(3, 3, 3, 3)); + estimator.update(Rotation3d.kZero, new MecanumDriveWheelPositions(3, 3, 3, 3)); assertAll( () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), () -> assertEquals( - Math.PI / 2, - estimator.getEstimatedPosition().getRotation().getRadians(), - kEpsilon)); + Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); // Test reset translation - estimator.resetTranslation(new Translation2d(-1, -1)); + estimator.resetTranslation(new Translation3d(-1, -1, -1)); assertAll( () -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> assertEquals(-1, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), () -> assertEquals( - Math.PI / 2, - estimator.getEstimatedPosition().getRotation().getRadians(), - kEpsilon)); + Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); // Test reset pose - estimator.resetPose(Pose2d.kZero); + estimator.resetPose(Pose3d.kZero); assertAll( () -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), - () -> - assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java index 4dc42e122cd..e3cd11a2815 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java @@ -10,9 +10,12 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -46,11 +49,11 @@ void testAccuracyFacingTrajectory() { var estimator = new SwerveDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, new SwerveModulePosition[] {fl, fr, bl, br}, - Pose2d.kZero, - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.5, 0.5, 0.5)); + Pose3d.kZero, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.5, 0.5, 0.5, 0.5)); var trajectory = TrajectoryGenerator.generateTrajectory( @@ -97,11 +100,11 @@ void testBadInitialPose() { var estimator = new SwerveDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, new SwerveModulePosition[] {fl, fr, bl, br}, - new Pose2d(-1, -1, Rotation2d.fromRadians(-1)), - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.9, 0.9, 0.9)); + Pose3d.kZero, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.9, 0.9, 0.9, 0.9)); var trajectory = TrajectoryGenerator.generateTrajectory( List.of( @@ -164,7 +167,7 @@ void testFollowTrajectory( new SwerveModulePosition() }; - estimator.resetPosition(Rotation2d.kZero, positions, startingPose); + estimator.resetPosition(Rotation3d.kZero, positions, new Pose3d(startingPose)); var rand = new Random(3538); @@ -195,7 +198,7 @@ void testFollowTrajectory( // since it was measured if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) { var visionEntry = visionUpdateQueue.pollFirstEntry(); - estimator.addVisionMeasurement(visionEntry.getValue(), visionEntry.getKey()); + estimator.addVisionMeasurement(new Pose3d(visionEntry.getValue()), visionEntry.getKey()); } var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState); @@ -212,15 +215,19 @@ void testFollowTrajectory( var xHat = estimator.updateWithTime( t, - groundTruthState - .poseMeters - .getRotation() - .plus(new Rotation2d(rand.nextGaussian() * 0.05)) - .minus(trajectory.getInitialPose().getRotation()), + new Rotation3d( + groundTruthState + .poseMeters + .getRotation() + .plus(new Rotation2d(rand.nextGaussian() * 0.05)) + .minus(trajectory.getInitialPose().getRotation())), positions); double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + groundTruthState + .poseMeters + .getTranslation() + .getDistance(xHat.getTranslation().toTranslation2d()); if (error > maxError) { maxError = error; } @@ -235,7 +242,7 @@ void testFollowTrajectory( endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y"); assertEquals( endingPose.getRotation().getRadians(), - estimator.getEstimatedPosition().getRotation().getRadians(), + estimator.getEstimatedPosition().getRotation().toRotation2d().getRadians(), 0.15, "Incorrect Final Theta"); @@ -267,13 +274,13 @@ void testSimultaneousVisionMeasurements() { var estimator = new SwerveDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, new SwerveModulePosition[] {fl, fr, bl, br}, - new Pose2d(1, 2, Rotation2d.kCW_Pi_2), - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.9, 0.9, 0.9)); + new Pose3d(1, 2, 0, new Rotation3d(0, 0, -Math.PI / 2)), + VecBuilder.fill(0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.9, 0.9, 0.9, 0.9)); - estimator.updateWithTime(0, Rotation2d.kZero, new SwerveModulePosition[] {fl, fr, bl, br}); + estimator.updateWithTime(0, Rotation3d.kZero, new SwerveModulePosition[] {fl, fr, bl, br}); var visionMeasurements = new Pose2d[] { @@ -284,7 +291,7 @@ void testSimultaneousVisionMeasurements() { for (int i = 0; i < 1000; i++) { for (var measurement : visionMeasurements) { - estimator.addVisionMeasurement(measurement, 0); + estimator.addVisionMeasurement(new Pose3d(measurement), 0); } } @@ -300,7 +307,7 @@ void testSimultaneousVisionMeasurements() { var dtheta = Math.abs( measurement.getRotation().getDegrees() - - estimator.getEstimatedPosition().getRotation().getDegrees()); + - estimator.getEstimatedPosition().getRotation().toRotation2d().getDegrees()); assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog); } @@ -317,16 +324,16 @@ void testDiscardsOldVisionMeasurements() { var estimator = new SwerveDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, new SwerveModulePosition[] { new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition() }, - Pose2d.kZero, - VecBuilder.fill(0.1, 0.1, 0.1), - VecBuilder.fill(0.9, 0.9, 0.9)); + Pose3d.kZero, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.9, 0.9, 0.9, 0.9)); double time = 0; @@ -334,7 +341,7 @@ void testDiscardsOldVisionMeasurements() { for (; time < 4; time += 0.02) { estimator.updateWithTime( time, - Rotation2d.kZero, + Rotation3d.kZero, new SwerveModulePosition[] { new SwerveModulePosition(), new SwerveModulePosition(), @@ -348,16 +355,33 @@ void testDiscardsOldVisionMeasurements() { // Apply a vision measurement made 3 seconds ago // This test passes if this does not cause a ConcurrentModificationException. estimator.addVisionMeasurement( - new Pose2d(new Translation2d(10, 10), new Rotation2d(0.1)), - 1, - VecBuilder.fill(0.1, 0.1, 0.1)); + new Pose3d(10, 10, 0, new Rotation3d(0, 0, 0.1)), 1, VecBuilder.fill(0.1, 0.1, 0.1, 0.1)); - assertEquals(odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X"); - assertEquals(odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y"); - assertEquals( - odometryPose.getRotation().getRadians(), - estimator.getEstimatedPosition().getRotation().getRadians(), - "Incorrect Final Theta"); + assertAll( + () -> + assertEquals( + odometryPose.getX(), estimator.getEstimatedPosition().getX(), "Incorrect Final X"), + () -> + assertEquals( + odometryPose.getY(), estimator.getEstimatedPosition().getY(), "Incorrect Final Y"), + () -> + assertEquals( + odometryPose.getZ(), estimator.getEstimatedPosition().getZ(), "Incorrect Final Y"), + () -> + assertEquals( + odometryPose.getRotation().getX(), + estimator.getEstimatedPosition().getRotation().getX(), + "Incorrect Final Roll"), + () -> + assertEquals( + odometryPose.getRotation().getY(), + estimator.getEstimatedPosition().getRotation().getY(), + "Incorrect Final Pitch"), + () -> + assertEquals( + odometryPose.getRotation().getZ(), + estimator.getEstimatedPosition().getRotation().getZ(), + "Incorrect Final Yaw")); } @Test @@ -371,16 +395,16 @@ void testSampleAt() { var estimator = new SwerveDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, new SwerveModulePosition[] { new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition() }, - Pose2d.kZero, - VecBuilder.fill(1, 1, 1), - VecBuilder.fill(1, 1, 1)); + Pose3d.kZero, + VecBuilder.fill(1, 1, 1, 1), + VecBuilder.fill(1, 1, 1, 1)); // Returns empty when null assertEquals(Optional.empty(), estimator.sampleAt(1)); @@ -395,35 +419,35 @@ void testSampleAt() { new SwerveModulePosition(time, Rotation2d.kZero), new SwerveModulePosition(time, Rotation2d.kZero) }; - estimator.updateWithTime(time, Rotation2d.kZero, wheelPositions); + estimator.updateWithTime(time, Rotation3d.kZero, wheelPositions); } // Sample at an added time - assertEquals(Optional.of(new Pose2d(1.02, 0, Rotation2d.kZero)), estimator.sampleAt(1.02)); + assertEquals(Optional.of(new Pose3d(1.02, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.02)); // Sample between updates (test interpolation) - assertEquals(Optional.of(new Pose2d(1.01, 0, Rotation2d.kZero)), estimator.sampleAt(1.01)); + assertEquals(Optional.of(new Pose3d(1.01, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.01)); // Sampling before the oldest value returns the oldest value - assertEquals(Optional.of(new Pose2d(1, 0, Rotation2d.kZero)), estimator.sampleAt(0.5)); + assertEquals(Optional.of(new Pose3d(1, 0, 0, Rotation3d.kZero)), estimator.sampleAt(0.5)); // Sampling after the newest value returns the newest value - assertEquals(Optional.of(new Pose2d(2, 0, Rotation2d.kZero)), estimator.sampleAt(2.5)); + assertEquals(Optional.of(new Pose3d(2, 0, 0, Rotation3d.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); + estimator.addVisionMeasurement(new Pose3d(2, 0, 0, new Rotation3d(0, 0, 1)), 2.2); // Make sure nothing changed (except the newest value) - 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)); + assertEquals(Optional.of(new Pose3d(1.02, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.02)); + assertEquals(Optional.of(new Pose3d(1.01, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.01)); + assertEquals(Optional.of(new Pose3d(1, 0, 0, Rotation3d.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, Rotation2d.kZero), 0.9); + estimator.addVisionMeasurement(new Pose3d(1, 0.2, 0, Rotation3d.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, 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)); + assertEquals(Optional.of(new Pose3d(1.02, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(1.02)); + assertEquals(Optional.of(new Pose3d(1.01, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(1.01)); + assertEquals(Optional.of(new Pose3d(1, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(0.5)); + assertEquals(Optional.of(new Pose3d(2, 0.1, 0, Rotation3d.kZero)), estimator.sampleAt(2.5)); } @Test @@ -437,39 +461,41 @@ void testReset() { var estimator = new SwerveDrivePoseEstimator3d( kinematics, - Rotation2d.kZero, + Rotation3d.kZero, new SwerveModulePosition[] { new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition() }, - Pose2d.kZero, - VecBuilder.fill(1, 1, 1), - VecBuilder.fill(1, 1, 1)); + Pose3d.kZero, + VecBuilder.fill(1, 1, 1, 1), + VecBuilder.fill(1, 1, 1, 1)); // Test reset position { var modulePosition = new SwerveModulePosition(1, Rotation2d.kZero); estimator.resetPosition( - Rotation2d.kZero, + Rotation3d.kZero, new SwerveModulePosition[] { modulePosition, modulePosition, modulePosition, modulePosition }, - new Pose2d(1, 0, Rotation2d.kZero)); + new Pose3d(1, 0, 0, Rotation3d.kZero)); } assertAll( () -> assertEquals(1, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), - () -> - assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); // Test orientation and wheel positions { var modulePosition = new SwerveModulePosition(2, Rotation2d.kZero); estimator.update( - Rotation2d.kZero, + Rotation3d.kZero, new SwerveModulePosition[] { modulePosition, modulePosition, modulePosition, modulePosition }); @@ -478,26 +504,29 @@ void testReset() { assertAll( () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), - () -> - assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); // Test reset rotation - estimator.resetRotation(Rotation2d.kCCW_Pi_2); + estimator.resetRotation(new Rotation3d(Rotation2d.kCCW_Pi_2)); assertAll( () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), () -> assertEquals( - Math.PI / 2, - estimator.getEstimatedPosition().getRotation().getRadians(), - kEpsilon)); + Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); // Test orientation { var modulePosition = new SwerveModulePosition(3, Rotation2d.kZero); estimator.update( - Rotation2d.kZero, + Rotation3d.kZero, new SwerveModulePosition[] { modulePosition, modulePosition, modulePosition, modulePosition }); @@ -506,31 +535,35 @@ void testReset() { assertAll( () -> assertEquals(2, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(1, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), () -> assertEquals( - Math.PI / 2, - estimator.getEstimatedPosition().getRotation().getRadians(), - kEpsilon)); + Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); // Test reset translation - estimator.resetTranslation(new Translation2d(-1, -1)); + estimator.resetTranslation(new Translation3d(-1, -1, -1)); assertAll( () -> assertEquals(-1, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(-1, estimator.getEstimatedPosition().getY(), kEpsilon), + () -> assertEquals(-1, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), () -> assertEquals( - Math.PI / 2, - estimator.getEstimatedPosition().getRotation().getRadians(), - kEpsilon)); + Math.PI / 2, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); // Test reset pose - estimator.resetPose(Pose2d.kZero); + estimator.resetPose(Pose3d.kZero); assertAll( () -> assertEquals(0, estimator.getEstimatedPosition().getX(), kEpsilon), () -> assertEquals(0, estimator.getEstimatedPosition().getY(), kEpsilon), - () -> - assertEquals(0, estimator.getEstimatedPosition().getRotation().getRadians(), kEpsilon)); + () -> assertEquals(0, estimator.getEstimatedPosition().getZ(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getX(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getY(), kEpsilon), + () -> assertEquals(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java index 3dae1c18deb..166a615ca9d 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java @@ -7,35 +7,42 @@ import static org.junit.jupiter.api.Assertions.assertAll; import static org.junit.jupiter.api.Assertions.assertEquals; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.util.Units; import org.junit.jupiter.api.Test; class DifferentialDriveOdometry3dTest { private static final double kEpsilon = 1E-9; private final DifferentialDriveOdometry3d m_odometry = - new DifferentialDriveOdometry3d(Rotation2d.kZero, 0, 0); + new DifferentialDriveOdometry3d(Rotation3d.kZero, 0, 0); @Test void testInitialize() { DifferentialDriveOdometry3d odometry = new DifferentialDriveOdometry3d( - Rotation2d.kZero, 0, 0, new Pose2d(1, 2, Rotation2d.fromDegrees(45))); + Rotation3d.kZero, + 0, + 0, + new Pose3d(1, 2, 0, new Rotation3d(0, 0, Units.degreesToRadians(45)))); var pose = odometry.getPoseMeters(); assertAll( () -> assertEquals(pose.getX(), 1.0, kEpsilon), () -> assertEquals(pose.getY(), 2.0, kEpsilon), - () -> assertEquals(pose.getRotation().getDegrees(), 45.0, kEpsilon)); + () -> assertEquals(pose.getZ(), 0.0, kEpsilon), + () -> assertEquals(pose.getRotation().toRotation2d().getDegrees(), 45.0, kEpsilon)); } @Test void testOdometryWithEncoderDistances() { - m_odometry.resetPosition(Rotation2d.fromDegrees(45), 0, 0, Pose2d.kZero); - var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI); + m_odometry.resetPosition(new Rotation3d(0, 0, Units.degreesToRadians(45)), 0, 0, Pose3d.kZero); + var pose = + m_odometry.update(new Rotation3d(0, 0, Units.degreesToRadians(135.0)), 0.0, 5 * Math.PI); assertAll( () -> assertEquals(pose.getX(), 5.0, kEpsilon), () -> assertEquals(pose.getY(), 5.0, kEpsilon), - () -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon)); + () -> assertEquals(pose.getZ(), 0.0, kEpsilon), + () -> assertEquals(pose.getRotation().toRotation2d().getDegrees(), 90.0, kEpsilon)); } } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java index 8ed5967417f..d417e2a120a 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java @@ -8,10 +8,14 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.math.util.Units; import java.util.List; import java.util.Random; import org.junit.jupiter.api.Test; @@ -28,48 +32,54 @@ class MecanumDriveOdometry3dTest { private final MecanumDriveWheelPositions zero = new MecanumDriveWheelPositions(); private final MecanumDriveOdometry3d m_odometry = - new MecanumDriveOdometry3d(m_kinematics, Rotation2d.kZero, zero); + new MecanumDriveOdometry3d(m_kinematics, Rotation3d.kZero, zero); @Test void testInitialize() { MecanumDriveOdometry3d odometry = new MecanumDriveOdometry3d( - m_kinematics, Rotation2d.kZero, zero, new Pose2d(1, 2, Rotation2d.fromDegrees(45))); + m_kinematics, + Rotation3d.kZero, + zero, + new Pose3d(1, 2, 0, new Rotation3d(0, 0, Units.degreesToRadians(45)))); var pose = odometry.getPoseMeters(); assertAll( () -> assertEquals(pose.getX(), 1.0, 1e-9), () -> assertEquals(pose.getY(), 2.0, 1e-9), - () -> assertEquals(pose.getRotation().getDegrees(), 45.0, 1e-9)); + () -> assertEquals(pose.getZ(), 0.0, 1e-9), + () -> assertEquals(pose.getRotation().toRotation2d().getDegrees(), 45.0, 1e-9)); } @Test void testMultipleConsecutiveUpdates() { var wheelPositions = new MecanumDriveWheelPositions(3.536, 3.536, 3.536, 3.536); - m_odometry.resetPosition(Rotation2d.kZero, wheelPositions, Pose2d.kZero); + m_odometry.resetPosition(Rotation3d.kZero, wheelPositions, Pose3d.kZero); - m_odometry.update(Rotation2d.kZero, wheelPositions); - var secondPose = m_odometry.update(Rotation2d.kZero, wheelPositions); + m_odometry.update(Rotation3d.kZero, wheelPositions); + var secondPose = m_odometry.update(Rotation3d.kZero, wheelPositions); assertAll( () -> assertEquals(secondPose.getX(), 0.0, 0.01), () -> assertEquals(secondPose.getY(), 0.0, 0.01), - () -> assertEquals(secondPose.getRotation().getDegrees(), 0.0, 0.01)); + () -> assertEquals(secondPose.getZ(), 0.0, 0.01), + () -> assertEquals(secondPose.getRotation().toRotation2d().getDegrees(), 0.0, 0.01)); } @Test 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(Rotation2d.kZero, new MecanumDriveWheelPositions(), Pose2d.kZero); + m_odometry.resetPosition(Rotation3d.kZero, new MecanumDriveWheelPositions(), Pose3d.kZero); - m_odometry.update(Rotation2d.kZero, new MecanumDriveWheelPositions()); - var pose = m_odometry.update(Rotation2d.kZero, wheelPositions); + m_odometry.update(Rotation3d.kZero, new MecanumDriveWheelPositions()); + var pose = m_odometry.update(Rotation3d.kZero, wheelPositions); assertAll( () -> assertEquals(0.3536, pose.getX(), 0.01), () -> assertEquals(0.0, pose.getY(), 0.01), - () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)); + () -> assertEquals(0.0, pose.getZ(), 0.01), + () -> assertEquals(0.0, pose.getRotation().toRotation2d().getDegrees(), 0.01)); } @Test @@ -77,23 +87,24 @@ void test90degreeTurn() { // This is a 90 degree turn about the point between front left and rear left wheels // fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946 final var wheelPositions = new MecanumDriveWheelPositions(-13.328, 39.986, -13.329, 39.986); - m_odometry.resetPosition(Rotation2d.kZero, new MecanumDriveWheelPositions(), Pose2d.kZero); + m_odometry.resetPosition(Rotation3d.kZero, new MecanumDriveWheelPositions(), Pose3d.kZero); - m_odometry.update(Rotation2d.kZero, new MecanumDriveWheelPositions()); - final var pose = m_odometry.update(Rotation2d.kCCW_Pi_2, wheelPositions); + m_odometry.update(Rotation3d.kZero, new MecanumDriveWheelPositions()); + final var pose = m_odometry.update(new Rotation3d(0, 0, Math.PI / 2), wheelPositions); assertAll( () -> assertEquals(8.4855, pose.getX(), 0.01), () -> assertEquals(8.4855, pose.getY(), 0.01), - () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)); + () -> assertEquals(0.0, pose.getZ(), 0.01), + () -> assertEquals(90.0, pose.getRotation().toRotation2d().getDegrees(), 0.01)); } @Test void testGyroAngleReset() { - var gyro = Rotation2d.kCCW_Pi_2; - var fieldAngle = Rotation2d.kZero; + var gyro = new Rotation3d(0, 0, Math.PI / 2); + var fieldAngle = Rotation3d.kZero; m_odometry.resetPosition( - gyro, new MecanumDriveWheelPositions(), new Pose2d(Translation2d.kZero, fieldAngle)); + gyro, new MecanumDriveWheelPositions(), new Pose3d(Translation3d.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); @@ -101,7 +112,8 @@ void testGyroAngleReset() { assertAll( () -> assertEquals(3.536, pose.getX(), 0.1), () -> assertEquals(0.0, pose.getY(), 0.1), - () -> assertEquals(0.0, pose.getRotation().getRadians(), 0.1)); + () -> assertEquals(0.0, pose.getZ(), 0.1), + () -> assertEquals(0.0, pose.getRotation().toRotation2d().getRadians(), 0.1)); } @Test @@ -114,7 +126,7 @@ void testAccuracyFacingTrajectory() { var wheelPositions = new MecanumDriveWheelPositions(); var odometry = - new MecanumDriveOdometry3d(kinematics, Rotation2d.kZero, wheelPositions, Pose2d.kZero); + new MecanumDriveOdometry3d(kinematics, Rotation3d.kZero, wheelPositions, Pose3d.kZero); var trajectory = TrajectoryGenerator.generateTrajectory( @@ -165,16 +177,20 @@ void testAccuracyFacingTrajectory() { var xHat = odometry.update( - groundTruthState - .poseMeters - .getRotation() - .plus(new Rotation2d(rand.nextGaussian() * 0.05)), + new Rotation3d( + groundTruthState + .poseMeters + .getRotation() + .plus(new Rotation2d(rand.nextGaussian() * 0.05))), wheelPositions); odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation()); double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + groundTruthState + .poseMeters + .getTranslation() + .getDistance(xHat.getTranslation().toTranslation2d()); if (error > maxError) { maxError = error; } @@ -203,7 +219,7 @@ void testAccuracyFacingXAxis() { var wheelPositions = new MecanumDriveWheelPositions(); var odometry = - new MecanumDriveOdometry3d(kinematics, Rotation2d.kZero, wheelPositions, Pose2d.kZero); + new MecanumDriveOdometry3d(kinematics, Rotation3d.kZero, wheelPositions, Pose3d.kZero); var trajectory = TrajectoryGenerator.generateTrajectory( @@ -253,12 +269,15 @@ void testAccuracyFacingXAxis() { var lastPose = odometry.getPoseMeters(); - var xHat = odometry.update(new Rotation2d(rand.nextGaussian() * 0.05), wheelPositions); + var xHat = odometry.update(new Rotation3d(0, 0, rand.nextGaussian() * 0.05), wheelPositions); odometryDistanceTravelled += lastPose.getTranslation().getDistance(xHat.getTranslation()); double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + groundTruthState + .poseMeters + .getTranslation() + .getDistance(xHat.getTranslation().toTranslation2d()); if (error > maxError) { maxError = error; } diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java index 9250fa6a306..dfc446fceea 100644 --- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java @@ -8,10 +8,14 @@ import static org.junit.jupiter.api.Assertions.assertEquals; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.trajectory.TrajectoryConfig; import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import edu.wpi.first.math.util.Units; import java.util.List; import java.util.Random; import org.junit.jupiter.api.Test; @@ -29,21 +33,22 @@ class SwerveDriveOdometry3dTest { private final SwerveDriveOdometry3d m_odometry = new SwerveDriveOdometry3d( - m_kinematics, Rotation2d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero}); + m_kinematics, Rotation3d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero}); @Test void testInitialize() { SwerveDriveOdometry3d odometry = new SwerveDriveOdometry3d( m_kinematics, - Rotation2d.kZero, + Rotation3d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero}, - new Pose2d(1, 2, Rotation2d.fromDegrees(45))); + new Pose3d(1, 2, 0, new Rotation3d(0, 0, Units.degreesToRadians(45)))); var pose = odometry.getPoseMeters(); assertAll( () -> assertEquals(pose.getX(), 1.0, 1e-9), () -> assertEquals(pose.getY(), 2.0, 1e-9), - () -> assertEquals(pose.getRotation().getDegrees(), 45.0, 1e-9)); + () -> assertEquals(pose.getZ(), 0.0, 1e-9), + () -> assertEquals(pose.getRotation().toRotation2d().getDegrees(), 45.0, 1e-9)); } @Test @@ -57,19 +62,20 @@ void testTwoIterations() { }; m_odometry.update( - Rotation2d.kZero, + Rotation3d.kZero, new SwerveModulePosition[] { new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition(), new SwerveModulePosition() }); - var pose = m_odometry.update(Rotation2d.kZero, wheelDeltas); + var pose = m_odometry.update(Rotation3d.kZero, wheelDeltas); assertAll( () -> assertEquals(5.0 / 10.0, pose.getX(), 0.01), () -> assertEquals(0, pose.getY(), 0.01), - () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)); + () -> assertEquals(0, pose.getZ(), 0.01), + () -> assertEquals(0.0, pose.getRotation().toRotation2d().getDegrees(), 0.01)); } @Test @@ -88,23 +94,24 @@ void test90degreeTurn() { }; final var zero = new SwerveModulePosition(); - m_odometry.update(Rotation2d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero}); - final var pose = m_odometry.update(Rotation2d.kCCW_Pi_2, wheelDeltas); + m_odometry.update(Rotation3d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero}); + final var pose = m_odometry.update(new Rotation3d(0, 0, Math.PI / 2), wheelDeltas); assertAll( () -> assertEquals(12.0, pose.getX(), 0.01), () -> assertEquals(12.0, pose.getY(), 0.01), - () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)); + () -> assertEquals(0.0, pose.getZ(), 0.01), + () -> assertEquals(90.0, pose.getRotation().toRotation2d().getDegrees(), 0.01)); } @Test void testGyroAngleReset() { - var gyro = Rotation2d.kCCW_Pi_2; - var fieldAngle = Rotation2d.kZero; + var gyro = new Rotation3d(0, 0, Math.PI / 2); + var fieldAngle = Rotation3d.kZero; m_odometry.resetPosition( gyro, new SwerveModulePosition[] {zero, zero, zero, zero}, - new Pose2d(Translation2d.kZero, fieldAngle)); + new Pose3d(Translation3d.kZero, fieldAngle)); var delta = new SwerveModulePosition(); m_odometry.update(gyro, new SwerveModulePosition[] {delta, delta, delta, delta}); delta = new SwerveModulePosition(1.0, Rotation2d.kZero); @@ -113,7 +120,8 @@ void testGyroAngleReset() { assertAll( () -> assertEquals(1.0, pose.getX(), 0.1), () -> assertEquals(0.00, pose.getY(), 0.1), - () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)); + () -> assertEquals(0.00, pose.getZ(), 0.1), + () -> assertEquals(0.00, pose.getRotation().toRotation2d().getRadians(), 0.1)); } @Test @@ -126,7 +134,7 @@ void testAccuracyFacingTrajectory() { new Translation2d(-1, 1)); var odometry = new SwerveDriveOdometry3d( - kinematics, Rotation2d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero}); + kinematics, Rotation3d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero}); SwerveModulePosition fl = new SwerveModulePosition(); SwerveModulePosition fr = new SwerveModulePosition(); @@ -177,14 +185,18 @@ void testAccuracyFacingTrajectory() { var xHat = odometry.update( - groundTruthState - .poseMeters - .getRotation() - .plus(new Rotation2d(rand.nextGaussian() * 0.05)), + new Rotation3d( + groundTruthState + .poseMeters + .getRotation() + .plus(new Rotation2d(rand.nextGaussian() * 0.05))), new SwerveModulePosition[] {fl, fr, bl, br}); double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + groundTruthState + .poseMeters + .getTranslation() + .getDistance(xHat.getTranslation().toTranslation2d()); if (error > maxError) { maxError = error; } @@ -195,9 +207,10 @@ void testAccuracyFacingTrajectory() { assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X"); assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y"); + assertEquals(0.0, odometry.getPoseMeters().getZ(), 1e-1, "Incorrect Final Y"); assertEquals( Math.PI / 4, - odometry.getPoseMeters().getRotation().getRadians(), + odometry.getPoseMeters().getRotation().toRotation2d().getRadians(), 10 * Math.PI / 180, "Incorrect Final Theta"); @@ -216,7 +229,7 @@ void testAccuracyFacingXAxis() { new Translation2d(-1, 1)); var odometry = new SwerveDriveOdometry3d( - kinematics, Rotation2d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero}); + kinematics, Rotation3d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero}); SwerveModulePosition fl = new SwerveModulePosition(); SwerveModulePosition fr = new SwerveModulePosition(); @@ -263,11 +276,14 @@ void testAccuracyFacingXAxis() { var xHat = odometry.update( - new Rotation2d(rand.nextGaussian() * 0.05), + new Rotation3d(0, 0, rand.nextGaussian() * 0.05), new SwerveModulePosition[] {fl, fr, bl, br}); double error = - groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation()); + groundTruthState + .poseMeters + .getTranslation() + .getDistance(xHat.getTranslation().toTranslation2d()); if (error > maxError) { maxError = error; } @@ -278,9 +294,10 @@ void testAccuracyFacingXAxis() { assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X"); assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y"); + assertEquals(0.0, odometry.getPoseMeters().getZ(), 1e-1, "Incorrect Final Y"); assertEquals( 0.0, - odometry.getPoseMeters().getRotation().getRadians(), + odometry.getPoseMeters().getRotation().toRotation2d().getRadians(), 10 * Math.PI / 180, "Incorrect Final Theta"); diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp index 116743d32ec..e43f662fddf 100644 --- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp @@ -36,8 +36,8 @@ void testFollowTrajectory( units::meter_t leftDistance = 0_m; units::meter_t rightDistance = 0_m; - estimator.ResetPosition(frc::Rotation2d{}, leftDistance, rightDistance, - startingPose); + estimator.ResetPosition(frc::Rotation3d{}, leftDistance, rightDistance, + frc::Pose3d{startingPose}); std::default_random_engine generator; std::normal_distribution distribution(0.0, 1.0); @@ -77,7 +77,8 @@ void testFollowTrajectory( if (!visionPoses.empty() && visionPoses.front().first + kVisionUpdateDelay < t) { auto visionEntry = visionPoses.front(); - estimator.AddVisionMeasurement(visionEntry.second, visionEntry.first); + estimator.AddVisionMeasurement(frc::Pose3d{visionEntry.second}, + visionEntry.first); visionPoses.erase(visionPoses.begin()); visionLog.push_back({t, visionEntry.first, visionEntry.second}); } @@ -91,22 +92,22 @@ void testFollowTrajectory( auto xhat = estimator.UpdateWithTime( t, - groundTruthState.pose.Rotation() + - frc::Rotation2d{distribution(generator) * 0.05_rad} - - trajectory.InitialPose().Rotation(), + frc::Rotation3d{groundTruthState.pose.Rotation() + + frc::Rotation2d{distribution(generator) * 0.05_rad} - + trajectory.InitialPose().Rotation()}, leftDistance, rightDistance); if (debug) { wpi::print( "{}, {}, {}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(), - xhat.Y().value(), xhat.Rotation().Radians().value(), + xhat.Y().value(), xhat.Rotation().ToRotation2d().Radians().value(), groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(), groundTruthState.pose.Rotation().Radians().value(), leftDistance.value(), rightDistance.value()); } double error = groundTruthState.pose.Translation() - .Distance(xhat.Translation()) + .Distance(xhat.Translation().ToTranslation2d()) .value(); if (error > maxError) { @@ -137,7 +138,11 @@ void testFollowTrajectory( EXPECT_NEAR(endingPose.Y().value(), estimator.GetEstimatedPosition().Y().value(), 0.08); EXPECT_NEAR(endingPose.Rotation().Radians().value(), - estimator.GetEstimatedPosition().Rotation().Radians().value(), + estimator.GetEstimatedPosition() + .Rotation() + .ToRotation2d() + .Radians() + .value(), 0.15); if (checkError) { @@ -150,9 +155,13 @@ void testFollowTrajectory( TEST(DifferentialDrivePoseEstimator3dTest, Accuracy) { frc::DifferentialDriveKinematics kinematics{1.0_m}; - frc::DifferentialDrivePoseEstimator3d estimator{ - kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{}, - {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}}; + frc::DifferentialDrivePoseEstimator3d estimator{kinematics, + frc::Rotation3d{}, + 0_m, + 0_m, + frc::Pose3d{}, + {0.02, 0.02, 0.02, 0.01}, + {0.1, 0.1, 0.1, 0.1}}; frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg}, @@ -175,9 +184,13 @@ TEST(DifferentialDrivePoseEstimator3dTest, Accuracy) { TEST(DifferentialDrivePoseEstimator3dTest, BadInitialPose) { frc::DifferentialDriveKinematics kinematics{1.0_m}; - frc::DifferentialDrivePoseEstimator3d estimator{ - kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{}, - {0.02, 0.02, 0.01}, {0.1, 0.1, 0.1}}; + frc::DifferentialDrivePoseEstimator3d estimator{kinematics, + frc::Rotation3d{}, + 0_m, + 0_m, + frc::Pose3d{}, + {0.02, 0.02, 0.02, 0.01}, + {0.1, 0.1, 0.1, 0.1}}; frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg}, @@ -222,29 +235,31 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) { frc::DifferentialDrivePoseEstimator3d estimator{ kinematics, - frc::Rotation2d{}, + frc::Rotation3d{}, 0_m, 0_m, - frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}}, - {0.02, 0.02, 0.01}, - {0.1, 0.1, 0.1}}; + frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 270_deg}}, + {0.02, 0.02, 0.02, 0.01}, + {0.1, 0.1, 0.1, 0.1}}; - estimator.UpdateWithTime(0_s, frc::Rotation2d{}, 0_m, 0_m); + estimator.UpdateWithTime(0_s, frc::Rotation3d{}, 0_m, 0_m); for (int i = 0; i < 1000; i++) { estimator.AddVisionMeasurement( - frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s); + frc::Pose3d{0_m, 0_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s); estimator.AddVisionMeasurement( - frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s); + frc::Pose3d{3_m, 1_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s); estimator.AddVisionMeasurement( - frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s); + frc::Pose3d{2_m, 4_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 180_deg}}, + 0_s); } { auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m); auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m); auto dtheta = units::math::abs( - estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg); + estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() - + 0_deg); EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); } @@ -253,7 +268,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) { auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m); auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m); auto dtheta = units::math::abs( - estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg); + estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() - + 90_deg); EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); } @@ -262,7 +278,8 @@ TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) { auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m); auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m); auto dtheta = units::math::abs( - estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg); + estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() - + 180_deg); EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); } @@ -272,35 +289,40 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) { frc::DifferentialDriveKinematics kinematics{1_m}; frc::DifferentialDrivePoseEstimator3d estimator{ - kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{}, - {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; + kinematics, frc::Rotation3d{}, 0_m, 0_m, frc::Pose3d{}, + {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}}; // Add enough measurements to fill up the buffer for (auto time = 0_s; time < 4_s; time += 20_ms) { - estimator.UpdateWithTime(time, frc::Rotation2d{}, 0_m, 0_m); + estimator.UpdateWithTime(time, frc::Rotation3d{}, 0_m, 0_m); } auto odometryPose = estimator.GetEstimatedPosition(); // Apply a vision measurement from 3 seconds ago estimator.AddVisionMeasurement( - frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}}, - 1_s, {0.1, 0.1, 0.1}); + frc::Pose3d{10_m, 10_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s, + {0.1, 0.1, 0.1, 0.1}); EXPECT_NEAR(odometryPose.X().value(), estimator.GetEstimatedPosition().X().value(), 1e-6); EXPECT_NEAR(odometryPose.Y().value(), estimator.GetEstimatedPosition().Y().value(), 1e-6); - EXPECT_NEAR(odometryPose.Rotation().Radians().value(), - estimator.GetEstimatedPosition().Rotation().Radians().value(), - 1e-6); + EXPECT_NEAR(odometryPose.Z().value(), + estimator.GetEstimatedPosition().Z().value(), 1e-6); + EXPECT_NEAR(odometryPose.Rotation().X().value(), + estimator.GetEstimatedPosition().Rotation().X().value(), 1e-6); + EXPECT_NEAR(odometryPose.Rotation().Y().value(), + estimator.GetEstimatedPosition().Rotation().Y().value(), 1e-6); + EXPECT_NEAR(odometryPose.Rotation().Z().value(), + estimator.GetEstimatedPosition().Rotation().Z().value(), 1e-6); } TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) { frc::DifferentialDriveKinematics kinematics{1_m}; frc::DifferentialDrivePoseEstimator3d estimator{ - kinematics, frc::Rotation2d{}, 0_m, 0_m, frc::Pose2d{}, - {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}}; + kinematics, frc::Rotation3d{}, 0_m, 0_m, frc::Pose3d{}, + {1.0, 1.0, 1.0, 1.0}, {1.0, 1.0, 1.0, 1.0}}; // Returns empty when null EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s)); @@ -309,49 +331,49 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestSampleAt) { // 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(units::second_t{time}, frc::Rotation2d{}, + estimator.UpdateWithTime(units::second_t{time}, frc::Rotation3d{}, units::meter_t{time}, units::meter_t{time}); } // Sample at an added time - EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.02_s)); // Sample between updates (test interpolation) - EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.01_s)); // Sampling before the oldest value returns the oldest value - EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(0.5_s)); // Sampling after the newest value returns the newest value - EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(2.5_s)); // Add a vision measurement after the odometry measurements (while keeping all // of the old odometry measurements) - estimator.AddVisionMeasurement(frc::Pose2d{2_m, 0_m, frc::Rotation2d{1_rad}}, - 2.2_s); + estimator.AddVisionMeasurement( + frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s); // Make sure nothing changed (except the newest value) - EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.02_s)); - EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.01_s)); - EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(0.5_s)); // Add a vision measurement before the odometry measurements that's still in // the buffer - estimator.AddVisionMeasurement(frc::Pose2d{1_m, 0.2_m, frc::Rotation2d{}}, - 0.9_s); + estimator.AddVisionMeasurement( + frc::Pose3d{1_m, 0.2_m, 0_m, frc::Rotation3d{}}, 0.9_s); // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2) - EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0.1_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.02_s)); - EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0.1_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.01_s)); - EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0.1_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(0.5_s)); - EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0.1_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(2.5_s)); } @@ -359,68 +381,82 @@ TEST(DifferentialDrivePoseEstimator3dTest, TestReset) { frc::DifferentialDriveKinematics kinematics{1_m}; frc::DifferentialDrivePoseEstimator3d estimator{ kinematics, - frc::Rotation2d{}, + frc::Rotation3d{}, 0_m, 0_m, - frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}}, - {1.0, 1.0, 1.0}, - {1.0, 1.0, 1.0}}; + frc::Pose3d{-1_m, -1_m, -1_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}}, + {1.0, 1.0, 1.0, 1.0}, + {1.0, 1.0, 1.0, 1.0}}; // Test initial pose EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - 1, estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Rotation().Z().value()); // Test reset position - estimator.ResetPosition(frc::Rotation2d{}, 1_m, 1_m, - frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}); + estimator.ResetPosition(frc::Rotation3d{}, 1_m, 1_m, + frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}); EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - 0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value()); // Test orientation and wheel positions - estimator.Update(frc::Rotation2d{}, 2_m, 2_m); + estimator.Update(frc::Rotation3d{}, 2_m, 2_m); EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - 0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value()); // Test reset rotation - estimator.ResetRotation(frc::Rotation2d{90_deg}); + estimator.ResetRotation(frc::Rotation3d{0_deg, 0_deg, 90_deg}); EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(std::numbers::pi / 2, + estimator.GetEstimatedPosition().Rotation().Z().value()); // Test orientation - estimator.Update(frc::Rotation2d{}, 3_m, 3_m); + estimator.Update(frc::Rotation3d{}, 3_m, 3_m); EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(std::numbers::pi / 2, + estimator.GetEstimatedPosition().Rotation().Z().value()); // Test reset translation - estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m}); + estimator.ResetTranslation(frc::Translation3d{-1_m, -1_m, -1_m}); EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(std::numbers::pi / 2, + estimator.GetEstimatedPosition().Rotation().Z().value()); // Test reset pose - estimator.ResetPose(frc::Pose2d{}); + estimator.ResetPose(frc::Pose3d{}); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - 0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value()); } diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp index 6a0e5119579..7ba98c9f998 100644 --- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp @@ -29,7 +29,8 @@ void testFollowTrajectory( const bool debug) { frc::MecanumDriveWheelPositions wheelPositions{}; - estimator.ResetPosition(frc::Rotation2d{}, wheelPositions, startingPose); + estimator.ResetPosition(frc::Rotation3d{}, wheelPositions, + frc::Pose3d{startingPose}); std::default_random_engine generator; std::normal_distribution distribution(0.0, 1.0); @@ -67,7 +68,8 @@ void testFollowTrajectory( if (!visionPoses.empty() && visionPoses.front().first + kVisionUpdateDelay < t) { auto visionEntry = visionPoses.front(); - estimator.AddVisionMeasurement(visionEntry.second, visionEntry.first); + estimator.AddVisionMeasurement(frc::Pose3d{visionEntry.second}, + visionEntry.first); visionPoses.erase(visionPoses.begin()); visionLog.push_back({t, visionEntry.first, visionEntry.second}); } @@ -83,21 +85,21 @@ void testFollowTrajectory( auto xhat = estimator.UpdateWithTime( t, - groundTruthState.pose.Rotation() + - frc::Rotation2d{distribution(generator) * 0.05_rad} - - trajectory.InitialPose().Rotation(), + frc::Rotation3d{groundTruthState.pose.Rotation() + + frc::Rotation2d{distribution(generator) * 0.05_rad} - + trajectory.InitialPose().Rotation()}, wheelPositions); if (debug) { - wpi::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(), - xhat.Y().value(), xhat.Rotation().Radians().value(), - groundTruthState.pose.X().value(), - groundTruthState.pose.Y().value(), - groundTruthState.pose.Rotation().Radians().value()); + wpi::print( + "{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(), + xhat.Y().value(), xhat.Rotation().ToRotation2d().Radians().value(), + groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(), + groundTruthState.pose.Rotation().Radians().value()); } double error = groundTruthState.pose.Translation() - .Distance(xhat.Translation()) + .Distance(xhat.Translation().ToTranslation2d()) .value(); if (error > maxError) { @@ -128,7 +130,11 @@ void testFollowTrajectory( EXPECT_NEAR(endingPose.Y().value(), estimator.GetEstimatedPosition().Y().value(), 0.08); EXPECT_NEAR(endingPose.Rotation().Radians().value(), - estimator.GetEstimatedPosition().Rotation().Radians().value(), + estimator.GetEstimatedPosition() + .Rotation() + .ToRotation2d() + .Radians() + .value(), 0.15); if (checkError) { @@ -146,8 +152,8 @@ TEST(MecanumDrivePoseEstimator3dTest, AccuracyFacingTrajectory) { frc::MecanumDriveWheelPositions wheelPositions; frc::MecanumDrivePoseEstimator3d estimator{ - kinematics, frc::Rotation2d{}, wheelPositions, - frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; + kinematics, frc::Rotation3d{}, wheelPositions, + frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}}; frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg}, @@ -175,8 +181,8 @@ TEST(MecanumDrivePoseEstimator3dTest, BadInitialPose) { frc::MecanumDriveWheelPositions wheelPositions; frc::MecanumDrivePoseEstimator3d estimator{ - kinematics, frc::Rotation2d{}, wheelPositions, - frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}}; + kinematics, frc::Rotation3d{}, wheelPositions, + frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.1}}; frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg}, @@ -224,26 +230,31 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) { frc::MecanumDriveWheelPositions wheelPositions; frc::MecanumDrivePoseEstimator3d estimator{ - kinematics, frc::Rotation2d{}, - wheelPositions, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}}, - {0.1, 0.1, 0.1}, {0.45, 0.45, 0.1}}; + kinematics, + frc::Rotation3d{}, + wheelPositions, + frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 270_deg}}, + {0.1, 0.1, 0.1, 0.1}, + {0.45, 0.45, 0.45, 0.1}}; - estimator.UpdateWithTime(0_s, frc::Rotation2d{}, wheelPositions); + estimator.UpdateWithTime(0_s, frc::Rotation3d{}, wheelPositions); for (int i = 0; i < 1000; i++) { estimator.AddVisionMeasurement( - frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s); + frc::Pose3d{0_m, 0_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s); estimator.AddVisionMeasurement( - frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s); + frc::Pose3d{3_m, 1_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s); estimator.AddVisionMeasurement( - frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s); + frc::Pose3d{2_m, 4_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 180_deg}}, + 0_s); } { auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m); auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m); auto dtheta = units::math::abs( - estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg); + estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() - + 0_deg); EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); } @@ -252,7 +263,8 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) { auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m); auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m); auto dtheta = units::math::abs( - estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg); + estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() - + 90_deg); EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); } @@ -261,7 +273,8 @@ TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) { auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m); auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m); auto dtheta = units::math::abs( - estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg); + estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() - + 180_deg); EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); } @@ -273,12 +286,12 @@ TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) { frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; frc::MecanumDrivePoseEstimator3d estimator{ - kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{}, - frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; + kinematics, frc::Rotation3d{}, frc::MecanumDriveWheelPositions{}, + frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}}; // Add enough measurements to fill up the buffer for (auto time = 0_s; time < 4_s; time += 20_ms) { - estimator.UpdateWithTime(time, frc::Rotation2d{}, + estimator.UpdateWithTime(time, frc::Rotation3d{}, frc::MecanumDriveWheelPositions{}); } @@ -286,16 +299,21 @@ TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) { // Apply a vision measurement from 3 seconds ago estimator.AddVisionMeasurement( - frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}}, - 1_s, {0.1, 0.1, 0.1}); + frc::Pose3d{10_m, 10_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s, + {0.1, 0.1, 0.1, 0.1}); EXPECT_NEAR(odometryPose.X().value(), estimator.GetEstimatedPosition().X().value(), 1e-6); EXPECT_NEAR(odometryPose.Y().value(), estimator.GetEstimatedPosition().Y().value(), 1e-6); - EXPECT_NEAR(odometryPose.Rotation().Radians().value(), - estimator.GetEstimatedPosition().Rotation().Radians().value(), - 1e-6); + EXPECT_NEAR(odometryPose.Z().value(), + estimator.GetEstimatedPosition().Z().value(), 1e-6); + EXPECT_NEAR(odometryPose.Rotation().X().value(), + estimator.GetEstimatedPosition().Rotation().X().value(), 1e-6); + EXPECT_NEAR(odometryPose.Rotation().Y().value(), + estimator.GetEstimatedPosition().Rotation().Y().value(), 1e-6); + EXPECT_NEAR(odometryPose.Rotation().Z().value(), + estimator.GetEstimatedPosition().Rotation().Z().value(), 1e-6); } TEST(MecanumDrivePoseEstimator3dTest, TestSampleAt) { @@ -303,8 +321,8 @@ TEST(MecanumDrivePoseEstimator3dTest, TestSampleAt) { frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m}, frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; frc::MecanumDrivePoseEstimator3d estimator{ - kinematics, frc::Rotation2d{}, frc::MecanumDriveWheelPositions{}, - frc::Pose2d{}, {1.0, 1.0, 1.0}, {1.0, 1.0, 1.0}}; + kinematics, frc::Rotation3d{}, frc::MecanumDriveWheelPositions{}, + frc::Pose3d{}, {1.0, 1.0, 1.0, 1.0}, {1.0, 1.0, 1.0, 1.0}}; // Returns empty when null EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s)); @@ -316,49 +334,49 @@ TEST(MecanumDrivePoseEstimator3dTest, TestSampleAt) { frc::MecanumDriveWheelPositions wheelPositions{ units::meter_t{time}, units::meter_t{time}, units::meter_t{time}, units::meter_t{time}}; - estimator.UpdateWithTime(units::second_t{time}, frc::Rotation2d{}, + estimator.UpdateWithTime(units::second_t{time}, frc::Rotation3d{}, wheelPositions); } // Sample at an added time - EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.02_s)); // Sample between updates (test interpolation) - EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.01_s)); // Sampling before the oldest value returns the oldest value - EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(0.5_s)); // Sampling after the newest value returns the newest value - EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(2.5_s)); // Add a vision measurement after the odometry measurements (while keeping all // of the old odometry measurements) - estimator.AddVisionMeasurement(frc::Pose2d{2_m, 0_m, frc::Rotation2d{1_rad}}, - 2.2_s); + estimator.AddVisionMeasurement( + frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s); // Make sure nothing changed (except the newest value) - EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.02_s)); - EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.01_s)); - EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(0.5_s)); // Add a vision measurement before the odometry measurements that's still in // the buffer - estimator.AddVisionMeasurement(frc::Pose2d{1_m, 0.2_m, frc::Rotation2d{}}, - 0.9_s); + estimator.AddVisionMeasurement( + frc::Pose3d{1_m, 0.2_m, 0_m, frc::Rotation3d{}}, 0.9_s); // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2) - EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0.1_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.02_s)); - EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0.1_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.01_s)); - EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0.1_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(0.5_s)); - EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0.1_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(2.5_s)); } @@ -368,67 +386,81 @@ TEST(MecanumDrivePoseEstimator3dTest, TestReset) { frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; frc::MecanumDrivePoseEstimator3d estimator{ kinematics, - frc::Rotation2d{}, + frc::Rotation3d{}, frc::MecanumDriveWheelPositions{}, - frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}}, - {1.0, 1.0, 1.0}, - {1.0, 1.0, 1.0}}; + frc::Pose3d{-1_m, -1_m, -1_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}}, + {1.0, 1.0, 1.0, 1.0}, + {1.0, 1.0, 1.0, 1.0}}; // Test initial pose EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - 1, estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Rotation().Z().value()); // Test reset position - estimator.ResetPosition(frc::Rotation2d{}, {1_m, 1_m, 1_m, 1_m}, - frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}); + estimator.ResetPosition(frc::Rotation3d{}, {1_m, 1_m, 1_m, 1_m}, + frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}); EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - 0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value()); // Test orientation and wheel positions - estimator.Update(frc::Rotation2d{}, {2_m, 2_m, 2_m, 2_m}); + estimator.Update(frc::Rotation3d{}, {2_m, 2_m, 2_m, 2_m}); EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - 0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value()); // Test reset rotation - estimator.ResetRotation(frc::Rotation2d{90_deg}); + estimator.ResetRotation(frc::Rotation3d{0_deg, 0_deg, 90_deg}); EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(std::numbers::pi / 2, + estimator.GetEstimatedPosition().Rotation().Z().value()); // Test orientation - estimator.Update(frc::Rotation2d{}, {3_m, 3_m, 3_m, 3_m}); + estimator.Update(frc::Rotation3d{}, {3_m, 3_m, 3_m, 3_m}); EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(std::numbers::pi / 2, + estimator.GetEstimatedPosition().Rotation().Z().value()); // Test reset translation - estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m}); + estimator.ResetTranslation(frc::Translation3d{-1_m, -1_m, -1_m}); EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(std::numbers::pi / 2, + estimator.GetEstimatedPosition().Rotation().Z().value()); // Test reset pose - estimator.ResetPose(frc::Pose2d{}); + estimator.ResetPose(frc::Pose3d{}); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - 0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value()); } diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp index 3c4090946b1..612862f4f15 100644 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp @@ -31,7 +31,8 @@ void testFollowTrajectory( const bool debug) { wpi::array positions{wpi::empty_array}; - estimator.ResetPosition(frc::Rotation2d{}, positions, startingPose); + estimator.ResetPosition(frc::Rotation3d{}, positions, + frc::Pose3d{startingPose}); std::default_random_engine generator; std::normal_distribution distribution(0.0, 1.0); @@ -69,7 +70,8 @@ void testFollowTrajectory( if (!visionPoses.empty() && visionPoses.front().first + kVisionUpdateDelay < t) { auto visionEntry = visionPoses.front(); - estimator.AddVisionMeasurement(visionEntry.second, visionEntry.first); + estimator.AddVisionMeasurement(frc::Pose3d{visionEntry.second}, + visionEntry.first); visionPoses.erase(visionPoses.begin()); visionLog.push_back({t, visionEntry.first, visionEntry.second}); } @@ -85,21 +87,21 @@ void testFollowTrajectory( auto xhat = estimator.UpdateWithTime( t, - groundTruthState.pose.Rotation() + - frc::Rotation2d{distribution(generator) * 0.05_rad} - - trajectory.InitialPose().Rotation(), + frc::Rotation3d{groundTruthState.pose.Rotation() + + frc::Rotation2d{distribution(generator) * 0.05_rad} - + trajectory.InitialPose().Rotation()}, positions); if (debug) { - wpi::print("{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(), - xhat.Y().value(), xhat.Rotation().Radians().value(), - groundTruthState.pose.X().value(), - groundTruthState.pose.Y().value(), - groundTruthState.pose.Rotation().Radians().value()); + wpi::print( + "{}, {}, {}, {}, {}, {}, {}\n", t.value(), xhat.X().value(), + xhat.Y().value(), xhat.Rotation().ToRotation2d().Radians().value(), + groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(), + groundTruthState.pose.Rotation().Radians().value()); } double error = groundTruthState.pose.Translation() - .Distance(xhat.Translation()) + .Distance(xhat.Translation().ToTranslation2d()) .value(); if (error > maxError) { @@ -130,7 +132,11 @@ void testFollowTrajectory( EXPECT_NEAR(endingPose.Y().value(), estimator.GetEstimatedPosition().Y().value(), 0.08); EXPECT_NEAR(endingPose.Rotation().Radians().value(), - estimator.GetEstimatedPosition().Rotation().Radians().value(), + estimator.GetEstimatedPosition() + .Rotation() + .ToRotation2d() + .Radians() + .value(), 0.15); if (checkError) { @@ -151,8 +157,8 @@ TEST(SwerveDrivePoseEstimator3dTest, AccuracyFacingTrajectory) { frc::SwerveModulePosition br; frc::SwerveDrivePoseEstimator3d<4> estimator{ - kinematics, frc::Rotation2d{}, {fl, fr, bl, br}, - frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; + kinematics, frc::Rotation3d{}, {fl, fr, bl, br}, + frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}}; frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg}, @@ -183,8 +189,8 @@ TEST(SwerveDrivePoseEstimator3dTest, BadInitialPose) { frc::SwerveModulePosition br; frc::SwerveDrivePoseEstimator3d<4> estimator{ - kinematics, frc::Rotation2d{}, {fl, fr, bl, br}, - frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.9, 0.9, 0.9}}; + kinematics, frc::Rotation3d{}, {fl, fr, bl, br}, + frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.9, 0.9, 0.9, 0.9}}; frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg}, @@ -235,26 +241,31 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) { frc::SwerveModulePosition br; frc::SwerveDrivePoseEstimator3d<4> estimator{ - kinematics, frc::Rotation2d{}, - {fl, fr, bl, br}, frc::Pose2d{1_m, 2_m, frc::Rotation2d{270_deg}}, - {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; + kinematics, + frc::Rotation3d{}, + {fl, fr, bl, br}, + frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 270_deg}}, + {0.1, 0.1, 0.1, 0.1}, + {0.45, 0.45, 0.45, 0.45}}; - estimator.UpdateWithTime(0_s, frc::Rotation2d{}, {fl, fr, bl, br}); + estimator.UpdateWithTime(0_s, frc::Rotation3d{}, {fl, fr, bl, br}); for (int i = 0; i < 1000; i++) { estimator.AddVisionMeasurement( - frc::Pose2d{0_m, 0_m, frc::Rotation2d{0_deg}}, 0_s); + frc::Pose3d{0_m, 0_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s); estimator.AddVisionMeasurement( - frc::Pose2d{3_m, 1_m, frc::Rotation2d{90_deg}}, 0_s); + frc::Pose3d{3_m, 1_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s); estimator.AddVisionMeasurement( - frc::Pose2d{2_m, 4_m, frc::Rotation2d{180_deg}}, 0_s); + frc::Pose3d{2_m, 4_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 180_deg}}, + 0_s); } { auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 0_m); auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 0_m); auto dtheta = units::math::abs( - estimator.GetEstimatedPosition().Rotation().Radians() - 0_deg); + estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() - + 0_deg); EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); } @@ -263,7 +274,8 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) { auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 3_m); auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 1_m); auto dtheta = units::math::abs( - estimator.GetEstimatedPosition().Rotation().Radians() - 90_deg); + estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() - + 90_deg); EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); } @@ -272,7 +284,8 @@ TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) { auto dx = units::math::abs(estimator.GetEstimatedPosition().X() - 2_m); auto dy = units::math::abs(estimator.GetEstimatedPosition().Y() - 4_m); auto dtheta = units::math::abs( - estimator.GetEstimatedPosition().Rotation().Radians() - 180_deg); + estimator.GetEstimatedPosition().Rotation().ToRotation2d().Radians() - + 180_deg); EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); } @@ -289,28 +302,33 @@ TEST(SwerveDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) { frc::SwerveModulePosition br; frc::SwerveDrivePoseEstimator3d<4> estimator{ - kinematics, frc::Rotation2d{}, {fl, fr, bl, br}, - frc::Pose2d{}, {0.1, 0.1, 0.1}, {0.45, 0.45, 0.45}}; + kinematics, frc::Rotation3d{}, {fl, fr, bl, br}, + frc::Pose3d{}, {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}}; // Add enough measurements to fill up the buffer for (auto time = 0_s; time < 4_s; time += 20_ms) { - estimator.UpdateWithTime(time, frc::Rotation2d{}, {fl, fr, bl, br}); + estimator.UpdateWithTime(time, frc::Rotation3d{}, {fl, fr, bl, br}); } auto odometryPose = estimator.GetEstimatedPosition(); // Apply a vision measurement from 3 seconds ago estimator.AddVisionMeasurement( - frc::Pose2d{frc::Translation2d{10_m, 10_m}, frc::Rotation2d{0.1_rad}}, - 1_s, {0.1, 0.1, 0.1}); + frc::Pose3d{10_m, 10_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 0.1_rad}}, 1_s, + {0.1, 0.1, 0.1, 0.1}); EXPECT_NEAR(odometryPose.X().value(), estimator.GetEstimatedPosition().X().value(), 1e-6); EXPECT_NEAR(odometryPose.Y().value(), estimator.GetEstimatedPosition().Y().value(), 1e-6); - EXPECT_NEAR(odometryPose.Rotation().Radians().value(), - estimator.GetEstimatedPosition().Rotation().Radians().value(), - 1e-6); + EXPECT_NEAR(odometryPose.Z().value(), + estimator.GetEstimatedPosition().Z().value(), 1e-6); + EXPECT_NEAR(odometryPose.Rotation().X().value(), + estimator.GetEstimatedPosition().Rotation().X().value(), 1e-6); + EXPECT_NEAR(odometryPose.Rotation().Y().value(), + estimator.GetEstimatedPosition().Rotation().Y().value(), 1e-6); + EXPECT_NEAR(odometryPose.Rotation().Z().value(), + estimator.GetEstimatedPosition().Rotation().Z().value(), 1e-6); } TEST(SwerveDrivePoseEstimator3dTest, TestSampleAt) { @@ -319,12 +337,12 @@ TEST(SwerveDrivePoseEstimator3dTest, TestSampleAt) { frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; frc::SwerveDrivePoseEstimator3d estimator{ kinematics, - frc::Rotation2d{}, + frc::Rotation3d{}, {frc::SwerveModulePosition{}, frc::SwerveModulePosition{}, frc::SwerveModulePosition{}, frc::SwerveModulePosition{}}, - frc::Pose2d{}, - {1.0, 1.0, 1.0}, - {1.0, 1.0, 1.0}}; + frc::Pose3d{}, + {1.0, 1.0, 1.0, 1.0}, + {1.0, 1.0, 1.0, 1.0}}; // Returns empty when null EXPECT_EQ(std::nullopt, estimator.SampleAt(1_s)); @@ -338,49 +356,49 @@ TEST(SwerveDrivePoseEstimator3dTest, TestSampleAt) { frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}}, frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}}, frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}}}}; - estimator.UpdateWithTime(units::second_t{time}, frc::Rotation2d{}, + estimator.UpdateWithTime(units::second_t{time}, frc::Rotation3d{}, wheelPositions); } // Sample at an added time - EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.02_s)); // Sample between updates (test interpolation) - EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.01_s)); // Sampling before the oldest value returns the oldest value - EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(0.5_s)); // Sampling after the newest value returns the newest value - EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(2.5_s)); // Add a vision measurement after the odometry measurements (while keeping all // of the old odometry measurements) - estimator.AddVisionMeasurement(frc::Pose2d{2_m, 0_m, frc::Rotation2d{1_rad}}, - 2.2_s); + estimator.AddVisionMeasurement( + frc::Pose3d{2_m, 0_m, 0_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}}, 2.2_s); // Make sure nothing changed (except the newest value) - EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.02_s)); - EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.01_s)); - EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(0.5_s)); // Add a vision measurement before the odometry measurements that's still in // the buffer - estimator.AddVisionMeasurement(frc::Pose2d{1_m, 0.2_m, frc::Rotation2d{}}, - 0.9_s); + estimator.AddVisionMeasurement( + frc::Pose3d{1_m, 0.2_m, 0_m, frc::Rotation3d{}}, 0.9_s); // Everything should be the same except Y is 0.1 (halfway between 0 and 0.2) - EXPECT_EQ(std::optional(frc::Pose2d{1.02_m, 0.1_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.02_m, 0.1_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.02_s)); - EXPECT_EQ(std::optional(frc::Pose2d{1.01_m, 0.1_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1.01_m, 0.1_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(1.01_s)); - EXPECT_EQ(std::optional(frc::Pose2d{1_m, 0.1_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{1_m, 0.1_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(0.5_s)); - EXPECT_EQ(std::optional(frc::Pose2d{2_m, 0.1_m, frc::Rotation2d{}}), + EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0.1_m, 0_m, frc::Rotation3d{}}), estimator.SampleAt(2.5_s)); } @@ -390,81 +408,95 @@ TEST(SwerveDrivePoseEstimator3dTest, TestReset) { frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}}; frc::SwerveDrivePoseEstimator3d estimator{ kinematics, - frc::Rotation2d{}, + frc::Rotation3d{}, {frc::SwerveModulePosition{}, frc::SwerveModulePosition{}, frc::SwerveModulePosition{}, frc::SwerveModulePosition{}}, - frc::Pose2d{-1_m, -1_m, frc::Rotation2d{1_rad}}, - {1.0, 1.0, 1.0}, - {1.0, 1.0, 1.0}}; + frc::Pose3d{-1_m, -1_m, -1_m, frc::Rotation3d{0_rad, 0_rad, 1_rad}}, + {1.0, 1.0, 1.0, 1.0}, + {1.0, 1.0, 1.0, 1.0}}; // Test initial pose EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - 1, estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Rotation().Z().value()); // Test reset position { frc::SwerveModulePosition modulePosition{1_m, frc::Rotation2d{}}; estimator.ResetPosition( - frc::Rotation2d{}, + frc::Rotation3d{}, {modulePosition, modulePosition, modulePosition, modulePosition}, - frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}); + frc::Pose3d{1_m, 0_m, 0_m, frc::Rotation3d{}}); } EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - 0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value()); // Test orientation and wheel positions { frc::SwerveModulePosition modulePosition{2_m, frc::Rotation2d{}}; - estimator.Update(frc::Rotation2d{}, {modulePosition, modulePosition, + estimator.Update(frc::Rotation3d{}, {modulePosition, modulePosition, modulePosition, modulePosition}); } EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - 0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value()); // Test reset rotation - estimator.ResetRotation(frc::Rotation2d{90_deg}); + estimator.ResetRotation(frc::Rotation3d{0_deg, 0_deg, 90_deg}); EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(std::numbers::pi / 2, + estimator.GetEstimatedPosition().Rotation().Z().value()); // Test orientation { frc::SwerveModulePosition modulePosition{3_m, frc::Rotation2d{}}; - estimator.Update(frc::Rotation2d{}, {modulePosition, modulePosition, + estimator.Update(frc::Rotation3d{}, {modulePosition, modulePosition, modulePosition, modulePosition}); } EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(std::numbers::pi / 2, + estimator.GetEstimatedPosition().Rotation().Z().value()); // Test reset translation - estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m}); + estimator.ResetTranslation(frc::Translation3d{-1_m, -1_m, -1_m}); EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(-1, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(std::numbers::pi / 2, + estimator.GetEstimatedPosition().Rotation().Z().value()); // Test reset pose - estimator.ResetPose(frc::Pose2d{}); + estimator.ResetPose(frc::Pose3d{}); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value()); EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_DOUBLE_EQ( - 0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Z().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Y().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Rotation().Z().value()); } diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp index f93b86b8606..76f5f3292c7 100644 --- a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp @@ -14,23 +14,27 @@ static constexpr double kEpsilon = 1E-9; using namespace frc; TEST(DifferentialDriveOdometry3dTest, Initialize) { - DifferentialDriveOdometry3d odometry{90_deg, 0_m, 0_m, - frc::Pose2d{1_m, 2_m, 45_deg}}; + DifferentialDriveOdometry3d odometry{ + frc::Rotation3d{0_deg, 0_deg, 90_deg}, 0_m, 0_m, + frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 45_deg}}}; - const frc::Pose2d& pose = odometry.GetPose(); + const frc::Pose3d& pose = odometry.GetPose(); EXPECT_NEAR(pose.X().value(), 1, kEpsilon); EXPECT_NEAR(pose.Y().value(), 2, kEpsilon); - EXPECT_NEAR(pose.Rotation().Degrees().value(), 45, kEpsilon); + EXPECT_NEAR(pose.Z().value(), 0, kEpsilon); + EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 45, kEpsilon); } TEST(DifferentialDriveOdometry3dTest, EncoderDistances) { - DifferentialDriveOdometry3d odometry{45_deg, 0_m, 0_m}; + DifferentialDriveOdometry3d odometry{frc::Rotation3d{0_deg, 0_deg, 45_deg}, + 0_m, 0_m}; - const auto& pose = - odometry.Update(135_deg, 0_m, units::meter_t{5 * std::numbers::pi}); + const auto& pose = odometry.Update(frc::Rotation3d{0_deg, 0_deg, 135_deg}, + 0_m, units::meter_t{5 * std::numbers::pi}); EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon); EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon); - EXPECT_NEAR(pose.Rotation().Degrees().value(), 90.0, kEpsilon); + EXPECT_NEAR(pose.Z().value(), 0.0, kEpsilon); + EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 90.0, kEpsilon); } diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp index 5c238d73b9d..b195e8507b7 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp @@ -22,70 +22,78 @@ class MecanumDriveOdometry3dTest : public ::testing::Test { MecanumDriveWheelPositions zero; MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br}; - MecanumDriveOdometry3d odometry{kinematics, 0_rad, zero}; + MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{}, zero}; }; TEST_F(MecanumDriveOdometry3dTest, Initialize) { - MecanumDriveOdometry3d odometry{kinematics, 0_rad, zero, - frc::Pose2d{1_m, 2_m, 45_deg}}; + MecanumDriveOdometry3d odometry{ + kinematics, frc::Rotation3d{}, zero, + frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 45_deg}}}; - const frc::Pose2d& pose = odometry.GetPose(); + const frc::Pose3d& pose = odometry.GetPose(); EXPECT_NEAR(pose.X().value(), 1, 1e-9); EXPECT_NEAR(pose.Y().value(), 2, 1e-9); - EXPECT_NEAR(pose.Rotation().Degrees().value(), 45, 1e-9); + EXPECT_NEAR(pose.Z().value(), 0, 1e-9); + EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 45, 1e-9); } TEST_F(MecanumDriveOdometry3dTest, MultipleConsecutiveUpdates) { MecanumDriveWheelPositions wheelDeltas{3.536_m, 3.536_m, 3.536_m, 3.536_m}; - odometry.ResetPosition(0_rad, wheelDeltas, Pose2d{}); + odometry.ResetPosition(frc::Rotation3d{}, wheelDeltas, Pose3d{}); - odometry.Update(0_deg, wheelDeltas); - auto secondPose = odometry.Update(0_deg, wheelDeltas); + odometry.Update(frc::Rotation3d{}, wheelDeltas); + auto secondPose = odometry.Update(frc::Rotation3d{}, wheelDeltas); EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01); EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01); - EXPECT_NEAR(secondPose.Rotation().Radians().value(), 0.0, 0.01); + EXPECT_NEAR(secondPose.Z().value(), 0.0, 0.01); + EXPECT_NEAR(secondPose.Rotation().ToRotation2d().Radians().value(), 0.0, + 0.01); } TEST_F(MecanumDriveOdometry3dTest, TwoIterations) { - odometry.ResetPosition(0_rad, zero, Pose2d{}); + odometry.ResetPosition(frc::Rotation3d{}, zero, Pose3d{}); MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m, 0.3536_m}; - odometry.Update(0_deg, MecanumDriveWheelPositions{}); - auto pose = odometry.Update(0_deg, wheelDeltas); + odometry.Update(frc::Rotation3d{}, MecanumDriveWheelPositions{}); + auto pose = odometry.Update(frc::Rotation3d{}, wheelDeltas); EXPECT_NEAR(pose.X().value(), 0.3536, 0.01); EXPECT_NEAR(pose.Y().value(), 0.0, 0.01); - EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01); + EXPECT_NEAR(pose.Z().value(), 0.0, 0.01); + EXPECT_NEAR(pose.Rotation().ToRotation2d().Radians().value(), 0.0, 0.01); } TEST_F(MecanumDriveOdometry3dTest, 90DegreeTurn) { - odometry.ResetPosition(0_rad, zero, Pose2d{}); + odometry.ResetPosition(frc::Rotation3d{}, zero, Pose3d{}); MecanumDriveWheelPositions wheelDeltas{-13.328_m, 39.986_m, -13.329_m, 39.986_m}; - odometry.Update(0_deg, MecanumDriveWheelPositions{}); - auto pose = odometry.Update(90_deg, wheelDeltas); + odometry.Update(frc::Rotation3d{}, MecanumDriveWheelPositions{}); + auto pose = + odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg}, wheelDeltas); EXPECT_NEAR(pose.X().value(), 8.4855, 0.01); EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01); - EXPECT_NEAR(pose.Rotation().Degrees().value(), 90.0, 0.01); + EXPECT_NEAR(pose.Z().value(), 0, 0.01); + EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 90.0, 0.01); } TEST_F(MecanumDriveOdometry3dTest, GyroAngleReset) { - odometry.ResetPosition(90_deg, zero, Pose2d{}); + odometry.ResetPosition(frc::Rotation3d{0_deg, 0_deg, 90_deg}, zero, Pose3d{}); MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m, 0.3536_m}; - odometry.Update(90_deg, MecanumDriveWheelPositions{}); - auto pose = odometry.Update(90_deg, wheelDeltas); + auto pose = + odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg}, wheelDeltas); EXPECT_NEAR(pose.X().value(), 0.3536, 0.01); EXPECT_NEAR(pose.Y().value(), 0.0, 0.01); - EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01); + EXPECT_NEAR(pose.Z().value(), 0.0, 0.01); + EXPECT_NEAR(pose.Rotation().ToRotation2d().Radians().value(), 0.0, 0.01); } TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) { @@ -95,8 +103,8 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) { frc::MecanumDriveWheelPositions wheelPositions; - frc::MecanumDriveOdometry3d odometry{kinematics, frc::Rotation2d{}, - wheelPositions, frc::Pose2d{}}; + frc::MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{}, + wheelPositions}; frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg}, @@ -131,12 +139,12 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) { wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt; wheelPositions.rearRight += wheelSpeeds.rearRight * dt; - auto xhat = - odometry.Update(groundTruthState.pose.Rotation() + - frc::Rotation2d{distribution(generator) * 0.05_rad}, - wheelPositions); + auto xhat = odometry.Update( + frc::Rotation3d{groundTruthState.pose.Rotation() + + frc::Rotation2d{distribution(generator) * 0.05_rad}}, + wheelPositions); double error = groundTruthState.pose.Translation() - .Distance(xhat.Translation()) + .Distance(xhat.Translation().ToTranslation2d()) .value(); if (error > maxError) { @@ -158,8 +166,8 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) { frc::MecanumDriveWheelPositions wheelPositions; - frc::MecanumDriveOdometry3d odometry{kinematics, frc::Rotation2d{}, - wheelPositions, frc::Pose2d{}}; + frc::MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{}, + wheelPositions}; frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg}, @@ -196,9 +204,10 @@ TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) { wheelPositions.rearRight += wheelSpeeds.rearRight * dt; auto xhat = odometry.Update( - frc::Rotation2d{distribution(generator) * 0.05_rad}, wheelPositions); + frc::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad}, + wheelPositions); double error = groundTruthState.pose.Translation() - .Distance(xhat.Translation()) + .Distance(xhat.Translation().ToTranslation2d()) .value(); if (error > maxError) { diff --git a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp index 4e48726449b..18e786cb71f 100644 --- a/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp @@ -85,7 +85,7 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingTrajectory) { frc::MecanumDriveWheelPositions wheelPositions; frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{}, - wheelPositions, frc::Pose2d{}}; + wheelPositions}; frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg}, @@ -148,7 +148,7 @@ TEST_F(MecanumDriveOdometryTest, AccuracyFacingXAxis) { frc::MecanumDriveWheelPositions wheelPositions; frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d{}, - wheelPositions, frc::Pose2d{}}; + wheelPositions}; frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory( std::vector{frc::Pose2d{0_m, 0_m, 45_deg}, frc::Pose2d{3_m, 0_m, -90_deg}, diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp index 597482b13c7..d9fd52a5594 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp @@ -27,35 +27,39 @@ class SwerveDriveOdometry3dTest : public ::testing::Test { SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br}; SwerveModulePosition zero; SwerveDriveOdometry3d<4> m_odometry{ - m_kinematics, 0_rad, {zero, zero, zero, zero}}; + m_kinematics, frc::Rotation3d{}, {zero, zero, zero, zero}}; }; TEST_F(SwerveDriveOdometry3dTest, Initialize) { - SwerveDriveOdometry3d odometry{m_kinematics, - 0_rad, - {zero, zero, zero, zero}, - frc::Pose2d{1_m, 2_m, 45_deg}}; + SwerveDriveOdometry3d odometry{ + m_kinematics, + frc::Rotation3d{}, + {zero, zero, zero, zero}, + frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 45_deg}}}; - const frc::Pose2d& pose = odometry.GetPose(); + const frc::Pose3d& pose = odometry.GetPose(); EXPECT_NEAR(pose.X().value(), 1, kEpsilon); EXPECT_NEAR(pose.Y().value(), 2, kEpsilon); - EXPECT_NEAR(pose.Rotation().Degrees().value(), 45, kEpsilon); + EXPECT_NEAR(pose.Z().value(), 0, kEpsilon); + EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 45, kEpsilon); } TEST_F(SwerveDriveOdometry3dTest, TwoIterations) { SwerveModulePosition position{0.5_m, 0_deg}; - m_odometry.ResetPosition(0_rad, {zero, zero, zero, zero}, Pose2d{}); + m_odometry.ResetPosition(frc::Rotation3d{}, {zero, zero, zero, zero}, + Pose3d{}); - m_odometry.Update(0_deg, {zero, zero, zero, zero}); + m_odometry.Update(frc::Rotation3d{}, {zero, zero, zero, zero}); - auto pose = - m_odometry.Update(0_deg, {position, position, position, position}); + auto pose = m_odometry.Update(frc::Rotation3d{}, + {position, position, position, position}); EXPECT_NEAR(0.5, pose.X().value(), kEpsilon); EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon); - EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon); + EXPECT_NEAR(0.0, pose.Z().value(), kEpsilon); + EXPECT_NEAR(0.0, pose.Rotation().ToRotation2d().Degrees().value(), kEpsilon); } TEST_F(SwerveDriveOdometry3dTest, 90DegreeTurn) { @@ -64,25 +68,30 @@ TEST_F(SwerveDriveOdometry3dTest, 90DegreeTurn) { SwerveModulePosition bl{18.85_m, -90_deg}; SwerveModulePosition br{42.15_m, -26.565_deg}; - m_odometry.ResetPosition(0_rad, {zero, zero, zero, zero}, Pose2d{}); - auto pose = m_odometry.Update(90_deg, {fl, fr, bl, br}); + m_odometry.ResetPosition(frc::Rotation3d{}, {zero, zero, zero, zero}, + Pose3d{}); + auto pose = m_odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg}, + {fl, fr, bl, br}); EXPECT_NEAR(12.0, pose.X().value(), kEpsilon); EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon); - EXPECT_NEAR(90.0, pose.Rotation().Degrees().value(), kEpsilon); + EXPECT_NEAR(0.0, pose.Z().value(), kEpsilon); + EXPECT_NEAR(90.0, pose.Rotation().ToRotation2d().Degrees().value(), kEpsilon); } TEST_F(SwerveDriveOdometry3dTest, GyroAngleReset) { - m_odometry.ResetPosition(90_deg, {zero, zero, zero, zero}, Pose2d{}); + m_odometry.ResetPosition(frc::Rotation3d{0_deg, 0_deg, 90_deg}, + {zero, zero, zero, zero}, Pose3d{}); SwerveModulePosition position{0.5_m, 0_deg}; - auto pose = - m_odometry.Update(90_deg, {position, position, position, position}); + auto pose = m_odometry.Update(frc::Rotation3d{0_deg, 0_deg, 90_deg}, + {position, position, position, position}); EXPECT_NEAR(0.5, pose.X().value(), kEpsilon); EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon); - EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon); + EXPECT_NEAR(0.0, pose.Z().value(), kEpsilon); + EXPECT_NEAR(0.0, pose.Rotation().ToRotation2d().Degrees().value(), kEpsilon); } TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingTrajectory) { @@ -91,7 +100,7 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingTrajectory) { Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}}; SwerveDriveOdometry3d<4> odometry{ - kinematics, 0_rad, {zero, zero, zero, zero}}; + kinematics, frc::Rotation3d{}, {zero, zero, zero, zero}}; SwerveModulePosition fl; SwerveModulePosition fr; @@ -130,12 +139,12 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingTrajectory) { bl.angle = moduleStates[2].angle; br.angle = moduleStates[3].angle; - auto xhat = - odometry.Update(groundTruthState.pose.Rotation() + - frc::Rotation2d{distribution(generator) * 0.05_rad}, - {fl, fr, bl, br}); + auto xhat = odometry.Update( + frc::Rotation3d{groundTruthState.pose.Rotation() + + frc::Rotation2d{distribution(generator) * 0.05_rad}}, + {fl, fr, bl, br}); double error = groundTruthState.pose.Translation() - .Distance(xhat.Translation()) + .Distance(xhat.Translation().ToTranslation2d()) .value(); if (error > maxError) { @@ -156,7 +165,7 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingXAxis) { Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}}; SwerveDriveOdometry3d<4> odometry{ - kinematics, 0_rad, {zero, zero, zero, zero}}; + kinematics, frc::Rotation3d{}, {zero, zero, zero, zero}}; SwerveModulePosition fl; SwerveModulePosition fr; @@ -196,9 +205,10 @@ TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingXAxis) { br.angle = groundTruthState.pose.Rotation(); auto xhat = odometry.Update( - frc::Rotation2d{distribution(generator) * 0.05_rad}, {fl, fr, bl, br}); + frc::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad}, + {fl, fr, bl, br}); double error = groundTruthState.pose.Translation() - .Distance(xhat.Translation()) + .Distance(xhat.Translation().ToTranslation2d()) .value(); if (error > maxError) { diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp index 9595335071b..80a3c85cd7d 100644 --- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp @@ -77,7 +77,8 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingTrajectory) { Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m}, Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}}; - SwerveDriveOdometry<4> odometry{kinematics, 0_rad, {zero, zero, zero, zero}}; + SwerveDriveOdometry<4> odometry{ + kinematics, frc::Rotation2d{}, {zero, zero, zero, zero}}; SwerveModulePosition fl; SwerveModulePosition fr; @@ -141,7 +142,8 @@ TEST_F(SwerveDriveOdometryTest, AccuracyFacingXAxis) { Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m}, Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}}; - SwerveDriveOdometry<4> odometry{kinematics, 0_rad, {zero, zero, zero, zero}}; + SwerveDriveOdometry<4> odometry{ + kinematics, frc::Rotation2d{}, {zero, zero, zero, zero}}; SwerveModulePosition fl; SwerveModulePosition fr;