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

896 lines
30 KiB
Python

"""Raycast sensor for terrain and obstacle detection.
Provides :class:`RayCastSensor` and :class:`RayCastSensorCfg` for BVH-accelerated
raycasting with grid, pinhole camera, and ring patterns. Supports multi-frame
attachment, configurable ray alignment, and geom group filtering.
See :doc:`/sensors/raycast_sensor` for usage guide and examples.
"""
from __future__ import annotations
import math
from dataclasses import dataclass, field
from typing import TYPE_CHECKING, Literal
import mujoco
import mujoco_warp as mjwarp
import torch
import warp as wp
from mujoco_warp import rays
from mjlab.entity import Entity
from mjlab.sensor.builtin_sensor import ObjRef
from mjlab.sensor.sensor import Sensor, SensorCfg
from mjlab.utils.lab_api.math import quat_from_matrix
if TYPE_CHECKING:
from mjlab.sensor.sensor_context import SensorContext
from mjlab.viewer.debug_visualizer import DebugVisualizer
RayAlignment = Literal["base", "yaw", "world"]
# NOTE: Not publicly exposed by mujoco_warp.
_vec6 = wp.types.vector(length=6, dtype=float)
_ALL_GROUPS = _vec6(-1, -1, -1, -1, -1, -1)
def _geom_groups_to_vec6(groups: tuple[int, ...] | None): # -> _vec6
"""Convert geom group tuple to mujoco_warp vec6 format.
In the vec6 format, -1 means include and 0 means exclude.
"""
if groups is None:
return _ALL_GROUPS
out = [0, 0, 0, 0, 0, 0]
for g in groups:
if not 0 <= g < mujoco.mjNGROUP:
raise ValueError(
f"include_geom_groups must be in [0, {mujoco.mjNGROUP}), got {g}"
)
out[g] = -1
return _vec6(*out)
@dataclass
class GridPatternCfg:
"""Grid pattern - parallel rays in a 2D grid."""
size: tuple[float, float] = (1.0, 1.0)
"""Grid size (length, width) in meters."""
resolution: float | tuple[float, float] = 0.1
"""Spacing between rays in meters, either shared or per axis as ``(x, y)``."""
offset: tuple[float, float] = (0.0, 0.0)
"""Grid center offset in frame-local XY (meters), rotated by ``ray_alignment``."""
direction: tuple[float, float, float] = (0.0, 0.0, -1.0)
"""Ray direction in frame-local coordinates."""
def generate_rays(
self, mj_model: mujoco.MjModel | None, device: str
) -> tuple[torch.Tensor, torch.Tensor]:
"""Generate ray pattern.
Args:
mj_model: MuJoCo model (unused for grid pattern).
device: Device for tensor operations.
Returns:
Tuple of (local_offsets [N, 3], local_directions [N, 3]).
"""
del mj_model # Unused for grid pattern
size_x, size_y = self.size
center_x, center_y = self.offset
res = self.resolution
if isinstance(res, (int, float)):
res = (res, res)
res_x, res_y = res
x = torch.arange(
-size_x / 2, size_x / 2 + res_x * 0.5, res_x, device=device, dtype=torch.float32
)
y = torch.arange(
-size_y / 2, size_y / 2 + res_y * 0.5, res_y, device=device, dtype=torch.float32
)
grid_x, grid_y = torch.meshgrid(x, y, indexing="xy")
num_rays = grid_x.numel()
local_offsets = torch.zeros((num_rays, 3), device=device, dtype=torch.float32)
local_offsets[:, 0] = grid_x.flatten() + center_x
local_offsets[:, 1] = grid_y.flatten() + center_y
# All rays share the same direction for grid pattern.
direction = torch.tensor(self.direction, device=device, dtype=torch.float32)
direction = direction / direction.norm()
local_directions = direction.unsqueeze(0).expand(num_rays, 3).clone()
return local_offsets, local_directions
@dataclass
class PinholeCameraPatternCfg:
"""Pinhole camera pattern - rays diverging from origin like a camera.
Can be configured with explicit parameters (width, height, fovy) or created
via factory methods like from_mujoco_camera() or from_intrinsic_matrix().
"""
width: int = 16
"""Image width in pixels."""
height: int = 12
"""Image height in pixels."""
fovy: float = 45.0
"""Vertical field of view in degrees (matches MuJoCo convention)."""
_camera_name: str | None = field(default=None, repr=False)
"""Internal: MuJoCo camera name for deferred parameter resolution."""
@classmethod
def from_mujoco_camera(cls, camera_name: str) -> PinholeCameraPatternCfg:
"""Create config that references a MuJoCo camera.
Camera parameters (resolution, FOV) are resolved at runtime from the model.
Args:
camera_name: Name of the MuJoCo camera to reference.
Returns:
Config that will resolve parameters from the MuJoCo camera.
"""
# Placeholder values; actual values resolved in generate_rays().
return cls(width=0, height=0, fovy=0.0, _camera_name=camera_name)
@classmethod
def from_intrinsic_matrix(
cls, intrinsic_matrix: list[float], width: int, height: int
) -> PinholeCameraPatternCfg:
"""Create from 3x3 intrinsic matrix [fx, 0, cx, 0, fy, cy, 0, 0, 1].
Args:
intrinsic_matrix: Flattened 3x3 intrinsic matrix.
width: Image width in pixels.
height: Image height in pixels.
Returns:
Config with fovy computed from the intrinsic matrix.
"""
fy = intrinsic_matrix[4] # fy is at position [1,1] in the matrix
fovy = 2 * math.atan(height / (2 * fy)) * 180 / math.pi
return cls(width=width, height=height, fovy=fovy)
def generate_rays(
self, mj_model: mujoco.MjModel | None, device: str
) -> tuple[torch.Tensor, torch.Tensor]:
"""Generate ray pattern.
Args:
mj_model: MuJoCo model (required if using from_mujoco_camera).
device: Device for tensor operations.
Returns:
Tuple of (local_offsets [N, 3], local_directions [N, 3]).
"""
# Resolve camera parameters.
if self._camera_name is not None:
if mj_model is None:
raise ValueError("MuJoCo model required when using from_mujoco_camera()")
# Get parameters from MuJoCo camera.
cam_id = mj_model.camera(self._camera_name).id
width, height = mj_model.cam_resolution[cam_id]
# MuJoCo has two camera modes:
# 1. fovy mode: sensorsize is zero, use cam_fovy directly
# 2. Physical sensor mode: sensorsize > 0, compute from focal/sensorsize
sensorsize = mj_model.cam_sensorsize[cam_id]
if sensorsize[0] > 0 and sensorsize[1] > 0:
# Physical sensor model.
intrinsic = mj_model.cam_intrinsic[cam_id] # [fx, fy, cx, cy]
focal = intrinsic[:2] # [fx, fy]
h_fov_rad = 2 * math.atan(sensorsize[0] / (2 * focal[0]))
v_fov_rad = 2 * math.atan(sensorsize[1] / (2 * focal[1]))
else:
# Read vertical FOV directly from MuJoCo.
v_fov_rad = math.radians(mj_model.cam_fovy[cam_id])
aspect = width / height
h_fov_rad = 2 * math.atan(math.tan(v_fov_rad / 2) * aspect)
else:
# Use explicit parameters.
width = self.width
height = self.height
v_fov_rad = math.radians(self.fovy)
aspect = width / height
h_fov_rad = 2 * math.atan(math.tan(v_fov_rad / 2) * aspect)
# Normalized pixel centers, row-major with row 0 at the top (MuJoCo's layout).
cols = torch.arange(width, device=device, dtype=torch.float32)
rows = torch.arange(height, device=device, dtype=torch.float32)
u = (cols + 0.5) / width * 2 - 1
v = 1 - (rows + 0.5) / height * 2
grid_u, grid_v = torch.meshgrid(u, v, indexing="xy")
# Convert to ray directions (MuJoCo camera: -Z forward, +X right, +Y up).
ray_x = grid_u.flatten() * math.tan(h_fov_rad / 2)
ray_y = grid_v.flatten() * math.tan(v_fov_rad / 2)
ray_z = -torch.ones_like(ray_x) # Negative Z for MuJoCo camera forward
num_rays = width * height
local_offsets = torch.zeros((num_rays, 3), device=device)
local_directions = torch.stack([ray_x, ray_y, ray_z], dim=1)
local_directions = local_directions / local_directions.norm(dim=1, keepdim=True)
return local_offsets, local_directions
@dataclass
class RingPatternCfg:
"""Ring pattern - rays in concentric rings around the origin.
Useful for per-site height sensing where you want to sample the
terrain around each attachment frame.
"""
@dataclass
class Ring:
"""A single ring in the pattern."""
radius: float
"""Radius of this ring in meters."""
num_samples: int
"""Number of evenly spaced sample points on this ring."""
rings: tuple[Ring, ...]
"""Ring definitions. Multiple rings create a concentric pattern."""
include_center: bool = True
"""Whether to include a ray at the center (zero offset)."""
direction: tuple[float, float, float] = (0.0, 0.0, -1.0)
"""Ray direction in frame-local coordinates."""
@classmethod
def single_ring(
cls,
radius: float = 0.1,
num_samples: int = 8,
include_center: bool = True,
direction: tuple[float, float, float] = (0.0, 0.0, -1.0),
) -> RingPatternCfg:
"""Create a single-ring pattern.
Args:
radius: Ring radius in meters.
num_samples: Number of evenly spaced points on the ring.
include_center: Whether to include a center ray.
direction: Ray direction in frame-local coordinates.
Returns:
RingPatternCfg with one ring.
"""
return cls(
rings=(cls.Ring(radius, num_samples),),
include_center=include_center,
direction=direction,
)
def generate_rays(
self, mj_model: mujoco.MjModel | None, device: str
) -> tuple[torch.Tensor, torch.Tensor]:
"""Generate ray pattern.
Args:
mj_model: MuJoCo model (unused for ring pattern).
device: Device for tensor operations.
Returns:
Tuple of (local_offsets [N, 3], local_directions [N, 3]).
"""
del mj_model
offsets: list[torch.Tensor] = []
if self.include_center:
offsets.append(torch.zeros(3, device=device, dtype=torch.float32))
for ring in self.rings:
for i in range(ring.num_samples):
angle = 2.0 * math.pi * i / ring.num_samples
offsets.append(
torch.tensor(
[
ring.radius * math.cos(angle),
ring.radius * math.sin(angle),
0.0,
],
device=device,
dtype=torch.float32,
)
)
local_offsets = torch.stack(offsets) # [N, 3]
num_rays = local_offsets.shape[0]
direction = torch.tensor(self.direction, device=device, dtype=torch.float32)
direction = direction / direction.norm()
local_directions = direction.unsqueeze(0).expand(num_rays, 3).clone()
return local_offsets, local_directions
PatternCfg = GridPatternCfg | PinholeCameraPatternCfg | RingPatternCfg
@dataclass
class RayCastData:
"""Raycast sensor output data.
Note:
Fields are views into GPU buffers and are valid until the next
``sense()`` call.
"""
distances: torch.Tensor
"""[B, N] Distance to hit point. -1 if no hit.
N = num_frames * num_rays_per_frame.
"""
normals_w: torch.Tensor
"""[B, N, 3] Surface normal at hit point (world frame). Zero if no hit."""
hit_pos_w: torch.Tensor
"""[B, N, 3] Hit position in world frame. Ray origin if no hit."""
pos_w: torch.Tensor
"""[B, 3] First frame position in world coordinates."""
quat_w: torch.Tensor
"""[B, 4] First frame orientation quaternion (w, x, y, z)."""
frame_pos_w: torch.Tensor
"""[B, F, 3] All frame positions in world coordinates."""
frame_quat_w: torch.Tensor
"""[B, F, 4] All frame orientations (w, x, y, z)."""
@dataclass
class RayCastSensorCfg(SensorCfg):
"""Raycast sensor configuration.
Supports multiple ray patterns (grid, pinhole camera, ring) and alignment modes.
"""
@dataclass
class VizCfg:
"""Visualization settings for debug rendering."""
hit_color: tuple[float, float, float, float] = (0.0, 1.0, 0.0, 0.8)
"""RGBA color for rays that hit a surface."""
miss_color: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.4)
"""RGBA color for rays that miss."""
hit_sphere_color: tuple[float, float, float, float] = (0.0, 1.0, 1.0, 1.0)
"""RGBA color for spheres drawn at hit points."""
hit_sphere_radius: float = 0.5
"""Radius of spheres drawn at hit points (multiplier of meansize)."""
show_rays: bool = False
"""Whether to draw ray arrows."""
show_normals: bool = False
"""Whether to draw surface normals at hit points."""
normal_color: tuple[float, float, float, float] = (1.0, 1.0, 0.0, 1.0)
"""RGBA color for surface normal arrows."""
normal_length: float = 5.0
"""Length of surface normal arrows (multiplier of meansize)."""
frame: ObjRef | tuple[ObjRef, ...]
"""Body, site, or geom to attach rays to.
Pass a single ``ObjRef`` for one frame, or a tuple for multi-frame
sensing (e.g. per-foot height sensors). Each frame's parent body is
excluded independently when ``exclude_parent_body`` is True.
"""
pattern: PatternCfg = field(default_factory=GridPatternCfg)
"""Ray pattern configuration. Defaults to GridPatternCfg."""
ray_alignment: RayAlignment = "base"
"""How rays are oriented relative to the frame. Controls direction only;
the ray origin is always the physical frame position (``site_xpos`` /
``geom_xpos`` / ``body_xpos``).
- "base": Full rotation (default). Rays rotate with the body.
- "yaw": Yaw only, ignores pitch/roll (good for height maps).
- "world": Fixed in world frame, rays always point the same direction.
"""
max_distance: float = 10.0
"""Maximum ray distance. Rays beyond this report -1."""
exclude_parent_body: bool = True
"""Exclude parent body from ray intersection tests."""
include_geom_groups: tuple[int, ...] | None = (0, 1, 2)
"""Geom groups (0-5) to include in raycasting.
Defaults to (0, 1, 2). Set to None to include all groups.
"""
debug_vis: bool = False
"""Enable debug visualization."""
viz: VizCfg = field(default_factory=VizCfg)
"""Visualization settings."""
def build(self) -> RayCastSensor:
return RayCastSensor(self)
class RayCastSensor(Sensor[RayCastData]):
"""Raycast sensor for terrain and obstacle detection."""
requires_sensor_context = True
def __init__(self, cfg: RayCastSensorCfg) -> None:
super().__init__()
self.cfg = cfg
self._data: mjwarp.Data | None = None
self._model: mjwarp.Model | None = None
self._mj_model: mujoco.MjModel | None = None
self._device: str | None = None
self._wp_device: wp.Device | None = None
# Per-frame info: list of (frame_type, obj_id, body_id).
self._frame_infos: list[tuple[Literal["body", "site", "geom"], int, int]] = []
self._num_frames: int = 0
self._num_rays_per_frame: int = 0
self._local_offsets: torch.Tensor | None = None
self._local_directions: torch.Tensor | None = None # [rays_per_frame, 3]
self._num_rays: int = 0
self._ray_pnt: wp.array | None = None
self._ray_vec: wp.array | None = None
self._ray_dist: wp.array | None = None
self._ray_geomid: wp.array | None = None
self._ray_normal: wp.array | None = None
self._ray_bodyexclude: wp.array | None = None
self._geomgroup = _vec6(-1, -1, -1, -1, -1, -1)
self._distances: torch.Tensor | None = None
self._normals_w: torch.Tensor | None = None
self._hit_pos_w: torch.Tensor | None = None
self._pos_w: torch.Tensor | None = None
self._quat_w: torch.Tensor | None = None
self._frame_pos_w: torch.Tensor | None = None
self._frame_quat_w: torch.Tensor | None = None
self._cached_world_origins: torch.Tensor | None = None
self._cached_world_rays: torch.Tensor | None = None
self._cached_frame_pos: torch.Tensor | None = None
self._cached_frame_mat: torch.Tensor | None = None
self._debug_vis_enabled: bool = True
self._ctx: SensorContext | None = None
def edit_spec(
self,
scene_spec: mujoco.MjSpec,
entities: dict[str, Entity],
) -> None:
del scene_spec, entities
def initialize(
self,
mj_model: mujoco.MjModel,
model: mjwarp.Model,
data: mjwarp.Data,
device: str,
) -> None:
self._data = data
self._model = model
self._mj_model = mj_model
self._device = device
self._wp_device = wp.get_device(device)
num_envs = data.nworld
# Normalize frame to tuple.
frames = self.cfg.frame
if isinstance(frames, ObjRef):
frames = (frames,)
# Resolve per-frame IDs.
self._frame_infos = []
for frame in frames:
frame_name = frame.prefixed_name()
info: tuple[Literal["body", "site", "geom"], int, int]
if frame.type == "body":
bid = mj_model.body(frame_name).id
info = ("body", bid, bid)
elif frame.type == "site":
sid = mj_model.site(frame_name).id
info = ("site", sid, int(mj_model.site_bodyid[sid]))
elif frame.type == "geom":
gid = mj_model.geom(frame_name).id
info = ("geom", gid, int(mj_model.geom_bodyid[gid]))
else:
raise ValueError(
f"RayCastSensor frame must be 'body', 'site', or 'geom', got '{frame.type}'"
)
self._frame_infos.append(info)
self._num_frames = len(self._frame_infos)
# Generate ray pattern.
pattern = self.cfg.pattern
self._local_offsets, self._local_directions = pattern.generate_rays(
mj_model, device
)
self._num_rays_per_frame = self._local_offsets.shape[0]
self._num_rays = self._num_frames * self._num_rays_per_frame
self._ray_pnt = wp.zeros((num_envs, self._num_rays), dtype=wp.vec3, device=device)
self._ray_vec = wp.zeros((num_envs, self._num_rays), dtype=wp.vec3, device=device)
self._ray_dist = wp.zeros((num_envs, self._num_rays), dtype=float, device=device)
self._ray_geomid = wp.zeros((num_envs, self._num_rays), dtype=int, device=device)
self._ray_normal = wp.zeros(
(num_envs, self._num_rays), dtype=wp.vec3, device=device
)
# Body exclusion: each frame's body_id repeated N times.
if self.cfg.exclude_parent_body:
body_excludes: list[int] = []
for _, _, body_id in self._frame_infos:
body_excludes.extend([body_id] * self._num_rays_per_frame)
else:
body_excludes = [-1] * self._num_rays
self._ray_bodyexclude = wp.array(body_excludes, dtype=int, device=device)
self._geomgroup = _geom_groups_to_vec6(self.cfg.include_geom_groups)
# Pre-allocate output tensors so shape inference works before
# the first sense() call.
F = self._num_frames
self._distances = torch.zeros(num_envs, self._num_rays, device=device)
self._normals_w = torch.zeros(num_envs, self._num_rays, 3, device=device)
self._hit_pos_w = torch.zeros(num_envs, self._num_rays, 3, device=device)
self._pos_w = torch.zeros(num_envs, 3, device=device)
self._quat_w = torch.zeros(num_envs, 4, device=device)
self._frame_pos_w = torch.zeros(num_envs, F, 3, device=device)
self._frame_quat_w = torch.zeros(num_envs, F, 4, device=device)
assert self._wp_device is not None
@property
def include_geom_groups(self) -> tuple[int, ...] | None:
return self.cfg.include_geom_groups
def set_context(self, ctx: SensorContext) -> None:
"""Wire this sensor to a SensorContext for BVH-accelerated raycasting."""
self._ctx = ctx
def _compute_data(self) -> RayCastData:
if self._ctx is None:
raise RuntimeError(
"RayCastSensor requires a SensorContext. "
"Ensure the sensor is part of a scene with "
"sim.sense() calls."
)
assert self._distances is not None and self._normals_w is not None
assert self._hit_pos_w is not None
assert self._pos_w is not None and self._quat_w is not None
assert self._frame_pos_w is not None and self._frame_quat_w is not None
return RayCastData(
distances=self._distances,
normals_w=self._normals_w,
hit_pos_w=self._hit_pos_w,
pos_w=self._pos_w,
quat_w=self._quat_w,
frame_pos_w=self._frame_pos_w,
frame_quat_w=self._frame_quat_w,
)
@property
def num_rays(self) -> int:
"""Total number of rays (num_frames * num_rays_per_frame)."""
return self._num_rays
@property
def num_frames(self) -> int:
"""Number of attachment frames."""
return self._num_frames
@property
def num_rays_per_frame(self) -> int:
"""Number of rays per attachment frame."""
return self._num_rays_per_frame
def debug_vis(self, visualizer: DebugVisualizer) -> None:
if not self.cfg.debug_vis or not self._debug_vis_enabled:
return
assert self._data is not None
assert self._local_offsets is not None
assert self._local_directions is not None
assert self._cached_frame_pos is not None
assert self._cached_frame_mat is not None
data = self.data
env_indices = list(visualizer.get_env_indices(data.distances.shape[0]))
if not env_indices:
return
F = self._num_frames
N = self._num_rays_per_frame
# Use cached frame poses [B, F, 3] / [B, F, 3, 3].
frame_pos = self._cached_frame_pos[env_indices] # [K, F, 3]
frame_mat = self._cached_frame_mat[env_indices] # [K, F, 3, 3]
K_envs = len(env_indices)
# Compute alignment rotation for all frames.
rot_mats = (
self._compute_alignment_rotation(frame_mat.view(K_envs * F, 3, 3))
.view(K_envs, F, 3, 3)
.cpu()
.numpy()
)
origins = frame_pos.cpu().numpy()
offsets = self._local_offsets.cpu().numpy()
directions = self._local_directions.cpu().numpy()
hit_positions = data.hit_pos_w[env_indices].cpu().numpy()
distances = data.distances[env_indices].cpu().numpy()
normals = data.normals_w[env_indices].cpu().numpy()
meansize = visualizer.meansize
ray_width = 0.1 * meansize
sphere_radius = self.cfg.viz.hit_sphere_radius * meansize
normal_length = self.cfg.viz.normal_length * meansize
normal_width = 0.1 * meansize
miss_extent = min(0.5, self.cfg.max_distance * 0.05)
name = self.cfg.name
for k in range(K_envs):
for f in range(F):
rot = rot_mats[k, f]
for i in range(N):
ray_idx = f * N + i
origin = origins[k, f] + rot @ offsets[i]
hit = distances[k, ray_idx] >= 0
if hit:
end = hit_positions[k, ray_idx]
color = self.cfg.viz.hit_color
else:
end = origin + rot @ directions[i] * miss_extent
color = self.cfg.viz.miss_color
if self.cfg.viz.show_rays:
visualizer.add_arrow(
start=origin,
end=end,
color=color,
width=ray_width,
label=f"{name}_ray_{ray_idx}",
)
if hit:
visualizer.add_sphere(
center=end,
radius=sphere_radius,
color=self.cfg.viz.hit_sphere_color,
label=f"{name}_hit_{ray_idx}",
)
if self.cfg.viz.show_normals:
normal_end = end + normals[k, ray_idx] * normal_length
visualizer.add_arrow(
start=end,
end=normal_end,
color=self.cfg.viz.normal_color,
width=normal_width,
label=f"{name}_normal_{ray_idx}",
)
# Private methods.
def prepare_rays(self) -> None:
"""PRE-GRAPH: Transform local rays to world frame.
Reads body/site/geom poses via PyTorch and writes world-frame ray
origins and directions into Warp arrays. Caches the frame pose and
world-frame tensors for postprocess_rays().
"""
assert self._data is not None and self._model is not None
assert self._local_offsets is not None and self._local_directions is not None
# Gather per-frame poses: [B, F, 3] and [B, F, 3, 3].
# Position is always the physical world position. Alignment only
# affects ray directions (applied to frame_mat below).
pos_list: list[torch.Tensor] = []
mat_list: list[torch.Tensor] = []
for frame_type, obj_id, _ in self._frame_infos:
if frame_type == "body":
pos_list.append(self._data.xpos[:, obj_id])
mat_list.append(self._data.xmat[:, obj_id].view(-1, 3, 3))
elif frame_type == "site":
pos_list.append(self._data.site_xpos[:, obj_id])
mat_list.append(self._data.site_xmat[:, obj_id].view(-1, 3, 3))
else: # geom
pos_list.append(self._data.geom_xpos[:, obj_id])
mat_list.append(self._data.geom_xmat[:, obj_id].view(-1, 3, 3))
frame_pos = torch.stack(pos_list, dim=1) # [B, F, 3]
frame_mat = torch.stack(mat_list, dim=1) # [B, F, 3, 3]
B, F = frame_pos.shape[:2]
N = self._num_rays_per_frame
# Compute alignment rotation for all frames at once.
rot_mat = self._compute_alignment_rotation(frame_mat.reshape(B * F, 3, 3)).reshape(
B, F, 3, 3
)
# Compute world offsets and directions: [B, F, N, 3].
world_offsets = torch.einsum("bfij,nj->bfni", rot_mat, self._local_offsets)
world_origins = frame_pos[:, :, None, :] + world_offsets
world_rays = torch.einsum("bfij,nj->bfni", rot_mat, self._local_directions)
# Flatten to [B, F*N, 3] for raycasting.
world_origins_flat = world_origins.reshape(B, F * N, 3)
world_rays_flat = world_rays.reshape(B, F * N, 3)
assert self._ray_pnt is not None and self._ray_vec is not None
pnt_torch = wp.to_torch(self._ray_pnt).view(B, self._num_rays, 3)
vec_torch = wp.to_torch(self._ray_vec).view(B, self._num_rays, 3)
pnt_torch.copy_(world_origins_flat)
vec_torch.copy_(world_rays_flat)
# Cache for postprocess_rays() and debug_vis().
self._cached_world_origins = world_origins_flat
self._cached_world_rays = world_rays_flat
self._cached_frame_pos = frame_pos # [B, F, 3]
self._cached_frame_mat = frame_mat # [B, F, 3, 3]
def raycast_kernel(self, rc: mjwarp.RenderContext) -> None:
"""IN-GRAPH: Execute BVH-accelerated raycast kernel."""
assert self._ray_pnt is not None
assert self._ray_vec is not None
assert self._ray_bodyexclude is not None
assert self._ray_dist is not None
assert self._ray_geomid is not None
assert self._ray_normal is not None
rays(
m=self._model.struct, # type: ignore[attr-defined]
d=self._data.struct, # type: ignore[attr-defined]
pnt=self._ray_pnt, # type: ignore[invalid-argument-type]
vec=self._ray_vec, # type: ignore[invalid-argument-type]
geomgroup=self._geomgroup, # pyright: ignore[reportArgumentType]
flg_static=True,
bodyexclude=self._ray_bodyexclude,
dist=self._ray_dist, # type: ignore[invalid-argument-type]
geomid=self._ray_geomid, # type: ignore[invalid-argument-type]
normal=self._ray_normal, # type: ignore[invalid-argument-type]
rc=rc,
)
def postprocess_rays(self) -> None:
"""POST-GRAPH: Convert Warp outputs to PyTorch, compute hit positions."""
assert self._cached_world_origins is not None
assert self._cached_world_rays is not None
assert self._cached_frame_pos is not None
assert self._cached_frame_mat is not None
B = self._cached_frame_pos.shape[0]
F = self._num_frames
assert self._ray_dist is not None and self._ray_normal is not None
distances = wp.to_torch(self._ray_dist)
normals_w = wp.to_torch(self._ray_normal).view(B, self._num_rays, 3)
distances.masked_fill_(distances > self.cfg.max_distance, -1.0)
hit_mask = distances >= 0
# ``origin + ray * max(distance, 0)`` collapses miss rays to ``origin``
# (clamped distance is 0) without any branching.
clamped = distances.clamp(min=0.0)
self._hit_pos_w = (
self._cached_world_origins + self._cached_world_rays * clamped.unsqueeze(-1)
)
# Zero out normals for misses.
normals_w.masked_fill_(~hit_mask.unsqueeze(-1), 0.0)
self._distances = distances
self._normals_w = normals_w
# All frames: [B, F, 3] / [B, F, 4].
self._frame_pos_w = self._cached_frame_pos
self._frame_quat_w = quat_from_matrix(
self._cached_frame_mat.reshape(B * F, 3, 3)
).reshape(B, F, 4)
# First frame for backward compat: [B, 3] / [B, 4].
assert self._frame_pos_w is not None and self._frame_quat_w is not None
self._pos_w = self._frame_pos_w[:, 0]
self._quat_w = self._frame_quat_w[:, 0]
# ``sense()`` runs after the decimation loop (which invalidates the cache)
# and after rewards may have already read ``.data``, repopulating the cache
# with pre-sense hit positions. Rebinding the tensors above leaves that
# cache pointing at the previous step's data, so invalidate here to force a
# recompute on the next ``.data`` access. Otherwise observations and debug
# visualization would lag one step behind the freshly sensed hits (#998).
self._invalidate_cache()
def _compute_alignment_rotation(self, frame_mat: torch.Tensor) -> torch.Tensor:
"""Compute rotation matrix based on ray_alignment setting."""
if self.cfg.ray_alignment == "base":
# Full rotation.
return frame_mat
elif self.cfg.ray_alignment == "yaw":
# Extract yaw only, zero out pitch/roll.
return self._extract_yaw_rotation(frame_mat)
elif self.cfg.ray_alignment == "world":
# Identity rotation (world-aligned).
num_envs = frame_mat.shape[0]
return (
torch.eye(3, device=frame_mat.device, dtype=frame_mat.dtype)
.unsqueeze(0)
.expand(num_envs, -1, -1)
)
else:
raise ValueError(f"Unknown ray_alignment: {self.cfg.ray_alignment}")
def _extract_yaw_rotation(self, rot_mat: torch.Tensor) -> torch.Tensor:
"""Extract yaw-only rotation matrix (rotation around Z axis).
Handles the singularity at ±90° pitch by falling back to the Y-axis
when the X-axis projection onto the XY plane is too small.
"""
batch_size = rot_mat.shape[0]
device = rot_mat.device
dtype = rot_mat.dtype
# Project X-axis onto XY plane.
x_axis = rot_mat[:, :, 0] # First column [B, 3]
x_proj = x_axis.clone()
x_proj[:, 2] = 0 # Zero out Z component
x_norm = x_proj.norm(dim=1) # [B]
# Check for singularity (X-axis nearly vertical).
threshold = 0.1
singular = x_norm < threshold # [B]
# For singular cases, use Y-axis instead.
if singular.any():
y_axis = rot_mat[:, :, 1] # Second column [B, 3]
y_proj = y_axis.clone()
y_proj[:, 2] = 0
y_norm = y_proj.norm(dim=1).clamp(min=1e-6)
y_proj = y_proj / y_norm.unsqueeze(-1)
# Y-axis points left; rotate -90° around Z to get forward direction.
# [y_x, y_y] -> [y_y, -y_x]
x_from_y = torch.zeros_like(y_proj)
x_from_y[:, 0] = y_proj[:, 1]
x_from_y[:, 1] = -y_proj[:, 0]
x_proj[singular] = x_from_y[singular]
x_norm[singular] = 1.0 # Already normalized
# Normalize X projection.
x_norm = x_norm.clamp(min=1e-6)
x_proj = x_proj / x_norm.unsqueeze(-1)
# Build yaw-only rotation matrix.
yaw_mat = torch.zeros((batch_size, 3, 3), device=device, dtype=dtype)
yaw_mat[:, 0, 0] = x_proj[:, 0]
yaw_mat[:, 1, 0] = x_proj[:, 1]
yaw_mat[:, 0, 1] = -x_proj[:, 1]
yaw_mat[:, 1, 1] = x_proj[:, 0]
yaw_mat[:, 2, 2] = 1
return yaw_mat