add vision - pose estimation, auto align, auto angle

simgui-ds.json

@@ -91,10 +91,10 @@
   ],
   "robotJoysticks": [
     {
+      "guid": "78696e70757401000000000000000000",
       "useGamepad": true
     },
     {
-      "guid": "78696e70757401000000000000000000",
       "useGamepad": true
     }
   ]

simgui.json

@@ -10,6 +10,7 @@
     "types": {
       "/AdvantageKit/RealOutputs/Climb/Mechanism2d": "Mechanism2d",
       "/AdvantageKit/RealOutputs/Visualizer/FullRobot": "Mechanism2d",
+      "/AdvantageKit/RealOutputs/Visualizer/M_main": "Mechanism2d",
       "/FMSInfo": "FMSInfo",
       "/SmartDashboard/Climb Down": "Command",
       "/SmartDashboard/Climb Up": "Command",
@@ -28,7 +29,8 @@
       "/SmartDashboard/Shooter 60": "Command",
       "/SmartDashboard/Shooter 75": "Command",
       "/SmartDashboard/Shooter 90": "Command",
-      "/SmartDashboard/Test": "Command"
+      "/SmartDashboard/Test": "Command",
+      "/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d"
     },
     "windows": {
       "/AdvantageKit/RealOutputs/Visualizer/M_main": {
@@ -77,12 +79,39 @@
         }
       },
       "/SmartDashboard/VisionSystemSim-main/Sim Field": {
+        "Robot": {
+          "color": [
+            206.58230590820313,
+            255.0,
+            0.0,
+            255.0
+          ]
+        },
         "bottom": 1476,
+        "cameras": {
+          "color": [
+            255.0,
+            238.86074829101563,
+            0.0,
+            255.0
+          ]
+        },
         "height": 8.210550308227539,
         "left": 150,
         "right": 2961,
         "top": 79,
-        "width": 16.541748046875
+        "visibleTargetPoses": {
+          "color": [
+            1.0,
+            0.0,
+            0.9113926887512207,
+            255.0
+          ]
+        },
+        "width": 16.541748046875,
+        "window": {
+          "visible": true
+        }
       }
     }
   },

src/main/java/frc/robot/Constants.java

@@ -13,7 +13,16 @@
 
 package frc.robot;
 
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+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.simulation.SingleJointedArmSim;
 
