utils.geometry

Geometry helper classes and transform builders for robot physical layout.

Provides:

transform_from_inches(): general-purpose Transform3d builder
CameraGeometry: camera with transform + FOV
MechanismMount: any physical mechanism with transform, supports parent chains

The actual robot-specific values live in constants/robot_geometry.py.

View Source

  1"""
  2Geometry helper classes and transform builders for robot physical layout.
  3
  4Provides:
  5  - transform_from_inches(): general-purpose Transform3d builder
  6  - CameraGeometry: camera with transform + FOV
  7  - MechanismMount: any physical mechanism with transform, supports parent chains
  8
  9The actual robot-specific values live in constants/robot_geometry.py.
 10"""
 11
 12from wpimath.geometry import (
 13    Pose3d,
 14    Rotation3d,
 15    Transform3d,
 16    Translation3d,
 17)
 18
 19
 20# ── Conversion constant ──────────────────────────────────────────────
 21
 22INCHES_TO_METERS = 0.0254
 23
 24
 25# ── Transform builders ───────────────────────────────────────────────
 26
 27def inches_to_meters(inches: float) -> float:
 28    return inches * INCHES_TO_METERS
 29
 30
 31def transform_from_inches(
 32    x_in: float = 0,
 33    y_in: float = 0,
 34    z_in: float = 0,
 35    roll_deg: float = 0,
 36    pitch_up_deg: float = 0,
 37    yaw_deg: float = 0,
 38) -> Transform3d:
 39    """Build a Transform3d from inches and intuitive degree conventions.
 40
 41    This is the general-purpose builder for any robot-relative transform:
 42    cameras, turrets, hoods, sensors, etc.
 43
 44    Args:
 45        x_in: forward/back in inches (positive = forward)
 46        y_in: left/right in inches (positive = left)
 47        z_in: height in inches (positive = up)
 48        roll_deg: rotation about forward axis
 49        pitch_up_deg: positive = tilts UP (negated internally for WPI)
 50        yaw_deg: 0 = forward, 90 = left, 180 = backward
 51
 52    Returns:
 53        Transform3d suitable for use as robot_to_X or parent_to_child.
 54    """
 55    c = INCHES_TO_METERS
 56    return Transform3d(
 57        Translation3d(x_in * c, y_in * c, z_in * c),
 58        Rotation3d.fromDegrees(roll_deg, -pitch_up_deg, yaw_deg),
 59    )
 60
 61
 62# Keep the old name as an alias for backward compat
 63camera_transform = transform_from_inches
 64
 65
 66def chain_transforms(*transforms: Transform3d) -> Transform3d:
 67    """Chain multiple transforms: parent_to_A + A_to_B + ... = parent_to_final.
 68
 69    Uses Pose3d as the accumulator since WPILib Transform3d doesn't
 70    support direct addition.
 71    """
 72    result = Pose3d()
 73    for tf in transforms:
 74        result = result.transformBy(tf)
 75    return Transform3d(Pose3d(), result)
 76
 77
 78# ── Camera geometry ──────────────────────────────────────────────────
 79
 80class CameraGeometry:
 81    """A vision camera on the robot.
 82
 83    Stores the canonical WPILib Transform3d plus the original inch/degree
 84    values for display, serialization, and the visualizer.
 85    """
 86
 87    __slots__ = ('name', 'robot_to_camera', 'fov_deg',
 88                 '_x_in', '_y_in', '_z_in',
 89                 '_roll_deg', '_pitch_up_deg', '_yaw_deg')
 90
 91    def __init__(
 92        self,
 93        name: str,
 94        robot_to_camera: Transform3d,
 95        fov_deg: float = 70.0,
 96        *,
 97        x_in: float = 0, y_in: float = 0, z_in: float = 0,
 98        roll_deg: float = 0, pitch_up_deg: float = 0, yaw_deg: float = 0,
 99    ):
