3D Pose Estimator

[Strflightmight09]

No description provided.

[coderabbitai]

Warning

Rate limit exceeded

@Strflightmight09 has exceeded the limit for the number of commits that can be reviewed per hour. Please wait 18 minutes and 46 seconds before requesting another review.

⌛ How to resolve this issue?

After the wait time has elapsed, a review can be triggered using the @coderabbitai review command as a PR comment. Alternatively, push new commits to this PR.

We recommend that you space out your commits to avoid hitting the rate limit.

🚦 How do rate limits work?

CodeRabbit enforces hourly rate limits for each developer per organization.

Our paid plans have higher rate limits than the trial, open-source and free plans. In all cases, we re-allow further reviews after a brief timeout.

Please see our FAQ for further information.

Walkthrough

The PR migrates many pose-related APIs from 2D to 3D types (Pose3d/Transform3d/Rotation3d) across the codebase, updating RobotPoseEstimator, AprilTagCamera, RelativeRobotPoseSource, and related classes. RobotPoseEstimator now exposes Pose3d and provides 2D helpers (getEstimated2DRobotPose, sample2DPoseAtTimestamp). Call sites were adjusted to use 3D poses or the new 2D helpers. Gyro handling was extended to include roll and pitch. A submodule reference under src/main/java/frc/trigon/lib was updated to a new commit hash.

🚥 Pre-merge checks | ✅ 1 | ❌ 2

❌ Failed checks (2 warnings)

Check name	Status	Explanation	Resolution
Description check	⚠️ Warning	No description was provided by the author, making it impossible to assess relevance to the changeset.	Add a description explaining the motivation for the 3D pose estimator migration, affected subsystems, and any breaking API changes.
Docstring Coverage	⚠️ Warning	Docstring coverage is 23.26% which is insufficient. The required threshold is 80.00%.	Write docstrings for the functions missing them to satisfy the coverage threshold.

✅ Passed checks (1 passed)

Check name	Status	Explanation
Title check	✅ Passed	The title clearly summarizes the main change: upgrading the pose estimator from 2D to 3D geometry types throughout the codebase.

_{✏️ Tip: You can configure your own custom pre-merge checks in the settings.}

---

Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out.

❤️ Share

• X
• Mastodon
• Reddit
• LinkedIn

_{Comment @coderabbitai help to get the list of available commands and usage tips.}

[coderabbitai]

Actionable comments posted: 7

Caution

Some comments are outside the diff and can’t be posted inline due to platform limitations.

⚠️ Outside diff range comments (2)

src/main/java/frc/trigon/robot/subsystems/turret/Turret.java (1)

82-85: Reset turret target on stop to avoid stale commands.

stop() halts the motor but preserves targetSelfRelativeAngle, which can re-issue a stale target when re-enabled. Reset the target field (and any related closed-loop state) in stop().

Based on learnings, ensure all subsystem stop() methods reset target fields.

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java (1)

28-41: Flatten camera-to-robot transform to match contract.

The Javadoc (line 29) states "only the x, y and yaw values will be used," but line 40 applies the full 3D inverse, which includes z, roll, and pitch if present in the input. This violates the documented contract. If robotCenterToCamera contains non-zero z/roll/pitch, they will propagate and skew the robot pose estimate.

Flatten to 2D before inversion to enforce the contract:

Proposed fix

-        this.cameraToRobotCenter = robotCenterToCamera.inverse();
+        final Translation3d translation = robotCenterToCamera.getTranslation();
+        final Rotation2d yaw = robotCenterToCamera.getRotation().toRotation2d();
+        final Transform3d flattened = new Transform3d(
+                new Translation3d(translation.getX(), translation.getY(), 0.0),
+                new Rotation3d(0.0, 0.0, yaw.getRadians())
+        );
+        this.cameraToRobotCenter = flattened.inverse();

♻️ Duplicate comments (1)

src/main/java/frc/trigon/robot/commands/commandfactories/AutonomousCommands.java (1)

129-129: Same 3D reset concern as in AutonomousConstants

[coderabbitai]

Actionable comments posted: 1

Caution

Some comments are outside the diff and can’t be posted inline due to platform limitations.

⚠️ Outside diff range comments (1)

src/main/java/frc/trigon/robot/subsystems/turret/Turret.java (1)

83-85: Reset target state in stop().

Line 83 stops the motor but leaves targetSelfRelativeAngle intact, so a subsequent enable can jump to a stale target. Reset the target to 0 in stop(). Based on learnings, subsystem stop methods must clear target fields.

