feat: add raycast camera perception phase one

examples/assets/raycast_camera_scene.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!-- Copyright Motphys Technology Co., Ltd. 2025, 2026
+     SPDX-License-Identifier: Apache-2.0 -->
+<mujoco>
+  <option gravity="0 0 -9.81"/>
+  <worldbody>
+    <geom name="floor" type="plane" size="10 10 0.1"/>
+    <geom name="back_wall" type="box" pos="0 5.5 2.5" size="4.5 0.15 2.5"/>
+    <geom name="left_wall" type="box" pos="-4.5 1.5 2.5" size="0.15 4.0 2.5"/>
+    <geom name="right_wall" type="box" pos="4.5 1.5 2.5" size="0.15 4.0 2.5"/>
+    <geom name="tower_left" type="box" pos="-2.0 2.0 1.4" size="0.65 0.65 1.4"/>
+    <geom name="tower_center" type="box" pos="0.0 3.2 0.8" size="0.55 0.55 0.8"/>
+    <geom name="tower_right" type="box" pos="2.0 2.4 2.0" size="0.75 0.75 2.0"/>
+    <geom name="sphere" type="sphere" pos="-0.9 1.5 1.0" size="0.75"/>
+    <geom name="cylinder" type="cylinder" pos="1.0 1.7 0.65" size="0.55 0.65"/>
+    <geom name="ramp" type="box" pos="0.0 4.3 0.35" size="1.8 0.7 0.35" euler="0 18 0"/>
+    <body name="camera_mount" pos="0 -4.0 1.6"/>
+  </worldbody>
+</mujoco>

examples/raycast_camera.py