@@ -43,7 +52,7 @@ public static enum Mode {
 
   public static final boolean isTuning = false;
 
-  public static class RobotMap {
+public static class RobotMap {
     public static class Drive {
       public static final int frontLeftDrive = 1;
       public static final int frontLeftTurn = 2;
@@ -306,4 +315,32 @@ public static class ShooterConstants {
   public static class SimConstants {
     public static final double loopTime = 0.02;
   }
+
+  public static class VisionConstants {
+    public static final Transform3d rightCamToRobot =
+        new Transform3d(
+            new Translation3d(
+                Units.inchesToMeters(12.161481),
+                -Units.inchesToMeters(12.050199),
+                Units.inchesToMeters(9.756915)),
+            new Rotation3d(
+                Units.degreesToRadians(0), -Units.degreesToRadians(25), -Units.degreesToRadians(35)));
+
+    public static final Transform3d leftCamToRobot =
+        new Transform3d(
+            new Translation3d(
+                Units.inchesToMeters(12.161481),
+                Units.inchesToMeters(12.050199),
+                Units.inchesToMeters(9.756915)),
+            new Rotation3d(
+                Units.degreesToRadians(0),
+                -Units.degreesToRadians(25),
+                Units.degreesToRadians(35)));
+
+    public static final Matrix<N3, N1> singleTagStdDev =
+        VecBuilder.fill(0.8, 0.8, Double.MAX_VALUE);
+    public static final Matrix<N3, N1> multiTagStdDev = VecBuilder.fill(0.5, 0.5, Double.MAX_VALUE);
+    public static final AprilTagFieldLayout aprilTagFieldLayout =
+        AprilTagFields.k2024Crescendo.loadAprilTagLayoutField();
+  }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -23,6 +23,9 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.RobotMap;
+import frc.robot.subsystems.apriltagvision.AprilTagVisionIOReal;
+import frc.robot.subsystems.apriltagvision.AprilTagVisionIOReplay;
+import frc.robot.subsystems.apriltagvision.AprilTagVisionIOSim;
 import frc.robot.subsystems.climber.Climb;
 import frc.robot.subsystems.climber.ClimbIOReplay;
 import frc.robot.subsystems.climber.ClimbIOSim;
@@ -91,7 +94,8 @@ public RobotContainer() {
                 new ModuleIOReal(0),
                 new ModuleIOReal(1),
                 new ModuleIOReal(2),
-                new ModuleIOReal(3));
+                new ModuleIOReal(3),
+                new AprilTagVisionIOReal());
         m_climber = new Climb(new ClimbIOSparkMax());
         m_intake = new Intake(new IntakeIOSparkMax());
         m_feeder =
@@ -111,7 +115,8 @@ public RobotContainer() {
                 new ModuleIOSim("FrontLeft"),
                 new ModuleIOSim("FrontRight"),
                 new ModuleIOSim("BackLeft"),
-                new ModuleIOSim("BackRight"));
+                new ModuleIOSim("BackRight"),
+                new AprilTagVisionIOSim());
         m_climber = new Climb(new ClimbIOSim());
         m_intake = new Intake(new IntakeIOSim());
         m_feeder =
@@ -131,7 +136,8 @@ public RobotContainer() {
                 new ModuleIOReplay() {},
                 new ModuleIOReplay() {},
                 new ModuleIOReplay() {},
-                new ModuleIOReplay() {});
+                new ModuleIOReplay() {},
+                new AprilTagVisionIOReplay() {});
         m_climber = new Climb(new ClimbIOReplay());
         m_intake = new Intake(new IntakeIOReplay());
         m_feeder =

src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionIO.java

@@ -0,0 +1,35 @@
+package frc.robot.subsystems.apriltagvision;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import java.util.List;
+import org.littletonrobotics.junction.AutoLog;
+import org.photonvision.EstimatedRobotPose;
+
+public interface AprilTagVisionIO {
+  @AutoLog
+  public static class AprilTagVisionIOInputs {
+    public Pose3d[] visionPoses =
+        List.of(new Pose3d(), new Pose3d(), new Pose3d()).toArray(new Pose3d[0]);
+    public double[] timestamps = new double[3];
+    public double[] latency = new double[3];
+    public double[] visionStdDevs = new double[9];
+  }
+
+  /** Updates the set of loggable inputs. */
+  public abstract void updateInputs(AprilTagVisionIOInputs inputs);
+
+  /** Update the reference pose of the vision system. Currently only used in sim. */
+  public abstract void updatePose(Pose2d pose);
+
+  /**
+   * The standard deviations of the estimated poses from vision cameras, for use with {@link
+   * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator SwerveDrivePoseEstimator}.
+   *
+   * @param estimatedPose The estimated pose to guess standard deviations for.
+   */
+  abstract Matrix<N3, N1> getEstimationStdDevs(EstimatedRobotPose estimatedPose);
+}

src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionIOReal.java

@@ -0,0 +1,141 @@
+package frc.robot.subsystems.apriltagvision;
+
+import static java.lang.System.arraycopy;
+import static org.photonvision.PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import frc.robot.Constants.VisionConstants;
+import java.util.Optional;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+
+public class AprilTagVisionIOReal implements AprilTagVisionIO {
+
+  // Left Side Camera
+  private final PhotonCamera leftCam;
+  private final PhotonPoseEstimator leftPhotonPoseEstimator;
+
+  // Right Side Camera
+  private final PhotonCamera rightCam;
+  private final PhotonPoseEstimator rightPhotonPoseEstimator;
+
+  private Pose3d[] poseArray = new Pose3d[2];
+  private double[] timestampArray = new double[2];
+  private double[] visionStdArray = new double[6];
+  private double[] latencyArray = new double[2];
+  private int count = 0;
+
+  public AprilTagVisionIOReal() {
+    // Left Side Camera
+    leftCam = new PhotonCamera("left");
+    leftPhotonPoseEstimator =
+        new PhotonPoseEstimator(
+            VisionConstants.aprilTagFieldLayout,
+            MULTI_TAG_PNP_ON_COPROCESSOR,
+            leftCam,
+            VisionConstants.leftCamToRobot);
+
+    // Right Side Camera
+    rightCam = new PhotonCamera("right");
+    rightPhotonPoseEstimator =
+        new PhotonPoseEstimator(
+            VisionConstants.aprilTagFieldLayout,
+            MULTI_TAG_PNP_ON_COPROCESSOR,
+            rightCam,
+            VisionConstants.rightCamToRobot);
+  }
+
+  @Override
+  public void updateInputs(AprilTagVisionIOInputs inputs) {
+    getEstimatedPoseUpdates();
+    inputs.visionPoses = poseArray;
+    inputs.timestamps = timestampArray;
+    inputs.visionStdDevs = visionStdArray;
+    inputs.latency = latencyArray;
+    count += 1;
+    if (count % 500 == 0) {
+      leftCam.takeOutputSnapshot();
+      rightCam.takeOutputSnapshot();
+    }
+  }
+
+  public void getEstimatedPoseUpdates() {
+    Matrix<N3, N1> infiniteStdevs =
+        VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+
+    Optional<EstimatedRobotPose> pose = leftPhotonPoseEstimator.update();
+    pose.ifPresentOrElse(
+        estimatedRobotPose -> {
+          poseArray[0] = estimatedRobotPose.estimatedPose;
+          timestampArray[0] = estimatedRobotPose.timestampSeconds;
+          Matrix<N3, N1> stdDevs = getEstimationStdDevs(estimatedRobotPose);
+          arraycopy(stdDevs.getData(), 0, visionStdArray, 0, 3);
+          latencyArray[0] = leftCam.getLatestResult().getLatencyMillis() / 1.0e3;
+        },
+        () -> {
+          poseArray[0] = new Pose3d();
+          timestampArray[0] = 0.0;
+          latencyArray[0] = 0.0;
+        });
+    pose = rightPhotonPoseEstimator.update();
+    pose.ifPresentOrElse(
+        estimatedRobotPose -> {
+          poseArray[1] = estimatedRobotPose.estimatedPose;
+          timestampArray[1] = estimatedRobotPose.timestampSeconds;
+          Matrix<N3, N1> stdDevs = getEstimationStdDevs(estimatedRobotPose);
+          arraycopy(stdDevs.getData(), 0, visionStdArray, 3, 3);
+          latencyArray[1] = leftCam.getLatestResult().getLatencyMillis() / 1.0e3;
+        },
+        () -> {
+          poseArray[1] = new Pose3d();
+          timestampArray[1] = 0.0;
+          latencyArray[1] = 0.0;
+        });
+  }
+
+  public Matrix<N3, N1> getEstimationStdDevs(EstimatedRobotPose estimatedPose) {
+    var estStdDevs = VisionConstants.singleTagStdDev;
+    var targets = estimatedPose.targetsUsed;
+    int numTags = 0;
+    double avgDist = 0;
+    for (var tgt : targets) {
+      var tagPose = VisionConstants.aprilTagFieldLayout.getTagPose(tgt.getFiducialId());
+      if (tagPose.isEmpty()) continue;
+      numTags++;
+      avgDist +=
+          tagPose
+              .get()
+              .toPose2d()
+              .minus(estimatedPose.estimatedPose.toPose2d())
+              .getTranslation()
+              .getNorm();
+    }
+
+    if (numTags == 0) return estStdDevs;
+    avgDist /= numTags;
+
+    // Decrease std devs if multiple targets are visible
+    if (numTags > 1 && avgDist > 5) {
+      estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+    } else {
+      estStdDevs = VisionConstants.multiTagStdDev;
+    }
+    // Increase std devs based on (average) distance
+    if (numTags == 1 && avgDist > 4) {
+      estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE);
+    } else {
+      estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 20));
+    }
+
+    return estStdDevs;
+  }
+
+  @Override
+  public void updatePose(Pose2d pose) {}
+}

src/main/java/frc/robot/subsystems/apriltagvision/AprilTagVisionIOReplay.java

@@ -0,0 +1,21 @@
+package frc.robot.subsystems.apriltagvision;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import org.photonvision.EstimatedRobotPose;
+
+public class AprilTagVisionIOReplay implements AprilTagVisionIO {
+
+  @Override
+  public void updateInputs(AprilTagVisionIOInputs inputs) {}
+
+  @Override
+  public void updatePose(Pose2d pose) {}
+
+  @Override
+  public Matrix<N3, N1> getEstimationStdDevs(EstimatedRobotPose estimatedPose) {
+    return null;
+  }
+}