3D Pose Estimator

src/main/java/frc/trigon/robot/commands/CommandConstants.java

@@ -52,7 +52,7 @@ public class CommandConstants {
                     RobotContainer.SWERVE
             ),
             CALCULATE_CAMERA_POSITION_COMMAND = new CameraPositionCalculationCommand(
-                    RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimatedRobotPose,
+                    RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimated2DRobotPose,
                     Rotation2d.fromDegrees(0),
                     (omegaRadiansPerSecond) -> RobotContainer.SWERVE.selfRelativeDrive(new ChassisSpeeds(0, 0, omegaRadiansPerSecond)),
                     RobotContainer.SWERVE

src/main/java/frc/trigon/robot/commands/commandclasses/GamePieceAutoDriveCommand.java

@@ -36,7 +36,7 @@ private Command getTrackGamePieceCommand() {
     }
 
     public static Translation2d calculateDistanceFromTrackedGamePiece() {
-        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose();
         final Translation2d trackedObjectPositionOnField = OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot();
         if (trackedObjectPositionOnField == null)
             return null;
@@ -65,7 +65,7 @@ public static boolean shouldMoveTowardsGamePiece(Translation2d distanceFromTrack
     }
 
     public static FlippableRotation2d calculateTargetAngle() {
-        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose();
         final Translation2d trackedObjectFieldRelativePosition = OBJECT_POSE_ESTIMATOR.getClosestObjectToRobot();
         if (trackedObjectFieldRelativePosition == null)
             return null;

src/main/java/frc/trigon/robot/commands/commandclasses/IntakeAssistCommand.java

@@ -112,7 +112,7 @@ private void trackGamePiece() {
     }
 
     private Translation2d calculateSelfRelativeDistanceFromBestGamePiece() {
-        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose();
         final Translation2d bestGamePieceFieldRelativePosition = getBestGamePieceFieldRelativePosition(robotPose);
 
         if (bestGamePieceFieldRelativePosition == null)

src/main/java/frc/trigon/robot/commands/commandclasses/LEDAutoSetupCommand.java

@@ -50,7 +50,7 @@ private Command getUpdateAutoStartPoseCommand() {
     }
 
     private Translation2d calculateRobotRelativeDifference() {
-        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose();
         final Translation2d robotRelativeRobotTranslation = robotPose.getTranslation().minus(this.autoStartPose.getTranslation());
         return robotRelativeRobotTranslation.rotateBy(robotPose.getRotation());
     }

src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java

@@ -2,6 +2,7 @@
 
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.*;
 import frc.trigon.lib.utilities.flippable.FlippablePose2d;
@@ -124,7 +125,7 @@ public static Command getResetPoseToAutoPoseCommand(Supplier<String> autoName) {
                 () -> {
                     if (DriverStation.isEnabled())
                         return;
-                    RobotContainer.ROBOT_POSE_ESTIMATOR.resetPose(getAutoStartPose(autoName.get()));
+                    RobotContainer.ROBOT_POSE_ESTIMATOR.resetPose(new Pose3d(getAutoStartPose(autoName.get())));
                 }
         ).ignoringDisable(true);
     }

src/main/java/frc/trigon/robot/constants/AutonomousConstants.java

@@ -9,6 +9,7 @@
 import com.pathplanner.lib.pathfinding.Pathfinding;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -63,8 +64,8 @@ public static void init() {
 
     private static void configureAutoBuilder() {
         AutoBuilder.configure(
-                RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimatedRobotPose,
-                RobotContainer.ROBOT_POSE_ESTIMATOR::resetPose,
+                RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimated2DRobotPose,
+                (resetPose2d) -> RobotContainer.ROBOT_POSE_ESTIMATOR.resetPose(new Pose3d(resetPose2d)),
                 RobotContainer.SWERVE::getSelfRelativeChassisSpeeds,
                 RobotContainer.SWERVE::drivePathPlanner,
                 AUTO_PATH_FOLLOWING_CONTROLLER,

src/main/java/frc/trigon/robot/misc/objectdetection/ObjectPoseEstimator.java

@@ -76,7 +76,7 @@ public void removeClosestObjectToRobot() {
      * @param intakeTransform the transform of the intake relative to the robot
      */
     public void removeClosestObjectToIntake(Transform2d intakeTransform) {
-        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose();
         final Translation2d closestObjectToIntake = getClosestTrackedObjectToPosition(robotPose.transformBy(intakeTransform).getTranslation());
         if (closestObjectToIntake == null)
             return;
@@ -129,7 +129,7 @@ public boolean hasObjects() {
      * @return the closest object to the robot
      */
     public Translation2d getClosestObjectToRobot() {
-        return getClosestTrackedObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation());
+        return getClosestTrackedObjectToPosition(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose().getTranslation());
     }
 
     private void updateTrackedObjectsPositions() {

src/main/java/frc/trigon/robot/misc/objectdetection/objectdetectioncamera/ObjectDetectionCamera.java

@@ -40,7 +40,7 @@ public void periodic() {
      */
     public Translation2d calculateClosestObjectPositionOnField(SimulatedGamePieceConstants.GamePieceType targetGamePiece) {
         final Translation2d[] targetObjectsTranslation = getObjectPositionsOnField(targetGamePiece);
-        final Translation2d currentRobotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation();
+        final Translation2d currentRobotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimated2DRobotPose().getTranslation();
         if (targetObjectsTranslation.length == 0)
             return null;
         Translation2d closestObjectTranslation = targetObjectsTranslation[0];
@@ -85,7 +85,7 @@ public Rotation3d[] getObjectsRotations(SimulatedGamePieceConstants.GamePieceTyp
      * @return the object's 2D position on the field (z is assumed to be 0)
      */
     private Translation2d calculateObjectPositionFromRotation(Rotation3d objectRotation) {
-        final Pose2d robotPoseAtResultTimestamp = RobotContainer.ROBOT_POSE_ESTIMATOR.samplePoseAtTimestamp(objectDetectionCameraInputs.latestResultTimestamp);
+        final Pose2d robotPoseAtResultTimestamp = RobotContainer.ROBOT_POSE_ESTIMATOR.sample2DPoseAtTimestamp(objectDetectionCameraInputs.latestResultTimestamp);
         if (robotPoseAtResultTimestamp == null)
             return new Translation2d();
         final Pose3d cameraPose = new Pose3d(robotPoseAtResultTimestamp).plus(robotCenterToCamera);

src/main/java/frc/trigon/robot/misc/objectdetection/objectdetectioncamera/io/SimulationObjectDetectionCameraIO.java

@@ -28,8 +28,8 @@ public SimulationObjectDetectionCameraIO(String hostname, Transform3d robotCente
 
     @Override
     protected void updateInputs(ObjectDetectionCameraInputsAutoLogged inputs) {
-        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
-        final Pose3d cameraPose = new Pose3d(robotPose).plus(robotCenterToCamera);
+        final Pose3d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+        final Pose3d cameraPose = robotPose.plus(robotCenterToCamera);
         final ArrayList<Pair<SimulatedGamePiece, Rotation3d>>[] visibleGamePieces = calculateAllVisibleGamePieces(cameraPose);
 
         boolean hasAnyObject = false;

src/main/java/frc/trigon/robot/misc/shootingphysics/ShootingCalculations.java

@@ -32,22 +32,22 @@ public ShootingState getTargetShootingState() {
 
     @AutoLogOutput(key = "Shooting/CurrentFuelExitPosition")
     public Translation3d calculateCurrentFuelExitPose() {
-        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+        final Pose3d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
         final Rotation2d hoodPitch = RobotContainer.HOOD.getCurrentAngle();
         final Rotation2d turretSelfRelativeYaw = RobotContainer.TURRET.getCurrentSelfRelativeAngle();
         return calculateFieldRelativeFuelExitPose(robotPose, hoodPitch, turretSelfRelativeYaw);
     }
 
     public Translation3d calculateTargetFuelExitPosition(double posePredictionTimeSeconds) {
-        final Pose2d predictedRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getPredictedRobotPose(posePredictionTimeSeconds);
+        final Pose3d predictedRobotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getPredictedRobotPose(posePredictionTimeSeconds);
         final Rotation2d hoodPitch = RobotContainer.HOOD.getTargetAngle();
         final Rotation2d turretSelfRelativeYaw = RobotContainer.TURRET.getTargetSelfRelativeAngle();
         return calculateFieldRelativeFuelExitPose(predictedRobotPose, hoodPitch, turretSelfRelativeYaw);
     }
 
-    public Translation3d calculateFieldRelativeFuelExitPose(Pose2d robotPose, Rotation2d hoodPitch, Rotation2d turretSelfRelativeYaw) {
+    public Translation3d calculateFieldRelativeFuelExitPose(Pose3d robotPose, Rotation2d hoodPitch, Rotation2d turretSelfRelativeYaw) {
         final Transform3d robotToFuelExitPosition = calculateRobotToFuelExitTransform(hoodPitch, turretSelfRelativeYaw);
-        return new Pose3d(robotPose).transformBy(robotToFuelExitPosition).getTranslation();
+        return robotPose.transformBy(robotToFuelExitPosition).getTranslation();
     }
 
     private Transform3d calculateRobotToFuelExitTransform(Rotation2d hoodPitch, Rotation2d turretSelfRelativeYaw) {

src/main/java/frc/trigon/robot/misc/simulatedfield/SimulatedGamePiece.java

@@ -93,14 +93,14 @@ private boolean isHeld() {
     }
 
     private Rotation2d calculateClosestSpindexerRotationFromCurrentPose() {
-        final Pose2d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
+        final Pose3d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
         final Pose3d spindexerRobotRelativePose = RobotContainer.SPINDEXER.calculateComponentPose();
         final Transform3d spindexerRobotRelativeTransform = new Transform3d(
                 new Pose3d(),
                 spindexerRobotRelativePose
         );
 
-        final double yOffset = new Pose3d(fieldRelativePosition, new Rotation3d()).relativeTo(new Pose3d(robotPose).plus(spindexerRobotRelativeTransform)).getY();
+        final double yOffset = new Pose3d(fieldRelativePosition, new Rotation3d()).relativeTo(robotPose.plus(spindexerRobotRelativeTransform)).getY();
         final double fuelTargetOffsetFromSpindexer = SimulatedGamePieceConstants.ROBOT_RELATIVE_HELD_FUEL_OFFSET_FROM_SPINDEXER_METERS.toTranslation2d().getNorm();
 
         final double closestSpindexerRotationRadians = Math.asin(MathUtil.clamp(yOffset / fuelTargetOffsetFromSpindexer, -1, 1));

src/main/java/frc/trigon/robot/misc/simulatedfield/SimulationFieldHandler.java

@@ -140,7 +140,7 @@ private static Translation3d calculateHeldFuelFieldRelativePosition(Rotation2d h
      * @return the field relative pose
      */
     private static Translation3d robotRelativeToFieldRelative(Translation3d robotRelativePose) {
-        final Pose3d robotPose = new Pose3d(RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose());
+        final Pose3d robotPose = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose();
         return robotPose.plus(new Transform3d(robotRelativePose, new Rotation3d())).getTranslation();
     }
 }

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java

@@ -15,20 +15,18 @@
 public class AprilTagCamera {
     protected final String name;
     private final AprilTagCameraInputsAutoLogged inputs = new AprilTagCameraInputsAutoLogged();
-    private final Transform2d cameraToRobotCenter;
+    private final Transform3d cameraToRobotCenter;
     private final StandardDeviations standardDeviations;
     private final AprilTagCameraIO aprilTagCameraIO;
-    private Pose2d estimatedRobotPose = new Pose2d();
+    private Pose3d estimatedRobotPose = new Pose3d();
 
     /**
      * Constructs a new AprilTagCamera.
      *
      * @param aprilTagCameraType  the type of camera
      * @param name                the camera's name
      * @param robotCenterToCamera the transform of the robot's origin point to the camera.
-     *                            only the x, y and yaw values will be used for transforming the camera pose to the robot's center,
-     *                            to avoid more inaccuracies like pitch and roll.
-     *                            The reset will be used for creating a camera in simulation
+     *                            The values will be used for creating a camera in simulation
      * @param standardDeviations  the initial calibrated standard deviations for the camera's estimated pose,
      *                            will be changed as the distance from the tag(s) changes and the number of tags changes
      */
@@ -37,7 +35,7 @@ public AprilTagCamera(AprilTagCameraConstants.AprilTagCameraType aprilTagCameraT
                           StandardDeviations standardDeviations) {
         this.name = name;
         this.standardDeviations = standardDeviations;
-        this.cameraToRobotCenter = toTransform2d(robotCenterToCamera).inverse();
+        this.cameraToRobotCenter = robotCenterToCamera.inverse();
 
         aprilTagCameraIO = AprilTagCameraIO.generateIO(aprilTagCameraType, name, robotCenterToCamera);
     }
@@ -50,7 +48,7 @@ public void update() {
         logCameraInfo();
     }
 
-    public Pose2d getEstimatedRobotPose() {
+    public Pose3d getEstimatedRobotPose() {
         return estimatedRobotPose;
     }
 
@@ -95,38 +93,38 @@ public StandardDeviations calculateStandardDeviations() {
         return new StandardDeviations(translationStandardDeviation, thetaStandardDeviation);
     }
 
-    private Pose2d calculateRobotPose() {
+    private Pose3d calculateRobotPose() {
         if (!hasValidResult())
             return null;
 
         return chooseBestRobotPose();
     }
 
-    private Pose2d chooseBestRobotPose() {
+    private Pose3d chooseBestRobotPose() {
         if (!inputs.hasConstrainedResult || isWithinBestTagRangeForAccurateSolvePNPResult())
             return chooseBestNormalSolvePNPPose();
 
-        return cameraPoseToRobotPose(inputs.constrainedSolvePNPPose.toPose2d());
+        return cameraPoseToRobotPose(inputs.constrainedSolvePNPPose);
     }
 
-    private Pose2d chooseBestNormalSolvePNPPose() {
-        final Pose2d bestPose = cameraPoseToRobotPose(inputs.bestCameraSolvePNPPose.toPose2d());
+    private Pose3d chooseBestNormalSolvePNPPose() {
+        final Pose3d bestPose = cameraPoseToRobotPose(inputs.bestCameraSolvePNPPose);
 
         if (inputs.bestCameraSolvePNPPose.equals(inputs.alternateCameraSolvePNPPose))
             return bestPose;
         if (inputs.alternateCameraSolvePNPPose.getTranslation().toTranslation2d().getDistance(FieldConstants.TAG_ID_TO_POSE.get(inputs.visibleTagIDs[0]).getTranslation().toTranslation2d()) < 0.1 || DriverStation.isDisabled())
             return bestPose;
 
-        final Pose2d alternatePose = cameraPoseToRobotPose(inputs.alternateCameraSolvePNPPose.toPose2d());
-        final Rotation2d robotAngleAtResultTime = RobotContainer.ROBOT_POSE_ESTIMATOR.samplePoseAtTimestamp(inputs.latestResultTimestampSeconds).getRotation();
+        final Pose3d alternatePose = cameraPoseToRobotPose(inputs.alternateCameraSolvePNPPose);
+        final Rotation2d robotAngleAtResultTime = RobotContainer.ROBOT_POSE_ESTIMATOR.sample2DPoseAtTimestamp(inputs.latestResultTimestampSeconds).getRotation();
 
-        final double bestAngleDifference = Math.abs(bestPose.getRotation().minus(robotAngleAtResultTime).getRadians());
-        final double alternateAngleDifference = Math.abs(alternatePose.getRotation().minus(robotAngleAtResultTime).getRadians());
+        final double bestAngleDifference = Math.abs(bestPose.toPose2d().getRotation().minus(robotAngleAtResultTime).getRadians());
+        final double alternateAngleDifference = Math.abs(alternatePose.toPose2d().getRotation().minus(robotAngleAtResultTime).getRadians());
 
         return bestAngleDifference > alternateAngleDifference ? alternatePose : bestPose;
     }
 
-    private Pose2d cameraPoseToRobotPose(Pose2d cameraPose) {
+    private Pose3d cameraPoseToRobotPose(Pose3d cameraPose) {
         return cameraPose.transformBy(cameraToRobotCenter);
     }
 
@@ -148,7 +146,7 @@ private void logCameraInfo() {
             logUsedTags();
 
         if (hasValidResult()) {
-            Logger.recordOutput("Poses/Robot/Cameras/" + name + "Pose", new Pose2d[]{estimatedRobotPose});
+            Logger.recordOutput("Poses/Robot/Cameras/" + name + "Pose", new Pose3d[]{estimatedRobotPose});
             return;
         }
 

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCameraConstants.java

@@ -1,24 +1,24 @@
 package frc.trigon.robot.poseestimation.apriltagcamera;
 
-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.geometry.Transform3d;
+import frc.trigon.lib.hardware.RobotHardwareStats;
 import frc.trigon.robot.constants.FieldConstants;
 import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagLimelightIO;
 import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagPhotonCameraIO;
 import frc.trigon.robot.poseestimation.apriltagcamera.io.AprilTagSimulationCameraIO;
 import org.photonvision.PhotonPoseEstimator;
 import org.photonvision.simulation.SimCameraProperties;
 import org.photonvision.simulation.VisionSystemSim;
-import frc.trigon.lib.hardware.RobotHardwareStats;
 
 import java.util.function.BiFunction;
 
 public class AprilTagCameraConstants {
     static final double
             MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_RESULT_METERS = 5,
             MAXIMUM_DISTANCE_FROM_TAG_FOR_ACCURATE_SOLVE_PNP_RESULT_METERS = 2;
-    static final Pose2d[] EMPTY_POSE_ARRAY = new Pose2d[0];
+    static final Pose3d[] EMPTY_POSE_ARRAY = new Pose3d[0];
     static final double MAXIMUM_AMBIGUITY = 0.4;
     public static final PhotonPoseEstimator.ConstrainedSolvepnpParams CONSTRAINED_SOLVE_PNP_PARAMS = new PhotonPoseEstimator.ConstrainedSolvepnpParams(false, 0.1);
 

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/io/AprilTagPhotonCameraIO.java

@@ -139,12 +139,13 @@ private Pose3d calculateConstrainedSolvePNPPose(PhotonPipelineResult result, Pos
             return null;
 
         Pose3d fieldToRobotSeed = bestCameraSolvePNPPose.transformBy(this.robotToCamera.inverse());
-        final Rotation2d robotYawAtTimestamp = RobotContainer.ROBOT_POSE_ESTIMATOR.samplePoseAtTimestamp(result.getTimestampSeconds()).getRotation();
+        final Rotation2d robotYawAtTimestamp = RobotContainer.ROBOT_POSE_ESTIMATOR.sample2DPoseAtTimestamp(result.getTimestampSeconds()).getRotation();
+        final Rotation3d robotYawAtTimestampRotation = new Rotation3d(robotYawAtTimestamp);
 
         if (!AprilTagCameraConstants.CONSTRAINED_SOLVE_PNP_PARAMS.headingFree()) {
             fieldToRobotSeed = new Pose3d(
                     fieldToRobotSeed.getTranslation(),
-                    new Rotation3d(robotYawAtTimestamp)
+                    robotYawAtTimestampRotation
             );
         }