mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
e9006a2803
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>
26 lines
1.1 KiB
Python
26 lines
1.1 KiB
Python
import math
|
|
|
|
from wpimath import Pose3d, Rotation2d, Transform3d
|
|
from wpimath.units import meters
|
|
|
|
|
|
class CameraTargetRelation:
|
|
def __init__(self, cameraPose: Pose3d, targetPose: Pose3d):
|
|
self.camPose = cameraPose
|
|
self.camToTarg = Transform3d(cameraPose, targetPose)
|
|
self.camToTargDist = self.camToTarg.translation().norm()
|
|
self.camToTargDistXY: meters = math.hypot(
|
|
self.camToTarg.translation().X(), self.camToTarg.translation().Y()
|
|
)
|
|
self.camToTargYaw = Rotation2d(self.camToTarg.X(), self.camToTarg.Y())
|
|
self.camToTargPitch = Rotation2d(self.camToTargDistXY, -self.camToTarg.Z())
|
|
self.camToTargAngle = Rotation2d(
|
|
math.hypot(self.camToTargYaw.radians(), self.camToTargPitch.radians())
|
|
)
|
|
self.targToCam = Transform3d(targetPose, cameraPose)
|
|
self.targToCamYaw = Rotation2d(self.targToCam.X(), self.targToCam.Y())
|
|
self.targToCamPitch = Rotation2d(self.camToTargDistXY, -self.targToCam.Z())
|
|
self.targtoCamAngle = Rotation2d(
|
|
math.hypot(self.targToCamYaw.radians(), self.targToCamPitch.radians())
|
|
)
|