100        self.name = name
101        self.robot_to_camera = robot_to_camera
102        self.fov_deg = fov_deg
103        self._x_in = x_in
104        self._y_in = y_in
105        self._z_in = z_in
106        self._roll_deg = roll_deg
107        self._pitch_up_deg = pitch_up_deg
108        self._yaw_deg = yaw_deg
109
110    def to_dict(self) -> dict:
111        """Serialize for the visualizer JSON."""
112        t = self.robot_to_camera.translation()
113        return {
114            "name": self.name,
115            "x_inches": self._x_in,
116            "y_inches": self._y_in,
117            "z_inches": self._z_in,
118            "roll_deg": self._roll_deg,
119            "pitch_deg": self._pitch_up_deg,
120            "yaw_deg": self._yaw_deg,
121            "fov_deg": self.fov_deg,
122            "x_meters": round(t.X(), 4),
123            "y_meters": round(t.Y(), 4),
124            "z_meters": round(t.Z(), 4),
125            "wpi_rotation_deg": {
126                "roll": self._roll_deg,
127                "pitch": -self._pitch_up_deg,
128                "yaw": self._yaw_deg,
129            },
130        }
131
132
133def make_camera(
134    name: str,
135    x_in: float = 0,
136    y_in: float = 0,
137    z_in: float = 0,
138    roll_deg: float = 0,
139    pitch_up_deg: float = 0,
140    yaw_deg: float = 0,
141    fov_deg: float = 70.0,
142) -> CameraGeometry:
143    """Create a CameraGeometry from inches and intuitive degrees."""
144    tf = transform_from_inches(
145        x_in, y_in, z_in, roll_deg, pitch_up_deg, yaw_deg
146    )
147    return CameraGeometry(
148        name, tf, fov_deg,
149        x_in=x_in, y_in=y_in, z_in=z_in,
150        roll_deg=roll_deg, pitch_up_deg=pitch_up_deg, yaw_deg=yaw_deg,
151    )
152
153
154# ── Mechanism mount ──────────────────────────────────────────────────
155
156class MechanismMount:
157    """A physical mechanism on the robot.
158
159    Each mechanism has:
160      - A local transform (relative to its parent frame)
161      - A resolved robot_to_mechanism transform (absolute from robot center)
162      - Visualization properties (type, size, color)
163
164    Supported types for the visualizer:
165      "swerve_module" - block with wheel direction arrow
166      "turret"        - circle with heading arrow
167      "generic"       - labeled block
168
169    Parent chaining example:
170        turret is at robot_to_turret
171        hood is at turret_to_hood (relative to turret)
172        hood's robot_to_mechanism = robot_to_turret + turret_to_hood
173    """
174
175    __slots__ = ('name', 'type', 'local_transform', 'robot_to_mechanism',
176                 'parent_name', 'width_m', 'length_m', 'radius_m', 'color')
177
178    def __init__(
179        self,
180        name: str,
181        mech_type: str,
182        local_transform: Transform3d,
183        *,
184        parent_transform: Transform3d = None,
185        parent_name: str = None,
186        width_in: float = 3.0,
187        length_in: float = 3.0,
188        radius_in: float = 0,
189        color: str = "",
190    ):
191        self.name = name
192        self.type = mech_type
193        self.local_transform = local_transform
194        self.parent_name = parent_name
195
196        # Resolve absolute transform from robot center
197        if parent_transform is not None:
198            self.robot_to_mechanism = chain_transforms(
199                parent_transform, local_transform
200            )
201        else:
202            self.robot_to_mechanism = local_transform
203
204        c = INCHES_TO_METERS
205        self.width_m = width_in * c
206        self.length_m = length_in * c
207        self.radius_m = radius_in * c
208        self.color = color
209
210    def to_dict(self) -> dict:
211        c = INCHES_TO_METERS
212        t = self.robot_to_mechanism.translation()
213        r = self.robot_to_mechanism.rotation()
214        return {
215            "name": self.name,
216            "type": self.type,
217            "parent": self.parent_name,
218            "x_inches": round(t.X() / c, 2),
219            "y_inches": round(t.Y() / c, 2),
220            "z_inches": round(t.Z() / c, 2),
221            "yaw_deg": round(r.z_degrees, 2),
222            "width_inches": round(self.width_m / c, 2),
223            "length_inches": round(self.length_m / c, 2),
224            "radius_inches": round(self.radius_m / c, 2),
225            "color": self.color,
226            "x_meters": round(t.X(), 4),
227            "y_meters": round(t.Y(), 4),
228            "z_meters": round(t.Z(), 4),
229            "width_meters": round(self.width_m, 4),
230            "length_meters": round(self.length_m, 4),
231            "radius_meters": round(self.radius_m, 4),
232        }
233
234
235# Convenience alias kept for old imports
236MechanismGeometry = MechanismMount

