"""Shared render context for camera and raycast sensors.""" from __future__ import annotations import warnings from collections.abc import Sequence from typing import TYPE_CHECKING import mujoco import mujoco_warp as mjwarp import torch import warp as wp if TYPE_CHECKING: from mjlab.sensor.camera_sensor import CameraSensor from mjlab.sensor.raycast_sensor import RayCastSensor @wp.kernel def _unpack_rgb_kernel( packed: wp.array2d(dtype=wp.uint32), # type: ignore[valid-type] rgb: wp.array3d(dtype=wp.uint8), # type: ignore[valid-type] ): """Unpack ABGR uint32 pixels into separate R, G, B uint8 channels.""" world_idx, pixel_idx = wp.tid() # type: ignore[attr-defined] val = packed[world_idx, pixel_idx] b = wp.uint8(val & wp.uint32(0xFF)) g = wp.uint8((val >> wp.uint32(8)) & wp.uint32(0xFF)) r = wp.uint8((val >> wp.uint32(16)) & wp.uint32(0xFF)) rgb[world_idx, pixel_idx, 0] = r rgb[world_idx, pixel_idx, 1] = g rgb[world_idx, pixel_idx, 2] = b # Fields that require per-pixel ray recomputation at render time. _RAY_FIELDS = frozenset({"cam_fovy", "cam_intrinsic"}) class SensorContext: """Container for shared sensing resources. Manages the RenderContext used by both camera sensors (for rendering) and raycast sensors (for BVH-accelerated ray intersection). The actual graph capture and execution is handled by Simulation. """ def __init__( self, mj_model: mujoco.MjModel, model: mjwarp.Model, data: mjwarp.Data, camera_sensors: list[CameraSensor], raycast_sensors: Sequence[RayCastSensor], device: str, ): self._model = model self._data = data self.wp_device = wp.get_device(device) # Sort camera sensors by camera index for consistent ordering. self.camera_sensors = sorted(camera_sensors, key=lambda s: s.camera_idx) self.raycast_sensors = raycast_sensors # MuJoCo camera ID -> sorted list index. self._cam_idx_to_list_idx = { s.camera_idx: idx for idx, s in enumerate(self.camera_sensors) } if self.camera_sensors: self._validate_sensor_settings() self._rgb_unpacked: wp.array | None = None self._rgb_torch: torch.Tensor | None = None self._depth_torch: torch.Tensor | None = None self._seg_torch: torch.Tensor | None = None self._rgb_adr_np: list[int] | None = None self._depth_adr_np: list[int] | None = None self._seg_adr_np: list[int] | None = None self._disable_precomputed_rays = False self._create_context(mj_model) # Wire up sensors to use this context. for sensor in self.camera_sensors: sensor.set_context(self) for sensor in self.raycast_sensors: sensor.set_context(self) @property def has_cameras(self) -> bool: return len(self.camera_sensors) > 0 @property def has_raycasts(self) -> bool: return len(self.raycast_sensors) > 0 @property def render_context(self) -> mjwarp.RenderContext: return self._ctx def recreate( self, mj_model: mujoco.MjModel, expanded_fields: set[str] | None = None, ) -> None: """Recreate the render context after model fields are expanded. Called by Simulation.expand_model_fields() for domain randomization. """ if expanded_fields is not None: self._disable_precomputed_rays = bool(expanded_fields & _RAY_FIELDS) self._create_context(mj_model) def prepare(self) -> None: """Pre-graph: transform rays to world frame.""" for sensor in self.raycast_sensors: sensor.prepare_rays() def finalize(self) -> None: """Post-graph: compute raycast hit positions.""" for sensor in self.raycast_sensors: sensor.postprocess_rays() # Fresh render results: drop camera data cached by a pre-sense read so # observations serve this step's frames (mirrors the raycast # invalidation in postprocess_rays). for sensor in self.camera_sensors: sensor._invalidate_cache() def get_rgb(self, cam_idx: int) -> torch.Tensor: """Get unpacked RGB data for a camera. Args: cam_idx: MuJoCo camera ID. Returns: Tensor of shape [num_envs, height, width, 3] (uint8). """ if self._rgb_unpacked is None: raise RuntimeError( "RGB rendering is not enabled. Ensure at least one camera sensor has 'rgb' in" " its data_types." ) if cam_idx not in self._cam_idx_to_list_idx: available = list(self._cam_idx_to_list_idx.keys()) raise KeyError( f"Camera ID {cam_idx} not found in SensorContext. " f"Available camera IDs: {available}" ) list_idx = self._cam_idx_to_list_idx[cam_idx] assert self._rgb_adr_np is not None assert self._rgb_torch is not None rgb_adr = self._rgb_adr_np[list_idx] if rgb_adr < 0: raise RuntimeError(f"Camera ID {cam_idx} does not have RGB rendering enabled.") sensor = self.camera_sensors[list_idx] w, h = sensor.cfg.width, sensor.cfg.height num_pixels = w * h nworld = self._data.nworld cam_data = self._rgb_torch[:, rgb_adr : rgb_adr + num_pixels, :] return cam_data.view(nworld, h, w, 3) def get_depth(self, cam_idx: int) -> torch.Tensor: """Get depth data for a camera. Args: cam_idx: MuJoCo camera ID. Returns: Tensor of shape [num_envs, height, width, 1] (float32). """ if cam_idx not in self._cam_idx_to_list_idx: available = list(self._cam_idx_to_list_idx.keys()) raise KeyError( f"Camera ID {cam_idx} not found in SensorContext. " f"Available camera IDs: {available}" ) list_idx = self._cam_idx_to_list_idx[cam_idx] assert self._depth_adr_np is not None assert self._depth_torch is not None depth_adr = self._depth_adr_np[list_idx] if depth_adr < 0: raise RuntimeError(f"Camera ID {cam_idx} does not have depth rendering enabled.") sensor = self.camera_sensors[list_idx] w, h = sensor.cfg.width, sensor.cfg.height num_pixels = w * h nworld = self._data.nworld cam_data = self._depth_torch[:, depth_adr : depth_adr + num_pixels] return cam_data.view(nworld, h, w, 1) def get_segmentation(self, cam_idx: int) -> torch.Tensor: """Get per-pixel segmentation data for a camera. Args: cam_idx: MuJoCo camera ID. Returns: Tensor of shape [num_envs, height, width, 2] (int32). Channel 0 stores object IDs. Channel 1 stores MuJoCo object types. """ if cam_idx not in self._cam_idx_to_list_idx: available = list(self._cam_idx_to_list_idx.keys()) raise KeyError( f"Camera ID {cam_idx} not found in SensorContext. " f"Available camera IDs: {available}" ) list_idx = self._cam_idx_to_list_idx[cam_idx] assert self._seg_adr_np is not None assert self._seg_torch is not None seg_adr = self._seg_adr_np[list_idx] if seg_adr < 0: raise RuntimeError( f"Camera ID {cam_idx} does not have segmentation rendering enabled." ) sensor = self.camera_sensors[list_idx] w, h = sensor.cfg.width, sensor.cfg.height num_pixels = w * h nworld = self._data.nworld cam_data = self._seg_torch[:, seg_adr : seg_adr + num_pixels, :] return cam_data.view(nworld, h, w, 2) # Private methods. def _validate_sensor_settings(self) -> None: """Validate that all camera sensors share render settings.""" ref = self.camera_sensors[0].cfg for s in self.camera_sensors[1:]: cfg = s.cfg if cfg.use_textures != ref.use_textures: raise ValueError( "All camera sensors must share the same use_textures " f"setting. '{s.cfg.name}' differs from " f"'{ref.name}'." ) if cfg.use_shadows != ref.use_shadows: raise ValueError( "All camera sensors must share the same use_shadows " f"setting. '{s.cfg.name}' differs from " f"'{ref.name}'." ) if cfg.enabled_geom_groups != ref.enabled_geom_groups: raise ValueError( "All camera sensors must share the same " f"enabled_geom_groups. '{s.cfg.name}' differs from " f"'{ref.name}'." ) def _raycast_geom_groups(self) -> set[int]: """Compute the union of geom groups across all raycast sensors.""" groups: set[int] = set() for s in self.raycast_sensors: if s.include_geom_groups is None: return {0, 1, 2, 3, 4, 5} groups.update(s.include_geom_groups) return groups def _create_context(self, mj_model: mujoco.MjModel) -> None: """Create the mujoco_warp RenderContext.""" ncam = mj_model.ncam cam_res: list[tuple[int, int]] | None = None render_rgb: list[bool] | None = None render_depth: list[bool] | None = None render_seg: list[bool] | None = None raycast_geom_groups = self._raycast_geom_groups() if self.camera_sensors: ref_cfg = self.camera_sensors[0].cfg use_textures = ref_cfg.use_textures use_shadows = ref_cfg.use_shadows cam_geom_groups = set(ref_cfg.enabled_geom_groups) if self.raycast_sensors and raycast_geom_groups != cam_geom_groups: merged = sorted(cam_geom_groups | raycast_geom_groups) warnings.warn( "Camera enabled_geom_groups " f"{sorted(cam_geom_groups)} and raycast " f"include_geom_groups {sorted(raycast_geom_groups)}" f" differ. Using union {merged}.", stacklevel=2, ) enabled_geom_groups = merged else: enabled_geom_groups = sorted(cam_geom_groups) # Build cam_active mask: only activate cameras that are sensors. # cam_res, render_rgb, render_depth follow sorted sensor order (by camera_idx). # This must match the order used by create_render_context for rgb_adr/depth_adr # indexing. cam_active = [False] * ncam cam_res = [] render_rgb = [] render_depth = [] render_seg = [] for s in self.camera_sensors: cam_active[s.camera_idx] = True cam_res.append((s.cfg.width, s.cfg.height)) render_rgb.append("rgb" in s.cfg.data_types) render_depth.append("depth" in s.cfg.data_types) render_seg.append("segmentation" in s.cfg.data_types) else: # Raycasts-only: need BVH but no camera rendering. use_textures = False use_shadows = False enabled_geom_groups = sorted(raycast_geom_groups) cam_active = [False] * ncam with wp.ScopedDevice(self.wp_device): self._ctx = mjwarp.create_render_context( mjm=mj_model, nworld=self._data.nworld, cam_res=cam_res, render_rgb=render_rgb, render_depth=render_depth, use_textures=use_textures, use_shadows=use_shadows, enabled_geom_groups=enabled_geom_groups, cam_active=cam_active, use_precomputed_rays=not self._disable_precomputed_rays, render_seg=render_seg, render_skybox=True, background_color=(0.0, 0.0, 0.0, 1.0), ) # Cache address arrays from the render context. An adr value of -1 means that data # type is not enabled for that camera. if self.camera_sensors: self._rgb_adr_np = self._ctx.rgb_adr.numpy().tolist() self._depth_adr_np = self._ctx.depth_adr.numpy().tolist() self._seg_adr_np = self._ctx.seg_adr.numpy().tolist() has_any_rgb = self._rgb_adr_np is not None and any(a >= 0 for a in self._rgb_adr_np) has_any_depth = self._depth_adr_np is not None and any( a >= 0 for a in self._depth_adr_np ) has_any_seg = self._seg_adr_np is not None and any(a >= 0 for a in self._seg_adr_np) # Allocate RGB unpack buffer if any camera wants RGB. if has_any_rgb: total_rgb_pixels = self._ctx.rgb_data.shape[1] nworld = self._data.nworld self._rgb_unpacked = wp.zeros( (nworld, total_rgb_pixels, 3), dtype=wp.uint8, device=self.wp_device, ) self._rgb_torch = wp.to_torch(self._rgb_unpacked) else: self._rgb_unpacked = None self._rgb_torch = None # Cache depth torch view (zero-copy). if has_any_depth: self._depth_torch = wp.to_torch(self._ctx.depth_data) else: self._depth_torch = None # Cache segmentation torch view (zero-copy). if has_any_seg: self._seg_torch = wp.to_torch(self._ctx.seg_data) else: self._seg_torch = None def unpack_rgb(self) -> None: """Unpack packed uint32 RGB data into separate channels. Called from Simulation._sense_kernel() so it gets captured in the CUDA graph. No-op if no cameras need RGB. """ if self._rgb_unpacked is not None: wp.launch( _unpack_rgb_kernel, dim=(self._data.nworld, self._ctx.rgb_data.shape[1]), inputs=[self._ctx.rgb_data], outputs=[self._rgb_unpacked], device=self.wp_device, )