Team2890/PhotonVision
4
0
Fork 0
mirror of https://github.com/PhotonVision/photonvision synced 2026-06-23 01:21:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add python simulation (#1532)

Browse Source
This commit is contained in:
Lucien Morey
2024-11-11 09:16:02 +11:00
committed by GitHub
parent b5d48a6503
commit b3d74e56a0
17 changed files with 2407 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

137
photon-lib/py/photonlibpy/estimation/targetModel.py Normal file
View File

@@ -0,0 +1,137 @@
import math
from typing import List, Self
from wpimath.geometry import Pose3d, Rotation2d, Rotation3d, Translation3d
from wpimath.units import meters
from . import RotTrlTransform3d
class TargetModel:
def __init__(
self,
*,
width: meters | None = None,
height: meters | None = None,
length: meters | None = None,
diameter: meters | None = None,
verts: List[Translation3d] | None = None
):
if (
width is not None
and height is not None
and length is None
and diameter is None
and verts is None
):
self.isPlanar = True
self.isSpherical = False
self.vertices = [
Translation3d(0.0, -width / 2.0, -height / 2.0),
Translation3d(0.0, width / 2.0, -height / 2.0),
Translation3d(0.0, width / 2.0, height / 2.0),
Translation3d(0.0, -width / 2.0, height / 2.0),
]
return
elif (
length is not None
and width is not None
and height is not None
and diameter is None
and verts is None
):
verts = [
Translation3d(length / 2.0, -width / 2.0, -height / 2.0),
Translation3d(length / 2.0, width / 2.0, -height / 2.0),
Translation3d(length / 2.0, width / 2.0, height / 2.0),
Translation3d(length / 2.0, -width / 2.0, height / 2.0),
Translation3d(-length / 2.0, -width / 2.0, height / 2.0),
Translation3d(-length / 2.0, width / 2.0, height / 2.0),
Translation3d(-length / 2.0, width / 2.0, -height / 2.0),
Translation3d(-length / 2.0, -width / 2.0, -height / 2.0),
]
# Handle the rest of this in the "default" case
elif (
diameter is not None
and width is None
and height is None
and length is None
and verts is None
):
self.isPlanar = False
self.isSpherical = True
self.vertices = [
Translation3d(0.0, -diameter / 2.0, 0.0),
Translation3d(0.0, 0.0, -diameter / 2.0),
Translation3d(0.0, diameter / 2.0, 0.0),
Translation3d(0.0, 0.0, diameter / 2.0),
]
return
elif (
verts is not None
and width is None
and height is None
and length is None
and diameter is None
):
# Handle this in the "default" case
pass
else:
raise Exception("Not a valid overload")
# TODO maybe remove this if there is a better/preferred way
# make the python type checking gods happy
assert verts is not None
self.isSpherical = False
if len(verts) <= 2:
self.vertices: List[Translation3d] = []
self.isPlanar = False
else:
cornersPlaner = True
for corner in verts:
if abs(corner.X() < 1e-4):
cornersPlaner = False
self.isPlanar = cornersPlaner
self.vertices = verts
def getFieldVertices(self, targetPose: Pose3d) -> List[Translation3d]:
basisChange = RotTrlTransform3d(targetPose.rotation(), targetPose.translation())
retVal = []
for vert in self.vertices:
retVal.append(basisChange.apply(vert))
return retVal
@classmethod
def getOrientedPose(cls, tgtTrl: Translation3d, cameraTrl: Translation3d):
relCam = cameraTrl - tgtTrl
orientToCam = Rotation3d(
0.0,
Rotation2d(math.hypot(relCam.X(), relCam.Y()), relCam.Z()).radians(),
Rotation2d(relCam.X(), relCam.Y()).radians(),
)
return Pose3d(tgtTrl, orientToCam)
def getVertices(self) -> List[Translation3d]:
return self.vertices
def getIsPlanar(self) -> bool:
return self.isPlanar
def getIsSpherical(self) -> bool:
return self.isSpherical
@classmethod
def AprilTag36h11(cls) -> Self:
return cls(width=6.5 * 0.0254, height=6.5 * 0.0254)
@classmethod
def AprilTag16h5(cls) -> Self:
return cls(width=6.0 * 0.0254, height=6.0 * 0.0254)