INCHES_TO_METERS = 0.0254

def inches_to_meters(inches: float) -> float: View Source

28def inches_to_meters(inches: float) -> float:
29    return inches * INCHES_TO_METERS

def transform_from_inches( x_in: float = 0, y_in: float = 0, z_in: float = 0, roll_deg: float = 0, pitch_up_deg: float = 0, yaw_deg: float = 0) -> wpimath.geometry._geometry.Transform3d: View Source

32def transform_from_inches(
33    x_in: float = 0,
34    y_in: float = 0,
35    z_in: float = 0,
36    roll_deg: float = 0,
37    pitch_up_deg: float = 0,
38    yaw_deg: float = 0,
39) -> Transform3d:
40    """Build a Transform3d from inches and intuitive degree conventions.
41
42    This is the general-purpose builder for any robot-relative transform:
43    cameras, turrets, hoods, sensors, etc.
44
45    Args:
46        x_in: forward/back in inches (positive = forward)
47        y_in: left/right in inches (positive = left)
48        z_in: height in inches (positive = up)
49        roll_deg: rotation about forward axis
50        pitch_up_deg: positive = tilts UP (negated internally for WPI)
51        yaw_deg: 0 = forward, 90 = left, 180 = backward
52
53    Returns:
54        Transform3d suitable for use as robot_to_X or parent_to_child.
55    """
56    c = INCHES_TO_METERS
57    return Transform3d(
58        Translation3d(x_in * c, y_in * c, z_in * c),
59        Rotation3d.fromDegrees(roll_deg, -pitch_up_deg, yaw_deg),
60    )

Build a Transform3d from inches and intuitive degree conventions.

This is the general-purpose builder for any robot-relative transform: cameras, turrets, hoods, sensors, etc.

Args: x_in: forward/back in inches (positive = forward) y_in: left/right in inches (positive = left) z_in: height in inches (positive = up) roll_deg: rotation about forward axis pitch_up_deg: positive = tilts UP (negated internally for WPI) yaw_deg: 0 = forward, 90 = left, 180 = backward

Returns: Transform3d suitable for use as robot_to_X or parent_to_child.

def camera_transform( x_in: float = 0, y_in: float = 0, z_in: float = 0, roll_deg: float = 0, pitch_up_deg: float = 0, yaw_deg: float = 0) -> wpimath.geometry._geometry.Transform3d: View Source

32def transform_from_inches(
33    x_in: float = 0,
34    y_in: float = 0,
35    z_in: float = 0,
36    roll_deg: float = 0,
37    pitch_up_deg: float = 0,
38    yaw_deg: float = 0,
39) -> Transform3d:
40    """Build a Transform3d from inches and intuitive degree conventions.
41
42    This is the general-purpose builder for any robot-relative transform:
43    cameras, turrets, hoods, sensors, etc.
44
45    Args:
46        x_in: forward/back in inches (positive = forward)
47        y_in: left/right in inches (positive = left)
48        z_in: height in inches (positive = up)
49        roll_deg: rotation about forward axis
50        pitch_up_deg: positive = tilts UP (negated internally for WPI)
51        yaw_deg: 0 = forward, 90 = left, 180 = backward
52
53    Returns:
54        Transform3d suitable for use as robot_to_X or parent_to_child.
55    """
56    c = INCHES_TO_METERS
57    return Transform3d(
58        Translation3d(x_in * c, y_in * c, z_in * c),
59        Rotation3d.fromDegrees(roll_deg, -pitch_up_deg, yaw_deg),
60    )

