mirror of
https://github.com/PhotonVision/photonvision
synced 2026-06-19 00:41:41 +00:00
sphinxify java docs for python code (#1575)
This commit is contained in:
@@ -4,24 +4,38 @@ from wpimath.geometry import Pose3d, Rotation3d, Transform3d, Translation3d
|
||||
|
||||
|
||||
class RotTrlTransform3d:
|
||||
"""Represents a transformation that first rotates a pose around the origin, and then translates it."""
|
||||
|
||||
def __init__(
|
||||
self, rot: Rotation3d = Rotation3d(), trl: Translation3d = Translation3d()
|
||||
):
|
||||
"""A rotation-translation transformation.
|
||||
|
||||
Applying this RotTrlTransform3d to poses will preserve their current origin-to-pose
|
||||
transform as if the origin was transformed by these components instead.
|
||||
|
||||
:param rot: The rotation component
|
||||
:param trl: The translation component
|
||||
"""
|
||||
self.rot = rot
|
||||
self.trl = trl
|
||||
|
||||
def inverse(self) -> Self:
|
||||
"""The inverse of this transformation. Applying the inverse will "undo" this transformation."""
|
||||
invRot = -self.rot
|
||||
invTrl = -(self.trl.rotateBy(invRot))
|
||||
return type(self)(invRot, invTrl)
|
||||
|
||||
def getTransform(self) -> Transform3d:
|
||||
"""This transformation as a Transform3d (as if of the origin)"""
|
||||
return Transform3d(self.trl, self.rot)
|
||||
|
||||
def getTranslation(self) -> Translation3d:
|
||||
"""The translation component of this transformation"""
|
||||
return self.trl
|
||||
|
||||
def getRotation(self) -> Rotation3d:
|
||||
"""The rotation component of this transformation"""
|
||||
return self.rot
|
||||
|
||||
def applyTranslation(self, trlToApply: Translation3d) -> Translation3d:
|
||||
@@ -44,6 +58,11 @@ class RotTrlTransform3d:
|
||||
|
||||
@classmethod
|
||||
def makeRelativeTo(cls, pose: Pose3d) -> Self:
|
||||
"""The rotation-translation transformation that makes poses in the world consider this pose as the
|
||||
new origin, or change the basis to this pose.
|
||||
|
||||
:param pose: The new origin
|
||||
"""
|
||||
return cls(pose.rotation(), pose.translation()).inverse()
|
||||
|
||||
@classmethod
|
||||
|
||||
Reference in New Issue
Block a user