Upgrade to wpilib alpha-6 (#2434)

Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Ryanforce08 <rradtke1208@gmail.com>
Co-authored-by: PJ Reiniger <pj.reiniger@gmail.com>
Co-authored-by: Jade Turner <spacey-sooty@proton.me>
Co-authored-by: Matt Morley <matthew.morley.ca@gmail.com>

photon-lib/py/photonlibpy/estimation/cameraTargetRelation.py

@@ -1,6 +1,6 @@
 import math
 
-from wpimath.geometry import Pose3d, Rotation2d, Transform3d
+from wpimath import Pose3d, Rotation2d, Transform3d
 from wpimath.units import meters
 
 

photon-lib/py/photonlibpy/estimation/openCVHelp.py

@@ -4,7 +4,7 @@ from typing import Any
 
 import cv2 as cv
 import numpy as np
-from wpimath.geometry import Rotation3d, Transform3d, Translation3d
+from wpimath import Rotation3d, Transform3d, Translation3d
 
 from ..targeting import PnpResult, TargetCorner
 from .rotTrlTransform3d import RotTrlTransform3d
@@ -26,7 +26,7 @@ class OpenCVHelp:
 
     @staticmethod
     def rotationNWUtoEDN(rot: Rotation3d) -> Rotation3d:
-        return -NWU_TO_EDN + (rot + NWU_TO_EDN)
+        return NWU_TO_EDN.inverse().rotateBy(rot.rotateBy(NWU_TO_EDN))
 
     @staticmethod
     def translationToTVec(translations: list[Translation3d]) -> np.ndarray:
@@ -147,7 +147,7 @@ class OpenCVHelp:
         in NWU, this would be {0, 0, 1} in EDN.
         """
 
-        return -EDN_TO_NWU + (rot + EDN_TO_NWU)
+        return EDN_TO_NWU.inverse().rotateBy(rot.rotateBy(EDN_TO_NWU))
 
     @staticmethod
     def tVecToTranslation(tvecInput: np.ndarray) -> Translation3d:

photon-lib/py/photonlibpy/estimation/rotTrlTransform3d.py

@@ -1,6 +1,6 @@
 from typing import Self
 
-from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
+from wpimath import Pose3d, Rotation3d, Transform3d, Translation3d
 
 
 class RotTrlTransform3d:
@@ -22,7 +22,7 @@ class RotTrlTransform3d:
 
     def inverse(self) -> Self:
         """The inverse of this transformation. Applying the inverse will "undo" this transformation."""
-        invRot = -self.rot
+        invRot = self.rot.inverse()
         invTrl = -(self.trl.rotateBy(invRot))
         return type(self)(invRot, invTrl)
 
@@ -42,7 +42,7 @@ class RotTrlTransform3d:
         return trlToApply.rotateBy(self.rot) + self.trl
 
     def applyRotation(self, rotToApply: Rotation3d) -> Rotation3d:
-        return rotToApply + self.rot
+        return rotToApply.rotateBy(self.rot)
 
     def applyPose(self, poseToApply: Pose3d) -> Pose3d:
         return Pose3d(
@@ -68,7 +68,9 @@ class RotTrlTransform3d:
     @classmethod
     def makeBetweenPoses(cls, initial: Pose3d, last: Pose3d) -> Self:
         return cls(
-            last.rotation() - initial.rotation(),
+            last.rotation().relativeTo(initial.rotation()),
             last.translation()
-            - initial.translation().rotateBy(last.rotation() - initial.rotation()),
+            - initial.translation().rotateBy(
+                last.rotation().relativeTo(initial.rotation())
+            ),
         )

photon-lib/py/photonlibpy/estimation/targetModel.py

@@ -1,7 +1,7 @@
 import math
 from typing import List, Self
 
-from wpimath.geometry import Pose3d, Rotation2d, Rotation3d, Translation3d
+from wpimath import Pose3d, Rotation2d, Rotation3d, Translation3d
 from wpimath.units import meters
 
 from . import RotTrlTransform3d

photon-lib/py/photonlibpy/estimation/visionEstimation.py

@@ -1,6 +1,6 @@
 import numpy as np
 from robotpy_apriltag import AprilTag, AprilTagFieldLayout
-from wpimath.geometry import Pose3d, Transform3d, Translation3d
+from wpimath import Pose3d, Transform3d, Translation3d
 
 from ..targeting import PhotonTrackedTarget, PnpResult, TargetCorner
 from . import OpenCVHelp, TargetModel