diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java index f81c9bb8a5..7918f9fbb8 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java @@ -4,23 +4,15 @@ 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.Rotation2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.DifferentialDriveKinematics; import edu.wpi.first.math.kinematics.DifferentialDriveOdometry; +import edu.wpi.first.math.kinematics.DifferentialDriveWheelPositions; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import java.util.Map; -import java.util.NoSuchElementException; -import java.util.Objects; /** * This class wraps {@link DifferentialDriveOdometry Differential Drive Odometry} to fuse @@ -35,17 +27,7 @@ import java.util.Objects; *

{@link DifferentialDrivePoseEstimator#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 DifferentialDrivePoseEstimator { - private final DifferentialDriveKinematics m_kinematics; - private final DifferentialDriveOdometry m_odometry; - private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1()); - private Matrix m_visionK = new Matrix<>(Nat.N3(), Nat.N3()); - - private static final double kBufferDuration = 1.5; - - private final TimeInterpolatableBuffer m_poseBuffer = - TimeInterpolatableBuffer.createBuffer(kBufferDuration); - +public class DifferentialDrivePoseEstimator extends PoseEstimator { /** * Constructs a DifferentialDrivePoseEstimator with default standard deviations for the model and * vision measurements. @@ -99,44 +81,12 @@ public class DifferentialDrivePoseEstimator { Pose2d initialPoseMeters, Matrix stateStdDevs, Matrix visionMeasurementStdDevs) { - m_kinematics = kinematics; - m_odometry = + super( + kinematics, new DifferentialDriveOdometry( - gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters); - - for (int i = 0; i < 3; ++i) { - m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); - } - - // Initialize vision R - 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, - * theta]ᵀ, with units in meters and radians. - */ - public void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) { - var r = new double[3]; - for (int i = 0; i < 3; ++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 < 3; ++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]))); - } - } + gyroAngle, leftDistanceMeters, rightDistanceMeters, initialPoseMeters), + stateStdDevs, + visionMeasurementStdDevs); } /** @@ -155,129 +105,10 @@ public class DifferentialDrivePoseEstimator { double leftPositionMeters, double rightPositionMeters, Pose2d poseMeters) { - // Reset state estimate and error covariance - m_odometry.resetPosition(gyroAngle, leftPositionMeters, rightPositionMeters, poseMeters); - m_poseBuffer.clear(); - } - - /** - * Gets the estimated robot pose. - * - * @return The estimated robot pose in meters. - */ - public Pose2d getEstimatedPosition() { - return 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 - * DifferentialDrivePoseEstimator#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 - * DifferentialDrivePoseEstimator#updateWithTime(double,Rotation2d,double,double)} then you - * must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is - * the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that - * you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source - * or sync the epochs. - */ - public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { - // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip. - try { - if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) { - return; - } - } catch (NoSuchElementException ex) { - return; - } - - // Step 1: Get the pose odometry measured at the moment the vision measurement was made. - var sample = m_poseBuffer.getSample(timestampSeconds); - - if (sample.isEmpty()) { - return; - } - - // Step 2: Measure the twist between the odometry pose and the vision pose. - var twist = sample.get().poseMeters.log(visionRobotPoseMeters); - - // Step 3: 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.dtheta)); - - // Step 4: Convert back to Twist2d. - var scaledTwist = - new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0)); - - // Step 5: Reset Odometry to state at sample with vision adjustment. - m_odometry.resetPosition( - sample.get().gyroAngle, - sample.get().leftMeters, - sample.get().rightMeters, - sample.get().poseMeters.exp(scaledTwist)); - - // Step 6: Record the current pose to allow multiple measurements from the same timestamp - m_poseBuffer.addSample( - timestampSeconds, - new InterpolationRecord( - getEstimatedPosition(), - sample.get().gyroAngle, - sample.get().leftMeters, - sample.get().rightMeters)); - - // Step 7: Replay odometry inputs between sample time and latest recorded sample to update the - // pose buffer and correct odometry. - for (Map.Entry entry : - m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) { - updateWithTime( - entry.getKey(), - entry.getValue().gyroAngle, - entry.getValue().leftMeters, - entry.getValue().rightMeters); - } - } - - /** - * 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 - * DifferentialDrivePoseEstimator#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 - * DifferentialDrivePoseEstimator#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 - * DifferentialDrivePoseEstimator#updateWithTime(double,Rotation2d,double,double)}, then you - * must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is - * the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that - * you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source - * in this case. - * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position - * in meters, y position in meters, and heading in radians). Increase these numbers to trust - * the vision pose measurement less. - */ - public void addVisionMeasurement( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs) { - setVisionMeasurementStdDevs(visionMeasurementStdDevs); - addVisionMeasurement(visionRobotPoseMeters, timestampSeconds); + resetPosition( + gyroAngle, + new DifferentialDriveWheelPositions(leftPositionMeters, rightPositionMeters), + poseMeters); } /** @@ -291,8 +122,8 @@ public class DifferentialDrivePoseEstimator { */ public Pose2d update( Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters) { - return updateWithTime( - MathSharedStore.getTimestamp(), gyroAngle, distanceLeftMeters, distanceRightMeters); + return update( + gyroAngle, new DifferentialDriveWheelPositions(distanceLeftMeters, distanceRightMeters)); } /** @@ -310,98 +141,9 @@ public class DifferentialDrivePoseEstimator { Rotation2d gyroAngle, double distanceLeftMeters, double distanceRightMeters) { - m_odometry.update(gyroAngle, distanceLeftMeters, distanceRightMeters); - m_poseBuffer.addSample( + return updateWithTime( currentTimeSeconds, - new InterpolationRecord( - getEstimatedPosition(), gyroAngle, distanceLeftMeters, distanceRightMeters)); - - return getEstimatedPosition(); - } - - /** - * Represents an odometry record. The record contains the inputs provided as well as the pose that - * was observed based on these inputs, as well as the previous record and its inputs. - */ - private class InterpolationRecord implements Interpolatable { - // The pose observed given the current sensor inputs and the previous pose. - private final Pose2d poseMeters; - - // The current gyro angle. - private final Rotation2d gyroAngle; - - // The distance traveled by the left encoder. - private final double leftMeters; - - // The distance traveled by the right encoder. - private final double rightMeters; - - /** - * Constructs an Interpolation Record with the specified parameters. - * - * @param poseMeters The pose observed given the current sensor inputs and the previous pose. - * @param gyro The current gyro angle. - * @param leftMeters The distance traveled by the left encoder. - * @param rightMeters The distanced traveled by the right encoder. - */ - private InterpolationRecord( - Pose2d poseMeters, Rotation2d gyro, double leftMeters, double rightMeters) { - this.poseMeters = poseMeters; - this.gyroAngle = gyro; - this.leftMeters = leftMeters; - this.rightMeters = rightMeters; - } - - /** - * Return the interpolated record. This object is assumed to be the starting position, or lower - * bound. - * - * @param endValue The upper bound, or end. - * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1]. - * @return The interpolated value. - */ - @Override - public InterpolationRecord interpolate(InterpolationRecord endValue, double t) { - if (t < 0) { - return this; - } else if (t >= 1) { - return endValue; - } else { - // Find the new left distance. - var left_lerp = MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t); - - // Find the new right distance. - var right_lerp = MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t); - - // Find the new gyro angle. - var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t); - - // Create a twist to represent this change based on the interpolated sensor inputs. - Twist2d twist = m_kinematics.toTwist2d(left_lerp - leftMeters, right_lerp - rightMeters); - twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians(); - - return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, left_lerp, right_lerp); - } - } - - @Override - public boolean equals(Object obj) { - if (this == obj) { - return true; - } - if (!(obj instanceof InterpolationRecord)) { - return false; - } - InterpolationRecord record = (InterpolationRecord) obj; - return Objects.equals(gyroAngle, record.gyroAngle) - && Double.compare(leftMeters, record.leftMeters) == 0 - && Double.compare(rightMeters, record.rightMeters) == 0 - && Objects.equals(poseMeters, record.poseMeters); - } - - @Override - public int hashCode() { - return Objects.hash(gyroAngle, leftMeters, rightMeters, poseMeters); - } + 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 03deaa1bc8..59bbb3235c 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 @@ -4,24 +4,15 @@ 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.Rotation2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.MecanumDriveKinematics; import edu.wpi.first.math.kinematics.MecanumDriveOdometry; import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import java.util.Map; -import java.util.NoSuchElementException; -import java.util.Objects; /** * This class wraps {@link MecanumDriveOdometry Mecanum Drive Odometry} to fuse latency-compensated @@ -34,17 +25,7 @@ import java.util.Objects; *

{@link MecanumDrivePoseEstimator#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 MecanumDrivePoseEstimator { - private final MecanumDriveKinematics m_kinematics; - private final MecanumDriveOdometry m_odometry; - private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1()); - private Matrix m_visionK = new Matrix<>(Nat.N3(), Nat.N3()); - - private static final double kBufferDuration = 1.5; - - private final TimeInterpolatableBuffer m_poseBuffer = - TimeInterpolatableBuffer.createBuffer(kBufferDuration); - +public class MecanumDrivePoseEstimator extends PoseEstimator { /** * Constructs a MecanumDrivePoseEstimator with default standard deviations for the model and * vision measurements. @@ -93,309 +74,10 @@ public class MecanumDrivePoseEstimator { Pose2d initialPoseMeters, Matrix stateStdDevs, Matrix visionMeasurementStdDevs) { - m_kinematics = kinematics; - m_odometry = new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters); - - for (int i = 0; i < 3; ++i) { - m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); - } - - // Initialize vision R - 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, - * theta]ᵀ, with units in meters and radians. - */ - public void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) { - var r = new double[3]; - for (int i = 0; i < 3; ++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 < 3; ++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]))); - } - } - } - - /** - * 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 angle reported by the gyroscope. - * @param wheelPositions The distances driven by each wheel. - * @param poseMeters The position on the field that your robot is at. - */ - public void resetPosition( - Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) { - // Reset state estimate and error covariance - m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters); - m_poseBuffer.clear(); - } - - /** - * Gets the estimated robot pose. - * - * @return The estimated robot pose in meters. - */ - public Pose2d getEstimatedPosition() { - return 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 - * MecanumDrivePoseEstimator#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 - * MecanumDrivePoseEstimator#updateWithTime(double,Rotation2d,MecanumDriveWheelPositions)} - * then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this - * timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) - * This means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as - * your time source or sync the epochs. - */ - public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { - // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip. - try { - if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) { - return; - } - } catch (NoSuchElementException ex) { - return; - } - - // Step 1: Get the pose odometry measured at the moment the vision measurement was made. - var sample = m_poseBuffer.getSample(timestampSeconds); - - if (sample.isEmpty()) { - return; - } - - // Step 2: Measure the twist between the odometry pose and the vision pose. - var twist = sample.get().poseMeters.log(visionRobotPoseMeters); - - // Step 3: 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.dtheta)); - - // Step 4: Convert back to Twist2d. - var scaledTwist = - new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0)); - - // Step 5: Reset Odometry to state at sample with vision adjustment. - m_odometry.resetPosition( - sample.get().gyroAngle, - sample.get().wheelPositions, - sample.get().poseMeters.exp(scaledTwist)); - - // Step 6: Record the current pose to allow multiple measurements from the same timestamp - m_poseBuffer.addSample( - timestampSeconds, - new InterpolationRecord( - getEstimatedPosition(), sample.get().gyroAngle, sample.get().wheelPositions)); - - // Step 7: Replay odometry inputs between sample time and latest recorded sample to update the - // pose buffer and correct odometry. - for (Map.Entry entry : - m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) { - updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().wheelPositions); - } - } - - /** - * 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 - * MecanumDrivePoseEstimator#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 - * MecanumDrivePoseEstimator#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 - * MecanumDrivePoseEstimator#updateWithTime(double,Rotation2d,MecanumDriveWheelPositions)}, - * then you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this - * timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). - * This means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as - * your time source in this case. - * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position - * in meters, y position in meters, and heading in radians). Increase these numbers to trust - * the vision pose measurement less. - */ - public void addVisionMeasurement( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs) { - setVisionMeasurementStdDevs(visionMeasurementStdDevs); - addVisionMeasurement(visionRobotPoseMeters, timestampSeconds); - } - - /** - * 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 driven by each wheel. - * @return The estimated pose of the robot in meters. - */ - public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions 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 gyroscope angle. - * @param wheelPositions The distances driven by each wheel. - * @return The estimated pose of the robot in meters. - */ - public Pose2d updateWithTime( - double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) { - m_odometry.update(gyroAngle, wheelPositions); - - m_poseBuffer.addSample( - currentTimeSeconds, - new InterpolationRecord( - getEstimatedPosition(), - gyroAngle, - new MecanumDriveWheelPositions( - wheelPositions.frontLeftMeters, - wheelPositions.frontRightMeters, - wheelPositions.rearLeftMeters, - wheelPositions.rearRightMeters))); - - return getEstimatedPosition(); - } - - /** - * Represents an odometry record. The record contains the inputs provided as well as the pose that - * was observed based on these inputs, as well as the previous record and its inputs. - */ - private class InterpolationRecord implements Interpolatable { - // The pose observed given the current sensor inputs and the previous pose. - private final Pose2d poseMeters; - - // The current gyro angle. - private final Rotation2d gyroAngle; - - // The distances traveled by each wheel encoder. - private final MecanumDriveWheelPositions wheelPositions; - - /** - * Constructs an Interpolation Record with the specified parameters. - * - * @param poseMeters The pose observed given the current sensor inputs and the previous pose. - * @param gyro The current gyro angle. - * @param wheelPositions The distances traveled by each wheel encoder. - */ - private InterpolationRecord( - Pose2d poseMeters, Rotation2d gyro, MecanumDriveWheelPositions wheelPositions) { - this.poseMeters = poseMeters; - this.gyroAngle = gyro; - this.wheelPositions = wheelPositions; - } - - /** - * Return the interpolated record. This object is assumed to be the starting position, or lower - * bound. - * - * @param endValue The upper bound, or end. - * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1]. - * @return The interpolated value. - */ - @Override - public InterpolationRecord interpolate(InterpolationRecord endValue, double t) { - if (t < 0) { - return this; - } else if (t >= 1) { - return endValue; - } else { - // Find the new wheel distances. - var wheels_lerp = - new MecanumDriveWheelPositions( - MathUtil.interpolate( - this.wheelPositions.frontLeftMeters, - endValue.wheelPositions.frontLeftMeters, - t), - MathUtil.interpolate( - this.wheelPositions.frontRightMeters, - endValue.wheelPositions.frontRightMeters, - t), - MathUtil.interpolate( - this.wheelPositions.rearLeftMeters, endValue.wheelPositions.rearLeftMeters, t), - MathUtil.interpolate( - this.wheelPositions.rearRightMeters, - endValue.wheelPositions.rearRightMeters, - t)); - - // Find the distance travelled between this measurement and the interpolated measurement. - var wheels_delta = - new MecanumDriveWheelPositions( - wheels_lerp.frontLeftMeters - this.wheelPositions.frontLeftMeters, - wheels_lerp.frontRightMeters - this.wheelPositions.frontRightMeters, - wheels_lerp.rearLeftMeters - this.wheelPositions.rearLeftMeters, - wheels_lerp.rearRightMeters - this.wheelPositions.rearRightMeters); - - // Find the new gyro angle. - var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t); - - // Create a twist to represent this change based on the interpolated sensor inputs. - Twist2d twist = m_kinematics.toTwist2d(wheels_delta); - twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians(); - - return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, wheels_lerp); - } - } - - @Override - public boolean equals(Object obj) { - if (this == obj) { - return true; - } - if (!(obj instanceof InterpolationRecord)) { - return false; - } - InterpolationRecord record = (InterpolationRecord) obj; - return Objects.equals(gyroAngle, record.gyroAngle) - && Objects.equals(wheelPositions, record.wheelPositions) - && Objects.equals(poseMeters, record.poseMeters); - } - - @Override - public int hashCode() { - return Objects.hash(gyroAngle, wheelPositions, poseMeters); - } + super( + kinematics, + new MecanumDriveOdometry(kinematics, gyroAngle, wheelPositions, initialPoseMeters), + stateStdDevs, + visionMeasurementStdDevs); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java new file mode 100644 index 0000000000..6e82cff3e3 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/estimator/PoseEstimator.java @@ -0,0 +1,336 @@ +// 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.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.Rotation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.interpolation.Interpolatable; +import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; +import edu.wpi.first.math.kinematics.Kinematics; +import edu.wpi.first.math.kinematics.Odometry; +import edu.wpi.first.math.kinematics.WheelPositions; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import java.util.Map; +import java.util.NoSuchElementException; +import java.util.Objects; + +/** + * This class wraps {@link 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., {@link DifferentialDrivePoseEstimator}). It is intended to be a drop-in + * replacement for {@link Odometry}; in fact, if you never call {@link + * PoseEstimator#addVisionMeasurement} and only call {@link PoseEstimator#update} then this will + * behave exactly the same as Odometry. + * + *

{@link PoseEstimator#update} should be called every robot loop. + * + *

{@link PoseEstimator#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 PoseEstimator> { + private final Kinematics m_kinematics; + private final Odometry m_odometry; + private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1()); + private final Matrix m_visionK = new Matrix<>(Nat.N3(), Nat.N3()); + + private static final double kBufferDuration = 1.5; + private final TimeInterpolatableBuffer m_poseBuffer = + TimeInterpolatableBuffer.createBuffer(kBufferDuration); + + /** + * Constructs a PoseEstimator. + * + * @param kinematics A correctly-configured kinematics object for your drivetrain. + * @param odometry A correctly-configured odometry object for your drivetrain. + * @param stateStdDevs Standard deviations of the pose estimate (x position in meters, y position + * in meters, and heading in radians). Increase these numbers to trust your state estimate + * less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position + * in meters, y position in meters, and heading in radians). Increase these numbers to trust + * the vision pose measurement less. + */ + public PoseEstimator( + Kinematics kinematics, + Odometry odometry, + Matrix stateStdDevs, + Matrix visionMeasurementStdDevs) { + m_kinematics = kinematics; + m_odometry = odometry; + + for (int i = 0; i < 3; ++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, + * theta]ᵀ, with units in meters and radians. + */ + public void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) { + var r = new double[3]; + for (int i = 0; i < 3; ++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 < 3; ++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]))); + } + } + } + + /** + * Resets the robot's position on the field. + * + *

The gyroscope angle does not need to be reset here on the user's robot code. The library + * automatically takes care of offsetting the gyro angle. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current encoder readings. + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) { + // Reset state estimate and error covariance + m_odometry.resetPosition(gyroAngle, wheelPositions, poseMeters); + m_poseBuffer.clear(); + } + + /** + * Gets the estimated robot pose. + * + * @return The estimated robot pose in meters. + */ + public Pose2d getEstimatedPosition() { + return 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 + * PoseEstimator#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 + * PoseEstimator#updateWithTime(double,Rotation2d,WheelPositions)} then you must use a + * timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same + * epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that you + * should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source or + * sync the epochs. + */ + public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip. + try { + if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) { + return; + } + } catch (NoSuchElementException ex) { + return; + } + + // Step 1: Get the pose odometry measured at the moment the vision measurement was made. + var sample = m_poseBuffer.getSample(timestampSeconds); + + if (sample.isEmpty()) { + return; + } + + // Step 2: Measure the twist between the odometry pose and the vision pose. + var twist = sample.get().poseMeters.log(visionRobotPoseMeters); + + // Step 3: 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.dtheta)); + + // Step 4: Convert back to Twist2d. + var scaledTwist = + new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0)); + + // Step 5: Reset Odometry to state at sample with vision adjustment. + m_odometry.resetPosition( + sample.get().gyroAngle, + sample.get().wheelPositions, + sample.get().poseMeters.exp(scaledTwist)); + + // Step 6: Record the current pose to allow multiple measurements from the same timestamp + m_poseBuffer.addSample( + timestampSeconds, + new InterpolationRecord( + getEstimatedPosition(), sample.get().gyroAngle, sample.get().wheelPositions)); + + // Step 7: Replay odometry inputs between sample time and latest recorded sample to update the + // pose buffer and correct odometry. + for (Map.Entry entry : + m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) { + updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().wheelPositions); + } + } + + /** + * 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 + * PoseEstimator#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 + * PoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you + * don't use your own time source by calling {@link #updateWithTime}, then you must use a + * timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is the same + * epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). This means that you + * should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source in + * this case. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position + * in meters, y position in meters, and heading in radians). Increase these numbers to trust + * the vision pose measurement less. + */ + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + setVisionMeasurementStdDevs(visionMeasurementStdDevs); + addVisionMeasurement(visionRobotPoseMeters, timestampSeconds); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This should be called every + * loop. + * + * @param gyroAngle The current gyro angle. + * @param wheelPositions The current encoder readings. + * @return The estimated pose of the robot in meters. + */ + public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { + return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, wheelPositions); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This should be called every + * loop. + * + * @param currentTimeSeconds Time at which this method was called, in seconds. + * @param gyroAngle The current gyro angle. + * @param wheelPositions The current encoder readings. + * @return The estimated pose of the robot in meters. + */ + public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle, T wheelPositions) { + m_odometry.update(gyroAngle, wheelPositions); + m_poseBuffer.addSample( + currentTimeSeconds, + new InterpolationRecord(getEstimatedPosition(), gyroAngle, wheelPositions.copy())); + + return getEstimatedPosition(); + } + + /** + * Represents an odometry record. The record contains the inputs provided as well as the pose that + * was observed based on these inputs, as well as the previous record and its inputs. + */ + private class InterpolationRecord implements Interpolatable { + // The pose observed given the current sensor inputs and the previous pose. + private final Pose2d poseMeters; + + // The current gyro angle. + private final Rotation2d gyroAngle; + + // The current encoder readings. + private final T wheelPositions; + + /** + * Constructs an Interpolation Record with the specified parameters. + * + * @param poseMeters The pose observed given the current sensor inputs and the previous pose. + * @param gyro The current gyro angle. + * @param wheelPositions The current encoder readings. + */ + private InterpolationRecord(Pose2d poseMeters, Rotation2d gyro, T wheelPositions) { + this.poseMeters = poseMeters; + this.gyroAngle = gyro; + this.wheelPositions = wheelPositions; + } + + /** + * Return the interpolated record. This object is assumed to be the starting position, or lower + * bound. + * + * @param endValue The upper bound, or end. + * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1]. + * @return The interpolated value. + */ + @Override + public InterpolationRecord interpolate(InterpolationRecord endValue, double t) { + if (t < 0) { + return this; + } else if (t >= 1) { + return endValue; + } else { + // Find the new wheel distances. + var wheelLerp = wheelPositions.interpolate(endValue.wheelPositions, t); + + // Find the distance travelled between this measurement and the interpolated measurement. + var wheelDelta = wheelLerp.minus(wheelPositions); + + // Find the new gyro angle. + var gyroLerp = gyroAngle.interpolate(endValue.gyroAngle, t); + + // Create a twist to represent this change based on the interpolated sensor inputs. + Twist2d twist = m_kinematics.toTwist2d(wheelDelta); + twist.dtheta = gyroLerp.minus(gyroAngle).getRadians(); + + return new InterpolationRecord(poseMeters.exp(twist), gyroLerp, wheelLerp); + } + } + + @Override + public boolean equals(Object obj) { + if (this == obj) { + return true; + } + if (!(obj instanceof PoseEstimator.InterpolationRecord)) { + return false; + } + var record = (PoseEstimator.InterpolationRecord) obj; + return Objects.equals(gyroAngle, record.gyroAngle) + && Objects.equals(wheelPositions, record.wheelPositions) + && Objects.equals(poseMeters, record.poseMeters); + } + + @Override + public int hashCode() { + return Objects.hash(gyroAngle, wheelPositions, poseMeters); + } + } +} 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 cbab887822..f778b621e3 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 @@ -4,25 +4,16 @@ 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.Rotation2d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.interpolation.Interpolatable; -import edu.wpi.first.math.interpolation.TimeInterpolatableBuffer; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveDriveWheelPositions; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; -import java.util.Arrays; -import java.util.Map; -import java.util.NoSuchElementException; -import java.util.Objects; /** * This class wraps {@link SwerveDriveOdometry Swerve Drive Odometry} to fuse latency-compensated @@ -34,17 +25,8 @@ import java.util.Objects; *

{@link SwerveDrivePoseEstimator#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 SwerveDrivePoseEstimator { - private final SwerveDriveKinematics m_kinematics; - private final SwerveDriveOdometry m_odometry; - private final Matrix m_q = new Matrix<>(Nat.N3(), Nat.N1()); +public class SwerveDrivePoseEstimator extends PoseEstimator { private final int m_numModules; - private Matrix m_visionK = new Matrix<>(Nat.N3(), Nat.N3()); - - private static final double kBufferDuration = 1.5; - - private final TimeInterpolatableBuffer m_poseBuffer = - TimeInterpolatableBuffer.createBuffer(kBufferDuration); /** * Constructs a SwerveDrivePoseEstimator with default standard deviations for the model and vision @@ -94,43 +76,13 @@ public class SwerveDrivePoseEstimator { Pose2d initialPoseMeters, Matrix stateStdDevs, Matrix visionMeasurementStdDevs) { - m_kinematics = kinematics; - m_odometry = new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters); - - for (int i = 0; i < 3; ++i) { - m_q.set(i, 0, stateStdDevs.get(i, 0) * stateStdDevs.get(i, 0)); - } + super( + kinematics, + new SwerveDriveOdometry(kinematics, gyroAngle, modulePositions, initialPoseMeters), + stateStdDevs, + visionMeasurementStdDevs); m_numModules = modulePositions.length; - - 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, - * theta]ᵀ, with units in meters and radians. - */ - public void setVisionMeasurementStdDevs(Matrix visionMeasurementStdDevs) { - var r = new double[3]; - for (int i = 0; i < 3; ++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 < 3; ++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]))); - } - } } /** @@ -145,121 +97,7 @@ public class SwerveDrivePoseEstimator { */ public void resetPosition( Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d poseMeters) { - // Reset state estimate and error covariance - m_odometry.resetPosition(gyroAngle, modulePositions, poseMeters); - m_poseBuffer.clear(); - } - - /** - * Gets the estimated robot pose. - * - * @return The estimated robot pose in meters. - */ - public Pose2d getEstimatedPosition() { - return 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 - * SwerveDrivePoseEstimator#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 - * SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])} then you - * must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this timestamp is - * the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}.) This means that - * you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as your time source - * or sync the epochs. - */ - public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { - // Step 0: If this measurement is old enough to be outside the pose buffer's timespan, skip. - try { - if (m_poseBuffer.getInternalBuffer().lastKey() - kBufferDuration > timestampSeconds) { - return; - } - } catch (NoSuchElementException ex) { - return; - } - - // Step 1: Get the pose odometry measured at the moment the vision measurement was made. - var sample = m_poseBuffer.getSample(timestampSeconds); - - if (sample.isEmpty()) { - return; - } - - // Step 2: Measure the twist between the odometry pose and the vision pose. - var twist = sample.get().poseMeters.log(visionRobotPoseMeters); - - // Step 3: 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.dtheta)); - - // Step 4: Convert back to Twist2d. - var scaledTwist = - new Twist2d(k_times_twist.get(0, 0), k_times_twist.get(1, 0), k_times_twist.get(2, 0)); - - // Step 5: Reset Odometry to state at sample with vision adjustment. - m_odometry.resetPosition( - sample.get().gyroAngle, - sample.get().modulePositions, - sample.get().poseMeters.exp(scaledTwist)); - - // Step 6: Record the current pose to allow multiple measurements from the same timestamp - m_poseBuffer.addSample( - timestampSeconds, - new InterpolationRecord( - getEstimatedPosition(), sample.get().gyroAngle, sample.get().modulePositions)); - - // Step 7: Replay odometry inputs between sample time and latest recorded sample to update the - // pose buffer and correct odometry. - for (Map.Entry entry : - m_poseBuffer.getInternalBuffer().tailMap(timestampSeconds).entrySet()) { - updateWithTime(entry.getKey(), entry.getValue().gyroAngle, entry.getValue().modulePositions); - } - } - - /** - * 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 - * SwerveDrivePoseEstimator#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 - * SwerveDrivePoseEstimator#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 - * SwerveDrivePoseEstimator#updateWithTime(double,Rotation2d,SwerveModulePosition[])}, then - * you must use a timestamp with an epoch since FPGA startup (i.e., the epoch of this - * timestamp is the same epoch as {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()}). - * This means that you should use {@link edu.wpi.first.wpilibj.Timer#getFPGATimestamp()} as - * your time source in this case. - * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement (x position - * in meters, y position in meters, and heading in radians). Increase these numbers to trust - * the vision pose measurement less. - */ - public void addVisionMeasurement( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs) { - setVisionMeasurementStdDevs(visionMeasurementStdDevs); - addVisionMeasurement(visionRobotPoseMeters, timestampSeconds); + resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), poseMeters); } /** @@ -271,7 +109,7 @@ public class SwerveDrivePoseEstimator { * @return The estimated pose of the robot in meters. */ public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { - return updateWithTime(MathSharedStore.getTimestamp(), gyroAngle, modulePositions); + return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions)); } /** @@ -285,118 +123,19 @@ public class SwerveDrivePoseEstimator { */ public Pose2d updateWithTime( double currentTimeSeconds, Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { - if (modulePositions.length != m_numModules) { + return updateWithTime( + currentTimeSeconds, gyroAngle, new SwerveDriveWheelPositions(modulePositions)); + } + + @Override + public Pose2d updateWithTime( + double currentTimeSeconds, Rotation2d gyroAngle, SwerveDriveWheelPositions wheelPositions) { + if (wheelPositions.positions.length != m_numModules) { throw new IllegalArgumentException( "Number of modules is not consistent with number of wheel locations provided in " + "constructor"); } - var internalModulePositions = new SwerveModulePosition[m_numModules]; - - for (int i = 0; i < m_numModules; i++) { - internalModulePositions[i] = - new SwerveModulePosition(modulePositions[i].distanceMeters, modulePositions[i].angle); - } - - m_odometry.update(gyroAngle, internalModulePositions); - - m_poseBuffer.addSample( - currentTimeSeconds, - new InterpolationRecord(getEstimatedPosition(), gyroAngle, internalModulePositions)); - - return getEstimatedPosition(); - } - - /** - * Represents an odometry record. The record contains the inputs provided as well as the pose that - * was observed based on these inputs, as well as the previous record and its inputs. - */ - private class InterpolationRecord implements Interpolatable { - // The pose observed given the current sensor inputs and the previous pose. - private final Pose2d poseMeters; - - // The current gyro angle. - private final Rotation2d gyroAngle; - - // The distances and rotations measured at each module. - private final SwerveModulePosition[] modulePositions; - - /** - * Constructs an Interpolation Record with the specified parameters. - * - * @param poseMeters The pose observed given the current sensor inputs and the previous pose. - * @param gyro The current gyro angle. - * @param modulePositions The distances and rotations measured at each wheel. - */ - private InterpolationRecord( - Pose2d poseMeters, Rotation2d gyro, SwerveModulePosition[] modulePositions) { - this.poseMeters = poseMeters; - this.gyroAngle = gyro; - this.modulePositions = modulePositions; - } - - /** - * Return the interpolated record. This object is assumed to be the starting position, or lower - * bound. - * - * @param endValue The upper bound, or end. - * @param t How far between the lower and upper bound we are. This should be bounded in [0, 1]. - * @return The interpolated value. - */ - @Override - public InterpolationRecord interpolate(InterpolationRecord endValue, double t) { - if (t < 0) { - return this; - } else if (t >= 1) { - return endValue; - } else { - // Find the new wheel distances. - var modulePositions = new SwerveModulePosition[m_numModules]; - - // Find the distance travelled between this measurement and the interpolated measurement. - var moduleDeltas = new SwerveModulePosition[m_numModules]; - - for (int i = 0; i < m_numModules; i++) { - double ds = - MathUtil.interpolate( - this.modulePositions[i].distanceMeters, - endValue.modulePositions[i].distanceMeters, - t); - Rotation2d theta = - this.modulePositions[i].angle.interpolate(endValue.modulePositions[i].angle, t); - modulePositions[i] = new SwerveModulePosition(ds, theta); - moduleDeltas[i] = - new SwerveModulePosition(ds - this.modulePositions[i].distanceMeters, theta); - } - - // Find the new gyro angle. - var gyro_lerp = gyroAngle.interpolate(endValue.gyroAngle, t); - - // Create a twist to represent this change based on the interpolated sensor inputs. - Twist2d twist = m_kinematics.toTwist2d(moduleDeltas); - twist.dtheta = gyro_lerp.minus(gyroAngle).getRadians(); - - return new InterpolationRecord(poseMeters.exp(twist), gyro_lerp, modulePositions); - } - } - - @Override - public boolean equals(Object obj) { - if (this == obj) { - return true; - } - if (!(obj instanceof InterpolationRecord)) { - return false; - } - InterpolationRecord record = (InterpolationRecord) obj; - return Objects.equals(gyroAngle, record.gyroAngle) - && Arrays.equals(modulePositions, record.modulePositions) - && Objects.equals(poseMeters, record.poseMeters); - } - - @Override - public int hashCode() { - return Objects.hash(gyroAngle, Arrays.hashCode(modulePositions), poseMeters); - } + return super.updateWithTime(currentTimeSeconds, gyroAngle, wheelPositions); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java index 0dfb016306..c02f4a6b63 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java @@ -16,7 +16,8 @@ import edu.wpi.first.math.geometry.Twist2d; * whereas forward kinematics converts left and right component velocities into a linear and angular * chassis speed. */ -public class DifferentialDriveKinematics { +public class DifferentialDriveKinematics + implements Kinematics { public final double trackWidthMeters; /** @@ -37,6 +38,7 @@ public class DifferentialDriveKinematics { * @param wheelSpeeds The left and right velocities. * @return The chassis speed. */ + @Override public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) { return new ChassisSpeeds( (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2, @@ -51,6 +53,7 @@ public class DifferentialDriveKinematics { * chassis' speed. * @return The left and right velocities. */ + @Override public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) { return new DifferentialDriveWheelSpeeds( chassisSpeeds.vxMetersPerSecond @@ -59,6 +62,11 @@ public class DifferentialDriveKinematics { + trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond); } + @Override + public Twist2d toTwist2d(DifferentialDriveWheelPositions wheelDeltas) { + return toTwist2d(wheelDeltas.leftMeters, wheelDeltas.rightMeters); + } + /** * Performs forward kinematics to return the resulting Twist2d from the given left and right side * distance deltas. This method is often used for odometry -- determining the robot's position on diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java index e8f97f53b1..db5e8a3387 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java @@ -8,7 +8,6 @@ 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.Rotation2d; -import edu.wpi.first.math.geometry.Twist2d; /** * Class for differential drive odometry. Odometry allows you to track the robot's position on the @@ -20,15 +19,7 @@ import edu.wpi.first.math.geometry.Twist2d; *

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 DifferentialDriveOdometry { - private Pose2d m_poseMeters; - - private Rotation2d m_gyroOffset; - private Rotation2d m_previousAngle; - - private double m_prevLeftDistance; - private double m_prevRightDistance; - +public class DifferentialDriveOdometry extends Odometry { /** * Constructs a DifferentialDriveOdometry object. * @@ -42,13 +33,11 @@ public class DifferentialDriveOdometry { double leftDistanceMeters, double rightDistanceMeters, Pose2d initialPoseMeters) { - m_poseMeters = initialPoseMeters; - m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); - m_previousAngle = initialPoseMeters.getRotation(); - - m_prevLeftDistance = leftDistanceMeters; - m_prevRightDistance = rightDistanceMeters; - + super( + new DifferentialDriveKinematics(1), + gyroAngle, + new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters), + initialPoseMeters); MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1); } @@ -80,21 +69,10 @@ public class DifferentialDriveOdometry { double leftDistanceMeters, double rightDistanceMeters, Pose2d poseMeters) { - m_poseMeters = poseMeters; - m_previousAngle = poseMeters.getRotation(); - m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); - - m_prevLeftDistance = leftDistanceMeters; - m_prevRightDistance = rightDistanceMeters; - } - - /** - * Returns the position of the robot on the field. - * - * @return The pose of the robot (x and y are in meters). - */ - public Pose2d getPoseMeters() { - return m_poseMeters; + super.resetPosition( + gyroAngle, + new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters), + poseMeters); } /** @@ -109,22 +87,7 @@ public class DifferentialDriveOdometry { */ public Pose2d update( Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) { - double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance; - double deltaRightDistance = rightDistanceMeters - m_prevRightDistance; - - m_prevLeftDistance = leftDistanceMeters; - m_prevRightDistance = rightDistanceMeters; - - double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0; - var angle = gyroAngle.plus(m_gyroOffset); - - var newPose = - m_poseMeters.exp( - new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians())); - - m_previousAngle = angle; - - m_poseMeters = new Pose2d(newPose.getTranslation(), angle); - return m_poseMeters; + return super.update( + gyroAngle, new DifferentialDriveWheelPositions(leftDistanceMeters, rightDistanceMeters)); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java new file mode 100644 index 0000000000..972390132e --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java @@ -0,0 +1,68 @@ +// 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.MathUtil; +import java.util.Objects; + +public class DifferentialDriveWheelPositions + implements WheelPositions { + /** Distance measured by the left side. */ + public double leftMeters; + + /** Distance measured by the right side. */ + public double rightMeters; + + /** + * Constructs a DifferentialDriveWheelPositions. + * + * @param leftMeters Distance measured by the left side. + * @param rightMeters Distance measured by the right side. + */ + public DifferentialDriveWheelPositions(double leftMeters, double rightMeters) { + this.leftMeters = leftMeters; + this.rightMeters = rightMeters; + } + + @Override + public boolean equals(Object obj) { + if (obj instanceof DifferentialDriveWheelPositions) { + DifferentialDriveWheelPositions other = (DifferentialDriveWheelPositions) obj; + return Math.abs(other.leftMeters - leftMeters) < 1E-9 + && Math.abs(other.rightMeters - rightMeters) < 1E-9; + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(leftMeters, rightMeters); + } + + @Override + public String toString() { + return String.format( + "DifferentialDriveWheelPositions(Left: %.2f m, Right: %.2f m", leftMeters, rightMeters); + } + + @Override + public DifferentialDriveWheelPositions copy() { + return new DifferentialDriveWheelPositions(leftMeters, rightMeters); + } + + @Override + public DifferentialDriveWheelPositions minus(DifferentialDriveWheelPositions other) { + return new DifferentialDriveWheelPositions( + this.leftMeters - other.leftMeters, this.rightMeters - other.rightMeters); + } + + @Override + public DifferentialDriveWheelPositions interpolate( + DifferentialDriveWheelPositions endValue, double t) { + return new DifferentialDriveWheelPositions( + MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t), + MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t)); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java new file mode 100644 index 0000000000..503df16b6d --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Kinematics.java @@ -0,0 +1,46 @@ +// 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.Twist2d; + +/** + * Helper class that converts a chassis velocity (dx and dtheta components) into wheel speeds. Robot + * code should not use this directly- Instead, use the particular type for your drivetrain (e.g., + * {@link DifferentialDriveKinematics}). + * + * @param The type of the wheel speeds. + * @param

The type of the wheel positions. + */ +public interface Kinematics { + /** + * Performs forward kinematics to return the resulting chassis speed from the wheel speeds. This + * method is often used for odometry -- determining the robot's position on the field using data + * from the real-world speed of each wheel on the robot. + * + * @param wheelSpeeds The speeds of the wheels. + * @return The chassis speed. + */ + ChassisSpeeds toChassisSpeeds(S wheelSpeeds); + + /** + * Performs inverse kinematics to return the wheel speeds from a desired chassis velocity. This + * method is often used to convert joystick values into wheel speeds. + * + * @param chassisSpeeds The desired chassis speed. + * @return The wheel speeds. + */ + S toWheelSpeeds(ChassisSpeeds chassisSpeeds); + + /** + * Performs forward kinematics to return the resulting from the given wheel deltas. This method is + * often used for odometry -- determining the robot's position on the field using changes in the + * distance driven by each wheel on the robot. + * + * @param wheelDeltas The distances driven by each wheel. + * @return The resulting Twist2d in the robot's movement. + */ + Twist2d toTwist2d(P wheelDeltas); +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java index 23204d4a6a..4a092f165c 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java @@ -30,7 +30,8 @@ import org.ejml.simple.SimpleMatrix; *

Forward kinematics is also used for odometry -- determining the position of the robot on the * field using encoders and a gyro. */ -public class MecanumDriveKinematics { +public class MecanumDriveKinematics + implements Kinematics { private final SimpleMatrix m_inverseKinematics; private final SimpleMatrix m_forwardKinematics; @@ -125,6 +126,7 @@ public class MecanumDriveKinematics { * @param chassisSpeeds The desired chassis speed. * @return The wheel speeds. */ + @Override public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) { return toWheelSpeeds(chassisSpeeds, new Translation2d()); } @@ -137,6 +139,7 @@ public class MecanumDriveKinematics { * @param wheelSpeeds The current mecanum drive wheel speeds. * @return The resulting chassis speed. */ + @Override public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) { var wheelSpeedsVector = new SimpleMatrix(4, 1); wheelSpeedsVector.setColumn( @@ -154,14 +157,7 @@ public class MecanumDriveKinematics { chassisSpeedsVector.get(2, 0)); } - /** - * Performs forward kinematics to return the resulting Twist2d from the given wheel deltas. This - * method is often used for odometry -- determining the robot's position on the field using - * changes in the distance driven by each wheel on the robot. - * - * @param wheelDeltas The distances driven by each wheel. - * @return The resulting Twist2d. - */ + @Override public Twist2d toTwist2d(MecanumDriveWheelPositions wheelDeltas) { var wheelDeltasVector = new SimpleMatrix(4, 1); wheelDeltasVector.setColumn( diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java index 40a77e23d7..32bc9bff05 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java @@ -16,14 +16,7 @@ import edu.wpi.first.math.geometry.Rotation2d; *

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 MecanumDriveOdometry { - private final MecanumDriveKinematics m_kinematics; - private Pose2d m_poseMeters; - private MecanumDriveWheelPositions m_previousWheelPositions; - - private Rotation2d m_gyroOffset; - private Rotation2d m_previousAngle; - +public class MecanumDriveOdometry extends Odometry { /** * Constructs a MecanumDriveOdometry object. * @@ -37,16 +30,7 @@ public class MecanumDriveOdometry { Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d initialPoseMeters) { - m_kinematics = kinematics; - m_poseMeters = initialPoseMeters; - m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); - m_previousAngle = initialPoseMeters.getRotation(); - m_previousWheelPositions = - new MecanumDriveWheelPositions( - wheelPositions.frontLeftMeters, - wheelPositions.frontRightMeters, - wheelPositions.rearLeftMeters, - wheelPositions.rearRightMeters); + super(kinematics, gyroAngle, wheelPositions, initialPoseMeters); MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1); } @@ -63,72 +47,4 @@ public class MecanumDriveOdometry { MecanumDriveWheelPositions wheelPositions) { this(kinematics, gyroAngle, wheelPositions, new Pose2d()); } - - /** - * 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 distances driven by each wheel. - * @param poseMeters The position on the field that your robot is at. - */ - public void resetPosition( - Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions, Pose2d poseMeters) { - m_poseMeters = poseMeters; - m_previousAngle = poseMeters.getRotation(); - m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); - m_previousWheelPositions = - new MecanumDriveWheelPositions( - wheelPositions.frontLeftMeters, - wheelPositions.frontRightMeters, - wheelPositions.rearLeftMeters, - wheelPositions.rearRightMeters); - } - - /** - * Returns the position of the robot on the field. - * - * @return The pose of the robot (x and y are in meters). - */ - public Pose2d getPoseMeters() { - return m_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 distances driven by each wheel. - * @return The new pose of the robot. - */ - public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelPositions wheelPositions) { - var angle = gyroAngle.plus(m_gyroOffset); - - var wheelDeltas = - new MecanumDriveWheelPositions( - wheelPositions.frontLeftMeters - m_previousWheelPositions.frontLeftMeters, - wheelPositions.frontRightMeters - m_previousWheelPositions.frontRightMeters, - wheelPositions.rearLeftMeters - m_previousWheelPositions.rearLeftMeters, - wheelPositions.rearRightMeters - m_previousWheelPositions.rearRightMeters); - - var twist = m_kinematics.toTwist2d(wheelDeltas); - twist.dtheta = angle.minus(m_previousAngle).getRadians(); - var newPose = m_poseMeters.exp(twist); - - m_previousAngle = angle; - m_poseMeters = new Pose2d(newPose.getTranslation(), angle); - m_previousWheelPositions = - new MecanumDriveWheelPositions( - wheelPositions.frontLeftMeters, - wheelPositions.frontRightMeters, - wheelPositions.rearLeftMeters, - wheelPositions.rearRightMeters); - - return m_poseMeters; - } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java index 9ff3341682..21c8eafa9a 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java @@ -4,7 +4,10 @@ package edu.wpi.first.math.kinematics; -public class MecanumDriveWheelPositions { +import edu.wpi.first.math.MathUtil; +import java.util.Objects; + +public class MecanumDriveWheelPositions implements WheelPositions { /** Distance measured by the front left wheel. */ public double frontLeftMeters; @@ -39,6 +42,23 @@ public class MecanumDriveWheelPositions { this.rearRightMeters = rearRightMeters; } + @Override + public boolean equals(Object obj) { + if (obj instanceof MecanumDriveWheelPositions) { + MecanumDriveWheelPositions other = (MecanumDriveWheelPositions) obj; + return Math.abs(other.frontLeftMeters - frontLeftMeters) < 1E-9 + && Math.abs(other.frontRightMeters - frontRightMeters) < 1E-9 + && Math.abs(other.rearLeftMeters - rearLeftMeters) < 1E-9 + && Math.abs(other.rearRightMeters - rearRightMeters) < 1E-9; + } + return false; + } + + @Override + public int hashCode() { + return Objects.hash(frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters); + } + @Override public String toString() { return String.format( @@ -46,4 +66,28 @@ public class MecanumDriveWheelPositions { + "Rear Left: %.2f m, Rear Right: %.2f m)", frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters); } + + @Override + public MecanumDriveWheelPositions copy() { + return new MecanumDriveWheelPositions( + frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters); + } + + @Override + public MecanumDriveWheelPositions minus(MecanumDriveWheelPositions other) { + return new MecanumDriveWheelPositions( + this.frontLeftMeters - other.frontLeftMeters, + this.frontRightMeters - other.frontRightMeters, + this.rearLeftMeters - other.rearLeftMeters, + this.rearRightMeters - other.rearRightMeters); + } + + @Override + public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions endValue, double t) { + return new MecanumDriveWheelPositions( + MathUtil.interpolate(this.frontLeftMeters, endValue.frontLeftMeters, t), + MathUtil.interpolate(this.frontRightMeters, endValue.frontRightMeters, t), + MathUtil.interpolate(this.rearLeftMeters, endValue.rearLeftMeters, t), + MathUtil.interpolate(this.rearRightMeters, endValue.rearRightMeters, t)); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java new file mode 100644 index 0000000000..c7ea01a8b4 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/Odometry.java @@ -0,0 +1,99 @@ +// 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.Rotation2d; + +/** + * Class for odometry. Robot code should not use this directly- Instead, use the particular type for + * your drivetrain (e.g., {@link DifferentialDriveOdometry}). 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. + * + *

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 Odometry> { + private final Kinematics m_kinematics; + private Pose2d m_poseMeters; + + private Rotation2d m_gyroOffset; + private Rotation2d m_previousAngle; + private T m_previousWheelPositions; + + /** + * Constructs an Odometry 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 Odometry( + Kinematics kinematics, + Rotation2d gyroAngle, + T wheelPositions, + Pose2d initialPoseMeters) { + m_kinematics = kinematics; + m_poseMeters = initialPoseMeters; + m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); + m_previousAngle = m_poseMeters.getRotation(); + m_previousWheelPositions = wheelPositions.copy(); + } + + /** + * Resets the robot's position on the field. + * + *

The gyroscope angle does not need to be reset here on the user's robot code. The library + * automatically takes care of offsetting the gyro angle. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current encoder readings. + * @param poseMeters The position on the field that your robot is at. + */ + public void resetPosition(Rotation2d gyroAngle, T wheelPositions, Pose2d poseMeters) { + m_poseMeters = poseMeters; + m_previousAngle = m_poseMeters.getRotation(); + m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); + m_previousWheelPositions = wheelPositions.copy(); + } + + /** + * Returns the position of the robot on the field. + * + * @return The pose of the robot (x and y are in meters). + */ + public Pose2d getPoseMeters() { + return m_poseMeters; + } + + /** + * Updates the robot's position on the field using forward kinematics and integration of the pose + * over time. This method takes in an angle parameter which is used instead of the angular rate + * that is calculated from forward kinematics, in addition to the current distance measurement at + * each wheel. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current encoder readings. + * @return The new pose of the robot. + */ + public Pose2d update(Rotation2d gyroAngle, T wheelPositions) { + T wheelDeltas = wheelPositions.minus(m_previousWheelPositions); + + var angle = gyroAngle.plus(m_gyroOffset); + + var twist = m_kinematics.toTwist2d(wheelDeltas); + twist.dtheta = angle.minus(m_previousAngle).getRadians(); + + var newPose = m_poseMeters.exp(twist); + + m_previousWheelPositions = wheelPositions.copy(); + m_previousAngle = angle; + m_poseMeters = new Pose2d(newPose.getTranslation(), angle); + + return m_poseMeters; + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java index 4d3a4c5e55..2f58217493 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java @@ -32,7 +32,24 @@ import org.ejml.simple.SimpleMatrix; *

Forward kinematics is also used for odometry -- determining the position of the robot on the * field using encoders and a gyro. */ -public class SwerveDriveKinematics { +public class SwerveDriveKinematics + implements Kinematics { + public static class SwerveDriveWheelStates { + public SwerveModuleState[] states; + + /** + * Creates a new SwerveDriveWheelStates instance. + * + * @param states The swerve module states. This will be deeply copied. + */ + public SwerveDriveWheelStates(SwerveModuleState[] states) { + this.states = new SwerveModuleState[states.length]; + for (int i = 0; i < states.length; i++) { + this.states[i] = new SwerveModuleState(states[i].speedMetersPerSecond, states[i].angle); + } + } + } + private final SimpleMatrix m_inverseKinematics; private final SimpleMatrix m_forwardKinematics; @@ -159,6 +176,11 @@ public class SwerveDriveKinematics { return toSwerveModuleStates(chassisSpeeds, new Translation2d()); } + @Override + public SwerveDriveWheelStates toWheelSpeeds(ChassisSpeeds chassisSpeeds) { + return new SwerveDriveWheelStates(toSwerveModuleStates(chassisSpeeds)); + } + /** * Performs forward kinematics to return the resulting chassis state from the given module states. * This method is often used for odometry -- determining the robot's position on the field using @@ -190,6 +212,11 @@ public class SwerveDriveKinematics { chassisSpeedsVector.get(2, 0)); } + @Override + public ChassisSpeeds toChassisSpeeds(SwerveDriveWheelStates wheelStates) { + return toChassisSpeeds(wheelStates.states); + } + /** * Performs forward kinematics to return the resulting chassis state from the given module states. * This method is often used for odometry -- determining the robot's position on the field using @@ -219,6 +246,11 @@ public class SwerveDriveKinematics { chassisDeltaVector.get(0, 0), chassisDeltaVector.get(1, 0), chassisDeltaVector.get(2, 0)); } + @Override + public Twist2d toTwist2d(SwerveDriveWheelPositions wheelDeltas) { + return toTwist2d(wheelDeltas.positions); + } + /** * Renormalizes the wheel speeds if any individual speed is above the specified maximum. * diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java index c2e188f23c..4f954e930e 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java @@ -17,14 +17,8 @@ import edu.wpi.first.math.geometry.Rotation2d; *

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 SwerveDriveOdometry { - private final SwerveDriveKinematics m_kinematics; - private Pose2d m_poseMeters; - - private Rotation2d m_gyroOffset; - private Rotation2d m_previousAngle; +public class SwerveDriveOdometry extends Odometry { private final int m_numModules; - private SwerveModulePosition[] m_previousModulePositions; /** * Constructs a SwerveDriveOdometry object. @@ -39,18 +33,9 @@ public class SwerveDriveOdometry { Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d initialPose) { - m_kinematics = kinematics; - m_poseMeters = initialPose; - m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); - m_previousAngle = initialPose.getRotation(); - m_numModules = modulePositions.length; + super(kinematics, gyroAngle, new SwerveDriveWheelPositions(modulePositions), initialPose); - m_previousModulePositions = new SwerveModulePosition[m_numModules]; - for (int index = 0; index < m_numModules; index++) { - m_previousModulePositions[index] = - new SwerveModulePosition( - modulePositions[index].distanceMeters, modulePositions[index].angle); - } + m_numModules = modulePositions.length; MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1); } @@ -83,29 +68,18 @@ public class SwerveDriveOdometry { */ public void resetPosition( Rotation2d gyroAngle, SwerveModulePosition[] modulePositions, Pose2d pose) { - if (modulePositions.length != m_numModules) { + resetPosition(gyroAngle, new SwerveDriveWheelPositions(modulePositions), pose); + } + + @Override + public void resetPosition( + Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions, Pose2d pose) { + if (modulePositions.positions.length != m_numModules) { throw new IllegalArgumentException( "Number of modules is not consistent with number of wheel locations provided in " + "constructor"); } - - m_poseMeters = pose; - m_previousAngle = pose.getRotation(); - m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle); - for (int index = 0; index < m_numModules; index++) { - m_previousModulePositions[index] = - new SwerveModulePosition( - modulePositions[index].distanceMeters, modulePositions[index].angle); - } - } - - /** - * Returns the position of the robot on the field. - * - * @return The pose of the robot (x and y are in meters). - */ - public Pose2d getPoseMeters() { - return m_poseMeters; + super.resetPosition(gyroAngle, modulePositions, pose); } /** @@ -121,32 +95,16 @@ public class SwerveDriveOdometry { * @return The new pose of the robot. */ public Pose2d update(Rotation2d gyroAngle, SwerveModulePosition[] modulePositions) { - if (modulePositions.length != m_numModules) { + return update(gyroAngle, new SwerveDriveWheelPositions(modulePositions)); + } + + @Override + public Pose2d update(Rotation2d gyroAngle, SwerveDriveWheelPositions modulePositions) { + if (modulePositions.positions.length != m_numModules) { throw new IllegalArgumentException( "Number of modules is not consistent with number of wheel locations provided in " + "constructor"); } - - var moduleDeltas = new SwerveModulePosition[m_numModules]; - for (int index = 0; index < m_numModules; index++) { - var current = modulePositions[index]; - var previous = m_previousModulePositions[index]; - - moduleDeltas[index] = - new SwerveModulePosition(current.distanceMeters - previous.distanceMeters, current.angle); - previous.distanceMeters = current.distanceMeters; - } - - var angle = gyroAngle.plus(m_gyroOffset); - - var twist = m_kinematics.toTwist2d(moduleDeltas); - twist.dtheta = angle.minus(m_previousAngle).getRadians(); - - var newPose = m_poseMeters.exp(twist); - - m_previousAngle = angle; - m_poseMeters = new Pose2d(newPose.getTranslation(), angle); - - return m_poseMeters; + return super.update(gyroAngle, modulePositions); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java new file mode 100644 index 0000000000..88c00c4b38 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveWheelPositions.java @@ -0,0 +1,72 @@ +// 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 java.util.Arrays; +import java.util.Objects; +import java.util.function.BinaryOperator; + +public class SwerveDriveWheelPositions implements WheelPositions { + public SwerveModulePosition[] positions; + + /** + * Creates a new SwerveDriveWheelPositions instance. + * + * @param positions The swerve module positions. This will be deeply copied. + */ + public SwerveDriveWheelPositions(SwerveModulePosition[] positions) { + this.positions = new SwerveModulePosition[positions.length]; + for (int i = 0; i < positions.length; i++) { + this.positions[i] = positions[i].copy(); + } + } + + @Override + public boolean equals(Object obj) { + if (obj instanceof SwerveDriveWheelPositions) { + SwerveDriveWheelPositions other = (SwerveDriveWheelPositions) obj; + return Arrays.equals(this.positions, other.positions); + } + return false; + } + + @Override + public int hashCode() { + // Cast to interpret positions as single argument, not array of the arguments + return Objects.hash((Object) positions); + } + + @Override + public String toString() { + return String.format("SwerveDriveWheelPositions(%s)", Arrays.toString(positions)); + } + + private SwerveDriveWheelPositions generate( + SwerveDriveWheelPositions other, BinaryOperator generator) { + if (other.positions.length != positions.length) { + throw new IllegalArgumentException("Inconsistent number of modules!"); + } + var newPositions = new SwerveModulePosition[positions.length]; + for (int i = 0; i < positions.length; i++) { + newPositions[i] = generator.apply(positions[i], other.positions[i]); + } + return new SwerveDriveWheelPositions(newPositions); + } + + @Override + public SwerveDriveWheelPositions copy() { + return new SwerveDriveWheelPositions(positions); + } + + @Override + public SwerveDriveWheelPositions minus(SwerveDriveWheelPositions other) { + return generate(other, (a, b) -> a.minus(b)); + } + + @Override + public SwerveDriveWheelPositions interpolate(SwerveDriveWheelPositions endValue, double t) { + return generate(endValue, (a, b) -> a.interpolate(b, t)); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java index 7968154ae5..b418744be3 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModulePosition.java @@ -4,11 +4,14 @@ package edu.wpi.first.math.kinematics; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.interpolation.Interpolatable; import java.util.Objects; /** Represents the state of one swerve module. */ -public class SwerveModulePosition implements Comparable { +public class SwerveModulePosition + implements Comparable, Interpolatable { /** Distance measured by the wheel of the module. */ public double distanceMeters; @@ -60,4 +63,32 @@ public class SwerveModulePosition implements Comparable { return String.format( "SwerveModulePosition(Distance: %.2f m, Angle: %s)", distanceMeters, angle); } + + /** + * Returns a copy of this swerve module position. + * + * @return A copy. + */ + public SwerveModulePosition copy() { + return new SwerveModulePosition(distanceMeters, angle); + } + + /** + * Calculates the difference between two swerve module positions. The difference has a length + * equal to the difference in lengths and an angle equal to the ending angle (this module + * position's angle). + * + * @param other The swerve module position to subtract. + * @return The difference. + */ + public SwerveModulePosition minus(SwerveModulePosition other) { + return new SwerveModulePosition(this.distanceMeters - other.distanceMeters, this.angle); + } + + @Override + public SwerveModulePosition interpolate(SwerveModulePosition endValue, double t) { + return new SwerveModulePosition( + MathUtil.interpolate(this.distanceMeters, endValue.distanceMeters, t), + this.angle.interpolate(endValue.angle, t)); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java new file mode 100644 index 0000000000..13888b3ea5 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java @@ -0,0 +1,24 @@ +// 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.interpolation.Interpolatable; + +public interface WheelPositions> extends Interpolatable { + /** + * Returns a copy of this instance. + * + * @return A copy. + */ + T copy(); + + /** + * Returns the difference with another set of wheel positions. + * + * @param other The other instance to compare to. + * @return The representation of how the wheels moved from other to this. + */ + T minus(T other); +} diff --git a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp index 4574c4a769..213ba99176 100644 --- a/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp @@ -4,41 +4,8 @@ #include "frc/estimator/DifferentialDrivePoseEstimator.h" -#include - -#include "frc/StateSpaceUtil.h" -#include "frc/estimator/AngleStatistics.h" -#include "wpimath/MathShared.h" - using namespace frc; -DifferentialDrivePoseEstimator::InterpolationRecord -DifferentialDrivePoseEstimator::InterpolationRecord::Interpolate( - DifferentialDriveKinematics& kinematics, InterpolationRecord endValue, - double i) const { - if (i < 0) { - return *this; - } else if (i > 1) { - return endValue; - } else { - // Find the interpolated left distance. - auto left = wpi::Lerp(this->leftDistance, endValue.leftDistance, i); - // Find the interpolated right distance. - auto right = wpi::Lerp(this->rightDistance, endValue.rightDistance, i); - - // Find the new gyro angle. - auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i); - - // Create a twist to represent this changed based on the interpolated - // sensor inputs. - auto twist = - kinematics.ToTwist2d(left - leftDistance, right - rightDistance); - twist.dtheta = (gyro - gyroAngle).Radians(); - - return {pose.Exp(twist), gyro, left, right}; - } -} - DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( DifferentialDriveKinematics& kinematics, const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, @@ -52,128 +19,7 @@ DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator( units::meter_t leftDistance, units::meter_t rightDistance, const Pose2d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : m_kinematics{kinematics}, - m_odometry{gyroAngle, leftDistance, rightDistance, initialPose} { - for (size_t i = 0; i < 3; ++i) { - m_q[i] = stateStdDevs[i] * stateStdDevs[i]; - } - - SetVisionMeasurementStdDevs(visionMeasurementStdDevs); -} - -void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs( - const wpi::array& visionMeasurementStdDevs) { - wpi::array r{wpi::empty_array}; - for (size_t i = 0; i < 3; ++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 < 3; ++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])); - } - } -} - -void DifferentialDrivePoseEstimator::ResetPosition(const Rotation2d& gyroAngle, - units::meter_t leftDistance, - units::meter_t rightDistance, - const Pose2d& pose) { - // Reset state estimate and error covariance - m_odometry.ResetPosition(gyroAngle, leftDistance, rightDistance, pose); - m_poseBuffer.Clear(); -} - -Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const { - return m_odometry.GetPose(); -} - -void DifferentialDrivePoseEstimator::AddVisionMeasurement( - const Pose2d& visionRobotPose, units::second_t timestamp) { - // Step 0: If this measurement is old enough to be outside the pose buffer's - // timespan, skip. - if (!m_poseBuffer.GetInternalBuffer().empty() && - m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration > - timestamp) { - return; - } - - // Step 1: Get the estimated pose from when the vision measurement was made. - auto sample = m_poseBuffer.Sample(timestamp); - - if (!sample.has_value()) { - return; - } - - // Step 2: Measure the twist between the odometry pose and the vision pose. - auto twist = sample.value().pose.Log(visionRobotPose); - - // Step 3: 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<3> k_times_twist = - m_visionK * - frc::Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dtheta.value()}; - - // Step 4: Convert back to Twist2d. - Twist2d scaledTwist{units::meter_t{k_times_twist(0)}, - units::meter_t{k_times_twist(1)}, - units::radian_t{k_times_twist(2)}}; - - // Step 5: Reset Odometry to state at sample with vision adjustment. - m_odometry.ResetPosition( - sample.value().gyroAngle, sample.value().leftDistance, - sample.value().rightDistance, sample.value().pose.Exp(scaledTwist)); - - // Step 6: Record the current pose to allow multiple measurements from the - // same timestamp - m_poseBuffer.AddSample( - timestamp, {GetEstimatedPosition(), sample.value().gyroAngle, - sample.value().leftDistance, sample.value().rightDistance}); - - // Step 7: Replay odometry inputs between sample time and latest recorded - // sample to update the pose buffer and correct odometry. - auto internal_buf = m_poseBuffer.GetInternalBuffer(); - - auto first_newer_record = - std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp, - [](const auto& pair, auto t) { return t > pair.first; }); - - for (auto entry = first_newer_record + 1; entry != internal_buf.end(); - entry++) { - UpdateWithTime(entry->first, entry->second.gyroAngle, - entry->second.leftDistance, entry->second.rightDistance); - } -} - -Pose2d DifferentialDrivePoseEstimator::Update(const Rotation2d& gyroAngle, - units::meter_t leftDistance, - units::meter_t rightDistance) { - return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle, - leftDistance, rightDistance); -} - -Pose2d DifferentialDrivePoseEstimator::UpdateWithTime( - units::second_t currentTime, const Rotation2d& gyroAngle, - units::meter_t leftDistance, units::meter_t rightDistance) { - m_odometry.Update(gyroAngle, leftDistance, rightDistance); - - // fmt::print("odo, {}, {}, {}, {}, {}, {}\n", - // gyroAngle.Radians(), - // leftDistance, - // rightDistance, - // GetEstimatedPosition().X(), - // GetEstimatedPosition().Y(), - // GetEstimatedPosition().Rotation().Radians() - // ); - - m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle, - leftDistance, rightDistance}); - - return GetEstimatedPosition(); -} + : PoseEstimator( + kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + m_odometryImpl{gyroAngle, leftDistance, rightDistance, initialPose} {} diff --git a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp index c2fbb10389..7c75e71ffe 100644 --- a/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp +++ b/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp @@ -12,46 +12,6 @@ using namespace frc; -frc::MecanumDrivePoseEstimator::InterpolationRecord -frc::MecanumDrivePoseEstimator::InterpolationRecord::Interpolate( - MecanumDriveKinematics& kinematics, InterpolationRecord endValue, - double i) const { - if (i < 0) { - return *this; - } else if (i > 1) { - return endValue; - } else { - // Find the new wheel distance measurements. - MecanumDriveWheelPositions wheels_lerp{ - wpi::Lerp(this->wheelPositions.frontLeft, - endValue.wheelPositions.frontLeft, i), - wpi::Lerp(this->wheelPositions.frontRight, - endValue.wheelPositions.frontRight, i), - wpi::Lerp(this->wheelPositions.rearLeft, - endValue.wheelPositions.rearLeft, i), - wpi::Lerp(this->wheelPositions.rearRight, - endValue.wheelPositions.rearRight, i)}; - - // Find the distance between this measurement and the - // interpolated measurement. - MecanumDriveWheelPositions wheels_delta{ - wheels_lerp.frontLeft - this->wheelPositions.frontLeft, - wheels_lerp.frontRight - this->wheelPositions.frontRight, - wheels_lerp.rearLeft - this->wheelPositions.rearLeft, - wheels_lerp.rearRight - this->wheelPositions.rearRight}; - - // Find the new gyro angle. - auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i); - - // Create a twist to represent this changed based on the interpolated - // sensor inputs. - auto twist = kinematics.ToTwist2d(wheels_delta); - twist.dtheta = (gyro - gyroAngle).Radians(); - - return {pose.Exp(twist), gyro, wheels_lerp}; - } -} - frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( MecanumDriveKinematics& kinematics, const Rotation2d& gyroAngle, const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose) @@ -64,121 +24,6 @@ frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator( const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : m_kinematics{kinematics}, - m_odometry{kinematics, gyroAngle, wheelPositions, initialPose} { - for (size_t i = 0; i < 3; ++i) { - m_q[i] = stateStdDevs[i] * stateStdDevs[i]; - } - - SetVisionMeasurementStdDevs(visionMeasurementStdDevs); -} - -void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs( - const wpi::array& visionMeasurementStdDevs) { - wpi::array r{wpi::empty_array}; - for (size_t i = 0; i < 3; ++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 < 3; ++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])); - } - } -} - -void frc::MecanumDrivePoseEstimator::ResetPosition( - const Rotation2d& gyroAngle, - const MecanumDriveWheelPositions& wheelPositions, const Pose2d& pose) { - // Reset state estimate and error covariance - m_odometry.ResetPosition(gyroAngle, wheelPositions, pose); - m_poseBuffer.Clear(); -} - -Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const { - return m_odometry.GetPose(); -} - -void frc::MecanumDrivePoseEstimator::AddVisionMeasurement( - const Pose2d& visionRobotPose, units::second_t timestamp) { - // Step 0: If this measurement is old enough to be outside the pose buffer's - // timespan, skip. - if (!m_poseBuffer.GetInternalBuffer().empty() && - m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration > - timestamp) { - return; - } - - // Step 1: Get the estimated pose from when the vision measurement was made. - auto sample = m_poseBuffer.Sample(timestamp); - - if (!sample.has_value()) { - return; - } - - // Step 2: Measure the twist between the odometry pose and the vision pose - auto twist = sample.value().pose.Log(visionRobotPose); - - // Step 3: 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<3> k_times_twist = - m_visionK * - frc::Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dtheta.value()}; - - // Step 4: Convert back to Twist2d - Twist2d scaledTwist{units::meter_t{k_times_twist(0)}, - units::meter_t{k_times_twist(1)}, - units::radian_t{k_times_twist(2)}}; - - // Step 5: Reset Odometry to state at sample with vision adjustment. - m_odometry.ResetPosition(sample.value().gyroAngle, - sample.value().wheelPositions, - sample.value().pose.Exp(scaledTwist)); - - // Step 6: Record the current pose to allow multiple measurements from the - // same timestamp - m_poseBuffer.AddSample(timestamp, - {GetEstimatedPosition(), sample.value().gyroAngle, - sample.value().wheelPositions}); - - // Step 7: Replay odometry inputs between sample time and latest recorded - // sample to update the pose buffer and correct odometry. - auto internal_buf = m_poseBuffer.GetInternalBuffer(); - - auto upper_bound = - std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp, - [](const auto& pair, auto t) { return t > pair.first; }); - - for (auto entry = upper_bound; entry != internal_buf.end(); entry++) { - UpdateWithTime(entry->first, entry->second.gyroAngle, - entry->second.wheelPositions); - } -} - -Pose2d frc::MecanumDrivePoseEstimator::Update( - const Rotation2d& gyroAngle, - const MecanumDriveWheelPositions& wheelPositions) { - return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle, - wheelPositions); -} - -Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime( - units::second_t currentTime, const Rotation2d& gyroAngle, - const MecanumDriveWheelPositions& wheelPositions) { - m_odometry.Update(gyroAngle, wheelPositions); - - MecanumDriveWheelPositions internalWheelPositions{ - wheelPositions.frontLeft, wheelPositions.frontRight, - wheelPositions.rearLeft, wheelPositions.rearRight}; - - m_poseBuffer.AddSample( - currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions}); - - return GetEstimatedPosition(); -} + : PoseEstimator( + kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + m_odometryImpl(kinematics, gyroAngle, wheelPositions, initialPose) {} diff --git a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp index 1ff7a8a0d2..346a232e75 100644 --- a/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp +++ b/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp @@ -11,32 +11,9 @@ using namespace frc; DifferentialDriveOdometry::DifferentialDriveOdometry( const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose2d& initialPose) - : m_pose(initialPose), - m_prevLeftDistance(leftDistance), - m_prevRightDistance(rightDistance) { - m_previousAngle = m_pose.Rotation(); - m_gyroOffset = m_pose.Rotation() - gyroAngle; + : Odometry( + m_kinematicsImpl, gyroAngle, {leftDistance, rightDistance}, + initialPose) { wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_DifferentialDrive, 1); } - -const Pose2d& DifferentialDriveOdometry::Update(const Rotation2d& gyroAngle, - units::meter_t leftDistance, - units::meter_t rightDistance) { - auto deltaLeftDistance = leftDistance - m_prevLeftDistance; - auto deltaRightDistance = rightDistance - m_prevRightDistance; - - m_prevLeftDistance = leftDistance; - m_prevRightDistance = rightDistance; - - auto averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0; - auto angle = gyroAngle + m_gyroOffset; - - auto newPose = m_pose.Exp( - {averageDeltaDistance, 0_m, (angle - m_previousAngle).Radians()}); - - m_previousAngle = angle; - m_pose = {newPose.Translation(), angle}; - - return m_pose; -} diff --git a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp index 394adaf128..55055be744 100644 --- a/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp +++ b/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp @@ -11,35 +11,9 @@ using namespace frc; MecanumDriveOdometry::MecanumDriveOdometry( MecanumDriveKinematics kinematics, const Rotation2d& gyroAngle, const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose) - : m_kinematics(kinematics), - m_pose(initialPose), - m_previousWheelPositions(wheelPositions) { - m_previousAngle = m_pose.Rotation(); - m_gyroOffset = m_pose.Rotation() - gyroAngle; + : Odometry( + m_kinematicsImpl, gyroAngle, wheelPositions, initialPose), + m_kinematicsImpl(kinematics) { wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_MecanumDrive, 1); } - -const Pose2d& MecanumDriveOdometry::Update( - const Rotation2d& gyroAngle, - const MecanumDriveWheelPositions& wheelPositions) { - auto angle = gyroAngle + m_gyroOffset; - - MecanumDriveWheelPositions wheelDeltas{ - wheelPositions.frontLeft - m_previousWheelPositions.frontLeft, - wheelPositions.frontRight - m_previousWheelPositions.frontRight, - wheelPositions.rearLeft - m_previousWheelPositions.rearLeft, - wheelPositions.rearRight - m_previousWheelPositions.rearRight, - }; - - auto twist = m_kinematics.ToTwist2d(wheelDeltas); - twist.dtheta = (angle - m_previousAngle).Radians(); - - auto newPose = m_pose.Exp(twist); - - m_previousAngle = angle; - m_previousWheelPositions = wheelPositions; - m_pose = {newPose.Translation(), angle}; - - return m_pose; -} diff --git a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h index 49f00e39e4..89dcd35cb0 100644 --- a/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h @@ -7,13 +7,11 @@ #include #include -#include "frc/EigenCore.h" +#include "frc/estimator/PoseEstimator.h" #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" -#include "frc/interpolation/TimeInterpolatableBuffer.h" #include "frc/kinematics/DifferentialDriveKinematics.h" #include "frc/kinematics/DifferentialDriveOdometry.h" -#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" #include "units/time.h" namespace frc { @@ -32,7 +30,9 @@ namespace frc { * 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 DifferentialDrivePoseEstimator { +class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator + : public PoseEstimator { public: /** * Constructs a DifferentialDrivePoseEstimator with default standard @@ -79,19 +79,6 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator { const Pose2d& initialPose, const wpi::array& stateStdDevs, const wpi::array& 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); - /** * Resets the robot's position on the field. * @@ -101,71 +88,10 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator { * @param pose The estimated pose of the robot on the field. */ void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance, - units::meter_t rightDistance, const Pose2d& pose); - - /** - * Gets the estimated robot pose. - * - * @return The estimated robot pose. - */ - Pose2d GetEstimatedPosition() const; - - /** - * Adds a vision measurement to the Kalman Filter. This will correct - * the odometry pose estimate while still accounting for measurement noise. - * - * This method can be called as infrequently as you want, as long as you are - * calling Update() every loop. - * - * To promote stability of the pose estimate and make it robust to bad vision - * data, we recommend only adding vision measurements that are already within - * one meter or so of the current pose estimate. - * - * @param visionRobotPose The pose of the robot as measured by the vision - * camera. - * @param timestamp The timestamp of the vision measurement in seconds. Note - * that if you don't use your own time source by calling UpdateWithTime(), - * then you must use a timestamp with an epoch since FPGA startup (i.e., - * the epoch of this timestamp is the same epoch as - * frc::Timer::GetFPGATimestamp(). This means that you should use - * frc::Timer::GetFPGATimestamp() as your time source in this case. - */ - void AddVisionMeasurement(const Pose2d& visionRobotPose, - units::second_t timestamp); - - /** - * Adds a vision measurement to the Kalman Filter. This will correct - * the odometry pose estimate while still accounting for measurement noise. - * - * This method can be called as infrequently as you want, as long as you are - * calling Update() every loop. - * - * To promote stability of the pose estimate and make it robust to bad vision - * data, we recommend only adding vision measurements that are already within - * one meter or so of the current pose estimate. - * - * Note that the vision measurement standard deviations passed into this - * method will continue to apply to future measurements until a subsequent - * call to SetVisionMeasurementStdDevs() or this method. - * - * @param visionRobotPose The pose of the robot as measured by the vision - * camera. - * @param timestamp The timestamp of the vision measurement in seconds. Note - * that if you don't use your own time source by calling UpdateWithTime(), - * then you must use a timestamp with an epoch since FPGA startup (i.e., - * the epoch of this timestamp is the same epoch as - * frc::Timer::GetFPGATimestamp(). This means that you should use - * frc::Timer::GetFPGATimestamp() as your time source in this case. - * @param visionMeasurementStdDevs Standard deviations of the vision pose - * measurement (x position in meters, y position in meters, and heading in - * radians). Increase these numbers to trust the vision pose measurement - * less. - */ - void AddVisionMeasurement( - const Pose2d& visionRobotPose, units::second_t timestamp, - const wpi::array& visionMeasurementStdDevs) { - SetVisionMeasurementStdDevs(visionMeasurementStdDevs); - AddVisionMeasurement(visionRobotPose, timestamp); + units::meter_t rightDistance, const Pose2d& pose) { + PoseEstimator:: + ResetPosition(gyroAngle, {leftDistance, rightDistance}, pose); } /** @@ -179,7 +105,12 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator { * @return The estimated pose of the robot. */ Pose2d Update(const Rotation2d& gyroAngle, units::meter_t leftDistance, - units::meter_t rightDistance); + units::meter_t rightDistance) { + return PoseEstimator< + DifferentialDriveWheelSpeeds, + DifferentialDriveWheelPositions>::Update(gyroAngle, + {leftDistance, rightDistance}); + } /** * Updates the pose estimator with wheel encoder and gyro information. This @@ -195,63 +126,16 @@ class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator { Pose2d UpdateWithTime(units::second_t currentTime, const Rotation2d& gyroAngle, units::meter_t leftDistance, - units::meter_t rightDistance); + units::meter_t rightDistance) { + return PoseEstimator< + DifferentialDriveWheelSpeeds, + DifferentialDriveWheelPositions>::UpdateWithTime(currentTime, gyroAngle, + {leftDistance, + rightDistance}); + } private: - struct InterpolationRecord { - // The pose observed given the current sensor inputs and the previous pose. - Pose2d pose; - - // The current gyro angle. - Rotation2d gyroAngle; - - // The distance traveled by the left encoder. - units::meter_t leftDistance; - - // The distance traveled by the right encoder. - units::meter_t rightDistance; - - /** - * Checks equality between this InterpolationRecord and another object. - * - * @param other The other object. - * @return Whether the two objects are equal. - */ - bool operator==(const InterpolationRecord& other) const = default; - - /** - * Checks inequality between this InterpolationRecord and another object. - * - * @param other The other object. - * @return Whether the two objects are not equal. - */ - bool operator!=(const InterpolationRecord& other) const = default; - - /** - * Interpolates between two InterpolationRecords. - * - * @param endValue The end value for the interpolation. - * @param i The interpolant (fraction). - * - * @return The interpolated state. - */ - InterpolationRecord Interpolate(DifferentialDriveKinematics& kinematics, - InterpolationRecord endValue, - double i) const; - }; - - static constexpr units::second_t kBufferDuration = 1.5_s; - - DifferentialDriveKinematics& m_kinematics; - DifferentialDriveOdometry m_odometry; - wpi::array m_q{wpi::empty_array}; - Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero(); - - TimeInterpolatableBuffer m_poseBuffer{ - kBufferDuration, [this](const InterpolationRecord& start, - const InterpolationRecord& end, double t) { - return start.Interpolate(this->m_kinematics, end, t); - }}; + DifferentialDriveOdometry m_odometryImpl; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h index 10fa4e9836..52794fb844 100644 --- a/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h @@ -10,6 +10,7 @@ #include #include "frc/EigenCore.h" +#include "frc/estimator/PoseEstimator.h" #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" #include "frc/interpolation/TimeInterpolatableBuffer.h" @@ -32,7 +33,9 @@ namespace frc { * never call it, then this class will behave mostly like regular encoder * odometry. */ -class WPILIB_DLLEXPORT MecanumDrivePoseEstimator { +class WPILIB_DLLEXPORT MecanumDrivePoseEstimator + : public PoseEstimator { public: /** * Constructs a MecanumDrivePoseEstimator with default standard deviations @@ -76,174 +79,8 @@ class WPILIB_DLLEXPORT MecanumDrivePoseEstimator { const Pose2d& initialPose, const wpi::array& stateStdDevs, const wpi::array& 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); - - /** - * 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 angle reported by the gyroscope. - * @param wheelPositions The distances measured at each wheel. - * @param pose The position on the field that your robot is at. - */ - void ResetPosition(const Rotation2d& gyroAngle, - const MecanumDriveWheelPositions& wheelPositions, - const Pose2d& pose); - - /** - * Gets the estimated robot pose. - * - * @return The estimated robot pose in meters. - */ - Pose2d GetEstimatedPosition() const; - - /** - * Add 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 or sync the epochs. - */ - void AddVisionMeasurement(const Pose2d& visionRobotPose, - units::second_t timestamp); - - /** - * Adds a vision measurement to the Kalman Filter. This will correct - * the odometry pose estimate while still accounting for measurement noise. - * - * This method can be called as infrequently as you want, as long as you are - * calling Update() every loop. - * - * To promote stability of the pose estimate and make it robust to bad vision - * data, we recommend only adding vision measurements that are already within - * one meter or so of the current pose estimate. - * - * Note that the vision measurement standard deviations passed into this - * method will continue to apply to future measurements until a subsequent - * call to SetVisionMeasurementStdDevs() or this method. - * - * @param visionRobotPose The pose of the robot as measured by the vision - * camera. - * @param timestamp The timestamp of the vision measurement in seconds. Note - * that if you don't use your own time source by calling UpdateWithTime(), - * then you must use a timestamp with an epoch since FPGA startup (i.e., - * the epoch of this timestamp is the same epoch as - * frc::Timer::GetFPGATimestamp(). This means that you should use - * frc::Timer::GetFPGATimestamp() as your time source in this case. - * @param visionMeasurementStdDevs Standard deviations of the vision pose - * measurement (x position in meters, y position in meters, and heading in - * radians). Increase these numbers to trust the vision pose measurement - * less. - */ - void AddVisionMeasurement( - const Pose2d& visionRobotPose, units::second_t timestamp, - const wpi::array& visionMeasurementStdDevs) { - SetVisionMeasurementStdDevs(visionMeasurementStdDevs); - AddVisionMeasurement(visionRobotPose, timestamp); - } - - /** - * 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 measured at each wheel. - * @return The estimated pose of the robot in meters. - */ - Pose2d Update(const Rotation2d& gyroAngle, - const MecanumDriveWheelPositions& wheelPositions); - - /** - * Updates the pose estimator with wheel encoder and gyro information. This - * should be called every loop. - * - * @param currentTime Time at which this method was called, in seconds. - * @param gyroAngle The current gyroscope angle. - * @param wheelPositions The distances measured at each wheel. - * @return The estimated pose of the robot in meters. - */ - Pose2d UpdateWithTime(units::second_t currentTime, - const Rotation2d& gyroAngle, - const MecanumDriveWheelPositions& wheelPositions); - private: - struct InterpolationRecord { - // The pose observed given the current sensor inputs and the previous pose. - Pose2d pose; - - // The current gyroscope angle. - Rotation2d gyroAngle; - - // The distances measured at each wheel. - MecanumDriveWheelPositions wheelPositions; - - /** - * Checks equality between this InterpolationRecord and another object. - * - * @param other The other object. - * @return Whether the two objects are equal. - */ - bool operator==(const InterpolationRecord& other) const = default; - - /** - * Checks inequality between this InterpolationRecord and another object. - * - * @param other The other object. - * @return Whether the two objects are not equal. - */ - bool operator!=(const InterpolationRecord& other) const = default; - - /** - * Interpolates between two InterpolationRecords. - * - * @param endValue The end value for the interpolation. - * @param i The interpolant (fraction). - * - * @return The interpolated state. - */ - InterpolationRecord Interpolate(MecanumDriveKinematics& kinematics, - InterpolationRecord endValue, - double i) const; - }; - - static constexpr units::second_t kBufferDuration = 1.5_s; - - MecanumDriveKinematics& m_kinematics; - MecanumDriveOdometry m_odometry; - wpi::array m_q{wpi::empty_array}; - Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero(); - - TimeInterpolatableBuffer m_poseBuffer{ - kBufferDuration, [this](const InterpolationRecord& start, - const InterpolationRecord& end, double t) { - return start.Interpolate(this->m_kinematics, end, t); - }}; + MecanumDriveOdometry m_odometryImpl; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h new file mode 100644 index 0000000000..542ab2e315 --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h @@ -0,0 +1,228 @@ +// 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/EigenCore.h" +#include "frc/geometry/Pose2d.h" +#include "frc/geometry/Rotation2d.h" +#include "frc/interpolation/TimeInterpolatableBuffer.h" +#include "frc/kinematics/Kinematics.h" +#include "frc/kinematics/Odometry.h" +#include "frc/kinematics/WheelPositions.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., + * DifferentialDrivePoseEstimator). It will correct for noisy vision + * measurements and encoder drift. It is intended to be an easy drop-in for + * Odometry. + * + * 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. + */ +template +class WPILIB_DLLEXPORT PoseEstimator { + public: + /** + * Constructs a PoseEstimator. + * + * @param kinematics A correctly-configured kinematics object for your + * drivetrain. + * @param odometry A correctly-configured odometry object for your drivetrain. + * @param stateStdDevs Standard deviations of the pose estimate (x position in + * meters, y position in meters, and heading in radians). Increase these + * numbers to trust your state estimate less. + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement (x position in meters, y position in meters, and heading in + * radians). Increase these numbers to trust the vision pose measurement + * less. + */ + PoseEstimator(Kinematics& kinematics, + Odometry& odometry, + const wpi::array& stateStdDevs, + const wpi::array& 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); + + /** + * Resets the robot's position on the field. + * + * The gyroscope angle does not need to be reset in the user's robot code. + * The library automatically takes care of offsetting the gyro angle. + * + * @param gyroAngle The current gyro angle. + * @param wheelPositions The distances traveled by the encoders. + * @param pose The estimated pose of the robot on the field. + */ + void ResetPosition(const Rotation2d& gyroAngle, + const WheelPositions& wheelPositions, const Pose2d& pose); + + /** + * Gets the estimated robot pose. + * + * @return The estimated robot pose in meters. + */ + Pose2d GetEstimatedPosition() const; + + /** + * Adds a vision measurement to the Kalman Filter. This will correct + * the odometry pose estimate while still accounting for measurement noise. + * + * This method can be called as infrequently as you want, as long as you are + * calling Update() every loop. + * + * To promote stability of the pose estimate and make it robust to bad vision + * data, we recommend only adding vision measurements that are already within + * one meter or so of the current pose estimate. + * + * @param visionRobotPose The pose of the robot as measured by the vision + * camera. + * @param timestamp The timestamp of the vision measurement in seconds. Note + * that if you don't use your own time source by calling UpdateWithTime(), + * then you must use a timestamp with an epoch since FPGA startup (i.e., + * the epoch of this timestamp is the same epoch as + * frc::Timer::GetFPGATimestamp(). This means that you should use + * frc::Timer::GetFPGATimestamp() as your time source in this case. + */ + void AddVisionMeasurement(const Pose2d& visionRobotPose, + units::second_t timestamp); + + /** + * Adds a vision measurement to the Kalman Filter. This will correct + * the odometry pose estimate while still accounting for measurement noise. + * + * This method can be called as infrequently as you want, as long as you are + * calling Update() every loop. + * + * To promote stability of the pose estimate and make it robust to bad vision + * data, we recommend only adding vision measurements that are already within + * one meter or so of the current pose estimate. + * + * Note that the vision measurement standard deviations passed into this + * method will continue to apply to future measurements until a subsequent + * call to SetVisionMeasurementStdDevs() or this method. + * + * @param visionRobotPose The pose of the robot as measured by the vision + * camera. + * @param timestamp The timestamp of the vision measurement in seconds. Note + * that if you don't use your own time source by calling UpdateWithTime(), + * then you must use a timestamp with an epoch since FPGA startup (i.e., + * the epoch of this timestamp is the same epoch as + * frc::Timer::GetFPGATimestamp(). This means that you should use + * frc::Timer::GetFPGATimestamp() as your time source in this case. + * @param visionMeasurementStdDevs Standard deviations of the vision pose + * measurement (x position in meters, y position in meters, and heading in + * radians). Increase these numbers to trust the vision pose measurement + * less. + */ + void AddVisionMeasurement( + const Pose2d& visionRobotPose, units::second_t timestamp, + const wpi::array& visionMeasurementStdDevs) { + SetVisionMeasurementStdDevs(visionMeasurementStdDevs); + AddVisionMeasurement(visionRobotPose, timestamp); + } + + /** + * Updates the pose estimator with wheel encoder and gyro information. This + * should be called every loop. + * + * @param gyroAngle The current gyro angle. + * @param wheelPositions The distances traveled by the encoders. + * + * @return The estimated pose of the robot in meters. + */ + Pose2d Update(const Rotation2d& gyroAngle, + const WheelPositions& wheelPositions); + + /** + * Updates the pose estimator with wheel encoder and gyro information. This + * should be called every loop. + * + * @param currentTime The time at which this method was called. + * @param gyroAngle The current gyro angle. + * @param wheelPositions The distances traveled by the encoders. + * + * @return The estimated pose of the robot in meters. + */ + Pose2d UpdateWithTime(units::second_t currentTime, + const Rotation2d& gyroAngle, + const WheelPositions& wheelPositions); + + private: + struct InterpolationRecord { + // The pose observed given the current sensor inputs and the previous pose. + Pose2d pose; + + // The current gyroscope angle. + Rotation2d gyroAngle; + + // The distances traveled by the wheels. + WheelPositions wheelPositions; + + /** + * Checks equality between this InterpolationRecord and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const InterpolationRecord& other) const = default; + + /** + * Checks inequality between this InterpolationRecord and another object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const InterpolationRecord& other) const = default; + + /** + * Interpolates between two InterpolationRecords. + * + * @param endValue The end value for the interpolation. + * @param i The interpolant (fraction). + * + * @return The interpolated state. + */ + InterpolationRecord Interpolate( + Kinematics& kinematics, + InterpolationRecord endValue, double i) const; + }; + + static constexpr units::second_t kBufferDuration = 1.5_s; + + Kinematics& m_kinematics; + Odometry& m_odometry; + wpi::array m_q{wpi::empty_array}; + Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero(); + + TimeInterpolatableBuffer m_poseBuffer{ + kBufferDuration, [this](const InterpolationRecord& start, + const InterpolationRecord& end, double t) { + return start.Interpolate(this->m_kinematics, end, t); + }}; +}; +} // namespace frc + +#include "frc/estimator/PoseEstimator.inc" diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc new file mode 100644 index 0000000000..9ca019c936 --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.inc @@ -0,0 +1,170 @@ +// 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 "frc/estimator/PoseEstimator.h" + +namespace frc { + +template +PoseEstimator::PoseEstimator( + Kinematics& kinematics, + Odometry& odometry, + const wpi::array& stateStdDevs, + const wpi::array& visionMeasurementStdDevs) + : m_kinematics(kinematics), m_odometry(odometry) { + for (size_t i = 0; i < 3; ++i) { + m_q[i] = stateStdDevs[i] * stateStdDevs[i]; + } + + SetVisionMeasurementStdDevs(visionMeasurementStdDevs); +} + +template +void PoseEstimator::SetVisionMeasurementStdDevs( + const wpi::array& visionMeasurementStdDevs) { + wpi::array r{wpi::empty_array}; + for (size_t i = 0; i < 3; ++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 < 3; ++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])); + } + } +} + +template +void PoseEstimator::ResetPosition( + const Rotation2d& gyroAngle, const WheelPositions& wheelPositions, + const Pose2d& pose) { + // Reset state estimate and error covariance + m_odometry.ResetPosition(gyroAngle, wheelPositions, pose); + m_poseBuffer.Clear(); +} + +template +Pose2d PoseEstimator::GetEstimatedPosition() + const { + return m_odometry.GetPose(); +} + +template +void PoseEstimator::AddVisionMeasurement( + const Pose2d& visionRobotPose, units::second_t timestamp) { + // Step 0: If this measurement is old enough to be outside the pose buffer's + // timespan, skip. + if (!m_poseBuffer.GetInternalBuffer().empty() && + m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration > + timestamp) { + return; + } + + // Step 1: Get the estimated pose from when the vision measurement was made. + auto sample = m_poseBuffer.Sample(timestamp); + + if (!sample.has_value()) { + return; + } + + // Step 2: Measure the twist between the odometry pose and the vision pose + auto twist = sample.value().pose.Log(visionRobotPose); + + // Step 3: 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. + Vectord<3> k_times_twist = + m_visionK * + Vectord<3>{twist.dx.value(), twist.dy.value(), twist.dtheta.value()}; + + // Step 4: Convert back to Twist2d + Twist2d scaledTwist{units::meter_t{k_times_twist(0)}, + units::meter_t{k_times_twist(1)}, + units::radian_t{k_times_twist(2)}}; + + // Step 5: Reset Odometry to state at sample with vision adjustment. + m_odometry.ResetPosition(sample.value().gyroAngle, + sample.value().wheelPositions, + sample.value().pose.Exp(scaledTwist)); + + // Step 6: Record the current pose to allow multiple measurements from the + // same timestamp + m_poseBuffer.AddSample(timestamp, + {GetEstimatedPosition(), sample.value().gyroAngle, + sample.value().wheelPositions}); + + // Step 7: Replay odometry inputs between sample time and latest recorded + // sample to update the pose buffer and correct odometry. + auto internal_buf = m_poseBuffer.GetInternalBuffer(); + + auto upper_bound = + std::lower_bound(internal_buf.begin(), internal_buf.end(), timestamp, + [](const auto& pair, auto t) { return t > pair.first; }); + + for (auto entry = upper_bound; entry != internal_buf.end(); entry++) { + UpdateWithTime(entry->first, entry->second.gyroAngle, + entry->second.wheelPositions); + } +} + +template +Pose2d PoseEstimator::Update( + const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) { + return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle, + wheelPositions); +} + +template +Pose2d PoseEstimator::UpdateWithTime( + units::second_t currentTime, const Rotation2d& gyroAngle, + const WheelPositions& wheelPositions) { + m_odometry.Update(gyroAngle, wheelPositions); + + // Copy? + WheelPositions internalWheelPositions = wheelPositions; + + m_poseBuffer.AddSample( + currentTime, {GetEstimatedPosition(), gyroAngle, internalWheelPositions}); + + return GetEstimatedPosition(); +} + +template +typename PoseEstimator::InterpolationRecord +PoseEstimator::InterpolationRecord::Interpolate( + Kinematics& kinematics, + InterpolationRecord endValue, double i) const { + if (i < 0) { + return *this; + } else if (i > 1) { + return endValue; + } else { + // Find the new wheel distance measurements. + WheelPositions wheels_lerp = + this->wheelPositions.Interpolate(endValue.wheelPositions, i); + + // Find the distance between this measurement and the + // interpolated measurement. + WheelPositions wheels_delta = wheels_lerp - this->wheelPositions; + + // Find the new gyro angle. + auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i); + + // Create a twist to represent this changed based on the interpolated + // sensor inputs. + auto twist = kinematics.ToTwist2d(wheels_delta); + twist.dtheta = (gyro - gyroAngle).Radians(); + + return {pose.Exp(twist), gyro, wheels_lerp}; + } +} + +} // 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 ba29a2b63d..43831d7aff 100644 --- a/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h @@ -6,19 +6,16 @@ #include -#include #include #include -#include -#include "frc/EigenCore.h" +#include "frc/estimator/PoseEstimator.h" #include "frc/geometry/Pose2d.h" #include "frc/geometry/Rotation2d.h" -#include "frc/interpolation/TimeInterpolatableBuffer.h" #include "frc/kinematics/SwerveDriveKinematics.h" #include "frc/kinematics/SwerveDriveOdometry.h" +#include "frc/kinematics/SwerveDriveWheelPositions.h" #include "units/time.h" -#include "wpimath/MathShared.h" namespace frc { @@ -34,7 +31,9 @@ namespace frc { * odometry. */ template -class SwerveDrivePoseEstimator { +class SwerveDrivePoseEstimator + : public PoseEstimator, + SwerveDriveWheelPositions> { public: /** * Constructs a SwerveDrivePoseEstimator with default standard deviations @@ -84,14 +83,10 @@ class SwerveDrivePoseEstimator { const wpi::array& modulePositions, const Pose2d& initialPose, const wpi::array& stateStdDevs, const wpi::array& visionMeasurementStdDevs) - : m_kinematics{kinematics}, - m_odometry{kinematics, gyroAngle, modulePositions, initialPose} { - for (size_t i = 0; i < 3; ++i) { - m_q[i] = stateStdDevs[i] * stateStdDevs[i]; - } - - SetVisionMeasurementStdDevs(visionMeasurementStdDevs); - } + : PoseEstimator, + SwerveDriveWheelPositions>( + kinematics, m_odometryImpl, stateStdDevs, visionMeasurementStdDevs), + m_odometryImpl{kinematics, gyroAngle, modulePositions, initialPose} {} /** * Resets the robot's position on the field. @@ -108,157 +103,11 @@ class SwerveDrivePoseEstimator { const Rotation2d& gyroAngle, const wpi::array& modulePositions, const Pose2d& pose) { - // Reset state estimate and error covariance - m_odometry.ResetPosition(gyroAngle, modulePositions, pose); - m_poseBuffer.Clear(); - } - - /** - * Gets the estimated robot pose. - * - * @return The estimated robot pose in meters. - */ - Pose2d GetEstimatedPosition() const { return m_odometry.GetPose(); } - - /** - * 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 < 3; ++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 < 3; ++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])); - } - } - } - - /** - * 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 or sync the epochs. - */ - void AddVisionMeasurement(const Pose2d& visionRobotPose, - units::second_t timestamp) { - // Step 0: If this measurement is old enough to be outside the pose buffer's - // timespan, skip. - if (!m_poseBuffer.GetInternalBuffer().empty() && - m_poseBuffer.GetInternalBuffer().front().first - kBufferDuration > - timestamp) { - return; - } - - // Step 1: Get the estimated pose from when the vision measurement was made. - auto sample = m_poseBuffer.Sample(timestamp); - - if (!sample.has_value()) { - return; - } - - // Step 2: Measure the twist between the odometry pose and the vision pose - auto twist = sample.value().pose.Log(visionRobotPose); - - // Step 3: 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<3> k_times_twist = - m_visionK * frc::Vectord<3>{twist.dx.value(), twist.dy.value(), - twist.dtheta.value()}; - - // Step 4: Convert back to Twist2d - Twist2d scaledTwist{units::meter_t{k_times_twist(0)}, - units::meter_t{k_times_twist(1)}, - units::radian_t{k_times_twist(2)}}; - - // Step 5: Reset Odometry to state at sample with vision adjustment. - m_odometry.ResetPosition(sample.value().gyroAngle, - sample.value().modulePositions, - sample.value().pose.Exp(scaledTwist)); - - // Step 6: Record the current pose to allow multiple measurements from the - // same timestamp - m_poseBuffer.AddSample(timestamp, - {GetEstimatedPosition(), sample.value().gyroAngle, - sample.value().modulePositions}); - - // Step 7: Replay odometry inputs between sample time and latest recorded - // sample to update the pose buffer and correct odometry. - auto internal_buf = m_poseBuffer.GetInternalBuffer(); - - auto upper_bound = std::lower_bound( - internal_buf.begin(), internal_buf.end(), timestamp, - [](const auto& pair, auto t) { return t > pair.first; }); - - for (auto entry = upper_bound; entry != internal_buf.end(); entry++) { - UpdateWithTime(entry->first, entry->second.gyroAngle, - entry->second.modulePositions); - } - } - - /** - * Adds a vision measurement to the Kalman Filter. This will correct the - * odometry pose estimate while still accounting for measurement noise. - * - * This method can be called as infrequently as you want, as long as you are - * calling Update() every loop. - * - * To promote stability of the pose estimate and make it robust to bad vision - * data, we recommend only adding vision measurements that are already within - * one meter or so of the current pose estimate. - * - * Note that the vision measurement standard deviations passed into this - * method will continue to apply to future measurements until a subsequent - * call to SetVisionMeasurementStdDevs() or this method. - * - * @param visionRobotPose The pose of the robot as measured by the vision - * camera. - * @param timestamp The timestamp of the vision measurement in seconds. Note - * that if you don't use your own time source by calling UpdateWithTime(), - * then you must use a timestamp with an epoch since FPGA startup (i.e., - * the epoch of this timestamp is the same epoch as - * frc::Timer::GetFPGATimestamp(). This means that you should use - * frc::Timer::GetFPGATimestamp() as your time source in this case. - * @param visionMeasurementStdDevs Standard deviations of the vision pose - * measurement (x position in meters, y position in meters, and heading in - * radians). Increase these numbers to trust the vision pose measurement - * less. - */ - void AddVisionMeasurement( - const Pose2d& visionRobotPose, units::second_t timestamp, - const wpi::array& visionMeasurementStdDevs) { - SetVisionMeasurementStdDevs(visionMeasurementStdDevs); - AddVisionMeasurement(visionRobotPose, timestamp); + PoseEstimator< + SwerveDriveWheelSpeeds, + SwerveDriveWheelPositions>::ResetPosition(gyroAngle, + {modulePositions}, + pose); } /** @@ -273,8 +122,10 @@ class SwerveDrivePoseEstimator { Pose2d Update( const Rotation2d& gyroAngle, const wpi::array& modulePositions) { - return UpdateWithTime(wpi::math::MathSharedStore::GetTimestamp(), gyroAngle, - modulePositions); + return PoseEstimator< + SwerveDriveWheelSpeeds, + SwerveDriveWheelPositions>::Update(gyroAngle, + {modulePositions}); } /** @@ -290,111 +141,13 @@ class SwerveDrivePoseEstimator { Pose2d UpdateWithTime( units::second_t currentTime, const Rotation2d& gyroAngle, const wpi::array& modulePositions) { - m_odometry.Update(gyroAngle, modulePositions); - - wpi::array internalModulePositions{ - wpi::empty_array}; - - for (size_t i = 0; i < NumModules; i++) { - internalModulePositions[i].distance = modulePositions[i].distance; - internalModulePositions[i].angle = modulePositions[i].angle; - } - - m_poseBuffer.AddSample(currentTime, {GetEstimatedPosition(), gyroAngle, - internalModulePositions}); - - return GetEstimatedPosition(); + return PoseEstimator, + SwerveDriveWheelPositions>:: + UpdateWithTime(currentTime, gyroAngle, {modulePositions}); } private: - struct InterpolationRecord { - // The pose observed given the current sensor inputs and the previous pose. - Pose2d pose; - - // The current gyroscope angle. - Rotation2d gyroAngle; - - // The distances traveled and rotations measured at each module. - wpi::array modulePositions; - - /** - * Checks equality between this InterpolationRecord and another object. - * - * @param other The other object. - * @return Whether the two objects are equal. - */ - bool operator==(const InterpolationRecord& other) const = default; - - /** - * Checks inequality between this InterpolationRecord and another object. - * - * @param other The other object. - * @return Whether the two objects are not equal. - */ - bool operator!=(const InterpolationRecord& other) const = default; - - /** - * Interpolates between two InterpolationRecords. - * - * @param endValue The end value for the interpolation. - * @param i The interpolant (fraction). - * - * @return The interpolated state. - */ - InterpolationRecord Interpolate( - SwerveDriveKinematics& kinematics, - InterpolationRecord endValue, double i) const { - if (i < 0) { - return *this; - } else if (i > 1) { - return endValue; - } else { - // Find the new module distances. - wpi::array modulePositions{ - wpi::empty_array}; - // Find the distance between this measurement and the - // interpolated measurement. - wpi::array modulesDelta{ - wpi::empty_array}; - - for (size_t i = 0; i < NumModules; i++) { - modulePositions[i].distance = - wpi::Lerp(this->modulePositions[i].distance, - endValue.modulePositions[i].distance, i); - modulePositions[i].angle = - wpi::Lerp(this->modulePositions[i].angle, - endValue.modulePositions[i].angle, i); - - modulesDelta[i].distance = - modulePositions[i].distance - this->modulePositions[i].distance; - modulesDelta[i].angle = modulePositions[i].angle; - } - - // Find the new gyro angle. - auto gyro = wpi::Lerp(this->gyroAngle, endValue.gyroAngle, i); - - // Create a twist to represent this changed based on the interpolated - // sensor inputs. - auto twist = kinematics.ToTwist2d(modulesDelta); - twist.dtheta = (gyro - gyroAngle).Radians(); - - return {pose.Exp(twist), gyro, modulePositions}; - } - } - }; - - static constexpr units::second_t kBufferDuration = 1.5_s; - - SwerveDriveKinematics& m_kinematics; - SwerveDriveOdometry m_odometry; - wpi::array m_q{wpi::empty_array}; - Eigen::Matrix3d m_visionK = Eigen::Matrix3d::Zero(); - - TimeInterpolatableBuffer m_poseBuffer{ - kBufferDuration, [this](const InterpolationRecord& start, - const InterpolationRecord& end, double t) { - return start.Interpolate(this->m_kinematics, end, t); - }}; + SwerveDriveOdometry m_odometryImpl; }; extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT) diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h index 930c7a6a02..ec6f7071f1 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h @@ -8,7 +8,9 @@ #include "frc/geometry/Twist2d.h" #include "frc/kinematics/ChassisSpeeds.h" +#include "frc/kinematics/DifferentialDriveWheelPositions.h" #include "frc/kinematics/DifferentialDriveWheelSpeeds.h" +#include "frc/kinematics/Kinematics.h" #include "units/angle.h" #include "units/length.h" #include "wpimath/MathShared.h" @@ -22,7 +24,9 @@ namespace frc { * velocity components whereas forward kinematics converts left and right * component velocities into a linear and angular chassis speed. */ -class WPILIB_DLLEXPORT DifferentialDriveKinematics { +class WPILIB_DLLEXPORT DifferentialDriveKinematics + : public Kinematics { public: /** * Constructs a differential drive kinematics object. @@ -46,7 +50,7 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics { * @return The chassis speed. */ constexpr ChassisSpeeds ToChassisSpeeds( - const DifferentialDriveWheelSpeeds& wheelSpeeds) const { + const DifferentialDriveWheelSpeeds& wheelSpeeds) const override { return {(wheelSpeeds.left + wheelSpeeds.right) / 2.0, 0_mps, (wheelSpeeds.right - wheelSpeeds.left) / trackWidth * 1_rad}; } @@ -60,7 +64,7 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics { * @return The left and right velocities. */ constexpr DifferentialDriveWheelSpeeds ToWheelSpeeds( - const ChassisSpeeds& chassisSpeeds) const { + const ChassisSpeeds& chassisSpeeds) const override { return {chassisSpeeds.vx - trackWidth / 2 * chassisSpeeds.omega / 1_rad, chassisSpeeds.vx + trackWidth / 2 * chassisSpeeds.omega / 1_rad}; } @@ -79,6 +83,11 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics { (rightDistance - leftDistance) / trackWidth * 1_rad}; } + Twist2d ToTwist2d( + const DifferentialDriveWheelPositions& wheelDeltas) const override { + return ToTwist2d(wheelDeltas.left, wheelDeltas.right); + } + units::meter_t trackWidth; }; } // 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 cc198ac470..279c1dcbf2 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h @@ -7,6 +7,10 @@ #include #include "frc/geometry/Pose2d.h" +#include "frc/kinematics/DifferentialDriveKinematics.h" +#include "frc/kinematics/DifferentialDriveWheelPositions.h" +#include "frc/kinematics/DifferentialDriveWheelSpeeds.h" +#include "frc/kinematics/Odometry.h" #include "units/length.h" namespace frc { @@ -22,7 +26,9 @@ namespace frc { * 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 DifferentialDriveOdometry { +class WPILIB_DLLEXPORT DifferentialDriveOdometry + : public Odometry { public: /** * Constructs a DifferentialDriveOdometry object. @@ -56,20 +62,13 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry { */ void ResetPosition(const Rotation2d& gyroAngle, units::meter_t leftDistance, units::meter_t rightDistance, const Pose2d& pose) { - m_pose = pose; - m_previousAngle = pose.Rotation(); - m_gyroOffset = m_pose.Rotation() - gyroAngle; - - m_prevLeftDistance = leftDistance; - m_prevRightDistance = rightDistance; + Odometry::ResetPosition(gyroAngle, + {leftDistance, + rightDistance}, + pose); } - /** - * Returns the position of the robot on the field. - * @return The pose of the robot. - */ - const Pose2d& GetPose() const { return m_pose; } - /** * Updates the robot position on the field using distance measurements from * encoders. This method is more numerically accurate than using velocities to @@ -82,15 +81,14 @@ class WPILIB_DLLEXPORT DifferentialDriveOdometry { * @return The new pose of the robot. */ const Pose2d& Update(const Rotation2d& gyroAngle, units::meter_t leftDistance, - units::meter_t rightDistance); + units::meter_t rightDistance) { + return Odometry::Update(gyroAngle, + {leftDistance, + rightDistance}); + } private: - Pose2d m_pose; - - Rotation2d m_gyroOffset; - Rotation2d m_previousAngle; - - units::meter_t m_prevLeftDistance = 0_m; - units::meter_t m_prevRightDistance = 0_m; + DifferentialDriveKinematics m_kinematicsImpl{units::meter_t{1}}; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h new file mode 100644 index 0000000000..d6a1dc2d8b --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelPositions.h @@ -0,0 +1,56 @@ +// 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 "units/length.h" + +namespace frc { +/** + * Represents the wheel positions for a differential drive drivetrain. + */ +struct WPILIB_DLLEXPORT DifferentialDriveWheelPositions { + /** + * Distance driven by the left side. + */ + units::meter_t left = 0_m; + + /** + * Distance driven by the right side. + */ + units::meter_t right = 0_m; + + /** + * Checks equality between this DifferentialDriveWheelPositions and another + * object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const DifferentialDriveWheelPositions& other) const = default; + + /** + * Checks inequality between this DifferentialDriveWheelPositions and another + * object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const DifferentialDriveWheelPositions& other) const = default; + + DifferentialDriveWheelPositions operator-( + const DifferentialDriveWheelPositions& other) const { + return {left - other.left, right - other.right}; + } + + DifferentialDriveWheelPositions Interpolate( + const DifferentialDriveWheelPositions& endValue, double t) const { + return {wpi::Lerp(left, endValue.left, t), + wpi::Lerp(right, endValue.right, t)}; + } +}; +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/Kinematics.h b/wpimath/src/main/native/include/frc/kinematics/Kinematics.h new file mode 100644 index 0000000000..4ee328d2d5 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/Kinematics.h @@ -0,0 +1,60 @@ +// 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/Twist2d.h" +#include "frc/kinematics/ChassisSpeeds.h" + +namespace frc { +/** + * Helper class that converts a chassis velocity (dx, dy, and dtheta components) + * into individual wheel speeds. Robot code should not use this directly- + * Instead, use the particular type for your drivetrain (e.g., + * DifferentialDriveKinematics). + * + * Inverse kinematics converts a desired chassis speed into wheel speeds whereas + * forward kinematics converts wheel speeds into chassis speed. + */ +template +class WPILIB_DLLEXPORT Kinematics { + public: + /** + * Performs forward kinematics to return the resulting chassis speed from the + * wheel speeds. This method is often used for odometry -- determining the + * robot's position on the field using data from the real-world speed of each + * wheel on the robot. + * + * @param wheelSpeeds The speeds of the wheels. + * @return The chassis speed. + */ + virtual ChassisSpeeds ToChassisSpeeds( + const WheelSpeeds& wheelSpeeds) const = 0; + + /** + * Performs inverse kinematics to return the wheel speeds from a desired + * chassis velocity. This method is often used to convert joystick values into + * wheel speeds. + * + * @param chassisSpeeds The desired chassis speed. + * @return The wheel speeds. + */ + virtual WheelSpeeds ToWheelSpeeds( + const ChassisSpeeds& chassisSpeeds) const = 0; + + /** + * Performs forward kinematics to return the resulting Twist2d from the given + * wheel deltas. This method is often used for odometry -- determining the + * robot's position on the field using changes in the distance driven by each + * wheel on the robot. + * + * @param wheelDeltas The distances driven by each wheel. + * + * @return The resulting Twist2d in the robot's movement. + */ + virtual Twist2d ToTwist2d(const WheelPositions& wheelDeltas) const = 0; +}; +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h index 2880cef9e6..5d67ade537 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h @@ -11,6 +11,7 @@ #include "frc/geometry/Translation2d.h" #include "frc/geometry/Twist2d.h" #include "frc/kinematics/ChassisSpeeds.h" +#include "frc/kinematics/Kinematics.h" #include "frc/kinematics/MecanumDriveWheelPositions.h" #include "frc/kinematics/MecanumDriveWheelSpeeds.h" #include "wpimath/MathShared.h" @@ -39,7 +40,8 @@ namespace frc { * Forward kinematics is also used for odometry -- determining the position of * the robot on the field using encoders and a gyro. */ -class WPILIB_DLLEXPORT MecanumDriveKinematics { +class WPILIB_DLLEXPORT MecanumDriveKinematics + : public Kinematics { public: /** * Constructs a mecanum drive kinematics object. @@ -101,7 +103,12 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics { */ MecanumDriveWheelSpeeds ToWheelSpeeds( const ChassisSpeeds& chassisSpeeds, - const Translation2d& centerOfRotation = Translation2d{}) const; + const Translation2d& centerOfRotation) const; + + MecanumDriveWheelSpeeds ToWheelSpeeds( + const ChassisSpeeds& chassisSpeeds) const override { + return ToWheelSpeeds(chassisSpeeds, {}); + } /** * Performs forward kinematics to return the resulting chassis state from the @@ -114,7 +121,7 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics { * @return The resulting chassis speed. */ ChassisSpeeds ToChassisSpeeds( - const MecanumDriveWheelSpeeds& wheelSpeeds) const; + const MecanumDriveWheelSpeeds& wheelSpeeds) const override; /** * Performs forward kinematics to return the resulting Twist2d from the @@ -126,7 +133,8 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics { * * @return The resulting chassis speed. */ - Twist2d ToTwist2d(const MecanumDriveWheelPositions& wheelDeltas) const; + Twist2d ToTwist2d( + const MecanumDriveWheelPositions& wheelDeltas) const override; private: mutable Matrixd<4, 3> m_inverseKinematics; diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h index 5e949caa64..0a6f537cd7 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h @@ -9,7 +9,9 @@ #include "frc/geometry/Pose2d.h" #include "frc/kinematics/MecanumDriveKinematics.h" +#include "frc/kinematics/MecanumDriveWheelPositions.h" #include "frc/kinematics/MecanumDriveWheelSpeeds.h" +#include "frc/kinematics/Odometry.h" #include "units/time.h" namespace frc { @@ -23,7 +25,8 @@ namespace frc { * path following. Furthermore, odometry can be used for latency compensation * when using computer-vision systems. */ -class WPILIB_DLLEXPORT MecanumDriveOdometry { +class WPILIB_DLLEXPORT MecanumDriveOdometry + : public Odometry { public: /** * Constructs a MecanumDriveOdometry object. @@ -38,52 +41,8 @@ class WPILIB_DLLEXPORT MecanumDriveOdometry { const MecanumDriveWheelPositions& wheelPositions, const Pose2d& initialPose = Pose2d{}); - /** - * Resets the robot's position on the field. - * - * The gyroscope angle does not need to be reset here on the user's robot - * code. The library automatically takes care of offsetting the gyro angle. - * - * @param gyroAngle The angle reported by the gyroscope. - * @param wheelPositions The current distances measured by each wheel. - * @param pose The position on the field that your robot is at. - */ - void ResetPosition(const Rotation2d& gyroAngle, - const MecanumDriveWheelPositions& wheelPositions, - const Pose2d& pose) { - m_pose = pose; - m_previousAngle = pose.Rotation(); - m_gyroOffset = m_pose.Rotation() - gyroAngle; - m_previousWheelPositions = wheelPositions; - } - - /** - * Returns the position of the robot on the field. - * @return The pose of the robot. - */ - const Pose2d& 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 Pose2d& Update(const Rotation2d& gyroAngle, - const MecanumDriveWheelPositions& wheelPositions); - private: - MecanumDriveKinematics m_kinematics; - Pose2d m_pose; - - MecanumDriveWheelPositions m_previousWheelPositions; - Rotation2d m_previousAngle; - Rotation2d m_gyroOffset; + MecanumDriveKinematics m_kinematicsImpl; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h index b69acebcd0..6c580a730f 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelPositions.h @@ -4,13 +4,14 @@ #pragma once +#include #include #include "units/length.h" namespace frc { /** - * Represents the wheel speeds for a mecanum drive drivetrain. + * Represents the wheel positions for a mecanum drive drivetrain. */ struct WPILIB_DLLEXPORT MecanumDriveWheelPositions { /** @@ -49,5 +50,19 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelPositions { * @return Whether the two objects are not equal. */ bool operator!=(const MecanumDriveWheelPositions& other) const = default; + + MecanumDriveWheelPositions operator-( + const MecanumDriveWheelPositions& other) const { + return {frontLeft - other.frontLeft, frontRight - other.frontRight, + rearLeft - other.rearLeft, rearRight - other.rearRight}; + } + + MecanumDriveWheelPositions Interpolate( + const MecanumDriveWheelPositions& endValue, double t) const { + return {wpi::Lerp(frontLeft, endValue.frontLeft, t), + wpi::Lerp(frontRight, endValue.frontRight, t), + wpi::Lerp(rearLeft, endValue.rearLeft, t), + wpi::Lerp(rearRight, endValue.rearRight, t)}; + } }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry.h b/wpimath/src/main/native/include/frc/kinematics/Odometry.h new file mode 100644 index 0000000000..55c074c02f --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/Odometry.h @@ -0,0 +1,88 @@ +// 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/Kinematics.h" +#include "frc/kinematics/WheelPositions.h" + +namespace frc { +/** + * Class for odometry. Robot code should not use this directly- Instead, use the + * particular type for your drivetrain (e.g., DifferentialDriveOdometry). + * 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. + */ +template +class WPILIB_DLLEXPORT Odometry { + public: + /** + * Constructs an Odometry 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 Odometry(const Kinematics& kinematics, + const Rotation2d& gyroAngle, + const WheelPositions& wheelPositions, + const Pose2d& initialPose = Pose2d{}); + + /** + * Resets the robot's position on the field. + * + * The gyroscope angle does not need to be reset here on the user's robot + * code. The library automatically takes care of offsetting the gyro angle. + * + * @param gyroAngle The angle reported by the gyroscope. + * @param wheelPositions The current distances measured by each wheel. + * @param pose The position on the field that your robot is at. + */ + void ResetPosition(const Rotation2d& gyroAngle, + const WheelPositions& wheelPositions, const Pose2d& pose) { + m_pose = pose; + m_previousAngle = pose.Rotation(); + m_gyroOffset = m_pose.Rotation() - gyroAngle; + m_previousWheelPositions = wheelPositions; + } + + /** + * Returns the position of the robot on the field. + * @return The pose of the robot. + */ + const Pose2d& 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 Pose2d& Update(const Rotation2d& gyroAngle, + const WheelPositions& wheelPositions); + + private: + const Kinematics& m_kinematics; + Pose2d m_pose; + + WheelPositions m_previousWheelPositions; + Rotation2d m_previousAngle; + Rotation2d m_gyroOffset; +}; +} // namespace frc + +#include "Odometry.inc" diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry.inc b/wpimath/src/main/native/include/frc/kinematics/Odometry.inc new file mode 100644 index 0000000000..3c6d705901 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/Odometry.inc @@ -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. + +#pragma once + +#include "frc/kinematics/Odometry.h" + +namespace frc { +template +Odometry::Odometry( + const Kinematics& kinematics, + const Rotation2d& gyroAngle, const WheelPositions& wheelPositions, + const Pose2d& initialPose) + : m_kinematics(kinematics), + m_pose(initialPose), + m_previousWheelPositions(wheelPositions) { + m_previousAngle = m_pose.Rotation(); + m_gyroOffset = m_pose.Rotation() - gyroAngle; +} + +template +const Pose2d& Odometry::Update( + const Rotation2d& gyroAngle, const WheelPositions& wheelPositions) { + auto angle = gyroAngle + m_gyroOffset; + + auto wheelDeltas = wheelPositions - m_previousWheelPositions; + + auto twist = m_kinematics.ToTwist2d(wheelDeltas); + twist.dtheta = (angle - m_previousAngle).Radians(); + + auto newPose = m_pose.Exp(twist); + + m_previousAngle = angle; + m_previousWheelPositions = wheelPositions; + m_pose = {newPose.Translation(), angle}; + + return m_pose; +} +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h index 4e472075a8..83edb4c6ee 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h @@ -16,12 +16,18 @@ #include "frc/geometry/Translation2d.h" #include "frc/geometry/Twist2d.h" #include "frc/kinematics/ChassisSpeeds.h" +#include "frc/kinematics/Kinematics.h" +#include "frc/kinematics/SwerveDriveWheelPositions.h" #include "frc/kinematics/SwerveModulePosition.h" #include "frc/kinematics/SwerveModuleState.h" #include "units/velocity.h" #include "wpimath/MathShared.h" namespace frc { + +template +using SwerveDriveWheelSpeeds = wpi::array; + /** * Helper class that converts a chassis velocity (dx, dy, and dtheta components) * into individual module states (speed and angle). @@ -45,7 +51,9 @@ namespace frc { * the robot on the field using encoders and a gyro. */ template -class SwerveDriveKinematics { +class SwerveDriveKinematics + : public Kinematics, + SwerveDriveWheelPositions> { public: /** * Constructs a swerve drive kinematics object. This takes in a variable @@ -130,6 +138,11 @@ class SwerveDriveKinematics { const ChassisSpeeds& chassisSpeeds, const Translation2d& centerOfRotation = Translation2d{}) const; + SwerveDriveWheelSpeeds ToWheelSpeeds( + const ChassisSpeeds& chassisSpeeds) const override { + return ToSwerveModuleStates(chassisSpeeds); + } + /** * Performs forward kinematics to return the resulting chassis state from the * given module states. This method is often used for odometry -- determining @@ -162,8 +175,8 @@ class SwerveDriveKinematics { * * @return The resulting chassis speed. */ - ChassisSpeeds ToChassisSpeeds( - const wpi::array& moduleStates) const; + ChassisSpeeds ToChassisSpeeds(const wpi::array& + moduleStates) const override; /** * Performs forward kinematics to return the resulting Twist2d from the @@ -201,6 +214,11 @@ class SwerveDriveKinematics { Twist2d ToTwist2d( wpi::array moduleDeltas) const; + Twist2d ToTwist2d( + const SwerveDriveWheelPositions& wheelDeltas) const override { + return ToTwist2d(wheelDeltas.positions); + } + /** * Renormalizes the wheel speeds if any individual speed is above the * specified maximum. diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h index 015c2c0038..2e0e553cbd 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h @@ -11,8 +11,11 @@ #include #include +#include "Odometry.h" #include "SwerveDriveKinematics.h" +#include "SwerveDriveWheelPositions.h" #include "SwerveModulePosition.h" +#include "SwerveModuleState.h" #include "frc/geometry/Pose2d.h" #include "units/time.h" @@ -28,7 +31,9 @@ namespace frc { * when using computer-vision systems. */ template -class SwerveDriveOdometry { +class SwerveDriveOdometry + : public Odometry, + SwerveDriveWheelPositions> { public: /** * Constructs a SwerveDriveOdometry object. @@ -56,13 +61,13 @@ class SwerveDriveOdometry { void ResetPosition( const Rotation2d& gyroAngle, const wpi::array& modulePositions, - const Pose2d& pose); - - /** - * Returns the position of the robot on the field. - * @return The pose of the robot. - */ - const Pose2d& GetPose() const { return m_pose; } + const Pose2d& pose) { + Odometry< + SwerveDriveWheelSpeeds, + SwerveDriveWheelPositions>::ResetPosition(gyroAngle, + {modulePositions}, + pose); + } /** * Updates the robot's position on the field using forward kinematics and @@ -79,17 +84,15 @@ class SwerveDriveOdometry { */ const Pose2d& Update( const Rotation2d& gyroAngle, - const wpi::array& modulePositions); + const wpi::array& modulePositions) { + return Odometry< + SwerveDriveWheelSpeeds, + SwerveDriveWheelPositions>::Update(gyroAngle, + {modulePositions}); + } private: - SwerveDriveKinematics m_kinematics; - Pose2d m_pose; - - Rotation2d m_previousAngle; - Rotation2d m_gyroOffset; - - wpi::array m_previousModulePositions{ - wpi::empty_array}; + SwerveDriveKinematics m_kinematicsImpl; }; extern template class EXPORT_TEMPLATE_DECLARE(WPILIB_DLLEXPORT) diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc index 64b46c1ff7..48ddfec233 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc @@ -13,58 +13,11 @@ SwerveDriveOdometry::SwerveDriveOdometry( SwerveDriveKinematics kinematics, const Rotation2d& gyroAngle, const wpi::array& modulePositions, const Pose2d& initialPose) - : m_kinematics(kinematics), m_pose(initialPose) { - m_previousAngle = m_pose.Rotation(); - m_gyroOffset = m_pose.Rotation() - gyroAngle; - - for (size_t i = 0; i < NumModules; i++) { - m_previousModulePositions[i] = {modulePositions[i].distance, - modulePositions[i].angle}; - } - + : Odometry, + SwerveDriveWheelPositions>( + m_kinematicsImpl, gyroAngle, {modulePositions}, initialPose), + m_kinematicsImpl(kinematics) { wpi::math::MathSharedStore::ReportUsage( wpi::math::MathUsageId::kOdometry_SwerveDrive, 1); } - -template -void SwerveDriveOdometry::ResetPosition( - const Rotation2d& gyroAngle, - const wpi::array& modulePositions, - const Pose2d& pose) { - m_pose = pose; - m_previousAngle = pose.Rotation(); - m_gyroOffset = m_pose.Rotation() - gyroAngle; - - for (size_t i = 0; i < NumModules; i++) { - m_previousModulePositions[i].distance = modulePositions[i].distance; - } -} - -template -const Pose2d& frc::SwerveDriveOdometry::Update( - const Rotation2d& gyroAngle, - const wpi::array& modulePositions) { - auto moduleDeltas = - wpi::array(wpi::empty_array); - for (size_t index = 0; index < NumModules; index++) { - auto lastPosition = m_previousModulePositions[index]; - auto currentPosition = modulePositions[index]; - moduleDeltas[index] = {currentPosition.distance - lastPosition.distance, - currentPosition.angle}; - - m_previousModulePositions[index].distance = modulePositions[index].distance; - } - - auto angle = gyroAngle + m_gyroOffset; - - auto twist = m_kinematics.ToTwist2d(moduleDeltas); - twist.dtheta = (angle - m_previousAngle).Radians(); - - auto newPose = m_pose.Exp(twist); - - m_previousAngle = angle; - m_pose = {newPose.Translation(), angle}; - - return m_pose; -} } // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h new file mode 100644 index 0000000000..cdfa033f1b --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveWheelPositions.h @@ -0,0 +1,61 @@ +// 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/kinematics/SwerveModulePosition.h" + +namespace frc { +/** + * Represents the wheel positions for a swerve drive drivetrain. + */ +template +struct WPILIB_DLLEXPORT SwerveDriveWheelPositions { + /** + * The distances driven by the wheels. + */ + wpi::array positions; + + /** + * Checks equality between this SwerveDriveWheelPositions and another object. + * + * @param other The other object. + * @return Whether the two objects are equal. + */ + bool operator==(const SwerveDriveWheelPositions& other) const = default; + + /** + * Checks inequality between this SwerveDriveWheelPositions and another + * object. + * + * @param other The other object. + * @return Whether the two objects are not equal. + */ + bool operator!=(const SwerveDriveWheelPositions& other) const = default; + + SwerveDriveWheelPositions operator-( + const SwerveDriveWheelPositions& other) const { + auto result = + wpi::array(wpi::empty_array); + for (size_t i = 0; i < NumModules; i++) { + result[i] = positions[i] - other.positions[i]; + } + return {result}; + } + + SwerveDriveWheelPositions Interpolate( + const SwerveDriveWheelPositions& endValue, double t) const { + auto result = + wpi::array(wpi::empty_array); + for (size_t i = 0; i < NumModules; i++) { + result[i] = positions[i].Interpolate(endValue.positions[i], t); + } + return {result}; + } +}; +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h index 0c0e1f7b07..63f68256ee 100644 --- a/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h +++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModulePosition.h @@ -4,6 +4,7 @@ #pragma once +#include #include #include "frc/geometry/Rotation2d.h" @@ -33,5 +34,15 @@ struct WPILIB_DLLEXPORT SwerveModulePosition { * @return Whether the two objects are equal. */ bool operator==(const SwerveModulePosition& other) const; + + SwerveModulePosition operator-(const SwerveModulePosition& other) const { + return {distance - other.distance, angle}; + } + + SwerveModulePosition Interpolate(const SwerveModulePosition& endValue, + double t) const { + return {wpi::Lerp(distance, endValue.distance, t), + wpi::Lerp(angle, endValue.angle, t)}; + } }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/kinematics/WheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/WheelPositions.h new file mode 100644 index 0000000000..56ceea24d3 --- /dev/null +++ b/wpimath/src/main/native/include/frc/kinematics/WheelPositions.h @@ -0,0 +1,16 @@ +// 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 + +namespace frc { +template +concept WheelPositions = + std::copy_constructible && requires(T a, T b, double t) { + { a - b } -> std::convertible_to; + { a.Interpolate(b, t) } -> std::convertible_to; + }; +} // namespace frc