1- """Ray-cast sensor with a configurable grid pattern, attached to a robot link.
1+ """Ray-cast sensor with a configurable pattern, attached to a robot link.
22
33Two intersection backends:
44
55* **Flat plane** (default when ``scene.terrain is None``): every ray hits the horizontal
6- plane at ``ground_height``. Closed-form intersection.
6+ plane at ``ground_height``. Closed-form intersection; rays going up or parallel are
7+ treated as misses and clamp to ``max_distance``.
78* **Height field** (when ``scene.terrain`` is a :class:`TerrainImporter`): bilinearly
89 samples ``terrain.heightfield_tensor`` at each ray's ``(x, y)`` origin and uses the
910 sampled elevation as the hit plane height. Accurate for vertical / near-vertical rays
1011 (the dominant case for height-scan grids); subclasses can override
1112 ``_intersect_world_rays`` for non-trivial BVH ray-tracing.
1213
13- Pattern shape mirrors mjlab's ``GridPatternCfg``; for now only a 2D grid is provided.
14+ Three built-in patterns share an informal protocol (``num_rays() -> int`` and
15+ ``generate(device) -> (starts, dirs)``):
16+
17+ * :class:`GridPattern` — 2D rectangular grid, all rays parallel. Default; mirrors mjlab.
18+ * :class:`RingPattern` — multi-line LIDAR-style sweep around the +z axis.
19+ * :class:`HemispherePattern` — Fibonacci-distributed rays on a spherical cap.
1420"""
1521
22+ import math
1623from dataclasses import dataclass , field
1724from typing import TYPE_CHECKING
1825
@@ -56,6 +63,142 @@ def generate(self, device: str) -> tuple[torch.Tensor, torch.Tensor]:
5663 return starts , dirs
5764
5865
66+ def _evenly_spaced_angles_rad (
67+ start_deg : float , end_deg : float , n : int , device : str
68+ ) -> torch .Tensor :
69+ """Return ``n`` azimuths in radians spanning ``[start_deg, end_deg]``.
70+
71+ A full ±360° span is treated as a wrap-around and the closing endpoint is dropped
72+ (``linspace(..., n+1)[:-1]``) so a 360° sweep with ``n`` rays does not duplicate the
73+ first/last angle. Any other span uses inclusive linspace.
74+ """
75+ span = end_deg - start_deg
76+ if n <= 0 :
77+ return torch .empty (0 , device = device , dtype = torch .float32 )
78+ if abs (abs (span ) - 360.0 ) < 1e-6 :
79+ angles_deg = torch .linspace (start_deg , end_deg , n + 1 , device = device )[:- 1 ]
80+ else :
81+ angles_deg = torch .linspace (start_deg , end_deg , n , device = device )
82+ return angles_deg * (math .pi / 180.0 )
83+
84+
85+ @dataclass
86+ class RingPattern :
87+ """Multi-line LIDAR sweep: ``num_horizontal`` azimuths × ``num_vertical`` elevations.
88+
89+ All rays emit from the sensor origin (``starts = zeros``). ``horizontal_fov_deg`` and
90+ ``vertical_fov_deg`` are inclusive on both endpoints except when the horizontal span is
91+ exactly ±360° — then the closing azimuth is dropped to avoid a duplicate ray. A single
92+ horizontal line (``num_vertical=1``) at ``vertical_fov_deg=(0, 0)`` gives a planar lidar.
93+ """
94+
95+ num_horizontal : int = 32
96+ num_vertical : int = 1
97+ horizontal_fov_deg : tuple [float , float ] = (- 180.0 , 180.0 )
98+ vertical_fov_deg : tuple [float , float ] = (- 15.0 , 15.0 )
99+
100+ def num_rays (self ) -> int :
101+ return self .num_horizontal * self .num_vertical
102+
103+ def generate (self , device : str ) -> tuple [torch .Tensor , torch .Tensor ]:
104+ n_h = self .num_horizontal
105+ n_v = self .num_vertical
106+ azimuth = _evenly_spaced_angles_rad (
107+ self .horizontal_fov_deg [0 ], self .horizontal_fov_deg [1 ], n_h , device
108+ )
109+ if n_v == 1 :
110+ elevation = torch .tensor (
111+ [(self .vertical_fov_deg [0 ] + self .vertical_fov_deg [1 ]) * 0.5 * math .pi / 180.0 ],
112+ device = device ,
113+ dtype = torch .float32 ,
114+ )
115+ else :
116+ elevation = torch .linspace (
117+ self .vertical_fov_deg [0 ],
118+ self .vertical_fov_deg [1 ],
119+ n_v ,
120+ device = device ,
121+ ) * (math .pi / 180.0 )
122+ # Outer product over (vertical, horizontal); reshape so vertical varies slowest.
123+ el_grid , az_grid = torch .meshgrid (elevation , azimuth , indexing = "ij" )
124+ cos_el = torch .cos (el_grid ).reshape (- 1 )
125+ sin_el = torch .sin (el_grid ).reshape (- 1 )
126+ cos_az = torch .cos (az_grid ).reshape (- 1 )
127+ sin_az = torch .sin (az_grid ).reshape (- 1 )
128+ dirs = torch .stack ([cos_el * cos_az , cos_el * sin_az , sin_el ], dim = - 1 ).contiguous ()
129+ starts = torch .zeros_like (dirs )
130+ return starts , dirs
131+
132+
133+ def _fibonacci_cap_dirs_z_aligned (n : int , polar_fov_deg : float , device : str ) -> torch .Tensor :
134+ """Return ``(n, 3)`` unit vectors evenly distributed on a spherical cap around +z.
135+
136+ Uses a Fibonacci lattice with area-uniform polar spacing on the cap. ``polar_fov_deg``
137+ is the half-angle from the pole; 90° covers a full hemisphere, 180° a full sphere.
138+ """
139+ fov_rad = polar_fov_deg * math .pi / 180.0
140+ cos_theta_min = math .cos (fov_rad )
141+ golden_angle = math .pi * (3.0 - math .sqrt (5.0 ))
142+
143+ i = torch .arange (n , device = device , dtype = torch .float32 )
144+ cos_theta = 1.0 - (i + 0.5 ) / float (n ) * (1.0 - cos_theta_min )
145+ sin_theta = (1.0 - cos_theta * cos_theta ).clamp (min = 0.0 ).sqrt ()
146+ phi = i * golden_angle
147+ x = sin_theta * torch .cos (phi )
148+ y = sin_theta * torch .sin (phi )
149+ z = cos_theta
150+ return torch .stack ([x , y , z ], dim = - 1 )
151+
152+
153+ def _rotate_z_to (z_aligned : torch .Tensor , target_axis : tuple [float , float , float ]) -> torch .Tensor :
154+ """Rotate ``(N, 3)`` vectors expressed in a frame where +z is the pole onto a frame
155+ where the pole has been mapped to ``target_axis`` (unit vector after normalisation)."""
156+ device = z_aligned .device
157+ dtype = z_aligned .dtype
158+ target = torch .tensor (target_axis , device = device , dtype = dtype )
159+ target = target / target .norm ().clamp_min (1e-9 )
160+ cos_a = float (target [2 ].item ())
161+ if cos_a > 1.0 - 1e-9 :
162+ return z_aligned
163+ if cos_a < - 1.0 + 1e-9 :
164+ # 180° rotation about +x: (x, y, z) → (x, -y, -z).
165+ flipped = z_aligned .clone ()
166+ flipped [..., 1 ] = - flipped [..., 1 ]
167+ flipped [..., 2 ] = - flipped [..., 2 ]
168+ return flipped
169+ # Rodrigues' rotation: axis = z × target, angle = arccos(target · z).
170+ z_unit = torch .tensor ([0.0 , 0.0 , 1.0 ], device = device , dtype = dtype )
171+ axis = torch .linalg .cross (z_unit , target )
172+ axis = axis / axis .norm ().clamp_min (1e-9 )
173+ sin_a = math .sqrt (max (0.0 , 1.0 - cos_a * cos_a ))
174+ cross = torch .linalg .cross (axis .expand_as (z_aligned ), z_aligned )
175+ dot = (axis * z_aligned ).sum (dim = - 1 , keepdim = True )
176+ return z_aligned * cos_a + cross * sin_a + axis * dot * (1.0 - cos_a )
177+
178+
179+ @dataclass
180+ class HemispherePattern :
181+ """Fibonacci-distributed rays on a spherical cap centred on ``pole_axis``.
182+
183+ All rays emit from the sensor origin (``starts = zeros``). ``polar_fov_deg`` is the
184+ half-angle from the pole: 90° is a full hemisphere, 180° a full sphere. ``num_rays``
185+ is the exact ray count produced by ``generate()`` (no Fibonacci rounding involved).
186+ """
187+
188+ num_rays_target : int = 64
189+ pole_axis : tuple [float , float , float ] = (0.0 , 0.0 , 1.0 )
190+ polar_fov_deg : float = 90.0
191+
192+ def num_rays (self ) -> int :
193+ return self .num_rays_target
194+
195+ def generate (self , device : str ) -> tuple [torch .Tensor , torch .Tensor ]:
196+ dirs_z = _fibonacci_cap_dirs_z_aligned (self .num_rays_target , self .polar_fov_deg , device )
197+ dirs = _rotate_z_to (dirs_z , self .pole_axis ).contiguous ()
198+ starts = torch .zeros_like (dirs )
199+ return starts , dirs
200+
201+
59202@dataclass
60203class RayCastData :
61204 distances : torch .Tensor # (B, M) — clamped to max_distance; equals max_distance on a miss
@@ -69,14 +212,19 @@ class RayCastData:
69212class RayCastSensorCfg (SensorCfg ):
70213 """Configuration for ``RayCastSensor``.
71214
72- ``link_name`` anchors the pattern. ``attach_yaw_only`` keeps the grid axis-aligned to the
73- horizon when the link rolls / pitches (typical for terrain height scans on a torso link);
74- set ``False`` to follow the full link orientation. ``ground_height`` is the z of the flat
75- plane the default backend intersects against; subclasses ignore it.
215+ ``link_name`` anchors the pattern. ``attach_yaw_only`` keeps the pattern axis-aligned to
216+ the horizon when the link rolls / pitches (typical for terrain height scans on a torso
217+ link); set ``False`` to follow the full link orientation. ``ground_height`` is the z of
218+ the flat plane the default backend intersects against; subclasses ignore it.
219+
220+ ``pattern`` accepts any of the bundled pattern dataclasses (:class:`GridPattern`,
221+ :class:`RingPattern`, :class:`HemispherePattern`); custom patterns satisfying the same
222+ informal protocol (``num_rays() -> int`` and ``generate(device) -> (starts, dirs)``)
223+ are accepted but should be re-exported through ``genelab.sensor`` for typing parity.
76224 """
77225
78226 link_name : str = ""
79- pattern : GridPattern = field (default_factory = GridPattern )
227+ pattern : GridPattern | RingPattern | HemispherePattern = field (default_factory = GridPattern )
80228 attach_yaw_only : bool = True
81229 max_distance : float = 10.0
82230 ground_height : float = 0.0
0 commit comments