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 new file mode 100644 index 00000000000..470a05aa373 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3d.java @@ -0,0 +1,159 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.estimator; + +import edu.wpi.first.math.Matrix; +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.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.N4; + +/** + * This class wraps {@link DifferentialDriveOdometry3d Differential Drive Odometry} to fuse + * latency-compensated vision measurements with differential drive encoder measurements. It is + * 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. 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. + * + *

{@link DifferentialDrivePoseEstimator3d#addVisionMeasurement} can be called as infrequently as + * you want; if you never call it then this class will behave exactly like regular encoder odometry. + */ +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, 0.02 meters for z, and 0.01 radians for angle. The default standard deviations of the vision + * measurements are 0.1 meters for x, 0.1 meters for y, 0.1 meters for z, and 0.1 radians for + * angle. + * + * @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, + Rotation3d gyroAngle, + double leftDistanceMeters, + double rightDistanceMeters, + Pose3d initialPoseMeters) { + this( + kinematics, + gyroAngle, + leftDistanceMeters, + rightDistanceMeters, + initialPoseMeters, + VecBuilder.fill(0.02, 0.02, 0.02, 0.01), + VecBuilder.fill(0.1, 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, + Rotation3d gyroAngle, + double leftDistanceMeters, + double rightDistanceMeters, + Pose3d initialPoseMeters, + Matrix stateStdDevs, + Matrix visionMeasurementStdDevs) { + super( + kinematics, + new DifferentialDriveOdometry3d( + gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters), + stateStdDevs, + visionMeasurementStdDevs); + } + + /** + * 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( + Rotation3d gyroAngle, + double leftPositionMeters, + double rightPositionMeters, + Pose3d poseMeters) { + resetPosition( + gyroAngle, + new DifferentialDriveWheelPositions(leftPositionMeters, rightPositionMeters), + 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 Pose3d update( + Rotation3d 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. + * + * @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 Pose3d updateWithTime( + double currentTimeSeconds, + Rotation3d gyroAngle, + double distanceLeftMeters, + double distanceRightMeters) { + return updateWithTime( + currentTimeSeconds, + gyroAngle, + new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters)); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java index 59bbb3235cd..e33b30d09ca 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java @@ -18,7 +18,7 @@ * This class wraps {@link MecanumDriveOdometry 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 - * edu.wpi.first.math.kinematics.MecanumDriveOdometry}. + * MecanumDriveOdometry}. * *

{@link MecanumDrivePoseEstimator#update} should be called every robot 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 new file mode 100644 index 00000000000..e2f7acfdb0c --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3d.java @@ -0,0 +1,92 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.estimator; + +import edu.wpi.first.math.Matrix; +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.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.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}. 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. + * + *

{@link MecanumDrivePoseEstimator3d#addVisionMeasurement} can be called as infrequently as you + * 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, + * 0.1 meters for z, and 0.1 radians for angle. The default standard deviations of the vision + * measurements are 0.45 meters for x, 0.45 meters for y, 0.45 meters for z, and 0.45 radians for + * angle. + * + * @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, + Rotation3d gyroAngle, + MecanumDriveWheelPositions wheelPositions, + Pose3d initialPoseMeters) { + this( + kinematics, + gyroAngle, + wheelPositions, + initialPoseMeters, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.45, 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, + Rotation3d gyroAngle, + MecanumDriveWheelPositions wheelPositions, + Pose3d initialPoseMeters, + Matrix stateStdDevs, + Matrix visionMeasurementStdDevs) { + super( + kinematics, + new MecanumDriveOdometry3d(kinematics, gyroAngle, wheelPositions, initialPoseMeters), + stateStdDevs, + 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 new file mode 100644 index 00000000000..9ce96f0243b --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator3d.java @@ -0,0 +1,424 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.estimator; + +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.Nat; +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.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Twist3d; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +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.N4; +import edu.wpi.first.math.numbers.N6; +import java.util.NavigableMap; +import java.util.Optional; +import java.util.TreeMap; + +/** + * This class wraps {@link Odometry3d} to fuse latency-compensated vision measurements with encoder + * measurements. Robot code should not use this directly- Instead, use the particular type for your + * 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. 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. + * + *

{@link PoseEstimator3d#addVisionMeasurement} can be called as infrequently as you want; if you + * never call it then this class will behave exactly like regular encoder odometry. + * + * @param Wheel positions type. + */ +public class PoseEstimator3d { + private final Odometry3d m_odometry; + private final Matrix m_q = new Matrix<>(Nat.N4(), Nat.N1()); + private final Matrix m_visionK = new Matrix<>(Nat.N6(), Nat.N6()); + + private static final double kBufferDuration = 1.5; + // Maps timestamps to odometry-only pose estimates + private final TimeInterpolatableBuffer m_odometryPoseBuffer = + TimeInterpolatableBuffer.createBuffer(kBufferDuration); + // Maps timestamps to vision updates + // Always contains one entry before the oldest entry in m_odometryPoseBuffer, unless there have + // been no vision measurements after the last reset + private final NavigableMap m_visionUpdates = new TreeMap<>(); + + 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, z 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, z position in meters, and angle in radians). Increase + * these numbers to trust the vision pose measurement less. + */ + @SuppressWarnings("PMD.UnusedFormalParameter") + public PoseEstimator3d( + Kinematics kinematics, + Odometry3d odometry, + Matrix stateStdDevs, + Matrix visionMeasurementStdDevs) { + m_odometry = odometry; + + 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)); + } + setVisionMeasurementStdDevs(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, z, + * theta]ᵀ, with units in meters and radians. + */ + 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); + } + + // Solve for closed form Kalman gain for continuous Kalman filter with A = 0 + // and C = I. See wpimath/algorithms.md. + for (int row = 0; row < 4; ++row) { + if (m_q.get(row, 0) == 0.0) { + m_visionK.set(row, row, 0.0); + } else { + m_visionK.set( + row, row, m_q.get(row, 0) / (m_q.get(row, 0) + Math.sqrt(m_q.get(row, 0) * r[row]))); + } + } + // Fill in the gains for the other components of the rotation vector + double angle_gain = m_visionK.get(3, 3); + m_visionK.set(4, 4, angle_gain); + 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(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters) { + // Reset state estimate and error covariance + m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters); + m_odometryPoseBuffer.clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.getPoseMeters(); + } + + /** + * Resets the robot's pose. + * + * @param pose The pose to reset to. + */ + public void resetPose(Pose3d pose) { + m_odometry.resetPose(pose); + m_odometryPoseBuffer.clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.getPoseMeters(); + } + + /** + * Resets the robot's translation. + * + * @param translation The pose to translation to. + */ + public void resetTranslation(Translation3d translation) { + m_odometry.resetTranslation(translation); + m_odometryPoseBuffer.clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.getPoseMeters(); + } + + /** + * Resets the robot's rotation. + * + * @param rotation The rotation to reset to. + */ + public void resetRotation(Rotation3d rotation) { + m_odometry.resetRotation(rotation); + m_odometryPoseBuffer.clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.getPoseMeters(); + } + + /** + * Gets the estimated robot pose. + * + * @return The estimated robot pose in meters. + */ + public Pose3d getEstimatedPosition() { + return m_poseEstimate; + } + + /** + * 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 sampleAt(double timestampSeconds) { + // Step 0: If there are no odometry updates to sample, skip. + if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) { + return Optional.empty(); + } + + // Step 1: Make sure timestamp matches the sample from the odometry pose buffer. (When sampling, + // the buffer will always use a timestamp between the first and last timestamps) + double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey(); + double newestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().lastKey(); + timestampSeconds = + MathUtil.clamp(timestampSeconds, oldestOdometryTimestamp, newestOdometryTimestamp); + + // Step 2: If there are no applicable vision updates, use the odometry-only information. + if (m_visionUpdates.isEmpty() || timestampSeconds < m_visionUpdates.firstKey()) { + return m_odometryPoseBuffer.getSample(timestampSeconds); + } + + // Step 3: Get the latest vision update from before or at the timestamp to sample at. + double floorTimestamp = m_visionUpdates.floorKey(timestampSeconds); + var visionUpdate = m_visionUpdates.get(floorTimestamp); + + // Step 4: Get the pose measured by odometry at the time of the sample. + var odometryEstimate = m_odometryPoseBuffer.getSample(timestampSeconds); + + // Step 5: Apply the vision compensation to the odometry pose. + return odometryEstimate.map(odometryPose -> visionUpdate.compensate(odometryPose)); + } + + /** Removes stale vision updates that won't affect sampling. */ + private void cleanUpVisionUpdates() { + // Step 0: If there are no odometry samples, skip. + if (m_odometryPoseBuffer.getInternalBuffer().isEmpty()) { + return; + } + + // Step 1: Find the oldest timestamp that needs a vision update. + double oldestOdometryTimestamp = m_odometryPoseBuffer.getInternalBuffer().firstKey(); + + // Step 2: If there are no vision updates before that timestamp, skip. + if (m_visionUpdates.isEmpty() || oldestOdometryTimestamp < m_visionUpdates.firstKey()) { + return; + } + + // Step 3: Find the newest vision update timestamp before or at the oldest timestamp. + double newestNeededVisionUpdateTimestamp = m_visionUpdates.floorKey(oldestOdometryTimestamp); + + // Step 4: Remove all entries strictly before the newest timestamp we need. + m_visionUpdates.headMap(newestNeededVisionUpdateTimestamp, false).clear(); + } + + /** + * 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,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 + * epochs. + */ + public void addVisionMeasurement(Pose3d visionRobotPoseMeters, double timestampSeconds) { + // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip. + if (m_odometryPoseBuffer.getInternalBuffer().isEmpty() + || m_odometryPoseBuffer.getInternalBuffer().lastKey() - kBufferDuration + > timestampSeconds) { + return; + } + + // Step 1: Clean up any old entries + cleanUpVisionUpdates(); + + // Step 2: Get the pose measured by odometry at the moment the vision measurement was made. + var odometrySample = m_odometryPoseBuffer.getSample(timestampSeconds); + + if (odometrySample.isEmpty()) { + return; + } + + // Step 3: Get the vision-compensated pose estimate at the moment the vision measurement was + // made. + var visionSample = sampleAt(timestampSeconds); + + if (visionSample.isEmpty()) { + return; + } + + // Step 4: Measure the twist between the old pose estimate and the vision pose. + var twist = visionSample.get().log(visionRobotPoseMeters); + + // Step 5: We should not trust the twist entirely, so instead we scale this twist by a Kalman + // gain matrix representing how much we trust vision measurements compared to our current pose. + var k_times_twist = + m_visionK.times( + VecBuilder.fill(twist.dx, twist.dy, twist.dz, twist.rx, twist.ry, twist.rz)); + + // Step 6: Convert back to Twist3d. + var scaledTwist = + new Twist3d( + k_times_twist.get(0, 0), + k_times_twist.get(1, 0), + k_times_twist.get(2, 0), + k_times_twist.get(3, 0), + k_times_twist.get(4, 0), + k_times_twist.get(5, 0)); + + // Step 7: Calculate and record the vision update. + var visionUpdate = new VisionUpdate(visionSample.get().exp(scaledTwist), odometrySample.get()); + m_visionUpdates.put(timestampSeconds, visionUpdate); + + // Step 8: Remove later vision measurements. (Matches previous behavior) + m_visionUpdates.tailMap(timestampSeconds, false).entrySet().clear(); + + // 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.getPoseMeters()); + } + + /** + * 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, z position in meters, and angle in radians). Increase + * these numbers to trust the vision pose measurement less. + */ + public void addVisionMeasurement( + Pose3d visionRobotPoseMeters, + double timestampSeconds, + Matrix 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 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 Pose3d updateWithTime(double currentTimeSeconds, Rotation3d gyroAngle, T wheelPositions) { + var odometryEstimate = m_odometry.update(gyroAngle, wheelPositions); + + m_odometryPoseBuffer.addSample(currentTimeSeconds, odometryEstimate); + + if (m_visionUpdates.isEmpty()) { + m_poseEstimate = odometryEstimate; + } else { + var visionUpdate = m_visionUpdates.get(m_visionUpdates.lastKey()); + m_poseEstimate = visionUpdate.compensate(odometryEstimate); + } + + return getEstimatedPosition(); + } + + /** + * Represents a vision update record. The record contains the vision-compensated pose estimate as + * well as the corresponding odometry pose estimate. + */ + private static final class VisionUpdate { + // The vision-compensated pose estimate. + private final Pose3d visionPose; + + // The pose estimated based solely on odometry. + private final Pose3d odometryPose; + + /** + * Constructs a vision update record with the specified parameters. + * + * @param visionPose The vision-compensated pose estimate. + * @param odometryPose The pose estimate based solely on odometry. + */ + private VisionUpdate(Pose3d visionPose, Pose3d odometryPose) { + this.visionPose = visionPose; + this.odometryPose = odometryPose; + } + + /** + * Returns the vision-compensated version of the pose. Specifically, changes the pose from being + * relative to this record's odometry pose to being relative to this record's vision pose. + * + * @param pose The pose to compensate. + * @return The compensated pose. + */ + public Pose3d compensate(Pose3d pose) { + var delta = pose.minus(this.odometryPose); + return this.visionPose.plus(delta); + } + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java index 42901670045..495cd893fc6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java @@ -17,7 +17,7 @@ /** * This class wraps {@link SwerveDriveOdometry 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 edu.wpi.first.math.kinematics.SwerveDriveOdometry}. + * drop-in replacement for {@link SwerveDriveOdometry}. * *

{@link SwerveDrivePoseEstimator#update} should be called every robot loop. * 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 new file mode 100644 index 00000000000..668f45c410e --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3d.java @@ -0,0 +1,107 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.estimator; + +import edu.wpi.first.math.Matrix; +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.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.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}. 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. + * + *

{@link SwerveDrivePoseEstimator3d#addVisionMeasurement} can be called as infrequently as you + * want; if you never call it, then this class will behave as regular encoder odometry. + */ +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, + * 0.1 meters for z, and 0.1 radians for angle. The default standard deviations of the vision + * measurements are 0.9 meters for x, 0.9 meters for y, 0.9 meters for z, and 0.9 radians for + * angle. + * + * @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, + Rotation3d gyroAngle, + SwerveModulePosition[] modulePositions, + Pose3d initialPoseMeters) { + this( + kinematics, + gyroAngle, + modulePositions, + initialPoseMeters, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1), + VecBuilder.fill(0.9, 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, + Rotation3d gyroAngle, + SwerveModulePosition[] modulePositions, + Pose3d initialPoseMeters, + Matrix stateStdDevs, + Matrix visionMeasurementStdDevs) { + super( + kinematics, + new SwerveDriveOdometry3d(kinematics, gyroAngle, modulePositions, initialPoseMeters), + stateStdDevs, + visionMeasurementStdDevs); + + m_numModules = modulePositions.length; + } + + @Override + public Pose3d updateWithTime( + double currentTimeSeconds, Rotation3d 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); + } +} 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 new file mode 100644 index 00000000000..7a5769a5777 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3d.java @@ -0,0 +1,149 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics; + +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; +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.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. + * + *

It is important that you reset your encoders to zero before using this class. Any subsequent + * 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( + Rotation3d gyroAngle, + double leftDistanceMeters, + double rightDistanceMeters, + Pose3d 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( + Rotation3d gyroAngle, + Distance leftDistance, + Distance rightDistance, + Pose3d 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( + Rotation3d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) { + this(gyroAngle, leftDistanceMeters, rightDistanceMeters, Pose3d.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( + Rotation3d gyroAngle, Distance leftDistance, Distance rightDistance) { + 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( + Rotation3d gyroAngle, + double leftDistanceMeters, + double rightDistanceMeters, + Pose3d 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( + Rotation3d gyroAngle, Distance leftDistance, Distance rightDistance, Pose3d poseMeters) { + 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 Pose3d update( + Rotation3d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) { + return super.update( + gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters)); + } +} 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 new file mode 100644 index 00000000000..00d66c0ab1b --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3d.java @@ -0,0 +1,59 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics; + +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; +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; + +/** + * 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, + Rotation3d gyroAngle, + MecanumDriveWheelPositions wheelPositions, + Pose3d 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, + Rotation3d gyroAngle, + MecanumDriveWheelPositions wheelPositions) { + this(kinematics, gyroAngle, wheelPositions, Pose3d.kZero); + } +} 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 new file mode 100644 index 00000000000..c1ccd3c9a16 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry3d.java @@ -0,0 +1,148 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics; + +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.geometry.Twist3d; + +/** + * Class for odometry. Robot code should not use this directly- Instead, use the particular type for + * your drivetrain (e.g., {@link DifferentialDriveOdometry3d}). Odometry allows you to track the + * 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. + * + * @param Wheel positions type. + */ +public class Odometry3d { + private final Kinematics m_kinematics; + 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, + Rotation3d gyroAngle, + T wheelPositions, + Pose3d initialPoseMeters) { + m_kinematics = kinematics; + m_poseMeters = initialPoseMeters; + 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(Rotation3d gyroAngle, T wheelPositions, Pose3d poseMeters) { + m_poseMeters = poseMeters; + 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(Pose3d poseMeters) { + m_gyroOffset = m_gyroOffset.plus(poseMeters.getRotation().minus(m_poseMeters.getRotation())); + m_poseMeters = poseMeters; + m_previousAngle = m_poseMeters.getRotation(); + } + + /** + * Resets the translation of the pose. + * + * @param translation The translation to reset to. + */ + public void resetTranslation(Translation3d translation) { + m_poseMeters = new Pose3d(translation, m_poseMeters.getRotation()); + } + + /** + * Resets the rotation of the pose. + * + * @param rotation The rotation to reset to. + */ + public void resetRotation(Rotation3d rotation) { + m_gyroOffset = m_gyroOffset.plus(rotation.minus(m_poseMeters.getRotation())); + m_poseMeters = new Pose3d(m_poseMeters.getTranslation(), rotation); + m_previousAngle = m_poseMeters.getRotation(); + } + + /** + * Returns the position of the robot on the field. + * + * @return The pose of the robot (x, y, and z are in meters). + */ + 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 Pose3d update(Rotation3d gyroAngle, T wheelPositions) { + var angle = gyroAngle.plus(m_gyroOffset); + var angle_difference = angle.minus(m_previousAngle).getQuaternion().toRotationVector(); + + var twist2d = m_kinematics.toTwist2d(m_previousWheelPositions, wheelPositions); + var twist = + new Twist3d( + twist2d.dx, + twist2d.dy, + 0, + angle_difference.get(0), + angle_difference.get(1), + angle_difference.get(2)); + + var newPose = m_poseMeters.exp(twist); + + m_kinematics.copyInto(wheelPositions, m_previousWheelPositions); + m_previousAngle = angle; + m_poseMeters = new Pose3d(newPose.getTranslation(), angle); + + 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 new file mode 100644 index 00000000000..35fc6fbc3a8 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3d.java @@ -0,0 +1,86 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics; + +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.math.MathUsageId; +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; + +/** + * 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, + Rotation3d gyroAngle, + SwerveModulePosition[] modulePositions, + Pose3d 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, + Rotation3d gyroAngle, + SwerveModulePosition[] modulePositions) { + this(kinematics, gyroAngle, modulePositions, Pose3d.kZero); + } + + @Override + public void resetPosition( + Rotation3d gyroAngle, SwerveModulePosition[] modulePositions, Pose3d 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 Pose3d update(Rotation3d 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); + } +} diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index 6ddc916a69e..e8ac341e4ce 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -21,9 +21,8 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( units::meter_t leftDistance, units::meter_t rightDistance, const Pose2d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : PoseEstimator( - kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + : PoseEstimator(kinematics, m_odometryImpl, stateStdDevs, + visionMeasurementStdDevs), m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} { - ResetPose(m_odometryImpl.GetPose()); + ResetPose(initialPose); } diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp new file mode 100644 index 00000000000..baa53b3eb44 --- /dev/null +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator3d.cpp @@ -0,0 +1,27 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/estimator/DifferentialDrivePoseEstimator3d.h" + +using namespace frc; + +DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d( + DifferentialDriveKinematics& kinematics, const Rotation3d& gyroAngle, + units::meter_t leftDistance, units::meter_t rightDistance, + const Pose3d& initialPose) + : DifferentialDrivePoseEstimator3d{ + kinematics, gyroAngle, leftDistance, + rightDistance, initialPose, {0.02, 0.02, 0.02, 0.01}, + {0.1, 0.1, 0.1, 0.1}} {} + +DifferentialDrivePoseEstimator3d::DifferentialDrivePoseEstimator3d( + DifferentialDriveKinematics& kinematics, const Rotation3d& gyroAngle, + units::meter_t leftDistance, units::meter_t rightDistance, + const Pose3d& initialPose, const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs) + : PoseEstimator3d(kinematics, m_odometryImpl, stateStdDevs, + visionMeasurementStdDevs), + m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} { + ResetPose(initialPose); +} diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index 5db1df74c06..f5d5ba4ffc4 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -24,8 +24,8 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : PoseEstimator( - kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + : PoseEstimator(kinematics, m_odometryImpl, stateStdDevs, + visionMeasurementStdDevs), m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) { - ResetPose(m_odometryImpl.GetPose()); + ResetPose(initialPose); } diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp new file mode 100644 index 00000000000..644b3da8dfc --- /dev/null +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator3d.cpp @@ -0,0 +1,32 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/estimator/MecanumDrivePoseEstimator3d.h" + +#include + +#include "frc/StateSpaceUtil.h" +#include "frc/estimator/AngleStatistics.h" +#include "wpimath/MathShared.h" + +using namespace frc; + +frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d( + MecanumDriveKinematics& kinematics, const Rotation3d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose) + : MecanumDrivePoseEstimator3d{ + kinematics, gyroAngle, + wheelPositions, initialPose, + {0.1, 0.1, 0.1, 0.1}, {0.45, 0.45, 0.45, 0.45}} {} + +frc::MecanumDrivePoseEstimator3d::MecanumDrivePoseEstimator3d( + MecanumDriveKinematics& kinematics, const Rotation3d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose, + const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs) + : PoseEstimator3d(kinematics, m_odometryImpl, stateStdDevs, + visionMeasurementStdDevs), + m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) { + ResetPose(initialPose); +} diff --git a/wpimath/src/main/native/cpp/estimator/SwerveDrivePoseEstimator3d.cpp b/wpimath/src/main/native/cpp/estimator/SwerveDrivePoseEstimator3d.cpp new file mode 100644 index 00000000000..7f36471b259 --- /dev/null +++ b/wpimath/src/main/native/cpp/estimator/SwerveDrivePoseEstimator3d.cpp @@ -0,0 +1,12 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/estimator/SwerveDrivePoseEstimator3d.h" + +namespace frc { + +template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT) + SwerveDrivePoseEstimator3d<4>; + +} // namespace frc diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp index 346a232e75c..d41a1c077f6 100644 --- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp +++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp @@ -11,9 +11,8 @@ using namespace frc; DifferentialDriveOdometry::DifferentialDriveOdometry( const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose2d& initialPose) - : Odometry( - m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance}, - initialPose) { + : Odometry(m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance}, + initialPose) { wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1); } diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp new file mode 100644 index 00000000000..93fcddbdf00 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry3d.cpp @@ -0,0 +1,18 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/kinematics/DifferentialDriveOdometry3d.h" + +#include "wpimath/MathShared.h" + +using namespace frc; + +DifferentialDriveOdometry3d::DifferentialDriveOdometry3d( + const Rotation3d& gyroAngle, units::meter_t leftDistance, + units::meter_t rightDistance, const Pose3d& initialPose) + : Odometry3d(m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance}, + initialPose) { + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1); +} diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp index 55055be744e..f3431e469ec 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp @@ -11,8 +11,7 @@ using namespace frc; MecanumDriveOdometry::MecanumDriveOdometry( MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle, const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose) - : Odometry( - m_kinematicsImpl, gyroAngle, wheelPositions, initialPose), + : Odometry(m_kinematicsImpl, gyroAngle, wheelPositions, initialPose), m_kinematicsImpl(kinematics) { wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_MecanumDrive, 1); diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp new file mode 100644 index 00000000000..bd6954006fe --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry3d.cpp @@ -0,0 +1,18 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/kinematics/MecanumDriveOdometry3d.h" + +#include "wpimath/MathShared.h" + +using namespace frc; + +MecanumDriveOdometry3d::MecanumDriveOdometry3d( + MecanumDriveKinematics kinematics, const Rotation3d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, const Pose3d& initialPose) + : Odometry3d(m_kinematicsImpl, gyroAngle, wheelPositions, initialPose), + m_kinematicsImpl(kinematics) { + wpi::math::MathSharedStore::ReportUsage( + wpi::math::MathUsageId::kOdometry_MecanumDrive, 1); +} diff --git a/wpimath/src/main/native/cpp/kinematics/SwerveDriveOdometry3d.cpp b/wpimath/src/main/native/cpp/kinematics/SwerveDriveOdometry3d.cpp new file mode 100644 index 00000000000..2325081c0e6 --- /dev/null +++ b/wpimath/src/main/native/cpp/kinematics/SwerveDriveOdometry3d.cpp @@ -0,0 +1,12 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "frc/kinematics/SwerveDriveOdometry3d.h" + +namespace frc { + +template class EXPORT_TEMPLATE_DEFINE(WPILIB_DLLEXPORT) + SwerveDriveOdometry3d<4>; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index 89dcd35cb06..ccd09a5d0a9 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -89,9 +89,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator */ void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose2d& pose) { - PoseEstimator:: - ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose); + PoseEstimator::ResetPosition(gyroAngle, {leftDistance, rightDistance}, + pose); } /** @@ -106,10 +105,7 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator */ Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance) { - return PoseEstimator< - DifferentialDriveWheelSpeeds, - DifferentialDriveWheelPositions>::Update(gyroAngle, - {leftDistance, rightDistance}); + return PoseEstimator::Update(gyroAngle, {leftDistance, rightDistance}); } /** @@ -127,11 +123,8 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance) { - return PoseEstimator< - DifferentialDriveWheelSpeeds, - DifferentialDriveWheelPositions>::UpdateWithTime(currentTime, gyroAngle, - {leftDistance, - rightDistance}); + return PoseEstimator::UpdateWithTime(currentTime, gyroAngle, + {leftDistance, rightDistance}); } private: diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h new file mode 100644 index 00000000000..371536b2980 --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator3d.h @@ -0,0 +1,139 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/estimator/PoseEstimator3d.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/kinematics/DifferentialDriveOdometry3d.h" +#include "units/time.h" + +namespace frc { +/** + * This class wraps Differential Drive Odometry to fuse latency-compensated + * vision measurements with differential drive encoder measurements. It will + * 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. 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 + * time via the constructor). + * + * AddVisionMeasurement() can be called as infrequently as you want; if you + * never call it, then this class will behave like regular encoder odometry. + */ +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, 0.02 meters for z, and 0.01 radians + * for angle. The default standard deviations of the vision measurements are + * 0.1 meters for x, 0.1 meters for y, 0.1 meters for z, and 0.1 radians for + * angle. + * + * @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 Rotation3d& gyroAngle, + units::meter_t leftDistance, + units::meter_t rightDistance, + const Pose3d& 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, z 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, z position in + * meters, and angle in radians). Increase these numbers to trust the vision + * pose measurement less. + */ + DifferentialDrivePoseEstimator3d( + DifferentialDriveKinematics& kinematics, const Rotation3d& gyroAngle, + units::meter_t leftDistance, units::meter_t rightDistance, + 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 Rotation3d& gyroAngle, units::meter_t leftDistance, + units::meter_t rightDistance, const Pose3d& pose) { + PoseEstimator3d::ResetPosition(gyroAngle, {leftDistance, rightDistance}, + 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. + */ + Pose3d Update(const Rotation3d& 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. + * + * @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. + */ + Pose3d UpdateWithTime(units::second_t currentTime, + const Rotation3d& gyroAngle, + units::meter_t leftDistance, + units::meter_t rightDistance) { + return PoseEstimator3d::UpdateWithTime(currentTime, gyroAngle, + {leftDistance, rightDistance}); + } + + private: + DifferentialDriveOdometry3d m_odometryImpl; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator3d.h new file mode 100644 index 00000000000..bb1c6ea6936 --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator3d.h @@ -0,0 +1,90 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include + +#include "frc/estimator/PoseEstimator3d.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/interpolation/TimeInterpolatableBuffer.h" +#include "frc/kinematics/MecanumDriveKinematics.h" +#include "frc/kinematics/MecanumDriveOdometry3d.h" +#include "units/time.h" + +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. 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 + * time by specifying it in the constructor. + * + * AddVisionMeasurement() can be called as infrequently as you want; if you + * never call it, then this class will behave mostly like regular encoder + * odometry. + */ +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, 0.1 meters for z, and 0.1 radians for + * angle. The default standard deviations of the vision measurements are 0.45 + * meters for x, 0.45 meters for y, 0.45 meters for z, and 0.45 radians for + * angle. + * + * @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 Rotation3d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, + const Pose3d& 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, z 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, z position in + * meters, and angle in radians). Increase these numbers to trust the vision + * pose measurement less. + */ + MecanumDrivePoseEstimator3d( + MecanumDriveKinematics& kinematics, const Rotation3d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, + const Pose3d& initialPose, const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs); + + private: + MecanumDriveOdometry3d m_odometryImpl; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h new file mode 100644 index 00000000000..bc8bdb08a66 --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator3d.h @@ -0,0 +1,451 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +#include "frc/EigenCore.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Translation2d.h" +#include "frc/interpolation/TimeInterpolatableBuffer.h" +#include "frc/kinematics/Kinematics.h" +#include "frc/kinematics/Odometry3d.h" +#include "units/time.h" +#include "wpimath/MathShared.h" + +namespace frc { + +/** + * This class wraps odometry to fuse latency-compensated + * vision measurements with encoder measurements. Robot code should not use this + * 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. 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. + * + * AddVisionMeasurement() can be called as infrequently as you want; if you + * never call it, then this class will behave like regular encoder odometry. + * + * @tparam WheelSpeeds Wheel speeds type. + * @tparam WheelPositions Wheel positions type. + */ +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 z position + * in meters, and angle 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) + : m_odometry(odometry) { + for (size_t i = 0; i < 4; ++i) { + m_q[i] = stateStdDevs[i] * stateStdDevs[i]; + } + + 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) { + wpi::array r{wpi::empty_array}; + for (size_t i = 0; i < 4; ++i) { + r[i] = visionMeasurementStdDevs[i] * visionMeasurementStdDevs[i]; + } + + // Solve for closed form Kalman gain for continuous Kalman filter with A = 0 + // and C = I. See wpimath/algorithms.md. + for (size_t row = 0; row < 4; ++row) { + if (m_q[row] == 0.0) { + m_visionK(row, row) = 0.0; + } else { + m_visionK(row, row) = + m_q[row] / (m_q[row] + std::sqrt(m_q[row] * r[row])); + } + } + double angle_gain = m_visionK(3, 3); + m_visionK(4, 4) = angle_gain; + 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 Rotation3d& gyroAngle, + const WheelPositions& wheelPositions, const Pose3d& pose) { + // Reset state estimate and error covariance + m_odometry.ResetPosition(gyroAngle, wheelPositions, pose); + m_odometryPoseBuffer.Clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.GetPose(); + } + + /** + * Resets the robot's pose. + * + * @param pose The pose to reset to. + */ + void ResetPose(const Pose3d& pose) { + m_odometry.ResetPose(pose); + m_odometryPoseBuffer.Clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.GetPose(); + } + + /** + * Resets the robot's translation. + * + * @param translation The pose to translation to. + */ + void ResetTranslation(const Translation3d& translation) { + m_odometry.ResetTranslation(translation); + m_odometryPoseBuffer.Clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.GetPose(); + } + + /** + * Resets the robot's rotation. + * + * @param rotation The rotation to reset to. + */ + void ResetRotation(const Rotation3d& rotation) { + m_odometry.ResetRotation(rotation); + m_odometryPoseBuffer.Clear(); + m_visionUpdates.clear(); + m_poseEstimate = m_odometry.GetPose(); + } + + /** + * Gets the estimated robot pose. + * + * @return The estimated robot pose in meters. + */ + Pose3d GetEstimatedPosition() 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 { + // Step 0: If there are no odometry updates to sample, skip. + if (m_odometryPoseBuffer.GetInternalBuffer().empty()) { + return std::nullopt; + } + + // Step 1: Make sure timestamp matches the sample from the odometry pose + // buffer. (When sampling, the buffer will always use a timestamp + // between the first and last timestamps) + units::second_t oldestOdometryTimestamp = + m_odometryPoseBuffer.GetInternalBuffer().front().first; + units::second_t newestOdometryTimestamp = + m_odometryPoseBuffer.GetInternalBuffer().back().first; + timestamp = + std::clamp(timestamp, oldestOdometryTimestamp, newestOdometryTimestamp); + + // Step 2: If there are no applicable vision updates, use the odometry-only + // information. + if (m_visionUpdates.empty() || timestamp < m_visionUpdates.begin()->first) { + return m_odometryPoseBuffer.Sample(timestamp); + } + + // Step 3: Get the latest vision update from before or at the timestamp to + // sample at. + // First, find the iterator past the sample timestamp, then go back one. + // Note that upper_bound() won't return begin() because we check begin() + // earlier. + auto floorIter = m_visionUpdates.upper_bound(timestamp); + --floorIter; + auto visionUpdate = floorIter->second; + + // Step 4: Get the pose measured by odometry at the time of the sample. + auto odometryEstimate = m_odometryPoseBuffer.Sample(timestamp); + + // Step 5: Apply the vision compensation to the odometry pose. + // TODO Replace with std::optional::transform() in C++23 + if (odometryEstimate) { + return visionUpdate.Compensate(*odometryEstimate); + } + 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 Pose3d& visionRobotPose, + units::second_t timestamp) { + // Step 0: If this measurement is old enough to be outside the pose buffer's + // timespan, skip. + if (m_odometryPoseBuffer.GetInternalBuffer().empty() || + m_odometryPoseBuffer.GetInternalBuffer().front().first - + kBufferDuration > + timestamp) { + return; + } + + // Step 1: Clean up any old entries + CleanUpVisionUpdates(); + + // Step 2: Get the pose measured by odometry at the moment the vision + // measurement was made. + auto odometrySample = m_odometryPoseBuffer.Sample(timestamp); + + if (!odometrySample) { + return; + } + + // Step 3: Get the vision-compensated pose estimate at the moment the vision + // measurement was made. + auto visionSample = SampleAt(timestamp); + + if (!visionSample) { + return; + } + + // Step 4: Measure the twist between the old pose estimate and the vision + // pose. + auto twist = visionSample.value().Log(visionRobotPose); + + // Step 5: We should not trust the twist entirely, so instead we scale this + // twist by a Kalman gain matrix representing how much we trust vision + // measurements compared to our current pose. + frc::Vectord<6> k_times_twist = + m_visionK * frc::Vectord<6>{twist.dx.value(), twist.dy.value(), + twist.dz.value(), twist.rx.value(), + twist.ry.value(), twist.rz.value()}; + + // Step 6: Convert back to Twist3d. + Twist3d scaledTwist{ + units::meter_t{k_times_twist(0)}, units::meter_t{k_times_twist(1)}, + units::meter_t{k_times_twist(2)}, units::radian_t{k_times_twist(3)}, + units::radian_t{k_times_twist(4)}, units::radian_t{k_times_twist(5)}}; + + // Step 7: Calculate and record the vision update. + VisionUpdate visionUpdate{visionSample->Exp(scaledTwist), *odometrySample}; + m_visionUpdates[timestamp] = visionUpdate; + + // Step 8: Remove later vision measurements. (Matches previous behavior) + auto firstAfter = m_visionUpdates.upper_bound(timestamp); + m_visionUpdates.erase(firstAfter, m_visionUpdates.end()); + + // 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.GetPose()); + } + + /** + * 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 Pose3d& visionRobotPose, units::second_t timestamp, + const wpi::array& visionMeasurementStdDevs) { + SetVisionMeasurementStdDevs(visionMeasurementStdDevs); + 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. + */ + Pose3d Update(const Rotation3d& gyroAngle, + const WheelPositions& wheelPositions) { + return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle, + 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. + */ + Pose3d UpdateWithTime(units::second_t currentTime, + const Rotation3d& gyroAngle, + const WheelPositions& wheelPositions) { + auto odometryEstimate = m_odometry.Update(gyroAngle, wheelPositions); + + m_odometryPoseBuffer.AddSample(currentTime, odometryEstimate); + + if (m_visionUpdates.empty()) { + m_poseEstimate = odometryEstimate; + } else { + auto visionUpdate = m_visionUpdates.rbegin()->second; + m_poseEstimate = visionUpdate.Compensate(odometryEstimate); + } + + return GetEstimatedPosition(); + } + + private: + /** + * Removes stale vision updates that won't affect sampling. + */ + void CleanUpVisionUpdates() { + // Step 0: If there are no odometry samples, skip. + if (m_odometryPoseBuffer.GetInternalBuffer().empty()) { + return; + } + + // Step 1: Find the oldest timestamp that needs a vision update. + units::second_t oldestOdometryTimestamp = + m_odometryPoseBuffer.GetInternalBuffer().front().first; + + // Step 2: If there are no vision updates before that timestamp, skip. + if (m_visionUpdates.empty() || + oldestOdometryTimestamp < m_visionUpdates.begin()->first) { + return; + } + + // Step 3: Find the newest vision update timestamp before or at the oldest + // timestamp. + // First, find the iterator past the oldest odometry timestamp, then go + // back one. Note that upper_bound() won't return begin() because we check + // begin() earlier. + auto newestNeededVisionUpdate = + m_visionUpdates.upper_bound(oldestOdometryTimestamp); + --newestNeededVisionUpdate; + + // Step 4: Remove all entries strictly before the newest timestamp we need. + m_visionUpdates.erase(m_visionUpdates.begin(), newestNeededVisionUpdate); + } + + struct VisionUpdate { + // The vision-compensated pose estimate + Pose3d visionPose; + + // The pose estimated based solely on odometry + Pose3d odometryPose; + + /** + * Returns the vision-compensated version of the pose. Specifically, changes + * the pose from being relative to this record's odometry pose to being + * relative to this record's vision pose. + * + * @param pose The pose to compensate. + * @return The compensated pose. + */ + Pose3d Compensate(const Pose3d& pose) const { + auto delta = pose - odometryPose; + return visionPose + delta; + } + }; + + static constexpr units::second_t kBufferDuration = 1.5_s; + + Odometry3d& m_odometry; + wpi::array m_q{wpi::empty_array}; + frc::Matrixd<6, 6> m_visionK = frc::Matrixd<6, 6>::Zero(); + + // Maps timestamps to odometry-only pose estimates + TimeInterpolatableBuffer m_odometryPoseBuffer{kBufferDuration}; + // Maps timestamps to vision updates + // Always contains one entry before the oldest entry in m_odometryPoseBuffer, + // unless there have been no vision measurements after the last reset + std::map m_visionUpdates; + + Pose3d m_poseEstimate; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h index 7205771e50b..a03dde93993 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -82,8 +82,7 @@ class SwerveDrivePoseEstimator const wpi::array& modulePositions, const Pose2d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : PoseEstimator, - wpi::array>( + : SwerveDrivePoseEstimator::PoseEstimator( kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} { this->ResetPose(initialPose); diff --git a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h new file mode 100644 index 00000000000..7efcbb573cd --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator3d.h @@ -0,0 +1,104 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include + +#include "frc/estimator/PoseEstimator3d.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/kinematics/SwerveDriveKinematics.h" +#include "frc/kinematics/SwerveDriveOdometry3d.h" +#include "units/time.h" + +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. 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. + * + * AddVisionMeasurement() can be called as infrequently as you want; if you + * never call it, then this class will behave as regular encoder + * odometry. + */ +template +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, 0.1 meters for z, and 0.1 radians for + * angle. The default standard deviations of the vision measurements are 0.9 + * meters for x, 0.9 meters for y, 0.9 meters for z, and 0.9 radians for + * angle. + * + * @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 Rotation3d& gyroAngle, + const wpi::array& modulePositions, + const Pose3d& initialPose) + : SwerveDrivePoseEstimator3d{kinematics, gyroAngle, + modulePositions, initialPose, + {0.1, 0.1, 0.1, 0.1}, {0.9, 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 Rotation3d& gyroAngle, + const wpi::array& modulePositions, + const Pose3d& 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); + } + + private: + SwerveDriveOdometry3d m_odometryImpl; +}; + +extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT) + SwerveDrivePoseEstimator3d<4>; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h index 279c1dcbf21..c0c406c22f1 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h @@ -62,11 +62,7 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry */ void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose2d& pose) { - Odometry::ResetPosition(gyroAngle, - {leftDistance, - rightDistance}, - pose); + Odometry::ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose); } /** @@ -82,10 +78,7 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry */ const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance) { - return Odometry::Update(gyroAngle, - {leftDistance, - rightDistance}); + return Odometry::Update(gyroAngle, {leftDistance, rightDistance}); } private: diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h new file mode 100644 index 00000000000..d6470442f27 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry3d.h @@ -0,0 +1,87 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/kinematics/DifferentialDriveWheelPositions.h" +#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" +#include "frc/kinematics/Odometry3d.h" +#include "units/length.h" + +namespace frc { +/** + * 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. + * + * 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. + * + * It is important that you reset your encoders to zero before using this class. + * Any subsequent pose resets also require the encoders to be reset to zero. + */ +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 Rotation3d& gyroAngle, + units::meter_t leftDistance, + 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 Rotation3d& gyroAngle, units::meter_t leftDistance, + units::meter_t rightDistance, const Pose3d& pose) { + 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 Pose3d& Update(const Rotation3d& gyroAngle, units::meter_t leftDistance, + units::meter_t rightDistance) { + return Odometry3d::Update(gyroAngle, {leftDistance, rightDistance}); + } + + private: + DifferentialDriveKinematics m_kinematicsImpl{units::meter_t{1}}; +}; +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry3d.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry3d.h new file mode 100644 index 00000000000..2f3726e45de --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry3d.h @@ -0,0 +1,48 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/kinematics/MecanumDriveKinematics.h" +#include "frc/kinematics/MecanumDriveWheelPositions.h" +#include "frc/kinematics/MecanumDriveWheelSpeeds.h" +#include "frc/kinematics/Odometry3d.h" +#include "units/time.h" + +namespace frc { + +/** + * 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. + * + * 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. + */ +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 Rotation3d& gyroAngle, + const MecanumDriveWheelPositions& wheelPositions, + const Pose3d& initialPose = Pose3d{}); + + private: + MecanumDriveKinematics m_kinematicsImpl; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h b/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h new file mode 100644 index 00000000000..ffdfb05bef1 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/Odometry3d.h @@ -0,0 +1,153 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Pose3d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/geometry/Rotation3d.h" +#include "frc/geometry/Translation2d.h" +#include "frc/geometry/Translation3d.h" +#include "frc/kinematics/Kinematics.h" + +namespace frc { + +/** + * Class for odometry. Robot code should not use this directly- Instead, use the + * particular type for your drivetrain (e.g., DifferentialDriveOdometry3d). + * Odometry allows you to track the robot's position on the field over a course + * of a match using readings from your wheel encoders. + * + * 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. + * + * @tparam WheelSpeeds Wheel speeds type. + * @tparam WheelPositions Wheel positions type. + */ +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 Rotation3d& gyroAngle, + const WheelPositions& wheelPositions, + const Pose3d& initialPose = Pose3d{}) + : m_kinematics(kinematics), + m_pose(initialPose), + m_previousWheelPositions(wheelPositions) { + 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 Rotation3d& gyroAngle, + const WheelPositions& wheelPositions, const Pose3d& pose) { + m_pose = pose; + 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 Pose3d& pose) { + m_gyroOffset = m_gyroOffset + (pose.Rotation() - m_pose.Rotation()); + m_pose = pose; + m_previousAngle = pose.Rotation(); + } + + /** + * Resets the translation of the pose. + * + * @param translation The translation to reset to. + */ + void ResetTranslation(const Translation3d& translation) { + m_pose = Pose3d{translation, m_pose.Rotation()}; + } + + /** + * Resets the rotation of the pose. + * + * @param rotation The rotation to reset to. + */ + void ResetRotation(const Rotation3d& rotation) { + m_gyroOffset = m_gyroOffset + (rotation - m_pose.Rotation()); + m_pose = Pose3d{m_pose.Translation(), rotation}; + m_previousAngle = rotation; + } + + /** + * Returns the position of the robot on the field. + * @return The pose of the robot. + */ + const Pose3d& GetPose() 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 Pose3d& Update(const Rotation3d& gyroAngle, + const WheelPositions& wheelPositions) { + auto angle = gyroAngle + m_gyroOffset; + auto angle_difference = + (angle - m_previousAngle).GetQuaternion().ToRotationVector(); + + auto twist2d = + m_kinematics.ToTwist2d(m_previousWheelPositions, wheelPositions); + Twist3d twist{twist2d.dx, + twist2d.dy, + 0_m, + units::radian_t{angle_difference(0)}, + units::radian_t{angle_difference(1)}, + units::radian_t{angle_difference(2)}}; + + auto newPose = m_pose.Exp(twist); + + m_previousWheelPositions = wheelPositions; + m_previousAngle = angle; + m_pose = {newPose.Translation(), angle}; + + return m_pose; + } + + private: + const Kinematics& m_kinematics; + Pose3d m_pose; + + WheelPositions m_previousWheelPositions; + Rotation3d m_previousAngle; + Rotation3d m_gyroOffset; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h index c00acd2e0da..91eb87d8a30 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h @@ -45,9 +45,8 @@ class SwerveDriveOdometry SwerveDriveKinematics kinematics, const Rotation2d& gyroAngle, const wpi::array& modulePositions, const Pose2d& initialPose = Pose2d{}) - : Odometry, - wpi::array>( - m_kinematicsImpl, gyroAngle, modulePositions, initialPose), + : SwerveDriveOdometry::Odometry(m_kinematicsImpl, gyroAngle, + modulePositions, initialPose), m_kinematicsImpl(kinematics) { wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_SwerveDrive, 1); diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h new file mode 100644 index 00000000000..0215a2c5dc8 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry3d.h @@ -0,0 +1,70 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include + +#include +#include + +#include "Odometry3d.h" +#include "SwerveDriveKinematics.h" +#include "SwerveModulePosition.h" +#include "SwerveModuleState.h" +#include "frc/geometry/Pose2d.h" +#include "units/time.h" + +namespace frc { + +/** + * 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. + * + * 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. + */ +template +class SwerveDriveOdometry3d + : public Odometry3d, + wpi::array> { + public: + /** + * 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, + const wpi::array& modulePositions, + const Pose3d& initialPose = Pose3d{}) + : 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__) + + private: + SwerveDriveKinematics m_kinematicsImpl; +}; + +extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT) + SwerveDriveOdometry3d<4>; + +} // namespace frc 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 new file mode 100644 index 00000000000..7912d3f9f54 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator3dTest.java @@ -0,0 +1,471 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.estimator; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +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; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import java.util.List; +import java.util.Optional; +import java.util.Random; +import java.util.TreeMap; +import java.util.function.Function; +import org.junit.jupiter.api.Test; + +class DifferentialDrivePoseEstimator3dTest { + private static final double kEpsilon = 1e-9; + + @Test + void testAccuracy() { + var kinematics = new DifferentialDriveKinematics(1); + + var estimator = + new DifferentialDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + 0, + 0, + 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( + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + new Pose2d(3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(135)), + new Pose2d(-3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(45))), + new TrajectoryConfig(2, 2)); + + testFollowTrajectory( + kinematics, + estimator, + trajectory, + state -> + new ChassisSpeeds( + state.velocityMetersPerSecond, + 0, + state.velocityMetersPerSecond * state.curvatureRadPerMeter), + state -> state.poseMeters, + trajectory.getInitialPose(), + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + 0.02, + 0.1, + 0.25, + true); + } + + @Test + void testBadInitialPose() { + var kinematics = new DifferentialDriveKinematics(1); + + var estimator = + new DifferentialDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + 0, + 0, + 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( + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + new Pose2d(3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(135)), + new Pose2d(-3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(45))), + new TrajectoryConfig(2, 2)); + + for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) { + for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) { + var pose_offset = Rotation2d.fromDegrees(offset_direction_degs); + var heading_offset = Rotation2d.fromDegrees(offset_heading_degs); + + var initial_pose = + trajectory + .getInitialPose() + .plus( + new Transform2d( + new Translation2d(pose_offset.getCos(), pose_offset.getSin()), + heading_offset)); + + testFollowTrajectory( + kinematics, + estimator, + trajectory, + state -> + new ChassisSpeeds( + state.velocityMetersPerSecond, + 0, + state.velocityMetersPerSecond * state.curvatureRadPerMeter), + state -> state.poseMeters, + initial_pose, + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + 0.02, + 0.1, + 0.25, + false); + } + } + } + + void testFollowTrajectory( + final DifferentialDriveKinematics kinematics, + final DifferentialDrivePoseEstimator3d estimator, + final Trajectory trajectory, + final Function chassisSpeedsGenerator, + final Function visionMeasurementGenerator, + final Pose2d startingPose, + final Pose2d endingPose, + final double dt, + final double visionUpdateRate, + final double visionUpdateDelay, + final boolean checkError) { + double leftDistanceMeters = 0; + double rightDistanceMeters = 0; + + estimator.resetPosition( + Rotation3d.kZero, leftDistanceMeters, rightDistanceMeters, new Pose3d(startingPose)); + + var rand = new Random(3538); + + double t = 0.0; + + final TreeMap visionUpdateQueue = new TreeMap<>(); + + double maxError = Double.NEGATIVE_INFINITY; + double errorSum = 0; + while (t <= trajectory.getTotalTimeSeconds()) { + var groundTruthState = trajectory.sample(t); + + // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the + // last vision measurement + if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) { + Pose2d newVisionPose = + visionMeasurementGenerator + .apply(groundTruthState) + .plus( + new Transform2d( + new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1), + new Rotation2d(rand.nextGaussian() * 0.05))); + + visionUpdateQueue.put(t, newVisionPose); + } + + // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds + // since it was measured + if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) { + var visionEntry = visionUpdateQueue.pollFirstEntry(); + estimator.addVisionMeasurement(new Pose3d(visionEntry.getValue()), visionEntry.getKey()); + } + + var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState); + + var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); + + leftDistanceMeters += wheelSpeeds.leftMetersPerSecond * dt; + rightDistanceMeters += wheelSpeeds.rightMetersPerSecond * dt; + + var xHat = + estimator.updateWithTime( + t, + 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().toTranslation2d()); + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + assertEquals( + endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X"); + assertEquals( + endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y"); + assertEquals( + endingPose.getRotation().getRadians(), + estimator.getEstimatedPosition().getRotation().toRotation2d().getRadians(), + 0.15, + "Incorrect Final Theta"); + + if (checkError) { + assertEquals( + 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error"); + assertEquals(0.0, maxError, 0.2, "Incorrect max error"); + } + } + + @Test + void testSimultaneousVisionMeasurements() { + // This tests for multiple vision measurements applied at the same time. The expected behavior + // is that all measurements affect the estimated pose. The alternative result is that only one + // vision measurement affects the outcome. If that were the case, after 1000 measurements, the + // estimated pose would converge to that measurement. + var kinematics = new DifferentialDriveKinematics(1); + + var estimator = + new DifferentialDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + 0, + 0, + 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, Rotation3d.kZero, 0, 0); + + var visionMeasurements = + new Pose2d[] { + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(3, 1, Rotation2d.kCCW_Pi_2), + new Pose2d(2, 4, Rotation2d.kPi), + }; + + for (int i = 0; i < 1000; i++) { + for (var measurement : visionMeasurements) { + estimator.addVisionMeasurement(new Pose3d(measurement), 0); + } + } + + for (var measurement : visionMeasurements) { + var errorLog = + "Estimator converged to one vision measurement: " + + estimator.getEstimatedPosition().toString() + + " -> " + + measurement; + + var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX()); + var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY()); + var dtheta = + Math.abs( + measurement.getRotation().getDegrees() + - estimator.getEstimatedPosition().getRotation().toRotation2d().getDegrees()); + + assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog); + } + } + + @Test + void testDiscardsStaleVisionMeasurements() { + var kinematics = new DifferentialDriveKinematics(1); + var estimator = + new DifferentialDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + 0, + 0, + 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, Rotation3d.kZero, 0, 0); + } + + var odometryPose = estimator.getEstimatedPosition(); + + // Apply a vision measurement made 3 seconds ago + // This test passes if this does not cause a ConcurrentModificationException. + estimator.addVisionMeasurement( + new Pose3d(10, 10, 0, new Rotation3d(0, 0, 0.1)), 1, VecBuilder.fill(0.1, 0.1, 0.1, 0.1)); + + 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 + void testSampleAt() { + var kinematics = new DifferentialDriveKinematics(1); + var estimator = + new DifferentialDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + 0, + 0, + Pose3d.kZero, + VecBuilder.fill(1, 1, 1, 1), + VecBuilder.fill(1, 1, 1, 1)); + + // Returns empty when null + assertEquals(Optional.empty(), estimator.sampleAt(1)); + + // 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, Rotation3d.kZero, time, time); + } + + // Sample at an added time + assertEquals(Optional.of(new Pose3d(1.02, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.02)); + // Sample between updates (test interpolation) + 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 Pose3d(1, 0, 0, Rotation3d.kZero)), estimator.sampleAt(0.5)); + // Sampling after the newest value returns the newest value + 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 Pose3d(2, 0, 0, new Rotation3d(0, 0, 1)), 2.2); + + // Make sure nothing changed (except the newest value) + 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 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 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 + void testReset() { + var kinematics = new DifferentialDriveKinematics(1); + var estimator = + new DifferentialDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + 0, + 0, + Pose3d.kZero, + VecBuilder.fill(1, 1, 1, 1), + VecBuilder.fill(1, 1, 1, 1)); + + // Test reset position + 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().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(Rotation3d.kZero, 2, 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(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); + + // Test reset rotation + 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().getZ(), kEpsilon)); + + // Test orientation + 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().getZ(), kEpsilon)); + + // Test reset translation + 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().getZ(), kEpsilon)); + + // Test reset pose + estimator.resetPose(Pose3d.kZero); + + assertAll( + () -> assertEquals(0, 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(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 new file mode 100644 index 00000000000..2ec6a200e7c --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator3dTest.java @@ -0,0 +1,500 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.estimator; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +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; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import java.util.List; +import java.util.Optional; +import java.util.Random; +import java.util.TreeMap; +import java.util.function.Function; +import org.junit.jupiter.api.Test; + +class MecanumDrivePoseEstimator3dTest { + private static final double kEpsilon = 1e-9; + + @Test + void testAccuracyFacingTrajectory() { + var kinematics = + new MecanumDriveKinematics( + new Translation2d(1, 1), new Translation2d(1, -1), + new Translation2d(-1, -1), new Translation2d(-1, 1)); + + var wheelPositions = new MecanumDriveWheelPositions(); + + var estimator = + new MecanumDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + wheelPositions, + 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( + List.of( + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + new Pose2d(3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(135)), + new Pose2d(-3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(45))), + new TrajectoryConfig(2, 2)); + + testFollowTrajectory( + kinematics, + estimator, + trajectory, + state -> + new ChassisSpeeds( + state.velocityMetersPerSecond, + 0, + state.velocityMetersPerSecond * state.curvatureRadPerMeter), + state -> state.poseMeters, + trajectory.getInitialPose(), + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + 0.02, + 0.1, + 0.25, + true); + } + + @Test + void testBadInitialPose() { + var kinematics = + new MecanumDriveKinematics( + new Translation2d(1, 1), new Translation2d(1, -1), + new Translation2d(-1, -1), new Translation2d(-1, 1)); + + var wheelPositions = new MecanumDriveWheelPositions(); + + var estimator = + new MecanumDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + wheelPositions, + 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( + List.of( + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + new Pose2d(3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(135)), + new Pose2d(-3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(45))), + new TrajectoryConfig(2, 2)); + + for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) { + for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) { + var pose_offset = Rotation2d.fromDegrees(offset_direction_degs); + var heading_offset = Rotation2d.fromDegrees(offset_heading_degs); + + var initial_pose = + trajectory + .getInitialPose() + .plus( + new Transform2d( + new Translation2d(pose_offset.getCos(), pose_offset.getSin()), + heading_offset)); + + testFollowTrajectory( + kinematics, + estimator, + trajectory, + state -> + new ChassisSpeeds( + state.velocityMetersPerSecond, + 0, + state.velocityMetersPerSecond * state.curvatureRadPerMeter), + state -> state.poseMeters, + initial_pose, + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + 0.02, + 0.1, + 0.25, + false); + } + } + } + + void testFollowTrajectory( + final MecanumDriveKinematics kinematics, + final MecanumDrivePoseEstimator3d estimator, + final Trajectory trajectory, + final Function chassisSpeedsGenerator, + final Function visionMeasurementGenerator, + final Pose2d startingPose, + final Pose2d endingPose, + final double dt, + final double visionUpdateRate, + final double visionUpdateDelay, + final boolean checkError) { + var wheelPositions = new MecanumDriveWheelPositions(); + + estimator.resetPosition(Rotation3d.kZero, wheelPositions, new Pose3d(startingPose)); + + var rand = new Random(3538); + + double t = 0.0; + + final TreeMap visionUpdateQueue = new TreeMap<>(); + + double maxError = Double.NEGATIVE_INFINITY; + double errorSum = 0; + while (t <= trajectory.getTotalTimeSeconds()) { + var groundTruthState = trajectory.sample(t); + + // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the + // last vision measurement + if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) { + Pose2d newVisionPose = + visionMeasurementGenerator + .apply(groundTruthState) + .plus( + new Transform2d( + new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1), + new Rotation2d(rand.nextGaussian() * 0.05))); + + visionUpdateQueue.put(t, newVisionPose); + } + + // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds + // since it was measured + if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) { + var visionEntry = visionUpdateQueue.pollFirstEntry(); + estimator.addVisionMeasurement(new Pose3d(visionEntry.getValue()), visionEntry.getKey()); + } + + var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState); + + var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds); + + wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt; + wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt; + wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt; + wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt; + + var xHat = + estimator.updateWithTime( + t, + 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().toTranslation2d()); + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + assertEquals( + endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X"); + assertEquals( + endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y"); + assertEquals( + endingPose.getRotation().getRadians(), + estimator.getEstimatedPosition().getRotation().toRotation2d().getRadians(), + 0.15, + "Incorrect Final Theta"); + + if (checkError) { + assertEquals( + 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error"); + assertEquals(0.0, maxError, 0.2, "Incorrect max error"); + } + } + + @Test + void testSimultaneousVisionMeasurements() { + // This tests for multiple vision measurements applied at the same time. The expected behavior + // is that all measurements affect the estimated pose. The alternative result is that only one + // vision measurement affects the outcome. If that were the case, after 1000 measurements, the + // estimated pose would converge to that measurement. + var kinematics = + new MecanumDriveKinematics( + new Translation2d(1, 1), new Translation2d(1, -1), + new Translation2d(-1, -1), new Translation2d(-1, 1)); + + var wheelPositions = new MecanumDriveWheelPositions(); + + var estimator = + new MecanumDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + wheelPositions, + 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, Rotation3d.kZero, wheelPositions); + + var visionMeasurements = + new Pose2d[] { + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(3, 1, Rotation2d.kCCW_Pi_2), + new Pose2d(2, 4, Rotation2d.kPi), + }; + + for (int i = 0; i < 1000; i++) { + for (var measurement : visionMeasurements) { + estimator.addVisionMeasurement(new Pose3d(measurement), 0); + } + } + + for (var measurement : visionMeasurements) { + var errorLog = + "Estimator converged to one vision measurement: " + + estimator.getEstimatedPosition().toString() + + " -> " + + measurement; + + var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX()); + var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY()); + var dtheta = + Math.abs( + measurement.getRotation().getDegrees() + - estimator.getEstimatedPosition().getRotation().toRotation2d().getDegrees()); + + assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog); + } + } + + @Test + void testDiscardsOldVisionMeasurements() { + var kinematics = + new MecanumDriveKinematics( + new Translation2d(1, 1), + new Translation2d(-1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1)); + var estimator = + new MecanumDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + new MecanumDriveWheelPositions(), + 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, Rotation3d.kZero, new MecanumDriveWheelPositions()); + } + + var odometryPose = estimator.getEstimatedPosition(); + + // Apply a vision measurement made 3 seconds ago + // This test passes if this does not cause a ConcurrentModificationException. + estimator.addVisionMeasurement( + new Pose3d(10, 10, 0, new Rotation3d(0, 0, 0.1)), 1, VecBuilder.fill(0.1, 0.1, 0.1, 0.1)); + + 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 + void testSampleAt() { + var kinematics = + new MecanumDriveKinematics( + new Translation2d(1, 1), + new Translation2d(-1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1)); + var estimator = + new MecanumDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + new MecanumDriveWheelPositions(), + Pose3d.kZero, + VecBuilder.fill(1, 1, 1, 1), + VecBuilder.fill(1, 1, 1, 1)); + + // Returns empty when null + assertEquals(Optional.empty(), estimator.sampleAt(1)); + + // 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) { + var wheelPositions = new MecanumDriveWheelPositions(time, time, time, time); + estimator.updateWithTime(time, Rotation3d.kZero, wheelPositions); + } + + // Sample at an added time + assertEquals(Optional.of(new Pose3d(1.02, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.02)); + // Sample between updates (test interpolation) + 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 Pose3d(1, 0, 0, Rotation3d.kZero)), estimator.sampleAt(0.5)); + // Sampling after the newest value returns the newest value + 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 Pose3d(2, 0, 0, new Rotation3d(0, 0, 1)), 2.2); + + // Make sure nothing changed (except the newest value) + 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 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 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 + void testReset() { + var kinematics = + new MecanumDriveKinematics( + new Translation2d(1, 1), + new Translation2d(-1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1)); + var estimator = + new MecanumDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + new MecanumDriveWheelPositions(), + Pose3d.kZero, + VecBuilder.fill(1, 1, 1, 1), + VecBuilder.fill(1, 1, 1, 1)); + + // Test reset position + estimator.resetPosition( + Rotation3d.kZero, + new MecanumDriveWheelPositions(1, 1, 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().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(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().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(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().getZ(), kEpsilon)); + + // Test orientation + 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().getZ(), kEpsilon)); + + // Test reset translation + 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().getZ(), kEpsilon)); + + // Test reset pose + estimator.resetPose(Pose3d.kZero); + + assertAll( + () -> assertEquals(0, 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(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 new file mode 100644 index 00000000000..e3cd11a2815 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator3dTest.java @@ -0,0 +1,569 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.estimator; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +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; +import edu.wpi.first.math.trajectory.Trajectory; +import edu.wpi.first.math.trajectory.TrajectoryConfig; +import edu.wpi.first.math.trajectory.TrajectoryGenerator; +import java.util.List; +import java.util.Optional; +import java.util.Random; +import java.util.TreeMap; +import java.util.function.Function; +import org.junit.jupiter.api.Test; + +class SwerveDrivePoseEstimator3dTest { + private static final double kEpsilon = 1e-9; + + @Test + void testAccuracyFacingTrajectory() { + var kinematics = + new SwerveDriveKinematics( + new Translation2d(1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1), + new Translation2d(-1, 1)); + + var fl = new SwerveModulePosition(); + var fr = new SwerveModulePosition(); + var bl = new SwerveModulePosition(); + var br = new SwerveModulePosition(); + + var estimator = + new SwerveDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + new SwerveModulePosition[] {fl, fr, bl, br}, + 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( + List.of( + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + new Pose2d(3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(135)), + new Pose2d(-3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(45))), + new TrajectoryConfig(2, 2)); + + testFollowTrajectory( + kinematics, + estimator, + trajectory, + state -> + new ChassisSpeeds( + state.velocityMetersPerSecond, + 0, + state.velocityMetersPerSecond * state.curvatureRadPerMeter), + state -> state.poseMeters, + trajectory.getInitialPose(), + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + 0.02, + 0.1, + 0.25, + true); + } + + @Test + void testBadInitialPose() { + var kinematics = + new SwerveDriveKinematics( + new Translation2d(1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1), + new Translation2d(-1, 1)); + + var fl = new SwerveModulePosition(); + var fr = new SwerveModulePosition(); + var bl = new SwerveModulePosition(); + var br = new SwerveModulePosition(); + + var estimator = + new SwerveDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + new SwerveModulePosition[] {fl, fr, bl, br}, + 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( + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + new Pose2d(3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(135)), + new Pose2d(-3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(45))), + new TrajectoryConfig(2, 2)); + + for (int offset_direction_degs = 0; offset_direction_degs < 360; offset_direction_degs += 45) { + for (int offset_heading_degs = 0; offset_heading_degs < 360; offset_heading_degs += 45) { + var pose_offset = Rotation2d.fromDegrees(offset_direction_degs); + var heading_offset = Rotation2d.fromDegrees(offset_heading_degs); + + var initial_pose = + trajectory + .getInitialPose() + .plus( + new Transform2d( + new Translation2d(pose_offset.getCos(), pose_offset.getSin()), + heading_offset)); + + testFollowTrajectory( + kinematics, + estimator, + trajectory, + state -> + new ChassisSpeeds( + state.velocityMetersPerSecond, + 0, + state.velocityMetersPerSecond * state.curvatureRadPerMeter), + state -> state.poseMeters, + initial_pose, + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + 0.02, + 0.1, + 1.0, + false); + } + } + } + + void testFollowTrajectory( + final SwerveDriveKinematics kinematics, + final SwerveDrivePoseEstimator3d estimator, + final Trajectory trajectory, + final Function chassisSpeedsGenerator, + final Function visionMeasurementGenerator, + final Pose2d startingPose, + final Pose2d endingPose, + final double dt, + final double visionUpdateRate, + final double visionUpdateDelay, + final boolean checkError) { + SwerveModulePosition[] positions = { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }; + + estimator.resetPosition(Rotation3d.kZero, positions, new Pose3d(startingPose)); + + var rand = new Random(3538); + + double t = 0.0; + + final TreeMap visionUpdateQueue = new TreeMap<>(); + + double maxError = Double.NEGATIVE_INFINITY; + double errorSum = 0; + while (t <= trajectory.getTotalTimeSeconds()) { + var groundTruthState = trajectory.sample(t); + + // We are due for a new vision measurement if it's been `visionUpdateRate` seconds since the + // last vision measurement + if (visionUpdateQueue.isEmpty() || visionUpdateQueue.lastKey() + visionUpdateRate < t) { + Pose2d newVisionPose = + visionMeasurementGenerator + .apply(groundTruthState) + .plus( + new Transform2d( + new Translation2d(rand.nextGaussian() * 0.1, rand.nextGaussian() * 0.1), + new Rotation2d(rand.nextGaussian() * 0.05))); + + visionUpdateQueue.put(t, newVisionPose); + } + + // We should apply the oldest vision measurement if it has been `visionUpdateDelay` seconds + // since it was measured + if (!visionUpdateQueue.isEmpty() && visionUpdateQueue.firstKey() + visionUpdateDelay < t) { + var visionEntry = visionUpdateQueue.pollFirstEntry(); + estimator.addVisionMeasurement(new Pose3d(visionEntry.getValue()), visionEntry.getKey()); + } + + var chassisSpeeds = chassisSpeedsGenerator.apply(groundTruthState); + + var moduleStates = kinematics.toSwerveModuleStates(chassisSpeeds); + + for (int i = 0; i < moduleStates.length; i++) { + positions[i].distanceMeters += + moduleStates[i].speedMetersPerSecond * (1 - rand.nextGaussian() * 0.05) * dt; + positions[i].angle = + moduleStates[i].angle.plus(new Rotation2d(rand.nextGaussian() * 0.005)); + } + + var xHat = + estimator.updateWithTime( + t, + 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().toTranslation2d()); + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + assertEquals( + endingPose.getX(), estimator.getEstimatedPosition().getX(), 0.08, "Incorrect Final X"); + assertEquals( + endingPose.getY(), estimator.getEstimatedPosition().getY(), 0.08, "Incorrect Final Y"); + assertEquals( + endingPose.getRotation().getRadians(), + estimator.getEstimatedPosition().getRotation().toRotation2d().getRadians(), + 0.15, + "Incorrect Final Theta"); + + if (checkError) { + assertEquals( + 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.07, "Incorrect mean error"); + assertEquals(0.0, maxError, 0.2, "Incorrect max error"); + } + } + + @Test + void testSimultaneousVisionMeasurements() { + // This tests for multiple vision measurements applied at the same time. The expected behavior + // is that all measurements affect the estimated pose. The alternative result is that only one + // vision measurement affects the outcome. If that were the case, after 1000 measurements, the + // estimated pose would converge to that measurement. + var kinematics = + new SwerveDriveKinematics( + new Translation2d(1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1), + new Translation2d(-1, 1)); + + var fl = new SwerveModulePosition(); + var fr = new SwerveModulePosition(); + var bl = new SwerveModulePosition(); + var br = new SwerveModulePosition(); + + var estimator = + new SwerveDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + new SwerveModulePosition[] {fl, fr, bl, br}, + 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, Rotation3d.kZero, new SwerveModulePosition[] {fl, fr, bl, br}); + + var visionMeasurements = + new Pose2d[] { + new Pose2d(0, 0, Rotation2d.kZero), + new Pose2d(3, 1, Rotation2d.kCCW_Pi_2), + new Pose2d(2, 4, Rotation2d.kPi), + }; + + for (int i = 0; i < 1000; i++) { + for (var measurement : visionMeasurements) { + estimator.addVisionMeasurement(new Pose3d(measurement), 0); + } + } + + for (var measurement : visionMeasurements) { + var errorLog = + "Estimator converged to one vision measurement: " + + estimator.getEstimatedPosition().toString() + + " -> " + + measurement; + + var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX()); + var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY()); + var dtheta = + Math.abs( + measurement.getRotation().getDegrees() + - estimator.getEstimatedPosition().getRotation().toRotation2d().getDegrees()); + + assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog); + } + } + + @Test + void testDiscardsOldVisionMeasurements() { + var kinematics = + new SwerveDriveKinematics( + new Translation2d(1, 1), + new Translation2d(-1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1)); + var estimator = + new SwerveDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }, + 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, + Rotation3d.kZero, + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }); + } + + var odometryPose = estimator.getEstimatedPosition(); + + // Apply a vision measurement made 3 seconds ago + // This test passes if this does not cause a ConcurrentModificationException. + estimator.addVisionMeasurement( + new Pose3d(10, 10, 0, new Rotation3d(0, 0, 0.1)), 1, VecBuilder.fill(0.1, 0.1, 0.1, 0.1)); + + 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 + void testSampleAt() { + var kinematics = + new SwerveDriveKinematics( + new Translation2d(1, 1), + new Translation2d(-1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1)); + var estimator = + new SwerveDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }, + Pose3d.kZero, + VecBuilder.fill(1, 1, 1, 1), + VecBuilder.fill(1, 1, 1, 1)); + + // Returns empty when null + assertEquals(Optional.empty(), estimator.sampleAt(1)); + + // 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) { + var wheelPositions = + new SwerveModulePosition[] { + new SwerveModulePosition(time, Rotation2d.kZero), + new SwerveModulePosition(time, Rotation2d.kZero), + new SwerveModulePosition(time, Rotation2d.kZero), + new SwerveModulePosition(time, Rotation2d.kZero) + }; + estimator.updateWithTime(time, Rotation3d.kZero, wheelPositions); + } + + // Sample at an added time + assertEquals(Optional.of(new Pose3d(1.02, 0, 0, Rotation3d.kZero)), estimator.sampleAt(1.02)); + // Sample between updates (test interpolation) + 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 Pose3d(1, 0, 0, Rotation3d.kZero)), estimator.sampleAt(0.5)); + // Sampling after the newest value returns the newest value + 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 Pose3d(2, 0, 0, new Rotation3d(0, 0, 1)), 2.2); + + // Make sure nothing changed (except the newest value) + 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 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 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 + void testReset() { + var kinematics = + new SwerveDriveKinematics( + new Translation2d(1, 1), + new Translation2d(-1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1)); + var estimator = + new SwerveDrivePoseEstimator3d( + kinematics, + Rotation3d.kZero, + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }, + 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( + Rotation3d.kZero, + new SwerveModulePosition[] { + modulePosition, modulePosition, modulePosition, modulePosition + }, + new Pose3d(1, 0, 0, Rotation3d.kZero)); + } + + assertAll( + () -> assertEquals(1, 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(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); + + // Test orientation and wheel positions + { + var modulePosition = new SwerveModulePosition(2, Rotation2d.kZero); + estimator.update( + Rotation3d.kZero, + new SwerveModulePosition[] { + modulePosition, modulePosition, modulePosition, modulePosition + }); + } + + 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(0, estimator.getEstimatedPosition().getRotation().getZ(), kEpsilon)); + + // Test reset rotation + 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().getZ(), kEpsilon)); + + // Test orientation + { + var modulePosition = new SwerveModulePosition(3, Rotation2d.kZero); + estimator.update( + Rotation3d.kZero, + new SwerveModulePosition[] { + modulePosition, modulePosition, modulePosition, modulePosition + }); + } + + 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().getZ(), kEpsilon)); + + // Test reset translation + 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().getZ(), kEpsilon)); + + // Test reset pose + estimator.resetPose(Pose3d.kZero); + + assertAll( + () -> assertEquals(0, 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(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 new file mode 100644 index 00000000000..166a615ca9d --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry3dTest.java @@ -0,0 +1,48 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics; + +import static org.junit.jupiter.api.Assertions.assertAll; +import static org.junit.jupiter.api.Assertions.assertEquals; + +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(Rotation3d.kZero, 0, 0); + + @Test + void testInitialize() { + DifferentialDriveOdometry3d odometry = + new DifferentialDriveOdometry3d( + 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.getZ(), 0.0, kEpsilon), + () -> assertEquals(pose.getRotation().toRotation2d().getDegrees(), 45.0, kEpsilon)); + } + + @Test + void testOdometryWithEncoderDistances() { + 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.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 new file mode 100644 index 00000000000..d417e2a120a --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry3dTest.java @@ -0,0 +1,298 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics; + +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.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; + +class MecanumDriveOdometry3dTest { + private final Translation2d m_fl = new Translation2d(12, 12); + private final Translation2d m_fr = new Translation2d(12, -12); + private final Translation2d m_bl = new Translation2d(-12, 12); + private final Translation2d m_br = new Translation2d(-12, -12); + + private final MecanumDriveKinematics m_kinematics = + new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br); + + private final MecanumDriveWheelPositions zero = new MecanumDriveWheelPositions(); + + private final MecanumDriveOdometry3d m_odometry = + new MecanumDriveOdometry3d(m_kinematics, Rotation3d.kZero, zero); + + @Test + void testInitialize() { + MecanumDriveOdometry3d odometry = + new MecanumDriveOdometry3d( + 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.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(Rotation3d.kZero, wheelPositions, Pose3d.kZero); + + 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.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(Rotation3d.kZero, new MecanumDriveWheelPositions(), Pose3d.kZero); + + 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.getZ(), 0.01), + () -> assertEquals(0.0, pose.getRotation().toRotation2d().getDegrees(), 0.01)); + } + + @Test + 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(Rotation3d.kZero, new MecanumDriveWheelPositions(), Pose3d.kZero); + + 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(0.0, pose.getZ(), 0.01), + () -> assertEquals(90.0, pose.getRotation().toRotation2d().getDegrees(), 0.01)); + } + + @Test + void testGyroAngleReset() { + var gyro = new Rotation3d(0, 0, Math.PI / 2); + var fieldAngle = Rotation3d.kZero; + m_odometry.resetPosition( + 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); + + assertAll( + () -> assertEquals(3.536, pose.getX(), 0.1), + () -> assertEquals(0.0, pose.getY(), 0.1), + () -> assertEquals(0.0, pose.getZ(), 0.1), + () -> assertEquals(0.0, pose.getRotation().toRotation2d().getRadians(), 0.1)); + } + + @Test + void testAccuracyFacingTrajectory() { + var kinematics = + new MecanumDriveKinematics( + new Translation2d(1, 1), new Translation2d(1, -1), + new Translation2d(-1, -1), new Translation2d(-1, 1)); + + var wheelPositions = new MecanumDriveWheelPositions(); + + var odometry = + new MecanumDriveOdometry3d(kinematics, Rotation3d.kZero, wheelPositions, Pose3d.kZero); + + var trajectory = + TrajectoryGenerator.generateTrajectory( + List.of( + Pose2d.kZero, + new Pose2d(20, 20, Rotation2d.kZero), + new Pose2d(10, 10, Rotation2d.kPi), + new Pose2d(30, 30, Rotation2d.kZero), + new Pose2d(20, 20, Rotation2d.kPi), + new Pose2d(10, 10, Rotation2d.kZero)), + new TrajectoryConfig(0.5, 2)); + + var rand = new Random(5190); + + final double dt = 0.02; + double t = 0.0; + + double maxError = Double.NEGATIVE_INFINITY; + double errorSum = 0; + double odometryDistanceTravelled = 0; + double trajectoryDistanceTravelled = 0; + while (t <= trajectory.getTotalTimeSeconds()) { + var groundTruthState = trajectory.sample(t); + + trajectoryDistanceTravelled += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + + var wheelSpeeds = + kinematics.toWheelSpeeds( + new ChassisSpeeds( + groundTruthState.velocityMetersPerSecond, + 0, + groundTruthState.velocityMetersPerSecond + * groundTruthState.curvatureRadPerMeter)); + + wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1; + + wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt; + wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt; + wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt; + wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt; + + var lastPose = odometry.getPoseMeters(); + + var xHat = + odometry.update( + 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().toTranslation2d()); + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + assertEquals( + 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.35, "Incorrect mean error"); + assertEquals(0.0, maxError, 0.35, "Incorrect max error"); + assertEquals( + 1.0, + odometryDistanceTravelled / trajectoryDistanceTravelled, + 0.05, + "Incorrect distance travelled"); + } + + @Test + void testAccuracyFacingXAxis() { + var kinematics = + new MecanumDriveKinematics( + new Translation2d(1, 1), new Translation2d(1, -1), + new Translation2d(-1, -1), new Translation2d(-1, 1)); + + var wheelPositions = new MecanumDriveWheelPositions(); + + var odometry = + new MecanumDriveOdometry3d(kinematics, Rotation3d.kZero, wheelPositions, Pose3d.kZero); + + var trajectory = + TrajectoryGenerator.generateTrajectory( + List.of( + Pose2d.kZero, + new Pose2d(20, 20, Rotation2d.kZero), + new Pose2d(10, 10, Rotation2d.kPi), + new Pose2d(30, 30, Rotation2d.kZero), + new Pose2d(20, 20, Rotation2d.kPi), + new Pose2d(10, 10, Rotation2d.kZero)), + new TrajectoryConfig(0.5, 2)); + + var rand = new Random(5190); + + final double dt = 0.02; + double t = 0.0; + + double maxError = Double.NEGATIVE_INFINITY; + double errorSum = 0; + double odometryDistanceTravelled = 0; + double trajectoryDistanceTravelled = 0; + while (t <= trajectory.getTotalTimeSeconds()) { + var groundTruthState = trajectory.sample(t); + + trajectoryDistanceTravelled += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + + var wheelSpeeds = + kinematics.toWheelSpeeds( + new ChassisSpeeds( + groundTruthState.velocityMetersPerSecond + * groundTruthState.poseMeters.getRotation().getCos(), + groundTruthState.velocityMetersPerSecond + * groundTruthState.poseMeters.getRotation().getSin(), + 0)); + + wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1; + wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1; + + wheelPositions.frontLeftMeters += wheelSpeeds.frontLeftMetersPerSecond * dt; + wheelPositions.frontRightMeters += wheelSpeeds.frontRightMetersPerSecond * dt; + wheelPositions.rearLeftMeters += wheelSpeeds.rearLeftMetersPerSecond * dt; + wheelPositions.rearRightMeters += wheelSpeeds.rearRightMetersPerSecond * dt; + + var lastPose = odometry.getPoseMeters(); + + 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().toTranslation2d()); + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + assertEquals( + 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.15, "Incorrect mean error"); + assertEquals(0.0, maxError, 0.3, "Incorrect max error"); + assertEquals( + 1.0, + odometryDistanceTravelled / trajectoryDistanceTravelled, + 0.05, + "Incorrect distance travelled"); + } +} 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 new file mode 100644 index 00000000000..dfc446fceea --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry3dTest.java @@ -0,0 +1,308 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package edu.wpi.first.math.kinematics; + +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.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; + +class SwerveDriveOdometry3dTest { + private final Translation2d m_fl = new Translation2d(12, 12); + private final Translation2d m_fr = new Translation2d(12, -12); + private final Translation2d m_bl = new Translation2d(-12, 12); + private final Translation2d m_br = new Translation2d(-12, -12); + + private final SwerveModulePosition zero = new SwerveModulePosition(); + + private final SwerveDriveKinematics m_kinematics = + new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br); + + private final SwerveDriveOdometry3d m_odometry = + new SwerveDriveOdometry3d( + m_kinematics, Rotation3d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero}); + + @Test + void testInitialize() { + SwerveDriveOdometry3d odometry = + new SwerveDriveOdometry3d( + m_kinematics, + Rotation3d.kZero, + new SwerveModulePosition[] {zero, zero, zero, 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.getZ(), 0.0, 1e-9), + () -> assertEquals(pose.getRotation().toRotation2d().getDegrees(), 45.0, 1e-9)); + } + + @Test + void testTwoIterations() { + // 5 units/sec in the x-axis (forward) + final SwerveModulePosition[] wheelDeltas = { + new SwerveModulePosition(0.5, Rotation2d.kZero), + new SwerveModulePosition(0.5, Rotation2d.kZero), + new SwerveModulePosition(0.5, Rotation2d.kZero), + new SwerveModulePosition(0.5, Rotation2d.kZero) + }; + + m_odometry.update( + Rotation3d.kZero, + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition() + }); + 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, pose.getZ(), 0.01), + () -> assertEquals(0.0, pose.getRotation().toRotation2d().getDegrees(), 0.01)); + } + + @Test + void test90degreeTurn() { + // This is a 90 degree turn about the point between front left and rear left wheels + // Module 0: speed 18.84955592153876 angle 90.0 + // Module 1: speed 42.14888838624436 angle 26.565051177077986 + // Module 2: speed 18.84955592153876 angle -90.0 + // Module 3: speed 42.14888838624436 angle -26.565051177077986 + + final SwerveModulePosition[] wheelDeltas = { + new SwerveModulePosition(18.85, Rotation2d.kCCW_Pi_2), + new SwerveModulePosition(42.15, Rotation2d.fromDegrees(26.565)), + new SwerveModulePosition(18.85, Rotation2d.kCW_Pi_2), + new SwerveModulePosition(42.15, Rotation2d.fromDegrees(-26.565)) + }; + final var zero = new SwerveModulePosition(); + + 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(0.0, pose.getZ(), 0.01), + () -> assertEquals(90.0, pose.getRotation().toRotation2d().getDegrees(), 0.01)); + } + + @Test + void testGyroAngleReset() { + var gyro = new Rotation3d(0, 0, Math.PI / 2); + var fieldAngle = Rotation3d.kZero; + m_odometry.resetPosition( + gyro, + new SwerveModulePosition[] {zero, zero, zero, zero}, + 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); + var pose = m_odometry.update(gyro, new SwerveModulePosition[] {delta, delta, delta, delta}); + + assertAll( + () -> assertEquals(1.0, pose.getX(), 0.1), + () -> assertEquals(0.00, pose.getY(), 0.1), + () -> assertEquals(0.00, pose.getZ(), 0.1), + () -> assertEquals(0.00, pose.getRotation().toRotation2d().getRadians(), 0.1)); + } + + @Test + void testAccuracyFacingTrajectory() { + var kinematics = + new SwerveDriveKinematics( + new Translation2d(1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1), + new Translation2d(-1, 1)); + var odometry = + new SwerveDriveOdometry3d( + kinematics, Rotation3d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero}); + + SwerveModulePosition fl = new SwerveModulePosition(); + SwerveModulePosition fr = new SwerveModulePosition(); + SwerveModulePosition bl = new SwerveModulePosition(); + SwerveModulePosition br = new SwerveModulePosition(); + + var trajectory = + TrajectoryGenerator.generateTrajectory( + List.of( + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + new Pose2d(3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(135)), + new Pose2d(-3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(45))), + new TrajectoryConfig(0.5, 2)); + + var rand = new Random(4915); + + final double dt = 0.02; + double t = 0.0; + + double maxError = Double.NEGATIVE_INFINITY; + double errorSum = 0; + while (t <= trajectory.getTotalTimeSeconds()) { + var groundTruthState = trajectory.sample(t); + + var moduleStates = + kinematics.toSwerveModuleStates( + new ChassisSpeeds( + groundTruthState.velocityMetersPerSecond, + 0.0, + groundTruthState.velocityMetersPerSecond + * groundTruthState.curvatureRadPerMeter)); + for (var moduleState : moduleStates) { + moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005)); + moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1; + } + + fl.distanceMeters += moduleStates[0].speedMetersPerSecond * dt; + fr.distanceMeters += moduleStates[1].speedMetersPerSecond * dt; + bl.distanceMeters += moduleStates[2].speedMetersPerSecond * dt; + br.distanceMeters += moduleStates[3].speedMetersPerSecond * dt; + + fl.angle = moduleStates[0].angle; + fr.angle = moduleStates[1].angle; + bl.angle = moduleStates[2].angle; + br.angle = moduleStates[3].angle; + + var xHat = + odometry.update( + 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().toTranslation2d()); + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X"); + assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y"); + assertEquals(0.0, odometry.getPoseMeters().getZ(), 1e-1, "Incorrect Final Y"); + assertEquals( + Math.PI / 4, + odometry.getPoseMeters().getRotation().toRotation2d().getRadians(), + 10 * Math.PI / 180, + "Incorrect Final Theta"); + + assertEquals( + 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.05, "Incorrect mean error"); + assertEquals(0.0, maxError, 0.125, "Incorrect max error"); + } + + @Test + void testAccuracyFacingXAxis() { + var kinematics = + new SwerveDriveKinematics( + new Translation2d(1, 1), + new Translation2d(1, -1), + new Translation2d(-1, -1), + new Translation2d(-1, 1)); + var odometry = + new SwerveDriveOdometry3d( + kinematics, Rotation3d.kZero, new SwerveModulePosition[] {zero, zero, zero, zero}); + + SwerveModulePosition fl = new SwerveModulePosition(); + SwerveModulePosition fr = new SwerveModulePosition(); + SwerveModulePosition bl = new SwerveModulePosition(); + SwerveModulePosition br = new SwerveModulePosition(); + + var trajectory = + TrajectoryGenerator.generateTrajectory( + List.of( + new Pose2d(0, 0, Rotation2d.fromDegrees(45)), + new Pose2d(3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(135)), + new Pose2d(-3, 0, Rotation2d.kCW_Pi_2), + new Pose2d(0, 0, Rotation2d.fromDegrees(45))), + new TrajectoryConfig(0.5, 2)); + + var rand = new Random(4915); + + final double dt = 0.02; + double t = 0.0; + + double maxError = Double.NEGATIVE_INFINITY; + double errorSum = 0; + while (t <= trajectory.getTotalTimeSeconds()) { + var groundTruthState = trajectory.sample(t); + + fl.distanceMeters += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + fr.distanceMeters += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + bl.distanceMeters += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + br.distanceMeters += + groundTruthState.velocityMetersPerSecond * dt + + 0.5 * groundTruthState.accelerationMetersPerSecondSq * dt * dt; + + fl.angle = groundTruthState.poseMeters.getRotation(); + fr.angle = groundTruthState.poseMeters.getRotation(); + bl.angle = groundTruthState.poseMeters.getRotation(); + br.angle = groundTruthState.poseMeters.getRotation(); + + var xHat = + odometry.update( + new Rotation3d(0, 0, rand.nextGaussian() * 0.05), + new SwerveModulePosition[] {fl, fr, bl, br}); + + double error = + groundTruthState + .poseMeters + .getTranslation() + .getDistance(xHat.getTranslation().toTranslation2d()); + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + assertEquals(0.0, odometry.getPoseMeters().getX(), 1e-1, "Incorrect Final X"); + assertEquals(0.0, odometry.getPoseMeters().getY(), 1e-1, "Incorrect Final Y"); + assertEquals(0.0, odometry.getPoseMeters().getZ(), 1e-1, "Incorrect Final Y"); + assertEquals( + 0.0, + odometry.getPoseMeters().getRotation().toRotation2d().getRadians(), + 10 * Math.PI / 180, + "Incorrect Final Theta"); + + assertEquals( + 0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.06, "Incorrect mean error"); + assertEquals(0.0, maxError, 0.125, "Incorrect max error"); + } +} diff --git a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp new file mode 100644 index 00000000000..e43f662fddf --- /dev/null +++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimator3dTest.cpp @@ -0,0 +1,462 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include +#include +#include + +#include +#include + +#include "frc/StateSpaceUtil.h" +#include "frc/estimator/DifferentialDrivePoseEstimator3d.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/trajectory/TrajectoryGenerator.h" +#include "units/angle.h" +#include "units/length.h" +#include "units/time.h" + +void testFollowTrajectory( + const frc::DifferentialDriveKinematics& kinematics, + frc::DifferentialDrivePoseEstimator3d& estimator, + const frc::Trajectory& trajectory, + std::function + chassisSpeedsGenerator, + std::function + visionMeasurementGenerator, + const frc::Pose2d& startingPose, const frc::Pose2d& endingPose, + const units::second_t dt, const units::second_t kVisionUpdateRate, + const units::second_t kVisionUpdateDelay, const bool checkError, + const bool debug) { + units::meter_t leftDistance = 0_m; + units::meter_t rightDistance = 0_m; + + estimator.ResetPosition(frc::Rotation3d{}, leftDistance, rightDistance, + frc::Pose3d{startingPose}); + + std::default_random_engine generator; + std::normal_distribution distribution(0.0, 1.0); + + units::second_t t = 0_s; + + std::vector> visionPoses; + std::vector> + visionLog; + + double maxError = -std::numeric_limits::max(); + double errorSum = 0; + + if (debug) { + wpi::print( + "time, est_x, est_y, est_theta, true_x, true_y, true_theta, left, " + "right\n"); + } + + while (t < trajectory.TotalTime()) { + frc::Trajectory::State groundTruthState = trajectory.Sample(t); + + // We are due for a new vision measurement if it's been `visionUpdateRate` + // seconds since the last vision measurement + if (visionPoses.empty() || + visionPoses.back().first + kVisionUpdateRate < t) { + auto visionPose = + visionMeasurementGenerator(groundTruthState) + + frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m, + distribution(generator) * 0.1_m}, + frc::Rotation2d{distribution(generator) * 0.05_rad}}; + visionPoses.push_back({t, visionPose}); + } + + // We should apply the oldest vision measurement if it has been + // `visionUpdateDelay` seconds since it was measured + if (!visionPoses.empty() && + visionPoses.front().first + kVisionUpdateDelay < t) { + auto visionEntry = visionPoses.front(); + estimator.AddVisionMeasurement(frc::Pose3d{visionEntry.second}, + visionEntry.first); + visionPoses.erase(visionPoses.begin()); + visionLog.push_back({t, visionEntry.first, visionEntry.second}); + } + + auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState); + + auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds); + + leftDistance += wheelSpeeds.left * dt; + rightDistance += wheelSpeeds.right * dt; + + auto xhat = estimator.UpdateWithTime( + t, + 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().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().ToTranslation2d()) + .value(); + + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + if (debug) { + wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n"); + + units::second_t apply_time; + units::second_t measure_time; + frc::Pose2d vision_pose; + for (auto record : visionLog) { + std::tie(apply_time, measure_time, vision_pose) = record; + wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(), + measure_time.value(), vision_pose.X().value(), + vision_pose.Y().value(), + vision_pose.Rotation().Radians().value()); + } + } + + EXPECT_NEAR(endingPose.X().value(), + estimator.GetEstimatedPosition().X().value(), 0.08); + EXPECT_NEAR(endingPose.Y().value(), + estimator.GetEstimatedPosition().Y().value(), 0.08); + EXPECT_NEAR(endingPose.Rotation().Radians().value(), + estimator.GetEstimatedPosition() + .Rotation() + .ToRotation2d() + .Radians() + .value(), + 0.15); + + if (checkError) { + // NOLINTNEXTLINE(bugprone-integer-division) + EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.05); + EXPECT_LT(maxError, 0.2); + } +} + +TEST(DifferentialDrivePoseEstimator3dTest, Accuracy) { + frc::DifferentialDriveKinematics kinematics{1.0_m}; + + 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}, + frc::Pose2d{0_m, 0_m, 135_deg}, + frc::Pose2d{-3_m, 0_m, -90_deg}, + frc::Pose2d{0_m, 0_m, 45_deg}}, + frc::TrajectoryConfig(2_mps, 2_mps_sq)); + + testFollowTrajectory( + kinematics, estimator, trajectory, + [&](frc::Trajectory::State& state) { + return frc::ChassisSpeeds{state.velocity, 0_mps, + state.velocity * state.curvature}; + }, + [&](frc::Trajectory::State& state) { return state.pose; }, + trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, + 100_ms, 250_ms, true, false); +} + +TEST(DifferentialDrivePoseEstimator3dTest, BadInitialPose) { + frc::DifferentialDriveKinematics kinematics{1.0_m}; + + 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}, + frc::Pose2d{0_m, 0_m, 135_deg}, + frc::Pose2d{-3_m, 0_m, -90_deg}, + frc::Pose2d{0_m, 0_m, 45_deg}}, + frc::TrajectoryConfig(2_mps, 2_mps_sq)); + + for (units::degree_t offset_direction_degs = 0_deg; + offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) { + for (units::degree_t offset_heading_degs = 0_deg; + offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) { + auto pose_offset = frc::Rotation2d{offset_direction_degs}; + auto heading_offset = frc::Rotation2d{offset_heading_degs}; + + auto initial_pose = + trajectory.InitialPose() + + frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m, + pose_offset.Sin() * 1_m}, + heading_offset}; + + testFollowTrajectory( + kinematics, estimator, trajectory, + [&](frc::Trajectory::State& state) { + return frc::ChassisSpeeds{state.velocity, 0_mps, + state.velocity * state.curvature}; + }, + [&](frc::Trajectory::State& state) { return state.pose; }, + initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms, + 250_ms, false, false); + } + } +} + +TEST(DifferentialDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) { + // This tests for multiple vision measurements applied at the same time. + // The expected behavior is that all measurements affect the estimated pose. + // The alternative result is that only one vision measurement affects the + // outcome. If that were the case, after 1000 measurements, the estimated + // pose would converge to that measurement. + frc::DifferentialDriveKinematics kinematics{1.0_m}; + + frc::DifferentialDrivePoseEstimator3d estimator{ + kinematics, + frc::Rotation3d{}, + 0_m, + 0_m, + 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::Rotation3d{}, 0_m, 0_m); + + for (int i = 0; i < 1000; i++) { + estimator.AddVisionMeasurement( + frc::Pose3d{0_m, 0_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s); + estimator.AddVisionMeasurement( + frc::Pose3d{3_m, 1_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s); + estimator.AddVisionMeasurement( + 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().ToRotation2d().Radians() - + 0_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } + + { + 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().ToRotation2d().Radians() - + 90_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } + + { + 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().ToRotation2d().Radians() - + 180_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } +} + +TEST(DifferentialDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) { + frc::DifferentialDriveKinematics kinematics{1_m}; + + frc::DifferentialDrivePoseEstimator3d estimator{ + 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::Rotation3d{}, 0_m, 0_m); + } + + auto odometryPose = estimator.GetEstimatedPosition(); + + // Apply a vision measurement from 3 seconds ago + estimator.AddVisionMeasurement( + 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.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::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)); + + // 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(units::second_t{time}, frc::Rotation3d{}, + units::meter_t{time}, units::meter_t{time}); + } + + // Sample at an added time + 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::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::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::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::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::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}), + estimator.SampleAt(1.02_s)); + 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::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::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::Pose3d{1.02_m, 0.1_m, 0_m, frc::Rotation3d{}}), + estimator.SampleAt(1.02_s)); + 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::Pose3d{1_m, 0.1_m, 0_m, frc::Rotation3d{}}), + estimator.SampleAt(0.5_s)); + EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0.1_m, 0_m, frc::Rotation3d{}}), + estimator.SampleAt(2.5_s)); +} + +TEST(DifferentialDrivePoseEstimator3dTest, TestReset) { + frc::DifferentialDriveKinematics kinematics{1_m}; + frc::DifferentialDrivePoseEstimator3d estimator{ + kinematics, + frc::Rotation3d{}, + 0_m, + 0_m, + 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().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::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().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::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().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::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(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::Rotation3d{}, 3_m, 3_m); + + EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value()); + EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().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::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(-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::Pose3d{}); + + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().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/DifferentialDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp index 029902b58cd..80986ea442a 100644 --- a/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp @@ -368,53 +368,60 @@ TEST(DifferentialDrivePoseEstimatorTest, TestReset) { {1.0, 1.0, 1.0}}; // Test initial pose - EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(1, estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test reset position estimator.ResetPosition(frc::Rotation2d{}, 1_m, 1_m, frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}); - EXPECT_EQ(1, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test orientation and wheel positions estimator.Update(frc::Rotation2d{}, 2_m, 2_m); - EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test reset rotation estimator.ResetRotation(frc::Rotation2d{90_deg}); - EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test orientation estimator.Update(frc::Rotation2d{}, 3_m, 3_m); - EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test reset translation estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m}); - EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test reset pose estimator.ResetPose(frc::Pose2d{}); - EXPECT_EQ(0, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); } diff --git a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp new file mode 100644 index 00000000000..7ba98c9f998 --- /dev/null +++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimator3dTest.cpp @@ -0,0 +1,466 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include +#include + +#include +#include + +#include "frc/estimator/MecanumDrivePoseEstimator3d.h" +#include "frc/geometry/Pose2d.h" +#include "frc/kinematics/MecanumDriveKinematics.h" +#include "frc/trajectory/TrajectoryGenerator.h" + +void testFollowTrajectory( + const frc::MecanumDriveKinematics& kinematics, + frc::MecanumDrivePoseEstimator3d& estimator, + const frc::Trajectory& trajectory, + std::function + chassisSpeedsGenerator, + std::function + visionMeasurementGenerator, + const frc::Pose2d& startingPose, const frc::Pose2d& endingPose, + const units::second_t dt, const units::second_t kVisionUpdateRate, + const units::second_t kVisionUpdateDelay, const bool checkError, + const bool debug) { + frc::MecanumDriveWheelPositions wheelPositions{}; + + estimator.ResetPosition(frc::Rotation3d{}, wheelPositions, + frc::Pose3d{startingPose}); + + std::default_random_engine generator; + std::normal_distribution distribution(0.0, 1.0); + + units::second_t t = 0_s; + + std::vector> visionPoses; + std::vector> + visionLog; + + double maxError = -std::numeric_limits::max(); + double errorSum = 0; + + if (debug) { + wpi::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n"); + } + + while (t < trajectory.TotalTime()) { + frc::Trajectory::State groundTruthState = trajectory.Sample(t); + + // We are due for a new vision measurement if it's been `visionUpdateRate` + // seconds since the last vision measurement + if (visionPoses.empty() || + visionPoses.back().first + kVisionUpdateRate < t) { + auto visionPose = + visionMeasurementGenerator(groundTruthState) + + frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m, + distribution(generator) * 0.1_m}, + frc::Rotation2d{distribution(generator) * 0.05_rad}}; + visionPoses.push_back({t, visionPose}); + } + + // We should apply the oldest vision measurement if it has been + // `visionUpdateDelay` seconds since it was measured + if (!visionPoses.empty() && + visionPoses.front().first + kVisionUpdateDelay < t) { + auto visionEntry = visionPoses.front(); + estimator.AddVisionMeasurement(frc::Pose3d{visionEntry.second}, + visionEntry.first); + visionPoses.erase(visionPoses.begin()); + visionLog.push_back({t, visionEntry.first, visionEntry.second}); + } + + auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState); + + auto wheelSpeeds = kinematics.ToWheelSpeeds(chassisSpeeds); + + wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt; + wheelPositions.frontRight += wheelSpeeds.frontRight * dt; + wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt; + wheelPositions.rearRight += wheelSpeeds.rearRight * dt; + + auto xhat = estimator.UpdateWithTime( + t, + 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().ToRotation2d().Radians().value(), + groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(), + groundTruthState.pose.Rotation().Radians().value()); + } + + double error = groundTruthState.pose.Translation() + .Distance(xhat.Translation().ToTranslation2d()) + .value(); + + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + if (debug) { + wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n"); + + units::second_t apply_time; + units::second_t measure_time; + frc::Pose2d vision_pose; + for (auto record : visionLog) { + std::tie(apply_time, measure_time, vision_pose) = record; + wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(), + measure_time.value(), vision_pose.X().value(), + vision_pose.Y().value(), + vision_pose.Rotation().Radians().value()); + } + } + + EXPECT_NEAR(endingPose.X().value(), + estimator.GetEstimatedPosition().X().value(), 0.08); + EXPECT_NEAR(endingPose.Y().value(), + estimator.GetEstimatedPosition().Y().value(), 0.08); + EXPECT_NEAR(endingPose.Rotation().Radians().value(), + estimator.GetEstimatedPosition() + .Rotation() + .ToRotation2d() + .Radians() + .value(), + 0.15); + + if (checkError) { + // NOLINTNEXTLINE(bugprone-integer-division) + EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.051); + EXPECT_LT(maxError, 0.2); + } +} + +TEST(MecanumDrivePoseEstimator3dTest, AccuracyFacingTrajectory) { + frc::MecanumDriveKinematics kinematics{ + 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::MecanumDriveWheelPositions wheelPositions; + + frc::MecanumDrivePoseEstimator3d estimator{ + 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}, + frc::Pose2d{0_m, 0_m, 135_deg}, + frc::Pose2d{-3_m, 0_m, -90_deg}, + frc::Pose2d{0_m, 0_m, 45_deg}}, + frc::TrajectoryConfig(2.0_mps, 2.0_mps_sq)); + + testFollowTrajectory( + kinematics, estimator, trajectory, + [&](frc::Trajectory::State& state) { + return frc::ChassisSpeeds{state.velocity, 0_mps, + state.velocity * state.curvature}; + }, + [&](frc::Trajectory::State& state) { return state.pose; }, + trajectory.InitialPose(), {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, + 100_ms, 250_ms, true, false); +} + +TEST(MecanumDrivePoseEstimator3dTest, BadInitialPose) { + frc::MecanumDriveKinematics kinematics{ + 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::MecanumDriveWheelPositions wheelPositions; + + frc::MecanumDrivePoseEstimator3d estimator{ + 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}, + frc::Pose2d{0_m, 0_m, 135_deg}, + frc::Pose2d{-3_m, 0_m, -90_deg}, + frc::Pose2d{0_m, 0_m, 45_deg}}, + frc::TrajectoryConfig(2.0_mps, 2.0_mps_sq)); + + for (units::degree_t offset_direction_degs = 0_deg; + offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) { + for (units::degree_t offset_heading_degs = 0_deg; + offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) { + auto pose_offset = frc::Rotation2d{offset_direction_degs}; + auto heading_offset = frc::Rotation2d{offset_heading_degs}; + + auto initial_pose = + trajectory.InitialPose() + + frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m, + pose_offset.Sin() * 1_m}, + heading_offset}; + + testFollowTrajectory( + kinematics, estimator, trajectory, + [&](frc::Trajectory::State& state) { + return frc::ChassisSpeeds{state.velocity, 0_mps, + state.velocity * state.curvature}; + }, + [&](frc::Trajectory::State& state) { return state.pose; }, + initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms, + 250_ms, false, false); + } + } +} + +TEST(MecanumDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) { + // This tests for multiple vision measurements applied at the same time. + // The expected behavior is that all measurements affect the estimated pose. + // The alternative result is that only one vision measurement affects the + // outcome. If that were the case, after 1000 measurements, the estimated + // pose would converge to that measurement. + frc::MecanumDriveKinematics kinematics{ + 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::MecanumDriveWheelPositions wheelPositions; + + frc::MecanumDrivePoseEstimator3d estimator{ + 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::Rotation3d{}, wheelPositions); + + for (int i = 0; i < 1000; i++) { + estimator.AddVisionMeasurement( + frc::Pose3d{0_m, 0_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s); + estimator.AddVisionMeasurement( + frc::Pose3d{3_m, 1_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s); + estimator.AddVisionMeasurement( + 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().ToRotation2d().Radians() - + 0_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } + + { + 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().ToRotation2d().Radians() - + 90_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } + + { + 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().ToRotation2d().Radians() - + 180_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } +} + +TEST(MecanumDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) { + frc::MecanumDriveKinematics kinematics{ + 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::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::Rotation3d{}, + frc::MecanumDriveWheelPositions{}); + } + + auto odometryPose = estimator.GetEstimatedPosition(); + + // Apply a vision measurement from 3 seconds ago + estimator.AddVisionMeasurement( + 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.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) { + frc::MecanumDriveKinematics kinematics{ + 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::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)); + + // 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) { + 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::Rotation3d{}, + wheelPositions); + } + + // Sample at an added time + 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::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::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::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::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::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}), + estimator.SampleAt(1.02_s)); + 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::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::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::Pose3d{1.02_m, 0.1_m, 0_m, frc::Rotation3d{}}), + estimator.SampleAt(1.02_s)); + 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::Pose3d{1_m, 0.1_m, 0_m, frc::Rotation3d{}}), + estimator.SampleAt(0.5_s)); + EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0.1_m, 0_m, frc::Rotation3d{}}), + estimator.SampleAt(2.5_s)); +} + +TEST(MecanumDrivePoseEstimator3dTest, TestReset) { + frc::MecanumDriveKinematics kinematics{ + 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::Rotation3d{}, + frc::MecanumDriveWheelPositions{}, + 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().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::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().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::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().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::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(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::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(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::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(-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::Pose3d{}); + + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().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/MecanumDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp index f1f7be98a17..bd3ce4f6ca0 100644 --- a/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp @@ -376,53 +376,60 @@ TEST(MecanumDrivePoseEstimatorTest, TestReset) { {1.0, 1.0, 1.0}}; // Test initial pose - EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(1, estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test reset position estimator.ResetPosition(frc::Rotation2d{}, {1_m, 1_m, 1_m, 1_m}, frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}); - EXPECT_EQ(1, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test orientation and wheel positions estimator.Update(frc::Rotation2d{}, {2_m, 2_m, 2_m, 2_m}); - EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test reset rotation estimator.ResetRotation(frc::Rotation2d{90_deg}); - EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test orientation estimator.Update(frc::Rotation2d{}, {3_m, 3_m, 3_m, 3_m}); - EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test reset translation estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m}); - EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test reset pose estimator.ResetPose(frc::Pose2d{}); - EXPECT_EQ(0, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); } diff --git a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp new file mode 100644 index 00000000000..612862f4f15 --- /dev/null +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimator3dTest.cpp @@ -0,0 +1,502 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "frc/estimator/SwerveDrivePoseEstimator3d.h" +#include "frc/geometry/Pose2d.h" +#include "frc/kinematics/SwerveDriveKinematics.h" +#include "frc/trajectory/TrajectoryGenerator.h" + +void testFollowTrajectory( + const frc::SwerveDriveKinematics<4>& kinematics, + frc::SwerveDrivePoseEstimator3d<4>& estimator, + const frc::Trajectory& trajectory, + std::function + chassisSpeedsGenerator, + std::function + visionMeasurementGenerator, + const frc::Pose2d& startingPose, const frc::Pose2d& endingPose, + const units::second_t dt, const units::second_t kVisionUpdateRate, + const units::second_t kVisionUpdateDelay, const bool checkError, + const bool debug) { + wpi::array positions{wpi::empty_array}; + + estimator.ResetPosition(frc::Rotation3d{}, positions, + frc::Pose3d{startingPose}); + + std::default_random_engine generator; + std::normal_distribution distribution(0.0, 1.0); + + units::second_t t = 0_s; + + std::vector> visionPoses; + std::vector> + visionLog; + + double maxError = -std::numeric_limits::max(); + double errorSum = 0; + + if (debug) { + wpi::print("time, est_x, est_y, est_theta, true_x, true_y, true_theta\n"); + } + + while (t < trajectory.TotalTime()) { + frc::Trajectory::State groundTruthState = trajectory.Sample(t); + + // We are due for a new vision measurement if it's been `visionUpdateRate` + // seconds since the last vision measurement + if (visionPoses.empty() || + visionPoses.back().first + kVisionUpdateRate < t) { + auto visionPose = + visionMeasurementGenerator(groundTruthState) + + frc::Transform2d{frc::Translation2d{distribution(generator) * 0.1_m, + distribution(generator) * 0.1_m}, + frc::Rotation2d{distribution(generator) * 0.05_rad}}; + visionPoses.push_back({t, visionPose}); + } + + // We should apply the oldest vision measurement if it has been + // `visionUpdateDelay` seconds since it was measured + if (!visionPoses.empty() && + visionPoses.front().first + kVisionUpdateDelay < t) { + auto visionEntry = visionPoses.front(); + estimator.AddVisionMeasurement(frc::Pose3d{visionEntry.second}, + visionEntry.first); + visionPoses.erase(visionPoses.begin()); + visionLog.push_back({t, visionEntry.first, visionEntry.second}); + } + + auto chassisSpeeds = chassisSpeedsGenerator(groundTruthState); + + auto moduleStates = kinematics.ToSwerveModuleStates(chassisSpeeds); + + for (size_t i = 0; i < 4; i++) { + positions[i].distance += moduleStates[i].speed * dt; + positions[i].angle = moduleStates[i].angle; + } + + auto xhat = estimator.UpdateWithTime( + t, + 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().ToRotation2d().Radians().value(), + groundTruthState.pose.X().value(), groundTruthState.pose.Y().value(), + groundTruthState.pose.Rotation().Radians().value()); + } + + double error = groundTruthState.pose.Translation() + .Distance(xhat.Translation().ToTranslation2d()) + .value(); + + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + if (debug) { + wpi::print("apply_time, measured_time, vision_x, vision_y, vision_theta\n"); + + units::second_t apply_time; + units::second_t measure_time; + frc::Pose2d vision_pose; + for (auto record : visionLog) { + std::tie(apply_time, measure_time, vision_pose) = record; + wpi::print("{}, {}, {}, {}, {}\n", apply_time.value(), + measure_time.value(), vision_pose.X().value(), + vision_pose.Y().value(), + vision_pose.Rotation().Radians().value()); + } + } + + EXPECT_NEAR(endingPose.X().value(), + estimator.GetEstimatedPosition().X().value(), 0.08); + EXPECT_NEAR(endingPose.Y().value(), + estimator.GetEstimatedPosition().Y().value(), 0.08); + EXPECT_NEAR(endingPose.Rotation().Radians().value(), + estimator.GetEstimatedPosition() + .Rotation() + .ToRotation2d() + .Radians() + .value(), + 0.15); + + if (checkError) { + // NOLINTNEXTLINE(bugprone-integer-division) + EXPECT_LT(errorSum / (trajectory.TotalTime() / dt), 0.058); + EXPECT_LT(maxError, 0.2); + } +} + +TEST(SwerveDrivePoseEstimator3dTest, AccuracyFacingTrajectory) { + frc::SwerveDriveKinematics<4> kinematics{ + 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::SwerveModulePosition fl; + frc::SwerveModulePosition fr; + frc::SwerveModulePosition bl; + frc::SwerveModulePosition br; + + frc::SwerveDrivePoseEstimator3d<4> estimator{ + 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}, + frc::Pose2d{0_m, 0_m, 135_deg}, + frc::Pose2d{-3_m, 0_m, -90_deg}, + frc::Pose2d{0_m, 0_m, 45_deg}}, + frc::TrajectoryConfig(2_mps, 2.0_mps_sq)); + + testFollowTrajectory( + kinematics, estimator, trajectory, + [&](frc::Trajectory::State& state) { + return frc::ChassisSpeeds{state.velocity, 0_mps, + state.velocity * state.curvature}; + }, + [&](frc::Trajectory::State& state) { return state.pose; }, + {0_m, 0_m, frc::Rotation2d{45_deg}}, {0_m, 0_m, frc::Rotation2d{45_deg}}, + 20_ms, 100_ms, 250_ms, true, false); +} + +TEST(SwerveDrivePoseEstimator3dTest, BadInitialPose) { + frc::SwerveDriveKinematics<4> kinematics{ + 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::SwerveModulePosition fl; + frc::SwerveModulePosition fr; + frc::SwerveModulePosition bl; + frc::SwerveModulePosition br; + + frc::SwerveDrivePoseEstimator3d<4> estimator{ + 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}, + frc::Pose2d{0_m, 0_m, 135_deg}, + frc::Pose2d{-3_m, 0_m, -90_deg}, + frc::Pose2d{0_m, 0_m, 45_deg}}, + frc::TrajectoryConfig(2_mps, 2.0_mps_sq)); + + for (units::degree_t offset_direction_degs = 0_deg; + offset_direction_degs < 360_deg; offset_direction_degs += 45_deg) { + for (units::degree_t offset_heading_degs = 0_deg; + offset_heading_degs < 360_deg; offset_heading_degs += 45_deg) { + auto pose_offset = frc::Rotation2d{offset_direction_degs}; + auto heading_offset = frc::Rotation2d{offset_heading_degs}; + + auto initial_pose = + trajectory.InitialPose() + + frc::Transform2d{frc::Translation2d{pose_offset.Cos() * 1_m, + pose_offset.Sin() * 1_m}, + heading_offset}; + + testFollowTrajectory( + kinematics, estimator, trajectory, + [&](frc::Trajectory::State& state) { + return frc::ChassisSpeeds{state.velocity, 0_mps, + state.velocity * state.curvature}; + }, + [&](frc::Trajectory::State& state) { return state.pose; }, + initial_pose, {0_m, 0_m, frc::Rotation2d{45_deg}}, 20_ms, 100_ms, + 250_ms, false, false); + } + } +} + +TEST(SwerveDrivePoseEstimator3dTest, SimultaneousVisionMeasurements) { + // This tests for multiple vision measurements applied at the same time. + // The expected behavior is that all measurements affect the estimated pose. + // The alternative result is that only one vision measurement affects the + // outcome. If that were the case, after 1000 measurements, the estimated + // pose would converge to that measurement. + frc::SwerveDriveKinematics<4> kinematics{ + 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::SwerveModulePosition fl; + frc::SwerveModulePosition fr; + frc::SwerveModulePosition bl; + frc::SwerveModulePosition br; + + frc::SwerveDrivePoseEstimator3d<4> estimator{ + 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::Rotation3d{}, {fl, fr, bl, br}); + + for (int i = 0; i < 1000; i++) { + estimator.AddVisionMeasurement( + frc::Pose3d{0_m, 0_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 0_deg}}, 0_s); + estimator.AddVisionMeasurement( + frc::Pose3d{3_m, 1_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 90_deg}}, 0_s); + estimator.AddVisionMeasurement( + 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().ToRotation2d().Radians() - + 0_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } + + { + 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().ToRotation2d().Radians() - + 90_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } + + { + 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().ToRotation2d().Radians() - + 180_deg); + + EXPECT_TRUE(dx > 0.08_m || dy > 0.08_m || dtheta > 0.08_rad); + } +} + +TEST(SwerveDrivePoseEstimator3dTest, TestDiscardStaleVisionMeasurements) { + frc::SwerveDriveKinematics<4> kinematics{ + 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::SwerveModulePosition fl; + frc::SwerveModulePosition fr; + frc::SwerveModulePosition bl; + frc::SwerveModulePosition br; + + frc::SwerveDrivePoseEstimator3d<4> estimator{ + 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::Rotation3d{}, {fl, fr, bl, br}); + } + + auto odometryPose = estimator.GetEstimatedPosition(); + + // Apply a vision measurement from 3 seconds ago + estimator.AddVisionMeasurement( + 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.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) { + frc::SwerveDriveKinematics<4> kinematics{ + 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::SwerveDrivePoseEstimator3d estimator{ + kinematics, + frc::Rotation3d{}, + {frc::SwerveModulePosition{}, frc::SwerveModulePosition{}, + frc::SwerveModulePosition{}, frc::SwerveModulePosition{}}, + 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)); + + // 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) { + wpi::array wheelPositions{ + {frc::SwerveModulePosition{units::meter_t{time}, frc::Rotation2d{}}, + 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::Rotation3d{}, + wheelPositions); + } + + // Sample at an added time + 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::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::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::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::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::Pose3d{1.02_m, 0_m, 0_m, frc::Rotation3d{}}), + estimator.SampleAt(1.02_s)); + 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::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::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::Pose3d{1.02_m, 0.1_m, 0_m, frc::Rotation3d{}}), + estimator.SampleAt(1.02_s)); + 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::Pose3d{1_m, 0.1_m, 0_m, frc::Rotation3d{}}), + estimator.SampleAt(0.5_s)); + EXPECT_EQ(std::optional(frc::Pose3d{2_m, 0.1_m, 0_m, frc::Rotation3d{}}), + estimator.SampleAt(2.5_s)); +} + +TEST(SwerveDrivePoseEstimator3dTest, TestReset) { + frc::SwerveDriveKinematics<4> kinematics{ + 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::SwerveDrivePoseEstimator3d estimator{ + kinematics, + frc::Rotation3d{}, + {frc::SwerveModulePosition{}, frc::SwerveModulePosition{}, + frc::SwerveModulePosition{}, frc::SwerveModulePosition{}}, + 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().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::Rotation3d{}, + {modulePosition, modulePosition, modulePosition, modulePosition}, + 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().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::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().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::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(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::Rotation3d{}, {modulePosition, modulePosition, + modulePosition, modulePosition}); + } + + EXPECT_DOUBLE_EQ(2, estimator.GetEstimatedPosition().X().value()); + EXPECT_DOUBLE_EQ(1, estimator.GetEstimatedPosition().Y().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::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(-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::Pose3d{}); + + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().X().value()); + EXPECT_DOUBLE_EQ(0, estimator.GetEstimatedPosition().Y().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/SwerveDrivePoseEstimatorTest.cpp b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp index d62200176c0..1655074209b 100644 --- a/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp +++ b/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp @@ -399,9 +399,10 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) { {1.0, 1.0, 1.0}}; // Test initial pose - EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(1, estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test reset position { @@ -412,9 +413,10 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) { frc::Pose2d{1_m, 0_m, frc::Rotation2d{}}); } - EXPECT_EQ(1, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test orientation and wheel positions { @@ -423,17 +425,19 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) { modulePosition, modulePosition}); } - EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test reset rotation estimator.ResetRotation(frc::Rotation2d{90_deg}); - EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test orientation { @@ -442,23 +446,26 @@ TEST(SwerveDrivePoseEstimatorTest, TestReset) { modulePosition, modulePosition}); } - EXPECT_EQ(2, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test reset translation estimator.ResetTranslation(frc::Translation2d{-1_m, -1_m}); - EXPECT_EQ(-1, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(-1, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(std::numbers::pi / 2, - estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); // Test reset pose estimator.ResetPose(frc::Pose2d{}); - EXPECT_EQ(0, estimator.GetEstimatedPosition().X().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Y().value()); - EXPECT_EQ(0, estimator.GetEstimatedPosition().Rotation().Radians().value()); + 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()); } diff --git a/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp new file mode 100644 index 00000000000..76f5f3292c7 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometry3dTest.cpp @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +#include + +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/kinematics/DifferentialDriveOdometry3d.h" + +static constexpr double kEpsilon = 1E-9; + +using namespace frc; + +TEST(DifferentialDriveOdometry3dTest, Initialize) { + 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::Pose3d& pose = odometry.GetPose(); + + EXPECT_NEAR(pose.X().value(), 1, kEpsilon); + EXPECT_NEAR(pose.Y().value(), 2, kEpsilon); + EXPECT_NEAR(pose.Z().value(), 0, kEpsilon); + EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 45, kEpsilon); +} + +TEST(DifferentialDriveOdometry3dTest, EncoderDistances) { + DifferentialDriveOdometry3d odometry{frc::Rotation3d{0_deg, 0_deg, 45_deg}, + 0_m, 0_m}; + + 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.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 new file mode 100644 index 00000000000..b195e8507b7 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometry3dTest.cpp @@ -0,0 +1,223 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include + +#include + +#include "frc/kinematics/MecanumDriveOdometry3d.h" +#include "frc/trajectory/TrajectoryGenerator.h" + +using namespace frc; + +class MecanumDriveOdometry3dTest : public ::testing::Test { + protected: + Translation2d m_fl{12_m, 12_m}; + Translation2d m_fr{12_m, -12_m}; + Translation2d m_bl{-12_m, 12_m}; + Translation2d m_br{-12_m, -12_m}; + + MecanumDriveWheelPositions zero; + + MecanumDriveKinematics kinematics{m_fl, m_fr, m_bl, m_br}; + MecanumDriveOdometry3d odometry{kinematics, frc::Rotation3d{}, zero}; +}; + +TEST_F(MecanumDriveOdometry3dTest, Initialize) { + MecanumDriveOdometry3d odometry{ + kinematics, frc::Rotation3d{}, zero, + frc::Pose3d{1_m, 2_m, 0_m, frc::Rotation3d{0_deg, 0_deg, 45_deg}}}; + + 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.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(frc::Rotation3d{}, wheelDeltas, Pose3d{}); + + 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.Z().value(), 0.0, 0.01); + EXPECT_NEAR(secondPose.Rotation().ToRotation2d().Radians().value(), 0.0, + 0.01); +} + +TEST_F(MecanumDriveOdometry3dTest, TwoIterations) { + odometry.ResetPosition(frc::Rotation3d{}, zero, Pose3d{}); + MecanumDriveWheelPositions wheelDeltas{0.3536_m, 0.3536_m, 0.3536_m, + 0.3536_m}; + + 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.Z().value(), 0.0, 0.01); + EXPECT_NEAR(pose.Rotation().ToRotation2d().Radians().value(), 0.0, 0.01); +} + +TEST_F(MecanumDriveOdometry3dTest, 90DegreeTurn) { + odometry.ResetPosition(frc::Rotation3d{}, zero, Pose3d{}); + MecanumDriveWheelPositions wheelDeltas{-13.328_m, 39.986_m, -13.329_m, + 39.986_m}; + 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.Z().value(), 0, 0.01); + EXPECT_NEAR(pose.Rotation().ToRotation2d().Degrees().value(), 90.0, 0.01); +} + +TEST_F(MecanumDriveOdometry3dTest, GyroAngleReset) { + 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}; + + 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.Z().value(), 0.0, 0.01); + EXPECT_NEAR(pose.Rotation().ToRotation2d().Radians().value(), 0.0, 0.01); +} + +TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingTrajectory) { + frc::MecanumDriveKinematics kinematics{ + 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::MecanumDriveWheelPositions wheelPositions; + + 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}, + frc::Pose2d{0_m, 0_m, 135_deg}, + frc::Pose2d{-3_m, 0_m, -90_deg}, + frc::Pose2d{0_m, 0_m, 45_deg}}, + frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq)); + + std::default_random_engine generator; + std::normal_distribution distribution(0.0, 1.0); + + units::second_t dt = 20_ms; + units::second_t t = 0_s; + + double maxError = -std::numeric_limits::max(); + double errorSum = 0; + + while (t < trajectory.TotalTime()) { + frc::Trajectory::State groundTruthState = trajectory.Sample(t); + + auto wheelSpeeds = kinematics.ToWheelSpeeds( + {groundTruthState.velocity, 0_mps, + groundTruthState.velocity * groundTruthState.curvature}); + + wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps; + wheelSpeeds.frontRight += distribution(generator) * 0.1_mps; + wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps; + wheelSpeeds.rearRight += distribution(generator) * 0.1_mps; + + wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt; + wheelPositions.frontRight += wheelSpeeds.frontRight * dt; + wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt; + wheelPositions.rearRight += wheelSpeeds.rearRight * dt; + + 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().ToTranslation2d()) + .value(); + + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06); + EXPECT_LT(maxError, 0.125); +} + +TEST_F(MecanumDriveOdometry3dTest, AccuracyFacingXAxis) { + frc::MecanumDriveKinematics kinematics{ + 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::MecanumDriveWheelPositions wheelPositions; + + 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}, + frc::Pose2d{0_m, 0_m, 135_deg}, + frc::Pose2d{-3_m, 0_m, -90_deg}, + frc::Pose2d{0_m, 0_m, 45_deg}}, + frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq)); + + std::default_random_engine generator; + std::normal_distribution distribution(0.0, 1.0); + + units::second_t dt = 20_ms; + units::second_t t = 0_s; + + double maxError = -std::numeric_limits::max(); + double errorSum = 0; + + while (t < trajectory.TotalTime()) { + frc::Trajectory::State groundTruthState = trajectory.Sample(t); + + auto wheelSpeeds = kinematics.ToWheelSpeeds( + {groundTruthState.velocity * groundTruthState.pose.Rotation().Cos(), + groundTruthState.velocity * groundTruthState.pose.Rotation().Sin(), + 0_rad_per_s}); + + wheelSpeeds.frontLeft += distribution(generator) * 0.1_mps; + wheelSpeeds.frontRight += distribution(generator) * 0.1_mps; + wheelSpeeds.rearLeft += distribution(generator) * 0.1_mps; + wheelSpeeds.rearRight += distribution(generator) * 0.1_mps; + + wheelPositions.frontLeft += wheelSpeeds.frontLeft * dt; + wheelPositions.frontRight += wheelSpeeds.frontRight * dt; + wheelPositions.rearLeft += wheelSpeeds.rearLeft * dt; + wheelPositions.rearRight += wheelSpeeds.rearRight * dt; + + auto xhat = odometry.Update( + frc::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad}, + wheelPositions); + double error = groundTruthState.pose.Translation() + .Distance(xhat.Translation().ToTranslation2d()) + .value(); + + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06); + EXPECT_LT(maxError, 0.125); +} 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 new file mode 100644 index 00000000000..d9fd52a5594 --- /dev/null +++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometry3dTest.cpp @@ -0,0 +1,224 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include + +#include + +#include "frc/kinematics/SwerveDriveKinematics.h" +#include "frc/kinematics/SwerveDriveOdometry3d.h" +#include "frc/trajectory/Trajectory.h" +#include "frc/trajectory/TrajectoryConfig.h" +#include "frc/trajectory/TrajectoryGenerator.h" + +using namespace frc; + +static constexpr double kEpsilon = 0.01; + +class SwerveDriveOdometry3dTest : public ::testing::Test { + protected: + Translation2d m_fl{12_m, 12_m}; + Translation2d m_fr{12_m, -12_m}; + Translation2d m_bl{-12_m, 12_m}; + Translation2d m_br{-12_m, -12_m}; + + SwerveDriveKinematics<4> m_kinematics{m_fl, m_fr, m_bl, m_br}; + SwerveModulePosition zero; + SwerveDriveOdometry3d<4> m_odometry{ + m_kinematics, frc::Rotation3d{}, {zero, zero, zero, zero}}; +}; + +TEST_F(SwerveDriveOdometry3dTest, Initialize) { + 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::Pose3d& pose = odometry.GetPose(); + + EXPECT_NEAR(pose.X().value(), 1, kEpsilon); + EXPECT_NEAR(pose.Y().value(), 2, 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(frc::Rotation3d{}, {zero, zero, zero, zero}, + Pose3d{}); + + m_odometry.Update(frc::Rotation3d{}, {zero, zero, zero, zero}); + + 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.Z().value(), kEpsilon); + EXPECT_NEAR(0.0, pose.Rotation().ToRotation2d().Degrees().value(), kEpsilon); +} + +TEST_F(SwerveDriveOdometry3dTest, 90DegreeTurn) { + SwerveModulePosition fl{18.85_m, 90_deg}; + SwerveModulePosition fr{42.15_m, 26.565_deg}; + SwerveModulePosition bl{18.85_m, -90_deg}; + SwerveModulePosition br{42.15_m, -26.565_deg}; + + 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(0.0, pose.Z().value(), kEpsilon); + EXPECT_NEAR(90.0, pose.Rotation().ToRotation2d().Degrees().value(), kEpsilon); +} + +TEST_F(SwerveDriveOdometry3dTest, GyroAngleReset) { + 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(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.Z().value(), kEpsilon); + EXPECT_NEAR(0.0, pose.Rotation().ToRotation2d().Degrees().value(), kEpsilon); +} + +TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingTrajectory) { + SwerveDriveKinematics<4> kinematics{ + Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m}, + Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}}; + + SwerveDriveOdometry3d<4> odometry{ + kinematics, frc::Rotation3d{}, {zero, zero, zero, zero}}; + + SwerveModulePosition fl; + SwerveModulePosition fr; + SwerveModulePosition bl; + SwerveModulePosition br; + + Trajectory trajectory = TrajectoryGenerator::GenerateTrajectory( + std::vector{Pose2d{0_m, 0_m, 45_deg}, Pose2d{3_m, 0_m, -90_deg}, + Pose2d{0_m, 0_m, 135_deg}, Pose2d{-3_m, 0_m, -90_deg}, + Pose2d{0_m, 0_m, 45_deg}}, + TrajectoryConfig(5.0_mps, 2.0_mps_sq)); + + std::default_random_engine generator; + std::normal_distribution distribution(0.0, 1.0); + + units::second_t dt = 20_ms; + units::second_t t = 0_s; + + double maxError = -std::numeric_limits::max(); + double errorSum = 0; + + while (t < trajectory.TotalTime()) { + Trajectory::State groundTruthState = trajectory.Sample(t); + + auto moduleStates = kinematics.ToSwerveModuleStates( + {groundTruthState.velocity, 0_mps, + groundTruthState.velocity * groundTruthState.curvature}); + + fl.distance += moduleStates[0].speed * dt; + fr.distance += moduleStates[1].speed * dt; + bl.distance += moduleStates[2].speed * dt; + br.distance += moduleStates[3].speed * dt; + + fl.angle = moduleStates[0].angle; + fr.angle = moduleStates[1].angle; + bl.angle = moduleStates[2].angle; + br.angle = moduleStates[3].angle; + + 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().ToTranslation2d()) + .value(); + + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.05); + EXPECT_LT(maxError, 0.125); +} + +TEST_F(SwerveDriveOdometry3dTest, AccuracyFacingXAxis) { + SwerveDriveKinematics<4> kinematics{ + Translation2d{1_m, 1_m}, Translation2d{1_m, -1_m}, + Translation2d{-1_m, -1_m}, Translation2d{-1_m, 1_m}}; + + SwerveDriveOdometry3d<4> odometry{ + kinematics, frc::Rotation3d{}, {zero, zero, zero, zero}}; + + SwerveModulePosition fl; + SwerveModulePosition fr; + SwerveModulePosition bl; + SwerveModulePosition br; + + Trajectory trajectory = TrajectoryGenerator::GenerateTrajectory( + std::vector{Pose2d{0_m, 0_m, 45_deg}, Pose2d{3_m, 0_m, -90_deg}, + Pose2d{0_m, 0_m, 135_deg}, Pose2d{-3_m, 0_m, -90_deg}, + Pose2d{0_m, 0_m, 45_deg}}, + TrajectoryConfig(5.0_mps, 2.0_mps_sq)); + + std::default_random_engine generator; + std::normal_distribution distribution(0.0, 1.0); + + units::second_t dt = 20_ms; + units::second_t t = 0_s; + + double maxError = -std::numeric_limits::max(); + double errorSum = 0; + + while (t < trajectory.TotalTime()) { + Trajectory::State groundTruthState = trajectory.Sample(t); + + fl.distance += groundTruthState.velocity * dt + + 0.5 * groundTruthState.acceleration * dt * dt; + fr.distance += groundTruthState.velocity * dt + + 0.5 * groundTruthState.acceleration * dt * dt; + bl.distance += groundTruthState.velocity * dt + + 0.5 * groundTruthState.acceleration * dt * dt; + br.distance += groundTruthState.velocity * dt + + 0.5 * groundTruthState.acceleration * dt * dt; + + fl.angle = groundTruthState.pose.Rotation(); + fr.angle = groundTruthState.pose.Rotation(); + bl.angle = groundTruthState.pose.Rotation(); + br.angle = groundTruthState.pose.Rotation(); + + auto xhat = odometry.Update( + frc::Rotation3d{0_rad, 0_rad, distribution(generator) * 0.05_rad}, + {fl, fr, bl, br}); + double error = groundTruthState.pose.Translation() + .Distance(xhat.Translation().ToTranslation2d()) + .value(); + + if (error > maxError) { + maxError = error; + } + errorSum += error; + + t += dt; + } + + EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.06); + EXPECT_LT(maxError, 0.125); +} 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;