Build a Transform3d from inches and intuitive degree conventions.

This is the general-purpose builder for any robot-relative transform: cameras, turrets, hoods, sensors, etc.

Returns: Transform3d suitable for use as robot_to_X or parent_to_child.

def chain_transforms( *transforms: wpimath.geometry._geometry.Transform3d) -> wpimath.geometry._geometry.Transform3d: View Source

67def chain_transforms(*transforms: Transform3d) -> Transform3d:
68    """Chain multiple transforms: parent_to_A + A_to_B + ... = parent_to_final.
69
70    Uses Pose3d as the accumulator since WPILib Transform3d doesn't
71    support direct addition.
72    """
73    result = Pose3d()
74    for tf in transforms:
75        result = result.transformBy(tf)
76    return Transform3d(Pose3d(), result)

Chain multiple transforms: parent_to_A + A_to_B + ... = parent_to_final.

Uses Pose3d as the accumulator since WPILib Transform3d doesn't support direct addition.

class CameraGeometry: View Source

 81class CameraGeometry:
 82    """A vision camera on the robot.
 83
 84    Stores the canonical WPILib Transform3d plus the original inch/degree
 85    values for display, serialization, and the visualizer.
 86    """
 87
 88    __slots__ = ('name', 'robot_to_camera', 'fov_deg',
 89                 '_x_in', '_y_in', '_z_in',
 90                 '_roll_deg', '_pitch_up_deg', '_yaw_deg')
 91
 92    def __init__(
 93        self,
 94        name: str,
 95        robot_to_camera: Transform3d,
 96        fov_deg: float = 70.0,
 97        *,
 98        x_in: float = 0, y_in: float = 0, z_in: float = 0,
 99        roll_deg: float = 0, pitch_up_deg: float = 0, yaw_deg: float = 0,
100    ):
101        self.name = name
102        self.robot_to_camera = robot_to_camera
103        self.fov_deg = fov_deg
104        self._x_in = x_in
105        self._y_in = y_in
106        self._z_in = z_in
107        self._roll_deg = roll_deg
108        self._pitch_up_deg = pitch_up_deg
109        self._yaw_deg = yaw_deg
110
111    def to_dict(self) -> dict:
112        """Serialize for the visualizer JSON."""
113        t = self.robot_to_camera.translation()
114        return {
115            "name": self.name,
116            "x_inches": self._x_in,
117            "y_inches": self._y_in,
118            "z_inches": self._z_in,
119            "roll_deg": self._roll_deg,
120            "pitch_deg": self._pitch_up_deg,
121            "yaw_deg": self._yaw_deg,
122            "fov_deg": self.fov_deg,
123            "x_meters": round(t.X(), 4),
124            "y_meters": round(t.Y(), 4),
125            "z_meters": round(t.Z(), 4),
126            "wpi_rotation_deg": {
127                "roll": self._roll_deg,
128                "pitch": -self._pitch_up_deg,
129                "yaw": self._yaw_deg,
130            },
131        }

A vision camera on the robot.

Stores the canonical WPILib Transform3d plus the original inch/degree values for display, serialization, and the visualizer.

CameraGeometry( name: str, robot_to_camera: wpimath.geometry._geometry.Transform3d, fov_deg: float = 70.0, *, x_in: float = 0, y_in: float = 0, z_in: float = 0, roll_deg: float = 0, pitch_up_deg: float = 0, yaw_deg: float = 0) View Source

 92    def __init__(
 93        self,
 94        name: str,
 95        robot_to_camera: Transform3d,
 96        fov_deg: float = 70.0,
 97        *,
 98        x_in: float = 0, y_in: float = 0, z_in: float = 0,
 99        roll_deg: float = 0, pitch_up_deg: float = 0, yaw_deg: float = 0,
100    ):
101        self.name = name
102        self.robot_to_camera = robot_to_camera
103        self.fov_deg = fov_deg
104        self._x_in = x_in
105        self._y_in = y_in
106        self._z_in = z_in
107        self._roll_deg = roll_deg
108        self._pitch_up_deg = pitch_up_deg
109        self._yaw_deg = yaw_deg