@@ -0,0 +1,191 @@
+# Copyright Motphys Technology Co., Ltd. 2025, 2026
+# SPDX-License-Identifier: Apache-2.0
+
+"""Show a RaycastCamera and its hit points in the MotrixSim viewer.
+
+The viewer shows the RaycastCamera depth image in an image widget, while the
+scene camera is shown as a green sphere with a sparse set of 3D rays. Yellow
+rays hit an obstacle; gray rays are misses. Hit points are shown as red spheres.
+Misses are blue in the depth widget; nearer surfaces are brighter.
+
+Run from the repository root::
+
+    python examples/raycast_camera.py
+
+Close the MotrixSim window (or press Escape) to exit.
+"""
+
+from __future__ import annotations
+
+import time
+from pathlib import Path
+
+import motrixsim as mtx
+import numpy as np
+from motrixsim.render import Color, Layout, RenderApp, RenderSettings
+
+from motrix_env_core.config import configclass
+from motrix_env_core.config.scene.base import SceneCfg, SceneObjsCfg, SceneSensorsCfg, SceneVisualCfg
+from motrix_env_core.config.scene.light import LightCfg
+from motrix_env_core.config.scene.sensor import PinholeCameraCfg, RaycastCameraSensorCfg
+from motrix_env_core.config.scene.site import SiteCfg
+from motrix_env_core.config.sim import SimCfg
+from motrix_env_motrixsim.compiler import build_scene_model
+
+SCENE_PATH = Path(__file__).with_name("assets") / "raycast_camera_scene.xml"
+CAMERA_SITE = "camera_mount"
+CAMERA_WIDTH = 48
+CAMERA_HEIGHT = 27
+CAMERA_FAR = 10.0
+CAMERA_POSITION = np.array([0.0, -4.0, 1.6], dtype=np.float32)
+CAMERA_ROTATION = np.array([-0.70710678, 0.0, 0.0, 0.70710678], dtype=np.float32)
+
+
+@configclass
+class ExampleObjects(SceneObjsCfg):
+    camera_mount: SiteCfg = SiteCfg(
+        parent_link_name="camera_mount",
+        rotation=tuple(float(value) for value in CAMERA_ROTATION),
+    )
+    key_light: LightCfg = LightCfg(
+        position=(-4.0, -4.0, 8.0),
+        direction=(0.35, 0.55, -1.0),
+        color=(1.0, 0.92, 0.78),
+        illuminance=18_000.0,
+        cast_shadows=True,
+    )
+    fill_light: LightCfg = LightCfg(
+        position=(4.0, -1.0, 5.0),
+        direction=(-0.4, 0.5, -0.8),
+        color=(0.65, 0.78, 1.0),
+        illuminance=6_000.0,
+        cast_shadows=False,
+    )
+
+
+@configclass
+class ExampleSensors(SceneSensorsCfg):
+    camera: RaycastCameraSensorCfg = RaycastCameraSensorCfg(
+        site=CAMERA_SITE,
+        width=CAMERA_WIDTH,
+        height=CAMERA_HEIGHT,
+        projection=PinholeCameraCfg.from_fov(
+            width=CAMERA_WIDTH,
+            height=CAMERA_HEIGHT,
+            horizontal_fov=70.0,
+        ),
+        near=0.05,
+        far=CAMERA_FAR,
+        outputs=("depth", "point"),
+        exclude="parent_body",
+    )
+
+
+def _camera_rays() -> tuple[np.ndarray, np.ndarray]:
+    """Return sparse local ray directions and their image coordinates."""
+    projection = ExampleSensors().camera.projection
+    assert isinstance(projection, PinholeCameraCfg)
+    rows = np.arange(1, CAMERA_HEIGHT, 3, dtype=np.float32)
+    cols = np.arange(1, CAMERA_WIDTH, 3, dtype=np.float32)
+    u, v = np.meshgrid(cols, rows)
+    directions = np.stack(
+        ((u + 0.5 - projection.cx) / projection.fx, (v + 0.5 - projection.cy) / projection.fy, np.ones_like(u)),
+        axis=-1,
+    )
+    directions /= np.linalg.norm(directions, axis=-1, keepdims=True)
+    return directions.reshape(-1, 3), np.stack((v, u), axis=-1).reshape(-1, 2).astype(np.int64)
+
+
+def _depth_image(depth: np.ndarray, max_depth: float) -> np.ndarray:
+    """Convert metric depth into an RGBA preview for the image widget."""
+    valid = depth >= 0.0
+    intensity = np.zeros(depth.shape, dtype=np.uint8)
+    intensity[valid] = (255.0 * (1.0 - np.clip(depth[valid] / max_depth, 0.0, 1.0))).astype(np.uint8)
+    pixels = np.full((*depth.shape, 4), 255, dtype=np.uint8)
+    pixels[..., :3] = intensity[..., None]
+    pixels[~valid, :3] = (24, 40, 80)
+    return pixels
+
+
+def _draw_camera_result(render: RenderApp, depth: np.ndarray, points: np.ndarray) -> None:
+    """Draw a sparse 3D visualization of the latest camera image."""
+    directions, pixels = _camera_rays()
+    # The camera is rotated -90 degrees around X: local +Z points toward +Y.
+    world_directions = np.stack((directions[:, 0], directions[:, 2], -directions[:, 1]), axis=-1)
+    render.gizmos.draw_sphere(0.08, CAMERA_POSITION, color=Color.rgb(0.1, 1.0, 0.2))
+    render.gizmos.draw_axes(CAMERA_POSITION, CAMERA_ROTATION.tolist(), 0.35)
+
+    for direction, (row, col) in zip(world_directions, pixels):
+        hit_depth = float(depth[row, col])
+        if hit_depth >= 0.0:
+            endpoint = points[row, col]
+            color = Color.rgb(1.0, 0.85, 0.1)
+            render.gizmos.draw_sphere(0.025, endpoint, color=Color.rgb(1.0, 0.1, 0.1))
+        else:
+            endpoint = CAMERA_POSITION + direction * CAMERA_FAR
+            color = Color.rgb(0.45, 0.45, 0.45)
+        render.gizmos.draw_ray(CAMERA_POSITION, endpoint - CAMERA_POSITION, color=color)
+
+
+def main() -> None:
+    if not hasattr(mtx.msd, "RaycastCameraSensor"):
+        raise RuntimeError(
+            "This example requires a MotrixSim build with RaycastCamera support; "
+            "the installed motrixsim-core does not expose msd.RaycastCameraSensor."
+        )
+    scene = SceneCfg(
+        file=SCENE_PATH,
+        objs=ExampleObjects(),
+        sensors=ExampleSensors(),
+        visual=SceneVisualCfg(
+            ambient_light_color=(0.22, 0.24, 0.30),
+            ambient_light_brightness=900.0,
+            head_light_color=(0.55, 0.62, 0.75),
+            head_light_luminous_power=700.0,
+            haze=(0.035, 0.045, 0.07, 1.0),
+            tone_mapping="aces",
+        ),
+    )
+    model = build_scene_model(scene, SimCfg(dt=0.01))
+    data = mtx.SceneData(model, batch=[1])
+    settings = RenderSettings.quality()
+    settings.enable_shadow = True
+    with RenderApp() as render:
+        render.launch(
+            model,
+            batch=1,
+            render_offset=[[0.0, 0.0, 0.0]],
+            render_settings=settings,
+        )
+        preview = render.create_image(_depth_image(np.full((CAMERA_HEIGHT, CAMERA_WIDTH), -1.0), CAMERA_FAR))
+        render.widgets.create_image_widget(
+            preview,
+            layout=Layout(right=16, top=16, width=384, height=216),
+        )
+        render.system_camera.set_view(
+            lookat=[0.0, 2.0, 1.4],
+            distance=10.0,
+            elevation=-18.0,
+            azimuth=180.0,
+        )
+        render.system_camera.active = True
+        print("RaycastCamera viewer started")
+        print("Lighting: warm key light, cool fill light, soft ambient light, and shadows.")
+        print("Depth image widget: near is bright, far is dark, blue means miss.")
+        print("Yellow rays hit geometry; gray rays miss; red spheres mark hit points.")
+        print("Close the MotrixSim window or press Escape to exit.")
+
+        while not render.is_closed:
+            model.step(data)
+            raw = model.get_sensor_value("camera", data)
+            rays = np.asarray(raw[0], dtype=np.float32).reshape(CAMERA_HEIGHT, CAMERA_WIDTH, 4)
+            depth = rays[..., 0]
+            points = rays[..., 1:4]
+            preview.pixels = _depth_image(depth, CAMERA_FAR)
+            _draw_camera_result(render, depth, points)
+            render.sync(data=data)
+            time.sleep(1.0 / 60.0)
+
+
+if __name__ == "__main__":
+    main()

