diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a68886440a..89af4e3018 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -47,6 +47,7 @@ import frc.robot.subsystems.swerve.factories.imu.IMUFactory; import frc.robot.subsystems.swerve.factories.modules.ModulesFactory; import frc.robot.statemachine.shooterstatehandler.TurretCalculations; +import frc.robot.vision.RobotPoseObservation; import frc.utils.GamePeriodUtils; import frc.utils.auto.AutonomousChooser; import frc.robot.subsystems.swerve.factories.modules.drive.KrakenX60DriveBuilder; @@ -64,6 +65,7 @@ import org.littletonrobotics.junction.Logger; import java.util.List; +import java.util.Optional; import java.util.function.Supplier; /** @@ -321,7 +323,12 @@ public void periodic() { getLimelights().forEach(Limelight::updateHardwareInputs); getLimelights().forEach(Limelight::updateMT1); - getLimelights().forEach(limelight -> limelight.getIndependentRobotPose().ifPresent(poseEstimator::updateVision)); + + RobotPoseObservation[] observationsArray = getLimelights().stream() + .map(Limelight::getIndependentRobotPose) + .flatMap(Optional::stream) + .toArray(RobotPoseObservation[]::new); + poseEstimator.updateVision(new RobotPoseObservation[][] {observationsArray}); poseEstimator.log(); ShootingCalculations diff --git a/src/main/java/frc/robot/poseestimator/IVisionEstimator.java b/src/main/java/frc/robot/poseestimator/IVisionEstimator.java index 555a8015c0..1d7ec4a8f4 100644 --- a/src/main/java/frc/robot/poseestimator/IVisionEstimator.java +++ b/src/main/java/frc/robot/poseestimator/IVisionEstimator.java @@ -5,7 +5,7 @@ public interface IVisionEstimator { - void updateVision(RobotPoseObservation... robotPoseVisionData); + void updateVision(RobotPoseObservation[]... robotPoseVisionData); Pose2d getEstimatedPose(); diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java index e305a3adb3..a944ce3e79 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorConstants.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import frc.constants.MathConstants; import frc.utils.math.StandardDeviations2D; @@ -15,6 +16,8 @@ public class WPILibPoseEstimatorConstants { public static final StandardDeviations2D VISION_STD_DEV_COLLISION_REDUCTION = new StandardDeviations2D(); + public static final StandardDeviations2D VISION_STD_DEV_SIMILARITY_REDUCTION = new StandardDeviations2D(0.1, 0.1, 0.1); + public static final double MINIMUM_COLLISION_IMU_ACCELERATION_G = 2; public static final Rotation2d MINIMUM_TILT_IMU_ROLL = Rotation2d.fromDegrees(4); @@ -37,4 +40,10 @@ public class WPILibPoseEstimatorConstants { public static double IMU_XY_ACCELERATION_G_BUFFER_SIZE_SECONDS = 2; + public static double SIMILAR_POSE_TRANSLATION_NORM_TOLERANCE_METERS = 1; + + public static Rotation2d SIMILAR_POSE_ROTATION_TOLERANCE = MathConstants.QUARTER_CIRCLE; + + public static double SIMILAR_POSE_TIMESTAMP_TOLERANCE_SECONDS = 0.2; + } diff --git a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java index ed1b98a326..e50cf11f89 100644 --- a/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java +++ b/src/main/java/frc/robot/poseestimator/WPILibPoseEstimator/WPILibPoseEstimatorWrapper.java @@ -1,6 +1,5 @@ package frc.robot.poseestimator.WPILibPoseEstimator; -import edu.wpi.first.math.Matrix; import edu.wpi.first.math.estimator.PoseEstimator; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -12,16 +11,18 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.Odometry; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.numbers.N3; import frc.robot.vision.RobotPoseObservation; import frc.robot.poseestimator.IPoseEstimator; import frc.robot.poseestimator.OdometryData; import frc.utils.buffers.RingBuffer.RingBuffer; +import frc.utils.math.StandardDeviations2D; import frc.utils.math.StatisticsMath; import frc.utils.pose.PoseUtil; import org.littletonrobotics.junction.Logger; +import java.util.Arrays; +import java.util.Comparator; +import java.util.List; import java.util.Optional; public class WPILibPoseEstimatorWrapper implements IPoseEstimator { @@ -33,8 +34,8 @@ public class WPILibPoseEstimatorWrapper implements IPoseEstimator { private final RingBuffer poseToIMUYawDifferenceBuffer; private final TimeInterpolatableBuffer imuYawBuffer; private final TimeInterpolatableBuffer imuXYAccelerationGBuffer; - private RobotPoseObservation lastVisionObservation; private OdometryData lastOdometryData; + private double lastVisionUpdateTimestamp; private boolean isIMUOffsetCalibrated; public WPILibPoseEstimatorWrapper( @@ -125,8 +126,8 @@ public void updateOdometry(OdometryData data) { } @Override - public void updateVision(RobotPoseObservation... visionRobotPoseObservations) { - for (RobotPoseObservation visionRobotPoseObservation : visionRobotPoseObservations) { + public void updateVision(RobotPoseObservation[]... visionRobotPoseObservations) { + for (RobotPoseObservation[] visionRobotPoseObservation : visionRobotPoseObservations) { updateVision(visionRobotPoseObservation); } } @@ -170,8 +171,8 @@ public void log() { Logger.recordOutput(logPath + "/estimatedPose", getEstimatedPose()); Logger.recordOutput(logPath + "/odometryPose", getOdometryPose()); Logger.recordOutput(logPath + "/predictedOdometryPose", getPredictedOdometryPose()); - if (lastVisionObservation != null) { - Logger.recordOutput(logPath + "/lastVisionUpdate", lastVisionObservation.timestampSeconds()); + if (lastVisionUpdateTimestamp != 0) { + Logger.recordOutput(logPath + "/lastVisionUpdate", lastVisionUpdateTimestamp); } Logger.recordOutput(logPath + "/lastOdometryUpdate", lastOdometryData.getTimestampSeconds()); Logger.recordOutput(logPath + "/isIMUOffsetCalibrated", isIMUOffsetCalibrated); @@ -207,9 +208,22 @@ public void log() { ); } - private void updateVision(RobotPoseObservation visionRobotPoseObservation) { - addVisionMeasurement(visionRobotPoseObservation); + private void updateVision(RobotPoseObservation[] visionRobotPoseObservations) { + List similarVisionRobotPoseObservations = Arrays.stream(visionRobotPoseObservations) + .filter(observation -> Arrays.stream(visionRobotPoseObservations).anyMatch(other -> getAreObservationsSimilar(observation, other))) + .toList(); + Arrays.stream(visionRobotPoseObservations) + .forEach(observation -> addVisionMeasurement(observation, similarVisionRobotPoseObservations.contains(observation))); + Arrays.stream(visionRobotPoseObservations).forEach(this::updateIMUOffset); + + lastVisionUpdateTimestamp = Arrays.stream(visionRobotPoseObservations) + .max(Comparator.comparingDouble(RobotPoseObservation::timestampSeconds)) + .orElse(new RobotPoseObservation()) + .timestampSeconds(); + } + + public void updateIMUOffset(RobotPoseObservation visionRobotPoseObservation) { getEstimatedPoseToIMUYawDifference( imuYawBuffer.getSample(visionRobotPoseObservation.timestampSeconds()), visionRobotPoseObservation.timestampSeconds() @@ -234,16 +248,15 @@ public void resetIsIMUOffsetCalibrated() { isIMUOffsetCalibrated = false; } - private void addVisionMeasurement(RobotPoseObservation visionObservation) { + private void addVisionMeasurement(RobotPoseObservation visionObservation, boolean isObservationSimilar) { poseEstimator.addVisionMeasurement( visionObservation.robotPose(), visionObservation.timestampSeconds(), - getCollisionCompensatedVisionStdDevs(visionObservation) + getSimilarityAccountedVisionStdDevs(getCollisionCompensatedVisionStdDevs(visionObservation), isObservationSimilar).asColumnVector() ); - this.lastVisionObservation = visionObservation; } - private Matrix getCollisionCompensatedVisionStdDevs(RobotPoseObservation visionObservation) { + private StandardDeviations2D getCollisionCompensatedVisionStdDevs(RobotPoseObservation visionObservation) { boolean isColliding = imuXYAccelerationGBuffer.getSample(visionObservation.timestampSeconds()) .map( (imuAccelerationG) -> PoseUtil @@ -252,10 +265,24 @@ private Matrix getCollisionCompensatedVisionStdDevs(RobotPoseObservation .orElse(false); return isColliding - ? visionObservation.stdDevs() - .asColumnVector() - .minus(WPILibPoseEstimatorConstants.VISION_STD_DEV_COLLISION_REDUCTION.asColumnVector()) - : visionObservation.stdDevs().asColumnVector(); + ? new StandardDeviations2D( + visionObservation.stdDevs() + .asColumnVector() + .minus(WPILibPoseEstimatorConstants.VISION_STD_DEV_COLLISION_REDUCTION.asColumnVector()) + ) + : visionObservation.stdDevs(); + } + + private StandardDeviations2D getSimilarityAccountedVisionStdDevs( + StandardDeviations2D visionStandardDeviations, + boolean isObservationSimilar + ) { + return isObservationSimilar + ? new StandardDeviations2D( + visionStandardDeviations.asColumnVector() + .minus(WPILibPoseEstimatorConstants.VISION_STD_DEV_SIMILARITY_REDUCTION.asColumnVector()) + ) + : visionStandardDeviations; } private Optional getEstimatedPoseToIMUYawDifference(Optional gyroYaw, double timestampSeconds) { @@ -271,4 +298,18 @@ private Pose2d getPredictedOdometryPose() { ); } + private boolean getAreObservationsSimilar(RobotPoseObservation observation, RobotPoseObservation other) { + return !observation.equals(other) + && Math.abs(observation.timestampSeconds() - other.timestampSeconds()) + < WPILibPoseEstimatorConstants.SIMILAR_POSE_TIMESTAMP_TOLERANCE_SECONDS + && getArePosesSimilar(observation.robotPose(), other.robotPose()); + } + + private boolean getArePosesSimilar(Pose2d pose, Pose2d other) { + return PoseUtil.getDifference(pose.getTranslation(), other.getTranslation()) + < WPILibPoseEstimatorConstants.SIMILAR_POSE_TRANSLATION_NORM_TOLERANCE_METERS + && PoseUtil.getDifferenceRadians(pose.getRotation(), other.getRotation()) + < WPILibPoseEstimatorConstants.SIMILAR_POSE_ROTATION_TOLERANCE.getRadians(); + } + } diff --git a/src/main/java/frc/utils/math/StandardDeviations2D.java b/src/main/java/frc/utils/math/StandardDeviations2D.java index 69db61a909..2841b2250e 100644 --- a/src/main/java/frc/utils/math/StandardDeviations2D.java +++ b/src/main/java/frc/utils/math/StandardDeviations2D.java @@ -19,6 +19,10 @@ public StandardDeviations2D(StandardDeviations2D standardDeviations2D) { this(standardDeviations2D.xStandardDeviations, standardDeviations2D.yStandardDeviations, standardDeviations2D.angleStandardDeviations); } + public StandardDeviations2D(Matrix columnVector) { + this(columnVector.get(0, 0), columnVector.get(1, 0), columnVector.get(2, 0)); + } + public Matrix asColumnVector() { return VecBuilder.fill(xStandardDeviations, yStandardDeviations, angleStandardDeviations); } diff --git a/src/main/java/frc/utils/pose/PoseUtil.java b/src/main/java/frc/utils/pose/PoseUtil.java index 24f15b9b8f..458c6664a5 100644 --- a/src/main/java/frc/utils/pose/PoseUtil.java +++ b/src/main/java/frc/utils/pose/PoseUtil.java @@ -87,6 +87,14 @@ public static double[] rotation3DToRotationArray(Rotation3d rotation3d, AngleUni }; } + public static double getDifference(Translation2d Translation1, Translation2d Translation2) { + return Translation1.minus(Translation2).getNorm(); + } + + public static double getDifferenceRadians(Rotation2d rotation1, Rotation2d rotation2) { + return Math.abs(rotation1.minus(rotation2).getRadians()); + } + public static boolean getIsColliding(Translation2d imuAccelerationG, double minimumCollisionIMUAccelerationG) { return imuAccelerationG.getNorm() >= minimumCollisionIMUAccelerationG; }