name

robot_to_camera

fov_deg

def to_dict(self) -> dict: View Source

111    def to_dict(self) -> dict:
112        """Serialize for the visualizer JSON."""
113        t = self.robot_to_camera.translation()
114        return {
115            "name": self.name,
116            "x_inches": self._x_in,
117            "y_inches": self._y_in,
118            "z_inches": self._z_in,
119            "roll_deg": self._roll_deg,
120            "pitch_deg": self._pitch_up_deg,
121            "yaw_deg": self._yaw_deg,
122            "fov_deg": self.fov_deg,
123            "x_meters": round(t.X(), 4),
124            "y_meters": round(t.Y(), 4),
125            "z_meters": round(t.Z(), 4),
126            "wpi_rotation_deg": {
127                "roll": self._roll_deg,
128                "pitch": -self._pitch_up_deg,
129                "yaw": self._yaw_deg,
130            },
131        }

Serialize for the visualizer JSON.

def make_camera( name: str, x_in: float = 0, y_in: float = 0, z_in: float = 0, roll_deg: float = 0, pitch_up_deg: float = 0, yaw_deg: float = 0, fov_deg: float = 70.0) -> CameraGeometry: View Source

134def make_camera(
135    name: str,
136    x_in: float = 0,
137    y_in: float = 0,
138    z_in: float = 0,
139    roll_deg: float = 0,
140    pitch_up_deg: float = 0,
141    yaw_deg: float = 0,
142    fov_deg: float = 70.0,
143) -> CameraGeometry:
144    """Create a CameraGeometry from inches and intuitive degrees."""
145    tf = transform_from_inches(
146        x_in, y_in, z_in, roll_deg, pitch_up_deg, yaw_deg
147    )
148    return CameraGeometry(
149        name, tf, fov_deg,
150        x_in=x_in, y_in=y_in, z_in=z_in,
151        roll_deg=roll_deg, pitch_up_deg=pitch_up_deg, yaw_deg=yaw_deg,
152    )

Create a CameraGeometry from inches and intuitive degrees.

class MechanismMount: View Source

157class MechanismMount:
158    """A physical mechanism on the robot.
159
160    Each mechanism has:
161      - A local transform (relative to its parent frame)
162      - A resolved robot_to_mechanism transform (absolute from robot center)
163      - Visualization properties (type, size, color)
164
165    Supported types for the visualizer:
166      "swerve_module" - block with wheel direction arrow
167      "turret"        - circle with heading arrow
168      "generic"       - labeled block
169
170    Parent chaining example:
171        turret is at robot_to_turret
172        hood is at turret_to_hood (relative to turret)
173        hood's robot_to_mechanism = robot_to_turret + turret_to_hood
174    """
175
176    __slots__ = ('name', 'type', 'local_transform', 'robot_to_mechanism',
177                 'parent_name', 'width_m', 'length_m', 'radius_m', 'color')
178
179    def __init__(
180        self,
181        name: str,
182        mech_type: str,
183        local_transform: Transform3d,
184        *,
185        parent_transform: Transform3d = None,
186        parent_name: str = None,
187        width_in: float = 3.0,
188        length_in: float = 3.0,
189        radius_in: float = 0,
190        color: str = "",
191    ):
192        self.name = name
193        self.type = mech_type
194        self.local_transform = local_transform
195        self.parent_name = parent_name
196
197        # Resolve absolute transform from robot center
198        if parent_transform is not None:
199            self.robot_to_mechanism = chain_transforms(
200                parent_transform, local_transform
201            )
202        else:
203            self.robot_to_mechanism = local_transform
204
205        c = INCHES_TO_METERS
206        self.width_m = width_in * c
207        self.length_m = length_in * c
208        self.radius_m = radius_in * c
209        self.color = color
210
211    def to_dict(self) -> dict:
212        c = INCHES_TO_METERS
213        t = self.robot_to_mechanism.translation()
214        r = self.robot_to_mechanism.rotation()
215        return {
216            "name": self.name,
217            "type": self.type,
218            "parent": self.parent_name,
219            "x_inches": round(t.X() / c, 2),
220            "y_inches": round(t.Y() / c, 2),
221            "z_inches": round(t.Z() / c, 2),
222            "yaw_deg": round(r.z_degrees, 2),
223            "width_inches": round(self.width_m / c, 2),
224            "length_inches": round(self.length_m / c, 2),
225            "radius_inches": round(self.radius_m / c, 2),
226            "color": self.color,
227            "x_meters": round(t.X(), 4),
228            "y_meters": round(t.Y(), 4),
229            "z_meters": round(t.Z(), 4),
230            "width_meters": round(self.width_m, 4),
231            "length_meters": round(self.length_m, 4),
232            "radius_meters": round(self.radius_m, 4),
233        }