motrix_env_core/src/motrix_env_core/config/scene/__init__.py

@@ -40,6 +40,7 @@
     FrameSensorCfg,
     FrameSensorType,
 )
+from motrix_env_core.config.scene.site import SiteCfg
 from motrix_env_core.config.scene.terrain import (
     CompositeTerrainGeneratorCfg,
     DiscreteObstaclesTerrainGeneratorCfg,
@@ -52,7 +53,6 @@
 )
 from motrix_env_core.config.scene.urdf import (
     JointCfg,
-    SiteCfg,
     UrdfFileCfg,
     UrdfGeomCfg,
 )

motrix_env_core/src/motrix_env_core/config/scene/sensor.py

@@ -1,11 +1,127 @@
 # Copyright Motphys Technology Co., Ltd. 2025, 2026
 # SPDX-License-Identifier: Apache-2.0
 
+import math
 from enum import Enum
+from typing import Literal, TypeAlias
 
 from motrix_env_core.config import configclass
 from motrix_env_core.config.scene.base import SceneSensorCfg
 
+RaycastCameraOutput: TypeAlias = Literal[
+    "depth",
+    "distance",
+    "point",
+    "normal",
+]
+RaycastCameraExclude: TypeAlias = Literal["none", "parent_body", "parent_subtree"]
+_RAYCAST_CAMERA_OUTPUTS: tuple[RaycastCameraOutput, ...] = (
+    "depth",
+    "distance",
+    "point",
+    "normal",
+)
+
+
+@configclass(kw_only=True)
+class PinholeCameraCfg:
+    """Pixel intrinsics for +X right, +Y down, +Z forward optical coordinates.
+
+    Rays pass through pixel centers ``(u + 0.5, v + 0.5)``. Principal points
+    outside the image are supported; only focal lengths must be positive.
+    """
+
+    fx: float
+    fy: float
+    cx: float
+    cy: float
+
+    def validate(self) -> None:
+        if not all(math.isfinite(value) for value in (self.fx, self.fy, self.cx, self.cy)):
+            raise ValueError("PinholeCameraCfg intrinsics must be finite")
+        if self.fx <= 0 or self.fy <= 0:
+            raise ValueError("PinholeCameraCfg focal lengths must be positive")
+
+    @classmethod
+    def from_fov(
+        cls, *, width: int, height: int, horizontal_fov: float, vertical_fov: float | None = None
+    ) -> "PinholeCameraCfg":
+        """Create centered intrinsics from FOV angles in degrees.
+
+        Omitting vertical FOV uses square pixels (``fy == fx``), deriving the
+        vertical field of view from the image aspect ratio.
+        """
+        if type(width) is not int or type(height) is not int or width <= 0 or height <= 0:
+            raise ValueError("Camera width and height must be positive integers")
+        for angle in (horizontal_fov, vertical_fov):
+            if angle is not None and (not math.isfinite(angle) or not 0 < angle < 180):
+                raise ValueError("Camera FOV must be finite and strictly between 0 and 180 degrees")
+        fx = width / (2 * math.tan(math.radians(horizontal_fov) / 2))
+        fy = fx if vertical_fov is None else height / (2 * math.tan(math.radians(vertical_fov) / 2))
+        return cls(fx=fx, fy=fy, cx=width / 2, cy=height / 2)
+
+
+@configclass(kw_only=True)
+class OrthographicCameraCfg:
+    """Spacing in meters between parallel +Z rays on a centered pixel grid."""
+
+    pixel_size_x: float
+    pixel_size_y: float
+
+    def validate(self) -> None:
+        if any(not math.isfinite(value) or value <= 0 for value in (self.pixel_size_x, self.pixel_size_y)):
+            raise ValueError("OrthographicCameraCfg pixel sizes must be finite and positive")
+
+
+@configclass(kw_only=True)
+class RaycastCameraSensorCfg(SceneSensorCfg):
+    """Physics raycast camera mounted on a named site in the assembled scene.
+
+    Near/far are inclusive optical-axis depths, not distances along oblique rays.
+    Filtering occurs before closest-hit selection; range rejection occurs after it,
+    so a surface closer than near still occludes farther surfaces. A zero-distance
+    nearest hit invalidates the pixel. ``update_period=0`` samples every physics
+    step; other values are seconds. Group IDs are collision geometry groups (0..255),
+    not collision masks: None includes all groups and [] includes none.
+    """
+
+    site: str
+    width: int
+    height: int
+    projection: PinholeCameraCfg | OrthographicCameraCfg
+    far: float
+    near: float = 0.0
+    update_period: float = 0.0
+    exclude: RaycastCameraExclude = "parent_body"
+    include_geom_groups: list[int] | None = None
+    outputs: tuple[RaycastCameraOutput, ...] = ("depth",)
+
+    def validate(self, name: str) -> None:
+        super().validate(name)
+        if not self.site:
+            raise ValueError("RaycastCameraSensorCfg.site must not be empty")
+        if type(self.width) is not int or type(self.height) is not int or self.width <= 0 or self.height <= 0:
+            raise ValueError("RaycastCameraSensorCfg width and height must be positive integers")
+        if not isinstance(self.projection, (PinholeCameraCfg, OrthographicCameraCfg)):
+            raise TypeError("RaycastCameraSensorCfg.projection must be PinholeCameraCfg or OrthographicCameraCfg")
+        self.projection.validate()
+        if not math.isfinite(self.near) or not math.isfinite(self.far) or not 0 <= self.near < self.far:
+            raise ValueError("RaycastCameraSensorCfg requires finite 0 <= near < far")
+        if not math.isfinite(self.update_period) or self.update_period < 0:
+            raise ValueError("RaycastCameraSensorCfg.update_period must be finite and non-negative")
+        if self.exclude not in {"none", "parent_body", "parent_subtree"}:
+            raise ValueError("RaycastCameraSensorCfg.exclude must be one of: none, parent_body, parent_subtree")
+        if self.include_geom_groups is not None and any(
+            type(group) is not int or not 0 <= group <= 255 for group in self.include_geom_groups
+        ):
+            raise ValueError("RaycastCameraSensorCfg.include_geom_groups must contain integers in 0..255")
+        if not self.outputs or any(field not in _RAYCAST_CAMERA_OUTPUTS for field in self.outputs):
+            raise ValueError(f"RaycastCameraSensorCfg.outputs must contain only {_RAYCAST_CAMERA_OUTPUTS}")
+        if len(set(self.outputs)) != len(self.outputs):
+            raise ValueError("RaycastCameraSensorCfg.outputs must be unique")
+        if not {"depth", "distance"}.intersection(self.outputs):
+            raise ValueError("RaycastCameraSensorCfg.outputs requires at least one distance field")
+
 
 class ContactReportField(str, Enum):
     """Fields reported by contact sensors."""