Proposed fix

     public void stop() {
         masterMotor.stopMotor();
+        targetSelfRelativeAngle = Rotation2d.fromDegrees(0);
     }

Based on learnings, ensure stop resets target fields to 0.

♻️ Duplicate comments (3)

src/main/java/frc/trigon/robot/constants/AutonomousConstants.java (1)

67-69: Resetting with new Pose3d(resetPose2d) drops z/roll/pitch.

Line 68 zeros z/roll/pitch. If the estimator tracks tilt, preserve existing z/roll/pitch when resetting from a 2D pose.

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

142-149: Add a null guard before calling getRotation().

Line 142 can return null when the pose buffer has no sample for that timestamp, which will throw a NullPointerException and abort constrained solve. Guard the sample and return null early.

Proposed fix

-import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
@@
-        final Rotation2d robotYawAtTimestamp = RobotContainer.ROBOT_POSE_ESTIMATOR.sample2DPoseAtTimestamp(result.getTimestampSeconds()).getRotation();
-        final Rotation3d robotYawAtTimestampRotation = new Rotation3d(robotYawAtTimestamp);
+        final Pose2d sampledPose = RobotContainer.ROBOT_POSE_ESTIMATOR.sample2DPoseAtTimestamp(result.getTimestampSeconds());
+        if (sampledPose == null)
+            return null;
+        final Rotation2d robotYawAtTimestamp = sampledPose.getRotation();
+        final Rotation3d robotYawAtTimestampRotation = new Rotation3d(robotYawAtTimestamp);

src/main/java/frc/trigon/robot/poseestimation/apriltagcamera/AprilTagCamera.java (1)

96-99: Avoid returning null from calculateRobotPose().
Returning null propagates into estimatedRobotPose, making getEstimatedRobotPose() nullable and prone to NPEs for callers that do not gate on hasValidResult().

Proposed fix

-        if (!hasValidResult())
-            return null;
+        if (!hasValidResult())
+            return estimatedRobotPose;

[coderabbitai]

Actionable comments posted: 1

♻️ Duplicate comments (2)

src/main/java/frc/trigon/robot/subsystems/swerve/Swerve.java (2)

310-316: Validate all gyro signal array lengths to prevent ArrayIndexOutOfBoundsException.

The code assumes roll, pitch, and yaw arrays have equal length but only checks the first. If any signal buffer has fewer samples, the loop at lines 320-327 will throw.

Suggested fix

         final double[][] odometryUpdatesDegrees = new double[][]{
                 gyro.getThreadedSignal(Pigeon2Signal.ROLL),
                 gyro.getThreadedSignal(Pigeon2Signal.PITCH),
                 gyro.getThreadedSignal(Pigeon2Signal.YAW)
         };
-        final int totalOdometryUpdates = odometryUpdatesDegrees[0].length;
+        final int totalOdometryUpdates = Math.min(
+                Math.min(odometryUpdatesDegrees[0].length, odometryUpdatesDegrees[1].length),
+                odometryUpdatesDegrees[2].length
+        );

---

329-329: Align timestamps with truncated odometry sample count.

If phoenix6SignalThread.getLatestTimestamps() returns more samples than totalOdometryUpdates, updatePoseEstimatorOdometry receives misaligned data. Include timestamps in the min calculation and slice to match.

Suggested fix

+import java.util.Arrays;
...
+        final double[] latestTimestamps = phoenix6SignalThread.getLatestTimestamps();
         final int totalOdometryUpdates = Math.min(
-                Math.min(odometryUpdatesDegrees[0].length, odometryUpdatesDegrees[1].length),
-                odometryUpdatesDegrees[2].length
+                Math.min(
+                        Math.min(odometryUpdatesDegrees[0].length, odometryUpdatesDegrees[1].length),
+                        odometryUpdatesDegrees[2].length
+                ),
+                latestTimestamps.length
         );
...
-        RobotContainer.ROBOT_POSE_ESTIMATOR.updatePoseEstimatorOdometry(swerveWheelPositions, gyroRotations, phoenix6SignalThread.getLatestTimestamps());
+        RobotContainer.ROBOT_POSE_ESTIMATOR.updatePoseEstimatorOdometry(
+                swerveWheelPositions,
+                gyroRotations,
+                Arrays.copyOf(latestTimestamps, totalOdometryUpdates)
+        );