diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java index 0d3dd5f..1bc7fe6 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/Drive.java @@ -11,10 +11,10 @@ import edu.wpi.first.hal.FRCNetComm.tResourceType; import edu.wpi.first.hal.HAL; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; -import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.filter.SlewRateLimiter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -26,7 +26,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; @@ -73,8 +72,8 @@ public class Drive extends SubsystemBase { new SwerveModulePosition(), new SwerveModulePosition() }; - private SwerveDrivePoseEstimator poseEstimator = - new SwerveDrivePoseEstimator(kinematics, rawGyroRotation, lastModulePositions, Pose2d.kZero); + private PoseEstimator poseEstimator = + new PoseEstimator(kinematics, lastModulePositions, rawGyroRotation); private final PIDController xController = new PIDController(10.0, 0.0, 0.0); private final PIDController yController = new PIDController(10.0, 0.0, 0.0); @@ -166,6 +165,8 @@ public void periodic() { lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; } + Logger.recordOutput("ModuleDeltas", moduleDeltas); + // Update gyro angle if (gyroInputs.connected) { // Use the real gyro angle @@ -177,7 +178,8 @@ public void periodic() { } // Apply update - poseEstimator.updateWithTime(sampleTimestamps[i], rawGyroRotation, modulePositions); + poseEstimator.updateWithTime( + modulePositions, rawGyroRotation, sampleTimestamps[i], VecBuilder.fill(1, 1, 1)); } // Update gyro alert @@ -551,7 +553,7 @@ public double getFFCharacterizationVelocity() { /** Returns the current odometry pose. */ @AutoLogOutput(key = "Odometry/Robot") public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); + return poseEstimator.getEstimatedPose(); } /** Returns the current odometry rotation. */ @@ -566,9 +568,7 @@ public void setPose(Pose2d pose) { /** Adds a new timestamped vision measurement. */ public void addVisionMeasurement( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs) { + Pose2d visionRobotPoseMeters, double timestampSeconds, Vector visionMeasurementStdDevs) { poseEstimator.addVisionMeasurement( visionRobotPoseMeters, timestampSeconds, visionMeasurementStdDevs); } diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/drive/PoseEstimator.java b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/PoseEstimator.java new file mode 100644 index 0000000..90be803 --- /dev/null +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/drive/PoseEstimator.java @@ -0,0 +1,151 @@ +package org.curtinfrc.frc2025.subsystems.drive; + +import edu.wpi.first.math.Vector; +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.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.RobotController; +import java.util.LinkedList; +import java.util.OptionalInt; +import org.littletonrobotics.junction.Logger; + +public class PoseEstimator { + private static final double kMaxSampleAge = 0.3; + + private final class TimestampedTwist2d extends Twist2d { + public final double timestamp; + + public TimestampedTwist2d(double dx, double dy, double dtheta, double timestamp) { + super(dx, dy, dtheta); + this.timestamp = timestamp; + } + } + + private final LinkedList samples = new LinkedList<>(); + private Pose2d rootPose = new Pose2d(); + private final SwerveModulePosition[] prevWheelPositions; + private final SwerveDriveKinematics kinematics; + private Rotation2d gyroOffset; + private Rotation2d prevAngle; + + public PoseEstimator( + SwerveDriveKinematics kinematics, + SwerveModulePosition[] wheelPositions, + Rotation2d gyroAngle) { + this.kinematics = kinematics; + prevWheelPositions = kinematics.copy(wheelPositions); + gyroOffset = rootPose.getRotation().minus(gyroAngle); + prevAngle = rootPose.getRotation(); + } + + public void resetPosition( + Rotation2d gyroAngle, SwerveModulePosition[] wheelPositions, Pose2d pose) { + rootPose = pose; + prevAngle = pose.getRotation(); + gyroOffset = pose.getRotation().minus(gyroAngle); + kinematics.copyInto(wheelPositions, prevWheelPositions); + samples.clear(); + } + + private OptionalInt indexForTimestamp(double timestamp) { + if (samples.isEmpty()) { + return OptionalInt.empty(); + } + + if (samples.getFirst().timestamp > timestamp) { + return OptionalInt.empty(); + } + if (samples.getLast().timestamp < timestamp) { + return OptionalInt.of(samples.size() - 1); + } + int low = 0; + int high = samples.size() - 1; + while (low <= high) { + int mid = (low + high) / 2; + double midTime = samples.get(mid).timestamp; + if (midTime < timestamp) { + low = mid + 1; + } else if (midTime > timestamp) { + high = mid - 1; + } else { + return OptionalInt.of(mid); + } + } + return OptionalInt.of(low - 1); + } + + private Pose2d poseAtIndex(int index) { + // switching this over to primitive math would be a good idea + Pose2d pose = rootPose; + for (int i = 0; i < index; i++) { + pose = pose.exp(samples.get(i)); + } + return pose; + } + + private void pruneToRoot() { + while (!samples.isEmpty() + && samples.getFirst().timestamp < RobotController.getTime() - kMaxSampleAge) { + rootPose = rootPose.exp(samples.removeFirst()); + } + } + + /** + * Adds a sample to the estimator + * + * @param pose the pose of the robot at the time of the sample + * @param timestamp the timestamp of the sample + * @param weight the weight of the sample (0.0 to 1.0) + */ + public void addVisionMeasurement(Pose2d pose, double timestamp, Vector weight) { + var opt = indexForTimestamp(timestamp); + if (opt.isEmpty()) { + // timestamp is before the first sample + return; + } + int index = opt.getAsInt(); + + Pose2d lastPose = poseAtIndex(index); + Twist2d twist = lastPose.log(pose); + samples.add( + index, + new TimestampedTwist2d( + twist.dx * weight.get(0), + twist.dy * weight.get(1), + twist.dtheta * weight.get(2), + timestamp)); + pruneToRoot(); + } + + public void updateWithTime( + SwerveModulePosition[] wheelPositions, + Rotation2d gyroAngle, + double timestamp, + Vector weight) { + var angle = gyroAngle.plus(gyroOffset); + var twist = kinematics.toTwist2d(prevWheelPositions, wheelPositions); + twist.dtheta = angle.minus(prevAngle).getRadians(); + Logger.recordOutput("PoseEstimator/PreviousWheelPositions", prevWheelPositions); + Logger.recordOutput("PoseEstimator/CurrentWheelPositions", wheelPositions); + Logger.recordOutput("PoseEstimator/Samples", samples.size()); + Logger.recordOutput("PoseEstimator/Twist", twist); + + samples.add( + new TimestampedTwist2d( + twist.dx * weight.get(0), + twist.dy * weight.get(1), + twist.dtheta * weight.get(2), + timestamp)); + + kinematics.copyInto(wheelPositions, prevWheelPositions); + prevAngle = angle; + pruneToRoot(); + } + + public Pose2d getEstimatedPose() { + return poseAtIndex(samples.size()); + } +} diff --git a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java index ba10c26..7c0041d 100644 --- a/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java +++ b/src/main/java/org/curtinfrc/frc2025/subsystems/vision/Vision.java @@ -15,12 +15,11 @@ import static org.curtinfrc.frc2025.subsystems.vision.VisionConstants.*; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.Vector; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; @@ -187,8 +186,6 @@ public void periodic() { @FunctionalInterface public static interface PoseEstimateConsumer { public void accept( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs); + Pose2d visionRobotPoseMeters, double timestampSeconds, Vector visionMeasurementStdDevs); } }