From b81e6fc81933bd50199497db0774c9496f803bb0 Mon Sep 17 00:00:00 2001 From: "Timothy P. Ellsworth Bowers" Date: Wed, 21 Jan 2026 18:10:13 -0700 Subject: [PATCH] Simulator has table CameraSweepEvaluator Can estimate the number of AprilTags seen as a function of camera location and angle as the robot is moved around the field. Teams can use this to judge where best to place their camera(s) on the robot for odometric vision. --- src/main/java/frc/robot/Constants.java | 17 +- src/main/java/frc/robot/RobotContainer.java | 44 ++-- .../vision/CameraPlacementEvaluator.java | 90 ------- .../vision/CameraSweepEvaluator.java | 220 +++++++++++++----- 4 files changed, 203 insertions(+), 168 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/vision/CameraPlacementEvaluator.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1ebb197..9e33d5d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -432,14 +432,19 @@ public static class Cameras { // Robot to camera transforms // (ONLY USED FOR PHOTONVISION -- Limelight: configure in web UI instead) - // Example Camera are mounted on the frame perimeter, 18" up from the floor, centered - // side-to-side + // Example Cameras are mounted in the back corners, 18" up from the floor, facing sideways public static Transform3d robotToCamera0 = new Transform3d( - Inches.of(13.0), Inches.of(0), Inches.of(18.0), new Rotation3d(0.0, 0.0, 0.0)); + Inches.of(-13.0), + Inches.of(13.0), + Inches.of(12.0), + new Rotation3d(0.0, 0.0, Math.PI / 2)); public static Transform3d robotToCamera1 = new Transform3d( - Inches.of(-13.0), Inches.of(0), Inches.of(18.0), new Rotation3d(0.0, 0.0, Math.PI)); + Inches.of(-13.0), + Inches.of(-13.0), + Inches.of(12.0), + new Rotation3d(0.0, 0.0, -Math.PI / 2)); // Standard deviation multipliers for each camera // (Adjust to trust some cameras more than others) @@ -456,7 +461,7 @@ public static class SimCameras { public static final SimCameraProperties kSimCamera1Props = new SimCameraProperties() { { - setCalibration(1280, 800, Rotation2d.fromDegrees(90)); + setCalibration(1280, 800, Rotation2d.fromDegrees(120)); setCalibError(0.25, 0.08); setFPS(30); setAvgLatencyMs(20); @@ -467,7 +472,7 @@ public static class SimCameras { public static final SimCameraProperties kSimCamera2Props = new SimCameraProperties() { { - setCalibration(1280, 800, Rotation2d.fromDegrees(90)); + setCalibration(1280, 800, Rotation2d.fromDegrees(120)); setCalibError(0.25, 0.08); setFPS(30); setAvgLatencyMs(20); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c8099d9..ce35545 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -62,7 +62,8 @@ import frc.robot.util.GetJoystickValue; import frc.robot.util.LoggedTunableNumber; import frc.robot.util.OverrideSwitches; -import frc.robot.util.RBSIEnum; +import frc.robot.util.RBSIEnum.AutoType; +import frc.robot.util.RBSIEnum.Mode; import frc.robot.util.RBSIPowerMonitor; import java.util.Set; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -379,24 +380,27 @@ private void configureBindings() { m_drivebase::stop, m_drivebase)); - // Double-press the A button on Joystick3 to run the CameraSweepEvaluator - // Use WPILib's built-in double-press binding - joystick3 - .button(1) - .multiPress(2, 0.2) - .onTrue( - Commands.runOnce( - () -> { - try { - sweep.runFullSweep( - Filesystem.getOperatingDirectory() - .toPath() - .resolve("camera_sweep.csv") - .toString()); - } catch (Exception e) { - e.printStackTrace(); - } - })); + if (Constants.getMode() == Mode.SIM) { + // IN SIMULATION ONLY: + // Double-press the A button on Joystick3 to run the CameraSweepEvaluator + // Use WPILib's built-in double-press binding + joystick3 + .button(1) + .multiPress(2, 0.2) + .onTrue( + Commands.runOnce( + () -> { + try { + sweep.runFullSweep( + Filesystem.getOperatingDirectory() + .toPath() + .resolve("camera_sweep.csv") + .toString()); + } catch (Exception e) { + e.printStackTrace(); + } + })); + } } /** @@ -467,7 +471,7 @@ public Drive getDrivebase() { *

NOTE: These are currently only accessible with Constants.AutoType.PATHPLANNER */ private void definesysIdRoutines() { - if (Constants.getAutoType() == RBSIEnum.AutoType.PATHPLANNER) { + if (Constants.getAutoType() == AutoType.PATHPLANNER) { // Drivebase characterization autoChooserPathPlanner.addOption( "Drive Wheel Radius Characterization", diff --git a/src/main/java/frc/robot/subsystems/vision/CameraPlacementEvaluator.java b/src/main/java/frc/robot/subsystems/vision/CameraPlacementEvaluator.java deleted file mode 100644 index aa907ee..0000000 --- a/src/main/java/frc/robot/subsystems/vision/CameraPlacementEvaluator.java +++ /dev/null @@ -1,90 +0,0 @@ -// Copyright (c) 2026 Az-FIRST -// http://github.com/AZ-First -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. -// -// 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 frc.robot.subsystems.vision; - -import edu.wpi.first.math.geometry.*; -import edu.wpi.first.wpilibj.Timer; -import frc.robot.Constants; -import frc.robot.subsystems.drive.Drive; -import java.util.List; -import org.photonvision.simulation.PhotonCameraSim; -import org.photonvision.simulation.SimCameraProperties; -import org.photonvision.simulation.VisionSystemSim; -import org.photonvision.simulation.VisionTargetSim; -import org.photonvision.targeting.PhotonPipelineResult; - -public class CameraPlacementEvaluator { - - private final Drive driveSim; - private final VisionSystemSim visionSim; - - public CameraPlacementEvaluator(Drive driveSim, VisionSystemSim visionSim) { - this.driveSim = driveSim; - this.visionSim = visionSim; - } - - public double evaluateCameraPose( - PhotonCameraSim simCam, Transform3d robotToCamera, SimCameraProperties props) { - - // Add or adjust camera transform in vision sim - visionSim.adjustCamera(simCam, robotToCamera); - - // Test field poses - Pose2d[] testPoses = - new Pose2d[] { - new Pose2d(1.0, 1.0, new Rotation2d(0)), - new Pose2d(3.0, 1.0, new Rotation2d(Math.PI / 2)), - new Pose2d(5.0, 3.0, new Rotation2d(Math.PI)), - new Pose2d(2.0, 4.0, new Rotation2d(-Math.PI / 2)), - }; - - double totalScore = 0.0; - double latencyMillis = 0.0; - - for (Pose2d pose : testPoses) { - - driveSim.resetPose(pose); - - for (int i = 0; i < 5; i++) { - driveSim.simulationPeriodic(); - visionSim.update(driveSim.getPose()); - Timer.delay(Constants.loopPeriodSecs); - } - - var maybeCamPose3d = visionSim.getCameraPose(simCam); - if (maybeCamPose3d.isEmpty()) { - continue; - } - Pose3d camFieldPose = maybeCamPose3d.get(); - - List allTargets = (List) visionSim.getVisionTargets(); - - PhotonPipelineResult result = simCam.process(latencyMillis, camFieldPose, allTargets); - - int tagsSeen = result.targets.size(); - - Pose2d estimatedPose = driveSim.getPose(); - double poseError = pose.getTranslation().getDistance(estimatedPose.getTranslation()); - - double score = tagsSeen - poseError; - totalScore += score; - } - - return totalScore / testPoses.length; - } -} diff --git a/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java b/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java index 5cb532d..05a0eb1 100644 --- a/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java +++ b/src/main/java/frc/robot/subsystems/vision/CameraSweepEvaluator.java @@ -17,9 +17,15 @@ package frc.robot.subsystems.vision; +import static frc.robot.Constants.Cameras.robotToCamera0; +import static frc.robot.Constants.Cameras.robotToCamera1; + import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Quaternion; 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.util.Units; import java.io.FileWriter; import java.io.IOException; import java.util.ArrayList; @@ -45,6 +51,62 @@ public CameraSweepEvaluator( this.camSim2 = camSim2; } + private static Quaternion quatFromAxisAngle(double ax, double ay, double az, double angleRad) { + double half = 0.5 * angleRad; + double s = Math.sin(half); + double c = Math.cos(half); + + // normalize axis defensively + double n = Math.sqrt(ax * ax + ay * ay + az * az); + if (n < 1e-12) { + return new Quaternion(1.0, 0.0, 0.0, 0.0); + } + ax /= n; + ay /= n; + az /= n; + + return new Quaternion(c, ax * s, ay * s, az * s); + } + + // Hamilton product: q = a ⊗ b + private static Quaternion quatMul(Quaternion a, Quaternion b) { + double aw = a.getW(), ax = a.getX(), ay = a.getY(), az = a.getZ(); + double bw = b.getW(), bx = b.getX(), by = b.getY(), bz = b.getZ(); + + double w = aw * bw - ax * bx - ay * by - az * bz; + double x = aw * bx + ax * bw + ay * bz - az * by; + double y = aw * by - ax * bz + ay * bw + az * bx; + double z = aw * bz + ax * by - ay * bx + az * bw; + + // normalize to keep drift down + double n = Math.sqrt(w * w + x * x + y * y + z * z); + if (n < 1e-12) return new Quaternion(1.0, 0.0, 0.0, 0.0); + return new Quaternion(w / n, x / n, y / n, z / n); + } + + /** + * Compose base rotation with an "extra" camera yaw/pitch. + * + *

Conventions: yawDeg = rotation about +Z (up) pitchDeg = rotation about +Y + * + *

Order: extra = yaw ⊗ pitch (yaw first, then pitch, both in the camera/base frame) combined = + * base ⊗ extra + */ + private static Rotation3d composeCameraExtra(Rotation3d base, double yawDeg, double pitchDeg) { + double yaw = Units.degreesToRadians(yawDeg); + double pitch = Units.degreesToRadians(pitchDeg); + + Quaternion qBase = base.getQuaternion(); + + Quaternion qYaw = quatFromAxisAngle(0.0, 0.0, 1.0, yaw); + Quaternion qPitch = quatFromAxisAngle(0.0, 1.0, 0.0, pitch); + + Quaternion qExtra = quatMul(qYaw, qPitch); + Quaternion qCombined = quatMul(qBase, qExtra); + + return new Rotation3d(qCombined); + } + /** * Run a full sweep of candidate camera placements. * @@ -52,66 +114,120 @@ public CameraSweepEvaluator( */ public void runFullSweep(String outputCsvPath) throws IOException { // Example field bounds (meters) -- tune these for your field size - double[] fieldX = {0.0, 2.0, 4.0, 6.0, 8.0}; // 0–8m along field length - double[] fieldY = {0.0, 1.0, 2.0, 3.0, 4.0}; // 0–4m along field width + double[] fieldX = { + 0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5, 5.0, 5.5, 6.0, 6.5, 7.0, 7.5, 8.0 + }; + double[] fieldY = {0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 3.5, 4.0, 4.5, 5.0, 5.5, 6.0, 6.5, 7.0, 7.5}; double[] robotZ = {0.0}; // Usually floor height - double[] camYaw = {-30, 0, 30}; // Camera yaw - double[] camPitch = {-10, 0, 10}; // Camera pitch + + // NEW: Sweep robot yaw at each location (degrees) + double[] robotYawDeg = {-180, -135, -90, -45, 0, 45, 90, 135}; + + // Camera “extra” rotation sweep (applied on top of the mount rotation) + double[] camYawDeg = {-15, 0, 15}; + double[] camPitchDeg = {-10, 0, 10}; + + // Pull fixed mount transforms from Constants once (and log/write them) + final Transform3d rToC0 = robotToCamera0; + final Transform3d rToC1 = robotToCamera1; + + final Translation3d c0T = rToC0.getTranslation(); + final Translation3d c1T = rToC1.getTranslation(); + final Quaternion c0Q = rToC0.getRotation().getQuaternion(); + final Quaternion c1Q = rToC1.getRotation().getQuaternion(); try (FileWriter writer = new FileWriter(outputCsvPath)) { - writer.write("robotX,robotY,robotZ," + "cam1Yaw,cam1Pitch,cam2Yaw,cam2Pitch,score\n"); + // Header includes robot yaw and BOTH robot->camera transforms (translation + quaternion) + writer.write( + "robotX,robotY,robotZ,robotYawDeg," + + "cam1YawDeg,cam1PitchDeg,cam2YawDeg,cam2PitchDeg,score," + + "rToC0_tx,rToC0_ty,rToC0_tz,rToC0_qw,rToC0_qx,rToC0_qy,rToC0_qz," + + "rToC1_tx,rToC1_ty,rToC1_tz,rToC1_qw,rToC1_qx,rToC1_qy,rToC1_qz\n"); // Sweep robot over the field for (double rx : fieldX) { for (double ry : fieldY) { for (double rz : robotZ) { - Pose3d robotPose = - new Pose3d( - new Translation3d(rx, ry, rz), - new Rotation3d(0, 0, 0) // robot heading = 0 for simplicity - ); - - // Sweep camera rotations - for (double c1Yaw : camYaw) { - for (double c1Pitch : camPitch) { - Pose3d cam1Pose = - new Pose3d( - robotPose.getTranslation(), - new Rotation3d(Math.toRadians(c1Pitch), 0, Math.toRadians(c1Yaw))); - - for (double c2Yaw : camYaw) { - for (double c2Pitch : camPitch) { - Pose3d cam2Pose = - new Pose3d( - robotPose.getTranslation(), - new Rotation3d(Math.toRadians(c2Pitch), 0, Math.toRadians(c2Yaw))); - - // Get all vision targets - Set simTargets = visionSim.getVisionTargets(); - List targetList = new ArrayList<>(simTargets); - - // Simulate camera processing - PhotonPipelineResult res1 = camSim1.process(0, cam1Pose, targetList); - PhotonPipelineResult res2 = camSim2.process(0, cam2Pose, targetList); - - // Score - double score = res1.getTargets().size() + res2.getTargets().size(); - if (res1.getTargets().size() >= 2) score += 2.0; - if (res2.getTargets().size() >= 2) score += 2.0; - - // Penalize ambiguity safely with loops - for (var t : res1.getTargets()) score -= t.getPoseAmbiguity() * 2.0; - for (var t : res2.getTargets()) score -= t.getPoseAmbiguity() * 2.0; - - // Write CSV row - writer.write( - String.format( - "%.2f,%.2f,%.2f,%.1f,%.1f,%.1f,%.1f,%.1f\n", - rx, ry, rz, c1Yaw, c1Pitch, c2Yaw, c2Pitch, score)); - - Logger.recordOutput("CameraSweep/Score", score); - Logger.recordOutput("CameraSweep/Cam1Pose", cam1Pose); - Logger.recordOutput("CameraSweep/Cam2Pose", cam2Pose); + + // NEW: Sweep robot yaw at each location + for (double rYaw : robotYawDeg) { + Pose3d robotPose = + new Pose3d( + new Translation3d(rx, ry, rz), + new Rotation3d(0.0, 0.0, Units.degreesToRadians(rYaw))); + + // Base camera poses from mount transforms (these rotate with robot yaw) + Pose3d cam1BasePose = robotPose.transformBy(rToC0); + Pose3d cam2BasePose = robotPose.transformBy(rToC1); + + // Sweep camera "extra" rotations + for (double c1Yaw : camYawDeg) { + for (double c1Pitch : camPitchDeg) { + + Rotation3d cam1Rot = + composeCameraExtra(cam1BasePose.getRotation(), c1Yaw, c1Pitch); + Pose3d cam1Pose = new Pose3d(cam1BasePose.getTranslation(), cam1Rot); + + for (double c2Yaw : camYawDeg) { + for (double c2Pitch : camPitchDeg) { + + Rotation3d cam2Rot = + composeCameraExtra(cam2BasePose.getRotation(), c2Yaw, c2Pitch); + Pose3d cam2Pose = new Pose3d(cam2BasePose.getTranslation(), cam2Rot); + + // Get all vision targets + Set simTargets = visionSim.getVisionTargets(); + List targetList = new ArrayList<>(simTargets); + + // Simulate camera processing + PhotonPipelineResult res1 = camSim1.process(0, cam1Pose, targetList); + PhotonPipelineResult res2 = camSim2.process(0, cam2Pose, targetList); + + // Score + double score = res1.getTargets().size() + res2.getTargets().size(); + if (res1.getTargets().size() >= 2) score += 2.0; + if (res2.getTargets().size() >= 2) score += 2.0; + + // Penalize ambiguity safely with loops + for (var t : res1.getTargets()) score -= t.getPoseAmbiguity() * 2.0; + for (var t : res2.getTargets()) score -= t.getPoseAmbiguity() * 2.0; + + // Write CSV row (note: we repeat mount transforms each row for + // “self-contained” CSV) + writer.write( + String.format( + "%.2f,%.2f,%.2f,%.1f,%.1f,%.1f,%.1f,%.1f,%.3f," + + "%.6f,%.6f,%.6f,%.16g,%.16g,%.16g,%.16g," + + "%.6f,%.6f,%.6f,%.16g,%.16g,%.16g,%.16g\n", + rx, + ry, + rz, + rYaw, + c1Yaw, + c1Pitch, + c2Yaw, + c2Pitch, + score, + c0T.getX(), + c0T.getY(), + c0T.getZ(), + c0Q.getW(), + c0Q.getX(), + c0Q.getY(), + c0Q.getZ(), + c1T.getX(), + c1T.getY(), + c1T.getZ(), + c1Q.getW(), + c1Q.getX(), + c1Q.getY(), + c1Q.getZ())); + + Logger.recordOutput("CameraSweep/Score", score); + Logger.recordOutput("CameraSweep/RobotPose", robotPose); + Logger.recordOutput("CameraSweep/Cam1Pose", cam1Pose); + Logger.recordOutput("CameraSweep/Cam2Pose", cam2Pose); + } } } }