motrix_env_core/src/motrix_env_core/config/scene/site.py

@@ -0,0 +1,32 @@
+# Copyright Motphys Technology Co., Ltd. 2025, 2026
+# SPDX-License-Identifier: Apache-2.0
+
+from motrix_env_core.config import configclass
+from motrix_env_core.config.scene._utils import Vec3, Vec4, optional_vec
+from motrix_env_core.config.scene.base import SceneObjCfg
+
+
+@configclass(kw_only=True)
+class SiteCfg(SceneObjCfg):
+    """Create a site on a named logical link.
+
+    ``parent_link_name`` uses the imported link name before a containing
+    ``BodyCfg`` applies its prefix or suffix. When used as a scene object, the
+    field name is the site name. When used in ``UrdfFileCfg.sites``, ``name``
+    must be provided explicitly. Sites are mounted after all bodies are
+    imported, so declaration order does not affect link resolution.
+    """
+
+    parent_link_name: str
+    name: str | None = None
+    position: Vec3 = (0.0, 0.0, 0.0)
+    rotation: Vec4 = (0.0, 0.0, 0.0, 1.0)
+
+    def validate(self, object_name: str | None = None) -> None:
+        super().validate(object_name or self.name or "site")
+        if self.name is not None and not self.name:
+            raise ValueError("SiteCfg.name must not be empty when provided")
+        if not self.parent_link_name:
+            raise ValueError("SiteCfg.parent_link_name must not be empty")
+        optional_vec("SiteCfg.position", self.position, 3)
+        optional_vec("SiteCfg.rotation", self.rotation, 4)