From 1fafbc2f7a6a2809c131940338598bd6fbd064f3 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Thu, 8 Feb 2024 18:00:21 -0500 Subject: [PATCH] finished implementing camera setup for testbed --- .../subsystems/drive/DriveSubsystem.java | 40 ++++++++++- .../subsystems/vision/VisionSubsystem.java | 68 ++++++++++++++++--- 2 files changed, 98 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 118065c9..5c2489f0 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -18,6 +18,8 @@ import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -26,12 +28,15 @@ 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.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.drive.module.Module; import frc.robot.subsystems.drive.module.ModuleIO; +import frc.robot.subsystems.vision.VisionSubsystem; import frc.robot.util.LocalADStarAK; import java.util.List; @@ -55,23 +60,45 @@ public class DriveSubsystem extends SubsystemBase { private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(getModuleTranslations()); private Pose2d pose = new Pose2d(); + private Pose2d visionPose = new Pose2d(); private Rotation2d lastGyroRotation = new Rotation2d(); + private final VisionSubsystem[] m_cameras; + private final Matrix m_stdDevs; + public DriveSubsystem( GyroIO gyroIO, ModuleIO flModuleIO, ModuleIO frModuleIO, ModuleIO blModuleIO, ModuleIO brModuleIO) { + this (gyroIO, + flModuleIO, + frModuleIO, + blModuleIO, + brModuleIO, + new VisionSubsystem[]{}); + } + + public DriveSubsystem( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO, + VisionSubsystem[] cameras) { this.gyroIO = gyroIO; modules[0] = new Module(flModuleIO); modules[1] = new Module(frModuleIO); modules[2] = new Module(blModuleIO); modules[3] = new Module(brModuleIO); + m_cameras = cameras; + m_stdDevs = VecBuilder.fill(Units.inchesToMeters(0.5), Units.inchesToMeters(0.5), Units.degreesToRadians(15)); + // Configure AutoBuilder for PathPlanner AutoBuilder.configureHolonomic( - this::getPose, + this::getVisionPose, this::setPose, () -> kinematics.toChassisSpeeds(getModuleStates()), this::runVelocity, @@ -143,6 +170,11 @@ public void periodic() { // Apply the twist (change since last sample) to the current pose pose = pose.exp(twist); } + + for (VisionSubsystem camera : m_cameras) { + var cameraPose = camera.getPose(getPose()); + cameraPose.ifPresent(visionUpdate -> visionPose = visionUpdate.apply(pose, m_stdDevs)); + } } /** @@ -218,6 +250,12 @@ public Pose2d getPose() { return pose; } + /** Returns the robot pose with vision updates */ + @AutoLogOutput(key = "Odometry/RobotVision") + public Pose2d getVisionPose() { + return visionPose; + } + /** Returns the current odometry rotation. */ public Rotation2d getRotation() { return pose.getRotation(); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 3de3677a..e99d74fe 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -3,11 +3,19 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +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.Transform3d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.littletonrobotics.junction.AutoLog; +import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -16,13 +24,14 @@ import java.io.IOException; import java.util.Optional; -public class VisionSubsystem extends SubsystemBase { +public class VisionSubsystem { private final PhotonCamera m_camera; private final Transform3d robotToCam; private PhotonPoseEstimator m_photonPoseEstimator; private DriverStation.Alliance m_alliance; private AprilTagFieldLayout m_aprilTagFieldLayout; + private final String m_name; private final PhotonVisionIOInputsAutoLogged inputs = new PhotonVisionIOInputsAutoLogged(); @@ -32,10 +41,13 @@ public void updateInputs(){ inputs.setTagIds(getTargetIDs()); inputs.setNumTagsFound(m_camera.getLatestResult().targets.size()); inputs.setTagsFound(m_camera.getLatestResult().hasTargets()); + + Logger.processInputs("PhotonVision/" + m_name, inputs); } public VisionSubsystem(String camName, Transform3d camPose) { m_camera = new PhotonCamera(camName); + m_name = camName; robotToCam = camPose; // if(DriverStation.getAlliance().isPresent()) { // m_alliance = DriverStation.getAlliance().get(); @@ -59,13 +71,7 @@ public VisionSubsystem(String camName, Transform3d camPose) { } } - @Override - public void periodic() { - updateInputs(); - super.periodic(); - } - - public Optional getPose(Pose2d prevEstimatedRobotPose) { + public Optional getPose(Pose2d prevEstimatedRobotPose) { m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); @@ -86,7 +92,16 @@ public Optional getPose(Pose2d prevEstimatedRobotPose) { // } PhotonPipelineResult camResult = m_camera.getLatestResult(); - return m_photonPoseEstimator.update(camResult); + Optional opPose = m_photonPoseEstimator.update(camResult); + + if (opPose.isEmpty()) { + return Optional.empty(); + } else { + EstimatedRobotPose estPose = opPose.get(); + return Optional.of(new VisionUpdate(estPose.estimatedPose.toPose2d(), + VecBuilder.fill(Units.inchesToMeters(0.5), Units.inchesToMeters(0.5), Units.degreesToRadians(15)), + estPose.timestampSeconds)); + } } public int[] getTargetIDs () { @@ -107,5 +122,40 @@ public int[] getTargetIDs () { return results; } + + public record VisionUpdate(Pose2d pose, Matrix stdDevs, double timestamp) { + public Pose2d apply(Pose2d lastPose, Matrix q) { + // Apply vision updates + // Calculate Kalman gains based on std devs + // (https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/estimator/) + Matrix visionK = new Matrix<>(Nat.N3(), Nat.N3()); + var r = new double[3]; + for (int i = 0; i < 3; ++i) { + r[i] = this.stdDevs().get(i, 0) * this.stdDevs().get(i, 0); + } + for (int row = 0; row < 3; ++row) { + if (q.get(row, 0) == 0.0) { + visionK.set(row, row, 0.0); + } else { + visionK.set( + row, row, q.get(row, 0) / (q.get(row, 0) + Math.sqrt(q.get(row, 0) * r[row]))); + } + } + + // Calculate twist between current and vision pose + var visionTwist = lastPose.log(this.pose()); + + // Multiply by Kalman gain matrix + var twistMatrix = + visionK.times(VecBuilder.fill(visionTwist.dx, visionTwist.dy, visionTwist.dtheta)); + + // Apply twist + lastPose = + lastPose.exp( + new Twist2d(twistMatrix.get(0, 0), twistMatrix.get(1, 0), twistMatrix.get(2, 0))); + + return lastPose; + } + } }