Files
2026-11-03 19:46:08 +08:00

95 lines
3.1 KiB
Python

"""Terrain height sensor for per-frame vertical clearance."""
from __future__ import annotations
from dataclasses import dataclass
import torch
from mjlab.sensor.raycast_sensor import RayCastData, RayCastSensor, RayCastSensorCfg
@dataclass
class TerrainHeightData(RayCastData):
"""Raycast data extended with per-frame terrain clearance.
Inherits all fields from :class:`RayCastData` (distances, hit positions, normals,
frame poses) and adds :attr:`heights`.
"""
heights: torch.Tensor
"""Vertical clearance per frame. Shape is ``[B, F]`` when a reduction is
applied, or ``[B, F, N]`` with ``reduction="none"``."""
@dataclass
class TerrainHeightSensorCfg(RayCastSensorCfg):
"""RayCastSensor that reports per-frame vertical clearance above terrain.
Inherits all raycasting configuration from :class:`RayCastSensorCfg`. The sensor
computes ``frame_z - hit_z`` for each ray and reduces across rays per frame using
the chosen :attr:`reduction`.
"""
reduction: str = "min"
"""How to aggregate rays within each frame: ``"min"``, ``"max"``, ``"mean"``,
or ``"none"`` (no reduction, returns ``[B, F, N]``).
Defaults to ``"min"`` (closest terrain point)."""
def build(self) -> TerrainHeightSensor:
return TerrainHeightSensor(self)
class TerrainHeightSensor(RayCastSensor):
"""Per-frame vertical clearance above terrain.
Inherits all raycasting from :class:`RayCastSensor`. Access terrain heights via
``sensor.data.heights`` (shape ``[B, F]``).
"""
cfg: TerrainHeightSensorCfg
@property
def data(self) -> TerrainHeightData:
"""Raycast data with per-frame terrain clearance heights."""
return super().data # type: ignore[return-value]
def _compute_data(self) -> TerrainHeightData:
raw = super()._compute_data()
F, N = self.num_frames, self.num_rays_per_frame
B = raw.distances.shape[0]
frame_z = raw.frame_pos_w[:, :, 2] # [B, F]
hit_z = raw.hit_pos_w[:, :, 2].view(B, F, N) # [B, F, N]
heights = frame_z.unsqueeze(-1) - hit_z # [B, F, N]
dists = raw.distances.view(B, F, N)
normal_z = raw.normals_w[:, :, 2].view(B, F, N)
# Backface hit: ray hit the underside of geometry (normal_z < 0).
# This means the ray origin is inside terrain, so clearance is 0.
backface = (dists >= 0) & (normal_z < 0)
heights = torch.where(backface, torch.zeros_like(heights), heights)
# True miss: no intersection at all.
miss = dists < 0
all_miss = miss.all(dim=-1, keepdim=True).expand_as(miss) # [B, F, N]
fallback = frame_z.unsqueeze(-1).clamp(0, self.cfg.max_distance)
fallback = fallback.expand_as(heights) # [B, F, N]
miss_value = torch.where(all_miss, fallback, self.cfg.max_distance)
heights = torch.where(miss, miss_value, heights)
reduction = self.cfg.reduction
if reduction == "min":
reduced = heights.min(dim=-1).values
elif reduction == "max":
reduced = heights.max(dim=-1).values
elif reduction == "mean":
reduced = heights.mean(dim=-1)
elif reduction == "none":
reduced = heights
else:
raise ValueError(f"Unknown reduction: {reduction!r}")
return TerrainHeightData(**vars(raw), heights=reduced)