A physical mechanism on the robot.

Each mechanism has:

A local transform (relative to its parent frame)
A resolved robot_to_mechanism transform (absolute from robot center)
Visualization properties (type, size, color)

Supported types for the visualizer: "swerve_module" - block with wheel direction arrow "turret" - circle with heading arrow "generic" - labeled block

Parent chaining example: turret is at robot_to_turret hood is at turret_to_hood (relative to turret) hood's robot_to_mechanism = robot_to_turret + turret_to_hood

MechanismMount( name: str, mech_type: str, local_transform: wpimath.geometry._geometry.Transform3d, *, parent_transform: wpimath.geometry._geometry.Transform3d = None, parent_name: str = None, width_in: float = 3.0, length_in: float = 3.0, radius_in: float = 0, color: str = '') View Source

179    def __init__(
180        self,
181        name: str,
182        mech_type: str,
183        local_transform: Transform3d,
184        *,
185        parent_transform: Transform3d = None,
186        parent_name: str = None,
187        width_in: float = 3.0,
188        length_in: float = 3.0,
189        radius_in: float = 0,
190        color: str = "",
191    ):
192        self.name = name
193        self.type = mech_type
194        self.local_transform = local_transform
195        self.parent_name = parent_name
196
197        # Resolve absolute transform from robot center
198        if parent_transform is not None:
199            self.robot_to_mechanism = chain_transforms(
200                parent_transform, local_transform
201            )
202        else:
203            self.robot_to_mechanism = local_transform
204
205        c = INCHES_TO_METERS
206        self.width_m = width_in * c
207        self.length_m = length_in * c
208        self.radius_m = radius_in * c
209        self.color = color

name

type

local_transform

parent_name

width_m

length_m

radius_m

color

def to_dict(self) -> dict: View Source

211    def to_dict(self) -> dict:
212        c = INCHES_TO_METERS
213        t = self.robot_to_mechanism.translation()
214        r = self.robot_to_mechanism.rotation()
215        return {
216            "name": self.name,
217            "type": self.type,
218            "parent": self.parent_name,
219            "x_inches": round(t.X() / c, 2),
220            "y_inches": round(t.Y() / c, 2),
221            "z_inches": round(t.Z() / c, 2),
222            "yaw_deg": round(r.z_degrees, 2),
223            "width_inches": round(self.width_m / c, 2),
224            "length_inches": round(self.length_m / c, 2),
225            "radius_inches": round(self.radius_m / c, 2),
226            "color": self.color,
227            "x_meters": round(t.X(), 4),
228            "y_meters": round(t.Y(), 4),
229            "z_meters": round(t.Z(), 4),
230            "width_meters": round(self.width_m, 4),
231            "length_meters": round(self.length_m, 4),
232            "radius_meters": round(self.radius_m, 4),
233        }

robot_to_mechanism

MechanismGeometry = <class 'MechanismMount'>