diff --git a/examples/assets/raycast_camera_scene.xml b/examples/assets/raycast_camera_scene.xml new file mode 100644 index 00000000..4ec126fa --- /dev/null +++ b/examples/assets/raycast_camera_scene.xml @@ -0,0 +1,19 @@ + + + + diff --git a/examples/raycast_camera.py b/examples/raycast_camera.py new file mode 100644 index 00000000..c9c984c6 --- /dev/null +++ b/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() diff --git a/motrix_env_core/src/motrix_env_core/config/scene/__init__.py b/motrix_env_core/src/motrix_env_core/config/scene/__init__.py index 013f8dad..e21df208 100644 --- a/motrix_env_core/src/motrix_env_core/config/scene/__init__.py +++ b/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, ) diff --git a/motrix_env_core/src/motrix_env_core/config/scene/mjcf.py b/motrix_env_core/src/motrix_env_core/config/scene/mjcf.py index 2e34d4f5..bd7f0671 100644 --- a/motrix_env_core/src/motrix_env_core/config/scene/mjcf.py +++ b/motrix_env_core/src/motrix_env_core/config/scene/mjcf.py @@ -3,8 +3,16 @@ from motrix_env_core.config import configclass from motrix_env_core.config.scene.base import ModelFileCfg +from motrix_env_core.config.scene.site import SiteCfg @configclass(kw_only=True) class MjcfFileCfg(ModelFileCfg): - """An MJCF model loaded from a file.""" + """An MJCF model loaded from a file, augmented with simulation-only sites.""" + + sites: list[SiteCfg] = [] + + def validate(self) -> None: + super().validate() + for site in self.sites: + site.validate() diff --git a/motrix_env_core/src/motrix_env_core/config/scene/sensor.py b/motrix_env_core/src/motrix_env_core/config/scene/sensor.py index 5a0ad129..c6a4cd61 100644 --- a/motrix_env_core/src/motrix_env_core/config/scene/sensor.py +++ b/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.""" diff --git a/motrix_env_core/src/motrix_env_core/config/scene/site.py b/motrix_env_core/src/motrix_env_core/config/scene/site.py new file mode 100644 index 00000000..fae21400 --- /dev/null +++ b/motrix_env_core/src/motrix_env_core/config/scene/site.py @@ -0,0 +1,35 @@ +# 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 the parent link belongs to a + ``BodyCfg`` model, the site mounts before attach, so both its name and its + parent follow that body's decoration. Otherwise the site mounts after all + bodies are imported with its plain name on the literal parent link. When + used as a scene object the field name is the site name; in a model file + config's ``sites`` (``UrdfFileCfg``/``MjcfFileCfg``) ``name`` must be + provided explicitly. + """ + + 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) diff --git a/motrix_env_core/src/motrix_env_core/config/scene/urdf.py b/motrix_env_core/src/motrix_env_core/config/scene/urdf.py index 7dc25b63..9a00a379 100644 --- a/motrix_env_core/src/motrix_env_core/config/scene/urdf.py +++ b/motrix_env_core/src/motrix_env_core/config/scene/urdf.py @@ -2,8 +2,9 @@ # 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, resolve_path +from motrix_env_core.config.scene._utils import Vec3, optional_vec, resolve_path from motrix_env_core.config.scene.base import ModelFileCfg +from motrix_env_core.config.scene.site import SiteCfg @configclass(kw_only=True) @@ -44,24 +45,6 @@ def validate(self) -> None: raise ValueError(f"UrdfGeomCfg.priority must be non-negative, got {self.priority}") -@configclass(kw_only=True) -class SiteCfg: - """A simulation-only reference site attached to an imported URDF link.""" - - name: str - parent_link_name: str - position: Vec3 = (0.0, 0.0, 0.0) - rotation: Vec4 = (0.0, 0.0, 0.0, 1.0) - - def validate(self) -> None: - if not self.name: - raise ValueError("SiteCfg.name must not be empty") - 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) - - @configclass(kw_only=True) class UrdfFileCfg(ModelFileCfg): """A URDF model augmented with simulation-only sites and joint properties.""" diff --git a/motrix_env_core/src/motrix_env_core/sim/__init__.py b/motrix_env_core/src/motrix_env_core/sim/__init__.py index a2f5c3a3..c8fe1536 100644 --- a/motrix_env_core/src/motrix_env_core/sim/__init__.py +++ b/motrix_env_core/src/motrix_env_core/sim/__init__.py @@ -46,6 +46,7 @@ LinkPositionQuery, LinkQuaternionQuery, PhysicsReadProgram, + RaycastCameraImageQuery, SensorValuesQuery, SimDataQuery, SimDataQueryCompiler, @@ -113,6 +114,7 @@ "SimModel", "SimModelCompiler", "PhysicsReadProgram", + "RaycastCameraImageQuery", "SensorValuesQuery", "SimDataQuery", "SimQueriesCfg", diff --git a/motrix_env_core/src/motrix_env_core/sim/read.py b/motrix_env_core/src/motrix_env_core/sim/read.py index 4c918749..18c31d32 100644 --- a/motrix_env_core/src/motrix_env_core/sim/read.py +++ b/motrix_env_core/src/motrix_env_core/sim/read.py @@ -158,6 +158,25 @@ def compile_with(self, compiler: SimDataQueryCompiler, *, key: str) -> None: compiler.compile_sensor_values(key, self.sensors) +@dataclass(frozen=True, kw_only=True) +class RaycastCameraImageQuery(SimDataQuery): + """One camera output: scalar ``(N,H,W)`` or world vector ``(N,H,W,3)``. + + Distances are in metres, misses are -1. Points and normals are world-space + and zero on misses. Pixels are row-major in the optical frame (+X right, + +Y down, +Z forward). Read-only views are updated by program execution; + querying never advances physics or forces sensor sampling. This reads the + backend cache: without native refresh/status support it may still contain + pre-reset measurements, and -1 cannot distinguish unready from all misses. + """ + + sensor: str + field: str = "depth" + + def compile_with(self, compiler: SimDataQueryCompiler, *, key: str) -> None: + compiler.compile_raycast_camera_image(key, self.sensor, self.field) + + @dataclass(frozen=True, kw_only=True) class SitePositionQuery(SimDataQuery): """World position of one named site, shape ``(rows, 3)``.""" @@ -381,6 +400,10 @@ def compile_sensor_values(self, key: str, sensors: tuple[str, ...]) -> None: sensors: Ordered sensor names to read. """ + def compile_raycast_camera_image(self, key: str, sensor: str, field: str) -> None: + """Compile a camera image field; unsupported backends fail explicitly.""" + raise NotImplementedError(f"{type(self).__name__} does not support raycast camera images") + @abc.abstractmethod def compile_site_position(self, key: str, site: str) -> None: """Compile the world position of one site. diff --git a/motrix_env_core/tests/test_raycast_camera_config.py b/motrix_env_core/tests/test_raycast_camera_config.py new file mode 100644 index 00000000..8282b38f --- /dev/null +++ b/motrix_env_core/tests/test_raycast_camera_config.py @@ -0,0 +1,83 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +import math + +import pytest + +from motrix_env_core.config.scene.sensor import ( + OrthographicCameraCfg, + PinholeCameraCfg, + RaycastCameraSensorCfg, +) + + +def camera(**kwargs): + return RaycastCameraSensorCfg( + **dict( + site="optical", + width=4, + height=2, + projection=PinholeCameraCfg.from_fov(width=4, height=2, horizontal_fov=90), + far=10, + ) + | kwargs + ) + + +def test_fov_uses_pixel_centers_and_square_pixels(): + p = PinholeCameraCfg.from_fov(width=640, height=480, horizontal_fov=90) + assert p.fx == pytest.approx(320) + assert p.fy == p.fx + assert (p.cx, p.cy) == (320, 240) + p = PinholeCameraCfg.from_fov(width=640, height=480, horizontal_fov=90, vertical_fov=60) + assert p.fy == pytest.approx(240 / math.tan(math.pi / 6)) + + +@pytest.mark.parametrize("fov", [0, 180, -1, math.inf, math.nan]) +def test_fov_rejects_degenerate_angles(fov): + with pytest.raises(ValueError, match="FOV"): + PinholeCameraCfg.from_fov(width=4, height=2, horizontal_fov=fov) + + +@pytest.mark.parametrize( + "kwargs", + [ + {"site": ""}, + {"width": 0}, + {"height": 1.5}, + {"width": True}, + {"near": -1}, + {"near": 10}, + {"far": math.inf}, + {"near": math.nan}, + {"update_period": -1}, + {"update_period": math.nan}, + {"include_geom_groups": [-1]}, + {"include_geom_groups": [256]}, + {"include_geom_groups": [True]}, + {"outputs": []}, + {"outputs": ["normal"]}, + {"outputs": ["distance"] * 2}, + {"projection": OrthographicCameraCfg(pixel_size_x=0, pixel_size_y=1)}, + {"projection": PinholeCameraCfg(fx=1, fy=-1, cx=0, cy=0)}, + {"projection": PinholeCameraCfg(fx=1, fy=1, cx=math.nan, cy=0)}, + ], +) +def test_camera_rejects_invalid_boundary_values(kwargs): + with pytest.raises(ValueError): + camera(**kwargs).validate("camera") + + +@pytest.mark.parametrize("groups", [None, [], [0, 255]]) +def test_camera_valid_outputs_and_filters(groups): + cfg = camera(include_geom_groups=groups, outputs=("depth", "distance", "point", "normal")) + cfg.validate("camera") + cfg.projection = OrthographicCameraCfg(pixel_size_x=0.1, pixel_size_y=0.2) + cfg.validate("camera") + + +def test_camera_mutable_defaults_are_independent(): + first, second = camera(), camera() + first.outputs = (*first.outputs, "point") + assert second.outputs == ("depth",) diff --git a/motrix_env_motrixsim/pyproject.toml b/motrix_env_motrixsim/pyproject.toml index aaf93f08..4bcdef03 100644 --- a/motrix_env_motrixsim/pyproject.toml +++ b/motrix_env_motrixsim/pyproject.toml @@ -12,7 +12,7 @@ readme = "README.md" license = "Apache-2.0" dependencies = [ "motrix-env-core==0.4.0", - "motrixsim==0.10.1", + "motrixsim-core===0.10.2.dev132873", "numpy>=1.26", ] diff --git a/motrix_env_motrixsim/src/motrix_env_motrixsim/compiler.py b/motrix_env_motrixsim/src/motrix_env_motrixsim/compiler.py index 1b092632..fa862a96 100644 --- a/motrix_env_motrixsim/src/motrix_env_motrixsim/compiler.py +++ b/motrix_env_motrixsim/src/motrix_env_motrixsim/compiler.py @@ -33,12 +33,16 @@ FrameRefKind, FrameSensorCfg, FrameSensorType, + OrthographicCameraCfg, + PinholeCameraCfg, + RaycastCameraSensorCfg, ) +from motrix_env_core.config.scene.site import SiteCfg from motrix_env_core.config.scene.urdf import UrdfFileCfg from motrix_env_core.config.scene.validation import validate_scene_cfg from motrix_env_core.config.sim import SimCfg from motrix_env_motrixsim.actuators import apply_actuator_cfgs -from motrix_env_motrixsim.urdf import apply_urdf_cfgs +from motrix_env_motrixsim.urdf import _iter_links, apply_link_sites, apply_urdf_cfgs _CONTACT_REDUCE = { ContactSensorReduce.none: mtx.msd.ContactSensorReduce.None_, @@ -75,8 +79,16 @@ def build_world(self, scene: SceneCfg) -> mtx.msd.World: for name, asset in scene.iter_assets(): self._add_asset(world, name, asset) self._apply_visual(world, scene.visual) + sites = [(name, obj) for name, obj in scene.iter_objs() if isinstance(obj, SiteCfg)] + claimed: dict[str, str] = {} for name, obj in scene.iter_objs(): - self._append_obj(world, name, obj) + if isinstance(obj, SiteCfg): + continue + if isinstance(obj, BodyCfg): + self._attach_body(world, obj, name, sites, claimed) + else: + self._append_obj(world, name, obj) + apply_link_sites(world, [(name, site) for name, site in sites if name not in claimed]) for name, sensor in scene.iter_sensors(): self._append_sensor(world, name, sensor) return world @@ -98,7 +110,9 @@ def compile(self, scene: SceneCfg, sim: SimCfg) -> mtx.SceneModel: def _load_model_world(self, model: ModelFileCfg) -> mtx.msd.World: if isinstance(model, MjcfFileCfg): - return mtx.msd.from_file(str(resolve_path(model.file))) + world = mtx.msd.from_file(str(resolve_path(model.file))) + self._apply_model_sites(world, model.sites) + return world if isinstance(model, UrdfFileCfg): path = resolve_path(model.file) world = mtx.msd.from_str(path.read_text(), format="urdf", file_path=str(path)) @@ -106,6 +120,13 @@ def _load_model_world(self, model: ModelFileCfg) -> mtx.msd.World: return world raise TypeError(f"MotrixSim does not support model file config {type(model).__name__}") + @staticmethod + def _apply_model_sites(world: mtx.msd.World, sites: list[SiteCfg]) -> None: + for site in sites: + if site.name is None: + raise ValueError("Model file site entries require SiteCfg.name") + apply_link_sites(world, [(site.name, site) for site in sites]) + def _add_asset(self, world: mtx.msd.World, name: str, cfg: SceneAssetCfg) -> None: if isinstance(cfg, TextureCfg): texture = mtx.msd.Texture() @@ -197,6 +218,41 @@ def _configure_geom(self, geom: mtx.msd.Geometry, cfg: GeomCfg) -> None: if cfg.material is not None: geom.visual.material = cfg.material + def _attach_body( + self, + world: mtx.msd.World, + cfg: BodyCfg, + body_name: str, + sites: list[tuple[str, SiteCfg]], + claimed: dict[str, str], + ) -> None: + model_world = self._load_model_world(cfg.model) + apply_actuator_cfgs(model_world, cfg.actuators) + # Sites whose parent link belongs to this model mount before attach so + # both the site name and its parent follow this body's decoration. + model_links = {link.name for body in model_world.hierarchy.bodies for link in _iter_links(body.link)} + owned = [] + for name, site in sites: + if site.parent_link_name not in model_links: + continue + if name in claimed: + raise ValueError( + f"Scene site {name!r} parent link {site.parent_link_name!r} exists in multiple " + f"attached bodies ({claimed[name]!r} and {body_name!r}). Use the decorated parent " + "link name or declare the site in the body's model file config." + ) + claimed[name] = body_name + owned.append((name, site)) + apply_link_sites(model_world, owned) + world.attach( + model_world, + other_link_name=cfg.base_link_name, + other_translation=cfg.translation, + other_rotation=cfg.rotation, + other_prefix=cfg.prefix, + other_suffix=cfg.suffix, + ) + def _append_obj(self, world: mtx.msd.World, name: str, cfg: SceneObjCfg) -> None: if isinstance(cfg, FlatTerrainCfg): geom = mtx.msd.Geometry() @@ -227,18 +283,6 @@ def _append_obj(self, world: mtx.msd.World, name: str, cfg: SceneObjCfg) -> None light.cast_shadows = cfg.cast_shadows world.hierarchy.lights.append(light) return - if isinstance(cfg, BodyCfg): - model_world = self._load_model_world(cfg.model) - apply_actuator_cfgs(model_world, cfg.actuators) - world.attach( - model_world, - other_link_name=cfg.base_link_name, - other_translation=cfg.translation, - other_rotation=cfg.rotation, - other_prefix=cfg.prefix, - other_suffix=cfg.suffix, - ) - return raise TypeError(f"MotrixSim does not support scene object config {type(cfg).__name__}") def _object_type(self, kind: FrameObjectKind, name: str) -> mtx.msd.ObjectType: @@ -274,6 +318,41 @@ def _append_sensor(self, world: mtx.msd.World, name: str, cfg: SceneSensorCfg) - sensor.ref_frame = self._frame_ref(cfg) world.sensors.frame.append(sensor) return + if isinstance(cfg, RaycastCameraSensorCfg): + sensor = mtx.msd.RaycastCameraSensor() + sensor.name = name + sensor.site = cfg.site + sensor.width = cfg.width + sensor.height = cfg.height + if isinstance(cfg.projection, PinholeCameraCfg): + p = cfg.projection + sensor.projection = mtx.msd.CameraProjection.pinhole([p.fx, p.fy, p.cx, p.cy]) + elif isinstance(cfg.projection, OrthographicCameraCfg): + p = cfg.projection + sensor.projection = mtx.msd.CameraProjection.orthographic([p.pixel_size_x, p.pixel_size_y]) + sensor.near = cfg.near + sensor.far = cfg.far + sensor.update_period = cfg.update_period + sensor.exclude = { + "none": mtx.msd.LidarExclude.None_, + "parent_body": mtx.msd.LidarExclude.ParentBody, + "parent_subtree": mtx.msd.LidarExclude.ParentSubtree, + }[cfg.exclude] + sensor.include_geom_groups = ( + None if cfg.include_geom_groups is None else np.asarray(cfg.include_geom_groups, dtype=np.uint32) + ) + outputs = mtx.msd.RaycastCameraOutputFields() + native_fields = { + "depth": "depth", + "distance": "distance", + "point": "point", + "normal": "normal", + } + for field, native_field in native_fields.items(): + setattr(outputs, native_field, field in cfg.outputs) + sensor.returns = outputs + world.sensors.raycast_camera.append(sensor) + return raise TypeError(f"MotrixSim does not support scene sensor config {type(cfg).__name__}") diff --git a/motrix_env_motrixsim/src/motrix_env_motrixsim/sim_data.py b/motrix_env_motrixsim/src/motrix_env_motrixsim/sim_data.py index 54e60b20..03d8e999 100644 --- a/motrix_env_motrixsim/src/motrix_env_motrixsim/sim_data.py +++ b/motrix_env_motrixsim/src/motrix_env_motrixsim/sim_data.py @@ -67,6 +67,13 @@ def __init__( declared: Mapping[str, SimDataQuery], canonical_keys: Mapping[str, str], ) -> None: + self._cameras = { + name: model.sensors[index] + for index, name in enumerate(model.sensors.names) + if isinstance(model.sensors[index], mtx.RaycastCameraSensor) + } + self._camera_sources: dict[str, str] = {} + self._image_views: dict[str, tuple[str, int, int, int, int, int]] = {} self._model = model self._source = source self._declared = declared @@ -75,6 +82,8 @@ def __init__( def _begin_compile(self) -> None: self._resolved = {} + self._camera_sources = {} + self._image_views = {} def _build_program(self) -> PhysicsReadProgram: native = tuple(self._resolved.values()) @@ -88,6 +97,7 @@ def _build_program(self) -> PhysicsReadProgram: self._source, declared=self._declared, canonical_keys=self._canonical_keys, + image_views=self._image_views, ) def _record( @@ -158,6 +168,32 @@ def compile_sensor_values(self, key: str, sensors: tuple[str, ...]) -> None: sensor_names = self._validate_names(sensors, key, "sensors") self._record(key, mtx.query.SensorValues(sensor_names), None) + def compile_raycast_camera_image(self, key: str, sensor: str, field: str) -> None: + self._validate_name(sensor, key) + camera = self._cameras.get(sensor) + if camera is None: + raise KeyError(f"Query {key!r} requires a compiled raycast camera sensor named {sensor!r}.") + if field not in ("depth", "distance", "point", "normal"): + raise ValueError(f"Query {key!r} requests unknown camera output {field!r}.") + if not getattr(camera.returns, field): + raise ValueError(f"Camera {sensor!r} does not enable output {field!r}.") + offset = 0 + field_offset = 0 + channels = 0 + for output in ("depth", "distance", "point", "normal"): + if not getattr(camera.returns, output): + continue + width = 3 if output in ("point", "normal") else 1 + if output == field: + field_offset, channels = offset, width + offset += width + source_key = self._camera_sources.get(sensor) + if source_key is None: + source_key = key + self._camera_sources[sensor] = key + self._record(key, mtx.query.SensorValues((sensor,))) + self._image_views[key] = (source_key, camera.height, camera.width, offset, field_offset, channels) + def compile_dof_position(self, key: str) -> None: self._record(key, mtx.query.DofPosition()) @@ -320,6 +356,7 @@ def __init__( source: mtx.SceneData, declared: Mapping[str, SimDataQuery], canonical_keys: Mapping[str, str], + image_views: Mapping[str, tuple[str, int, int, int, int, int]], ) -> None: self._read = read self._source = source @@ -340,6 +377,13 @@ def __init__( field.offset, shape, ) + # Camera fields alias a single native pixel-major sensor read. Slice + # before squeezing so vector/scalar views retain the native row stride. + raw_views = dict(self._views) + for key, (source_key, height, width, stride, offset, channels) in image_views.items(): + pixels = raw_views[source_key].reshape(num_envs, height, width, stride) + view = pixels[..., offset : offset + channels] + self._views[key] = view[..., 0] if channels == 1 else view # Duplicate declarations alias the canonical region: one physical read. for alias, canonical in canonical_keys.items(): if alias != canonical: diff --git a/motrix_env_motrixsim/src/motrix_env_motrixsim/urdf.py b/motrix_env_motrixsim/src/motrix_env_motrixsim/urdf.py index 8f0e854a..70bdd26a 100644 --- a/motrix_env_motrixsim/src/motrix_env_motrixsim/urdf.py +++ b/motrix_env_motrixsim/src/motrix_env_motrixsim/urdf.py @@ -6,9 +6,9 @@ import motrixsim as mtx +from motrix_env_core.config.scene.site import SiteCfg from motrix_env_core.config.scene.urdf import ( JointCfg, - SiteCfg, UrdfFileCfg, UrdfGeomCfg, ) @@ -36,14 +36,34 @@ def _apply_geom_cfg(cfg: UrdfGeomCfg, geom: Any) -> None: geom.priority = cfg.priority -def _build_site(cfg: SiteCfg) -> mtx.msd.Site: +def _build_site(name: str, cfg: SiteCfg) -> mtx.msd.Site: site = mtx.msd.Site() - site.name = cfg.name + site.name = name site.position = cfg.position site.orientation = cfg.rotation return site +def apply_link_sites( + world: mtx.msd.World, + sites: list[tuple[str, SiteCfg]], +) -> None: + """Mount named sites without replacing imported sites or duplicating name ownership.""" + if not sites: + return + links = {link.name: link for body in world.hierarchy.bodies for link in _iter_links(body.link)} + existing = {site.name for site in world.hierarchy.sites} + existing.update(site.name for link in links.values() for site in link.sites) + for name, cfg in sites: + if name in existing: + raise ValueError(f"Configured site name already exists in model: {name!r}") + if cfg.parent_link_name not in links: + raise ValueError(f"Configured site parent link does not exist in model: {cfg.parent_link_name!r}") + existing.add(name) + for name, cfg in sites: + links[cfg.parent_link_name].sites.append(_build_site(name, cfg)) + + def apply_urdf_cfgs(world: mtx.msd.World, cfg: UrdfFileCfg) -> None: """Apply declarative URDF augmentations to a MotrixSim MSD world.""" cfg_by_geom: dict[str, UrdfGeomCfg] = {} @@ -55,7 +75,9 @@ def apply_urdf_cfgs(world: mtx.msd.World, cfg: UrdfFileCfg) -> None: cfg_by_site: dict[str, SiteCfg] = {} for site_cfg in cfg.sites: - site_cfg.validate() + site_cfg.validate(site_cfg.name) + if site_cfg.name is None: + raise ValueError("UrdfFileCfg.sites entries require SiteCfg.name") if site_cfg.name in cfg_by_site: raise ValueError(f"Duplicate configured site name {site_cfg.name!r}") cfg_by_site[site_cfg.name] = site_cfg @@ -111,5 +133,4 @@ def apply_geom(geom: Any) -> None: if unknown_site_links: raise ValueError(f"Configured site parent links do not exist in model: {unknown_site_links}") - for site_cfg in cfg.sites: - links_by_name[site_cfg.parent_link_name].sites.append(_build_site(site_cfg)) + apply_link_sites(world, [(site.name, site) for site in cfg.sites]) diff --git a/motrix_env_motrixsim/tests/test_raycast_camera_compiler.py b/motrix_env_motrixsim/tests/test_raycast_camera_compiler.py new file mode 100644 index 00000000..3a6f1e3d --- /dev/null +++ b/motrix_env_motrixsim/tests/test_raycast_camera_compiler.py @@ -0,0 +1,195 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +import numpy as np +import pytest + +from motrix_env_core.config import configclass +from motrix_env_core.config.scene.base import BodyCfg, SceneCfg, SceneObjsCfg, SceneSensorsCfg +from motrix_env_core.config.scene.mjcf import MjcfFileCfg +from motrix_env_core.config.scene.sensor import ( + OrthographicCameraCfg, + PinholeCameraCfg, + RaycastCameraSensorCfg, +) +from motrix_env_core.config.scene.site import SiteCfg +from motrix_env_core.config.scene.urdf import UrdfFileCfg + +mtx = pytest.importorskip("motrixsim") + +from motrix_env_motrixsim.compiler import build_scene_world # noqa: E402 + + +def make_scene(tmp_path, source="mjcf", **camera_kwargs): + if source == "mjcf": + path = tmp_path / "model.xml" + path.write_text( + '' + '' + ) + model = MjcfFileCfg(file=path) + else: + path = tmp_path / "model.urdf" + path.write_text( + '' + '' + "" + ) + model = UrdfFileCfg(file=path, sites=[SiteCfg(name="old", parent_link_name="base")]) + + @configclass + class Objects(SceneObjsCfg): + # Deliberately before body: mounting must resolve the complete scene. + optical: SiteCfg = SiteCfg(parent_link_name="base", position=(0, 0, 0.2)) + body: BodyCfg = BodyCfg(model=model, base_link_name="base", prefix="p_", suffix="_s") + + @configclass + class Sensors(SceneSensorsCfg): + camera: RaycastCameraSensorCfg = RaycastCameraSensorCfg( + **dict( + site="p_optical_s", + width=4, + height=2, + projection=PinholeCameraCfg(fx=2, fy=3, cx=2, cy=1), + far=12, + near=0.1, + update_period=0.02, + outputs=("depth", "distance", "point", "normal"), + exclude="parent_subtree", + include_geom_groups=[0, 255], + ) + | camera_kwargs + ) + + return SceneCfg(objs=Objects(), sensors=Sensors()) + + +@pytest.mark.parametrize("source", ["mjcf", "urdf"]) +@pytest.mark.parametrize( + "projection", [PinholeCameraCfg(fx=2, fy=3, cx=2, cy=1), OrthographicCameraCfg(pixel_size_x=0.1, pixel_size_y=0.2)] +) +def test_camera_maps_native_projection_outputs_filters_and_link_site(tmp_path, source, projection): + scene = make_scene(tmp_path, source, projection=projection) + world = build_scene_world(scene) + sensor = world.sensors.raycast_camera[0] + assert (sensor.name, sensor.site, sensor.width, sensor.height) == ("camera", "p_optical_s", 4, 2) + assert sensor.near == pytest.approx(0.1) + assert sensor.far == 12 + assert sensor.update_period == pytest.approx(0.02) + assert sensor.exclude == mtx.msd.LidarExclude.ParentSubtree + np.testing.assert_array_equal(sensor.include_geom_groups, [0, 255]) + assert all(getattr(sensor.returns, field) for field in ("depth", "distance", "point", "normal")) + if isinstance(projection, PinholeCameraCfg): + assert sensor.projection.variant == "pinhole" + np.testing.assert_allclose(sensor.projection.value["intrinsics"], [2, 3, 2, 1]) + else: + assert sensor.projection.variant == "orthographic" + np.testing.assert_allclose(sensor.projection.value["spacing"], [0.1, 0.2]) + model = world.build() + assert model.get_site("p_optical_s") is not None + assert model.get_site("p_old_s") is not None + + +def test_only_requested_outputs_are_enabled(tmp_path): + sensor = build_scene_world(make_scene(tmp_path, outputs=("distance", "normal"))).sensors.raycast_camera[0] + assert not sensor.returns.depth + assert sensor.returns.distance + assert not sensor.returns.point + assert sensor.returns.normal + + +@pytest.mark.parametrize("groups", [None, []]) +def test_filter_none_and_empty_are_distinct(tmp_path, groups): + sensor = build_scene_world(make_scene(tmp_path, include_geom_groups=groups)).sensors.raycast_camera[0] + if groups is None: + assert sensor.include_geom_groups is None + else: + assert len(sensor.include_geom_groups) == 0 + + +def test_link_site_rejects_unknown_parent_and_existing_site(tmp_path): + scene = make_scene(tmp_path) + scene.objs.optical.parent_link_name = "missing" + with pytest.raises(ValueError, match="parent link"): + build_scene_world(scene) + scene.objs.optical.parent_link_name = "p_base_s" + scene.objs.optical = None + + @configclass + class Objects(SceneObjsCfg): + body: BodyCfg = scene.objs.body + old: SiteCfg = SiteCfg(parent_link_name="base") + + scene.objs = Objects() + with pytest.raises(ValueError, match="already exist"): + build_scene_world(scene) + + +def test_two_bodies_resolve_site_parent_by_owning_body(tmp_path): + base_model = _write_urdf(tmp_path, "l.urdf", "base") + tool_model = _write_urdf(tmp_path, "r.urdf", "tool") + + @configclass + class Objects(SceneObjsCfg): + optical: SiteCfg = SiteCfg(parent_link_name="base") + tool_site: SiteCfg = SiteCfg(parent_link_name="tool") + left: BodyCfg = BodyCfg(model=UrdfFileCfg(file=base_model), base_link_name="base", prefix="a_") + right: BodyCfg = BodyCfg(model=UrdfFileCfg(file=tool_model), base_link_name="tool", prefix="b_") + + world = build_scene_world(SceneCfg(objs=Objects())) + mounted = {site.name: link.name for link in _all_links(world) for site in link.sites if site.name} + assert mounted["a_optical"] == "a_base" + assert mounted["b_tool_site"] == "b_tool" + + +def test_two_bodies_with_same_link_name_require_decorated_parent(tmp_path): + model = _write_urdf(tmp_path, "l.urdf", "base") + + @configclass + class Objects(SceneObjsCfg): + optical: SiteCfg = SiteCfg(parent_link_name="base") + left: BodyCfg = BodyCfg(model=UrdfFileCfg(file=model), base_link_name="base", prefix="a_") + right: BodyCfg = BodyCfg(model=UrdfFileCfg(file=model), base_link_name="base", prefix="b_") + + objs = Objects() + with pytest.raises(ValueError, match="multiple attached bodies"): + build_scene_world(SceneCfg(objs=objs)) + + objs.optical.parent_link_name = "b_base" + world = build_scene_world(SceneCfg(objs=objs)) + mounted = {site.name: link.name for link in _all_links(world) for site in link.sites if site.name} + assert mounted["optical"] == "b_base" + + +def test_mjcf_model_level_sites_are_applied_and_decorated(tmp_path): + path = tmp_path / "model.xml" + path.write_text( + '' + ) + + @configclass + class Objects(SceneObjsCfg): + body: BodyCfg = BodyCfg( + model=MjcfFileCfg(file=path, sites=[SiteCfg(name="marker", parent_link_name="base")]), + base_link_name="base", + prefix="p_", + suffix="_s", + ) + + model = build_scene_world(SceneCfg(objs=Objects())).build() + assert model.get_site("p_marker_s") is not None + + +def _write_urdf(tmp_path, filename, link): + path = tmp_path / filename + path.write_text( + f'' + f'' + ) + return path + + +def _all_links(world): + from motrix_env_motrixsim.urdf import _iter_links + + return [link for body in world.hierarchy.bodies for link in _iter_links(body.link)] diff --git a/motrix_env_motrixsim/tests/test_raycast_camera_read.py b/motrix_env_motrixsim/tests/test_raycast_camera_read.py new file mode 100644 index 00000000..3cbaf1ac --- /dev/null +++ b/motrix_env_motrixsim/tests/test_raycast_camera_read.py @@ -0,0 +1,213 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +import numpy as np +import pytest + +from motrix_env_core.config import SimCfg, configclass +from motrix_env_core.config.scene.base import SceneCfg, SceneSensorsCfg +from motrix_env_core.config.scene.sensor import ( + OrthographicCameraCfg, + PinholeCameraCfg, + RaycastCameraSensorCfg, +) +from motrix_env_core.sim.read import DofPositionQuery, RaycastCameraImageQuery, SensorValuesQuery +from motrix_env_core.sim.write import JointPositionWrite, JointVelocityWrite +from motrix_env_motrixsim.runtime import MotrixSimBackend + +pytestmark = pytest.mark.integration + + +def camera_backend(tmp_path, *, moving=False, foreground=False, **overrides): + path = tmp_path / "camera.xml" + mount = ( + '' + '' + if moving + else '' + ) + obstruction = '' if foreground else "" + path.write_text( + '" + ) + + @configclass + class Sensors(SceneSensorsCfg): + camera: RaycastCameraSensorCfg = RaycastCameraSensorCfg( + **dict( + site="optical", + width=4, + height=3, + projection=PinholeCameraCfg(fx=4, fy=4, cx=2, cy=1.5), + far=10, + outputs=("depth", "distance", "point", "normal"), + ) + | overrides + ) + + return MotrixSimBackend(SceneCfg(file=path, sensors=Sensors()), SimCfg(dt=0.01), 2) + + +def test_image_fields_match_geometry_and_native_flat_layout(tmp_path): + backend = camera_backend(tmp_path) + queries = { + field: RaycastCameraImageQuery(sensor="camera", field=field) + for field in ("depth", "distance", "point", "normal") + } + queries["alias"] = queries["depth"] + queries["raw"] = SensorValuesQuery(sensors=("camera",)) + program = backend.compile_reads(queries) + backend.step(1) + program.execute() + depth = program["depth"] + points = program["point"] + normals = program["normal"] + assert depth.shape == (2, 3, 4) + assert points.shape == normals.shape == (2, 3, 4, 3) + np.testing.assert_allclose(depth, 4.9, atol=1e-5) + np.testing.assert_allclose(points[..., 2], depth, atol=1e-5) + np.testing.assert_allclose(np.linalg.norm(points, axis=-1), program["distance"], atol=1e-5) + np.testing.assert_allclose(normals[..., 2], -1) + flat = program["raw"].reshape(2, 3, 4, 8) + np.testing.assert_array_equal(depth, flat[..., 0]) + np.testing.assert_array_equal(program["distance"], flat[..., 1]) + np.testing.assert_array_equal(points, flat[..., 2:5]) + np.testing.assert_array_equal(normals, flat[..., 5:8]) + # Disjoint interleaved channels share an allocation, not overlapping bytes. + assert depth.base is points.base + assert program["alias"] is depth + assert not depth.flags.writeable + before = depth.copy() + program.execute(np.array([1], dtype=np.int64)) + np.testing.assert_array_equal(depth, before) + # Camera field layout is independent of declaration order. + reverse = backend.compile_reads( + { + "normal": RaycastCameraImageQuery(sensor="camera", field="normal"), + "depth": RaycastCameraImageQuery(sensor="camera"), + } + ) + reverse.execute() + np.testing.assert_array_equal(reverse["normal"], normals) + np.testing.assert_array_equal(reverse["depth"], depth) + assert reverse.arena_bytes == 2 * 3 * 4 * 8 * np.dtype(np.float32).itemsize + + +def test_reset_invalidates_cached_image_but_intra_episode_fk_respects_update_period(tmp_path): + # MotrixSim contract: reset leaves no valid frame, so the next FK write + # resamples; without reset the cached image is kept until update_period + # elapses on a physics step. + backend = camera_backend(tmp_path, moving=True, update_period=100.0) + reads = backend.compile_reads({"depth": RaycastCameraImageQuery(sensor="camera")}) + reset = backend.compile_writes({"q": JointPositionWrite(joints=("slide",))}, reset=True, forward_kinematics=True) + fk = backend.compile_writes({"q": JointPositionWrite(joints=("slide",))}, reset=False, forward_kinematics=True) + + backend.step(1) + reads.execute() + np.testing.assert_allclose(reads["depth"], 4.9, atol=1e-5) + + fk.buffer("q")[:] = [[1.0], [2.0]] + fk.execute() + reads.execute() + np.testing.assert_allclose(reads["depth"], 4.9, atol=1e-5) + + reset.buffer("q")[:] = [[1.0], [2.0]] + reset.execute() + reads.execute() + np.testing.assert_allclose(reads["depth"][0], 3.9, atol=1e-5) + np.testing.assert_allclose(reads["depth"][1], 2.9, atol=1e-5) + + +def test_partial_reset_resamples_only_selected_rows(tmp_path): + backend = camera_backend(tmp_path, moving=True, update_period=100.0) + reads = backend.compile_reads({"depth": RaycastCameraImageQuery(sensor="camera")}) + reset = backend.compile_writes({"q": JointPositionWrite(joints=("slide",))}, reset=True, forward_kinematics=True) + backend.step(1) + reads.execute() + reset.buffer("q")[:] = [[0.0], [2.0]] + reset.execute(np.array([1], dtype=np.int64)) + reads.execute() + np.testing.assert_allclose(reads["depth"][0], 4.9, atol=1e-5) + np.testing.assert_allclose(reads["depth"][1], 2.9, atol=1e-5) + + +def test_non_integer_update_period_resamples_on_step_at_elapsed_times(tmp_path): + backend = camera_backend(tmp_path, moving=True, update_period=0.015) # 1.5 * dt + reads = backend.compile_reads({"depth": RaycastCameraImageQuery(sensor="camera")}) + vel = backend.compile_writes({"v": JointVelocityWrite(joints=("slide",))}) + backend.step(1) + vel.buffer("v")[:] = [[1.0], [1.0]] + vel.execute() + backend.step(1) + backend.step(2) # resamples at t=0.015 and t=0.03; sampling precedes integration, so z=0.02 at t=0.03 + reads.execute() + np.testing.assert_allclose(reads["depth"], 4.9 - 0.02, atol=1e-5) + + +def test_partial_read_updates_only_selected_row_with_interleaved_fields(tmp_path): + backend = camera_backend(tmp_path, moving=True) + reads = backend.compile_reads( + { + "depth": RaycastCameraImageQuery(sensor="camera"), + "normal": RaycastCameraImageQuery(sensor="camera", field="normal"), + "q": DofPositionQuery(), + } + ) + backend.step(1) + reads.execute() + before = reads["depth"].copy() + writes = backend.compile_writes({"q": JointPositionWrite(joints=("slide",))}) + writes.buffer("q")[:] = [[1.0], [2.0]] + writes.execute() + backend.step(1) + reads.execute(np.array([1], dtype=np.int64)) + np.testing.assert_array_equal(reads["depth"][0], before[0]) + np.testing.assert_allclose(reads["depth"][1], 2.9, atol=1e-5) + np.testing.assert_array_equal(reads["q"], [[0], [2]]) + np.testing.assert_allclose(reads["normal"][..., 2], -1) + reads.execute() + np.testing.assert_allclose(reads["depth"][0], 3.9, atol=1e-5) + + +def test_orthographic_grid_has_parallel_rays_and_metric_spacing(tmp_path): + backend = camera_backend(tmp_path, projection=OrthographicCameraCfg(pixel_size_x=0.2, pixel_size_y=0.3)) + reads = backend.compile_reads( + { + field: RaycastCameraImageQuery(sensor="camera", field=field) + for field in ("depth", "distance", "point", "normal") + } + ) + backend.step(1) + reads.execute() + np.testing.assert_allclose(reads["distance"], reads["depth"]) + np.testing.assert_allclose(reads["depth"], 4.9, atol=1e-5) + points = reads["point"] + np.testing.assert_allclose(np.diff(points[..., 0], axis=2), 0.2, atol=1e-5) + np.testing.assert_allclose(np.diff(points[..., 1], axis=1), 0.3, atol=1e-5) + + +def test_near_surface_occludes_far_surface_instead_of_seeing_through(tmp_path): + backend = camera_backend(tmp_path, foreground=True, near=0.5) + reads = backend.compile_reads( + { + "depth": RaycastCameraImageQuery(sensor="camera"), + "point": RaycastCameraImageQuery(sensor="camera", field="point"), + } + ) + backend.step(1) + reads.execute() + np.testing.assert_array_equal(reads["depth"], -1) + np.testing.assert_array_equal(reads["point"], 0) + + +def test_disabled_field_is_rejected_and_miss_is_not_normalized(tmp_path): + backend = camera_backend(tmp_path, far=1, outputs=("depth",)) + with pytest.raises(ValueError, match="does not enable"): + backend.compile_reads({"points": RaycastCameraImageQuery(sensor="camera", field="point")}) + with pytest.raises(KeyError, match="raycast camera sensor"): + backend.compile_reads({"unknown": RaycastCameraImageQuery(sensor="missing")}) + program = backend.compile_reads({"depth": RaycastCameraImageQuery(sensor="camera")}) + backend.step(1) + program.execute() + np.testing.assert_array_equal(program["depth"], -1) diff --git a/motrix_env_motrixsim/tests/test_sim_data_provider.py b/motrix_env_motrixsim/tests/test_sim_data_provider.py index 33fa3b8c..dacdf2f8 100644 --- a/motrix_env_motrixsim/tests/test_sim_data_provider.py +++ b/motrix_env_motrixsim/tests/test_sim_data_provider.py @@ -253,6 +253,7 @@ def __init__(self) -> None: self.links = (_Link(2, "root"), _Link(7, "foot")) self._links = {link.name: link for link in self.links} self.reports = SimpleNamespace(contact_forces=False) + self.sensors = SimpleNamespace(names=()) self.plans: list[_QueryPlan] = [] self.calls: list[str] = [] self.sites = (_PoseObject(1, "tip", 40.0, self.calls, "site"),) diff --git a/motrix_env_mujoco/src/motrix_env_mujoco/compiler.py b/motrix_env_mujoco/src/motrix_env_mujoco/compiler.py index 946bd983..0920e057 100644 --- a/motrix_env_mujoco/src/motrix_env_mujoco/compiler.py +++ b/motrix_env_mujoco/src/motrix_env_mujoco/compiler.py @@ -40,12 +40,14 @@ FrameRefKind, FrameSensorCfg, FrameSensorType, + RaycastCameraSensorCfg, ) +from motrix_env_core.config.scene.site import SiteCfg from motrix_env_core.config.scene.urdf import UrdfFileCfg from motrix_env_core.config.scene.validation import validate_scene_cfg from motrix_env_core.config.sim import SimCfg from motrix_env_mujoco.actuators import apply_actuator_cfgs -from motrix_env_mujoco.urdf import apply_urdf_cfgs +from motrix_env_mujoco.urdf import apply_spec_sites, apply_urdf_cfgs _CONTACT_DATA_BITS = { ContactReportField.found: 1, @@ -101,8 +103,16 @@ def build_spec(self, scene: SceneCfg) -> mj.MjSpec: for name, asset in scene.iter_assets(): self._add_asset(spec, name, asset) self._apply_visual(spec, scene.visual) + sites = [(name, obj) for name, obj in scene.iter_objs() if isinstance(obj, SiteCfg)] + claimed: dict[str, str] = {} for name, obj in scene.iter_objs(): - self._append_obj(spec, name, obj) + if isinstance(obj, SiteCfg): + continue + if isinstance(obj, BodyCfg): + self._attach_body(spec, obj, name, sites, claimed) + else: + self._append_obj(spec, name, obj) + apply_spec_sites(spec, [(name, site) for name, site in sites if name not in claimed]) for name, sensor in scene.iter_sensors(): self._append_sensor(spec, name, sensor) return spec @@ -128,7 +138,9 @@ def compile(self, scene: SceneCfg, sim: SimCfg) -> mj.MjModel: def _load_model_spec(self, model: ModelFileCfg) -> mj.MjSpec: if isinstance(model, MjcfFileCfg): - return mj.MjSpec.from_file(str(resolve_path(model.file))) + spec = mj.MjSpec.from_file(str(resolve_path(model.file))) + apply_spec_sites(spec, [(site.name, site) for site in model.sites]) + return spec if isinstance(model, UrdfFileCfg): spec = mj.MjSpec.from_file(str(resolve_path(model.file))) apply_urdf_cfgs(spec, model) @@ -231,6 +243,46 @@ def _geom_kwargs(self, cfg: GeomCfg) -> dict[str, object]: "priority": cfg.priority, } + def _attach_body( + self, + spec: mj.MjSpec, + cfg: BodyCfg, + body_name: str, + sites: list[tuple[str, SiteCfg]], + claimed: dict[str, str], + ) -> None: + robot_spec = self._load_model_spec(cfg.model) + apply_actuator_cfgs(robot_spec, cfg.actuators) + base_link = robot_spec.body(cfg.base_link_name) + if base_link is None: + raise ValueError(f"Attached body base link {cfg.base_link_name!r} does not exist in model") + if base_link.parent is not robot_spec.worldbody: + raise ValueError(f"MuJoCo requires BodyCfg.base_link_name {cfg.base_link_name!r} to be a root body") + first_root = robot_spec.worldbody.first_body() + if first_root is None or first_root.name != cfg.base_link_name: + raise ValueError(f"MuJoCo requires BodyCfg.base_link_name {cfg.base_link_name!r} to be the first root body") + # Sites whose parent link belongs to this model mount before attach so + # both the site name and its parent follow this body's decoration. + model_bodies = {body.name for body in robot_spec.bodies} + owned = [] + for name, site in sites: + if site.parent_link_name not in model_bodies: + continue + if name in claimed: + raise ValueError( + f"Scene site {name!r} parent link {site.parent_link_name!r} exists in multiple " + f"attached bodies ({claimed[name]!r} and {body_name!r}). Use the decorated parent " + "link name or declare the site in the body's model file config." + ) + claimed[name] = body_name + owned.append((name, site)) + apply_spec_sites(robot_spec, owned) + placement = spec.worldbody.add_frame( + pos=cfg.translation, + quat=_mujoco_quat(cfg.rotation), + ) + spec.attach(robot_spec, prefix=cfg.prefix or "", suffix=cfg.suffix or "", frame=placement) + def _append_obj(self, spec: mj.MjSpec, name: str, cfg: SceneObjCfg) -> None: if isinstance(cfg, FlatTerrainCfg): spec.worldbody.add_geom( @@ -260,28 +312,11 @@ def _append_obj(self, spec: mj.MjSpec, name: str, cfg: SceneObjCfg) -> None: castshadow=cfg.cast_shadows, ) return - if isinstance(cfg, BodyCfg): - robot_spec = self._load_model_spec(cfg.model) - apply_actuator_cfgs(robot_spec, cfg.actuators) - base_link = robot_spec.body(cfg.base_link_name) - if base_link is None: - raise ValueError(f"Attached body base link {cfg.base_link_name!r} does not exist in model") - if base_link.parent is not robot_spec.worldbody: - raise ValueError(f"MuJoCo requires BodyCfg.base_link_name {cfg.base_link_name!r} to be a root body") - first_root = robot_spec.worldbody.first_body() - if first_root is None or first_root.name != cfg.base_link_name: - raise ValueError( - f"MuJoCo requires BodyCfg.base_link_name {cfg.base_link_name!r} to be the first root body" - ) - placement = spec.worldbody.add_frame( - pos=cfg.translation, - quat=_mujoco_quat(cfg.rotation), - ) - spec.attach(robot_spec, prefix=cfg.prefix or "", suffix=cfg.suffix or "", frame=placement) - return raise TypeError(f"MuJoCo does not support scene object config {type(cfg).__name__}") def _append_sensor(self, spec: mj.MjSpec, name: str, cfg: SceneSensorCfg) -> None: + if isinstance(cfg, RaycastCameraSensorCfg): + raise NotImplementedError(f"MuJoCo does not support RaycastCameraSensorCfg sensor {name!r}") if isinstance(cfg, ContactSensorCfg): data_mask = sum(_CONTACT_DATA_BITS[field] for field in cfg.data) spec.add_sensor( diff --git a/motrix_env_mujoco/src/motrix_env_mujoco/urdf.py b/motrix_env_mujoco/src/motrix_env_mujoco/urdf.py index 843f11c6..5c7163ec 100644 --- a/motrix_env_mujoco/src/motrix_env_mujoco/urdf.py +++ b/motrix_env_mujoco/src/motrix_env_mujoco/urdf.py @@ -3,6 +3,7 @@ import mujoco as mj +from motrix_env_core.config.scene.site import SiteCfg from motrix_env_core.config.scene.urdf import UrdfFileCfg @@ -12,6 +13,38 @@ def _mujoco_quat(rotation: tuple[float, float, float, float]) -> list[float]: return [w, x, y, z] +def apply_spec_sites(spec: mj.MjSpec, sites: list[tuple[str, SiteCfg]]) -> None: + """Mount configured sites onto a MuJoCo specification using literal parent link names.""" + for name, site in sites: + if not name: + raise ValueError("Configured model file site entries require a site name") + if site.name is not None and site.name != name: + raise ValueError(f"SiteCfg.name {site.name!r} does not match the configured site name {name!r}") + cfg_by_site = {name: site for name, site in sites} + if len(cfg_by_site) != len(sites): + raise ValueError("Duplicate configured site name") + + existing_site_names = {site.name for site in spec.sites} + duplicate_site_names = sorted(existing_site_names.intersection(cfg_by_site)) + if duplicate_site_names: + raise ValueError(f"Configured site names already exist in model: {duplicate_site_names}") + + existing_body_names = {body.name for body in spec.bodies} + parent_link_names = {site.parent_link_name for _, site in sites} + unknown_site_links = sorted(parent_link_names.difference(existing_body_names)) + if unknown_site_links: + raise ValueError(f"Configured site parent links do not exist in model: {unknown_site_links}") + + for name, site_cfg in sites: + parent = spec.body(site_cfg.parent_link_name) + assert parent is not None + parent.add_site( + name=name, + pos=site_cfg.position, + quat=_mujoco_quat(site_cfg.rotation), + ) + + def apply_urdf_cfgs(spec: mj.MjSpec, cfg: UrdfFileCfg) -> None: """Apply declarative URDF augmentations to a MuJoCo specification.""" cfg_by_geom = {geom.geom_name: geom for geom in cfg.geoms} @@ -22,10 +55,6 @@ def apply_urdf_cfgs(spec: mj.MjSpec, cfg: UrdfFileCfg) -> None: if len(cfg_by_joint) != len(cfg.joints): raise ValueError("Multiple joint configs target the same joint") - cfg_by_site = {site.name: site for site in cfg.sites} - if len(cfg_by_site) != len(cfg.sites): - raise ValueError("Duplicate configured site name") - existing_geom_names = {geom.name for geom in spec.geoms} unknown_geoms = sorted(set(cfg_by_geom).difference(existing_geom_names)) if unknown_geoms: @@ -36,17 +65,6 @@ def apply_urdf_cfgs(spec: mj.MjSpec, cfg: UrdfFileCfg) -> None: if unknown_joints: raise ValueError(f"Configured joints do not exist in model: {unknown_joints}") - existing_site_names = {site.name for site in spec.sites} - duplicate_site_names = sorted(existing_site_names.intersection(cfg_by_site)) - if duplicate_site_names: - raise ValueError(f"Configured site names already exist in model: {duplicate_site_names}") - - existing_body_names = {body.name for body in spec.bodies} - parent_link_names = {site.parent_link_name for site in cfg.sites} - unknown_site_links = sorted(parent_link_names.difference(existing_body_names)) - if unknown_site_links: - raise ValueError(f"Configured site parent links do not exist in model: {unknown_site_links}") - for name, geom_cfg in cfg_by_geom.items(): geom = spec.geom(name) assert geom is not None @@ -65,11 +83,4 @@ def apply_urdf_cfgs(spec: mj.MjSpec, cfg: UrdfFileCfg) -> None: if joint_cfg.friction_loss is not None: joint.frictionloss = joint_cfg.friction_loss - for site_cfg in cfg.sites: - parent = spec.body(site_cfg.parent_link_name) - assert parent is not None - parent.add_site( - name=site_cfg.name, - pos=site_cfg.position, - quat=_mujoco_quat(site_cfg.rotation), - ) + apply_spec_sites(spec, [(site.name, site) for site in cfg.sites]) diff --git a/motrix_env_mujoco/tests/test_link_site_compiler.py b/motrix_env_mujoco/tests/test_link_site_compiler.py new file mode 100644 index 00000000..7e373076 --- /dev/null +++ b/motrix_env_mujoco/tests/test_link_site_compiler.py @@ -0,0 +1,147 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +import numpy as np +import pytest + +from motrix_env_core.config import configclass +from motrix_env_core.config.scene.base import BodyCfg, SceneCfg, SceneObjsCfg, SceneSensorsCfg +from motrix_env_core.config.scene.mjcf import MjcfFileCfg +from motrix_env_core.config.scene.sensor import PinholeCameraCfg, RaycastCameraSensorCfg +from motrix_env_core.config.scene.site import SiteCfg +from motrix_env_core.config.scene.urdf import UrdfFileCfg + +mj = pytest.importorskip("mujoco") + +from motrix_env_mujoco.compiler import build_mujoco_spec # noqa: E402 + + +@pytest.fixture(params=["mjcf", "urdf"]) +def scene(request, tmp_path): + if request.param == "mjcf": + path = tmp_path / "tiny.xml" + path.write_text( + '' + '' + ) + model = MjcfFileCfg(file=path) + else: + path = tmp_path / "tiny.urdf" + path.write_text( + '' + '' + "" + ) + model = UrdfFileCfg(file=path, sites=[SiteCfg(name="old", parent_link_name="base")]) + + @configclass + class Objects(SceneObjsCfg): + optical: SiteCfg = SiteCfg(parent_link_name="base", position=(0.1, 0.2, 0.3), rotation=(1, 0, 0, 0)) + body: BodyCfg = BodyCfg(model=model, base_link_name="base", prefix="p_") + + return SceneCfg(objs=Objects()) + + +def test_two_bodies_resolve_site_parent_by_owning_body(tmp_path): + @configclass + class Objects(SceneObjsCfg): + optical: SiteCfg = SiteCfg(parent_link_name="base") + second: SiteCfg = SiteCfg(parent_link_name="tool") + left: BodyCfg = BodyCfg( + model=MjcfFileCfg(file=_write_link_model(tmp_path, "l.xml", "base")), base_link_name="base", prefix="a_" + ) + right: BodyCfg = BodyCfg( + model=MjcfFileCfg(file=_write_link_model(tmp_path, "r.xml", "tool")), base_link_name="tool", prefix="b_" + ) + + spec = build_mujoco_spec(SceneCfg(objs=Objects())) + assert spec.site("a_optical").parent.name == "a_base" + assert spec.site("b_second").parent.name == "b_tool" + + +def test_two_bodies_with_same_link_name_require_decorated_parent(tmp_path): + @configclass + class Objects(SceneObjsCfg): + optical: SiteCfg = SiteCfg(parent_link_name="base") + left: BodyCfg = BodyCfg( + model=MjcfFileCfg(file=_write_link_model(tmp_path, "l.xml", "base")), base_link_name="base", prefix="a_" + ) + right: BodyCfg = BodyCfg( + model=MjcfFileCfg(file=_write_link_model(tmp_path, "r.xml", "base")), base_link_name="base", prefix="b_" + ) + + objs = Objects() + with pytest.raises(ValueError, match="multiple attached bodies"): + build_mujoco_spec(SceneCfg(objs=objs)) + + objs.optical.parent_link_name = "b_base" + spec = build_mujoco_spec(SceneCfg(objs=objs)) + assert spec.site("optical").parent.name == "b_base" + + +def _write_link_model(tmp_path, filename, link): + path = tmp_path / filename + path.write_text( + f'' + ) + return path + + +def test_mounts_before_attach_with_decorated_name_and_xyzw_rotation(scene): + spec = build_mujoco_spec(scene) + site = spec.site("p_optical") + assert site.parent.name == "p_base" + np.testing.assert_allclose(site.pos, [0.1, 0.2, 0.3]) + np.testing.assert_allclose(site.quat, [0, 1, 0, 0]) + model = spec.compile() + assert model.site("p_optical").id >= 0 + assert model.site("p_old").id >= 0 + + +def test_site_rejects_unknown_parent(scene): + scene.objs.optical.parent_link_name = "missing" + with pytest.raises(ValueError, match="parent link"): + build_mujoco_spec(scene) + + +def test_site_does_not_replace_imported_site(scene): + @configclass + class Objects(SceneObjsCfg): + old: SiteCfg = SiteCfg(parent_link_name="base") + body: BodyCfg = scene.objs.body + + scene.objs = Objects() + with pytest.raises(ValueError, match="already exist"): + build_mujoco_spec(scene) + + +def test_mjcf_model_level_sites_are_applied_and_decorated(tmp_path): + @configclass + class Objects(SceneObjsCfg): + body: BodyCfg = BodyCfg( + model=MjcfFileCfg( + file=_write_link_model(tmp_path, "m.xml", "base"), + sites=[SiteCfg(name="marker", parent_link_name="base")], + ), + base_link_name="base", + prefix="p_", + ) + + spec = build_mujoco_spec(SceneCfg(objs=Objects())) + assert spec.site("p_marker").parent.name == "p_base" + + +def test_raycast_camera_is_explicitly_unsupported(scene): + @configclass + class Sensors(SceneSensorsCfg): + camera: RaycastCameraSensorCfg = RaycastCameraSensorCfg( + site="optical", + width=4, + height=2, + far=10, + projection=PinholeCameraCfg.from_fov(width=4, height=2, horizontal_fov=90), + ) + + scene.sensors = Sensors() + with pytest.raises(NotImplementedError, match="MuJoCo does not support RaycastCameraSensorCfg"): + build_mujoco_spec(scene) diff --git a/motrixlab/pyproject.toml b/motrixlab/pyproject.toml index afd6fd13..0eb8adf5 100644 --- a/motrixlab/pyproject.toml +++ b/motrixlab/pyproject.toml @@ -24,7 +24,7 @@ classifiers = [ dependencies = [ "gymnasium==1.1.1", "hydra-core>=1.3,<1.4", - "motrixsim==0.10.1", + "motrixsim-core===0.10.2.dev132873", "numpy>=1.26", "omegaconf>=2.3,<2.4", "motrix-env-core==0.4.0", diff --git a/pyproject.toml b/pyproject.toml index 4187605a..eb80c97f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -87,6 +87,7 @@ members = [ environments = ["sys_platform == 'darwin'", "sys_platform != 'darwin'"] package = false prerelease = "if-necessary-or-explicit" +constraint-dependencies = ["motrixsim-core===0.10.2.dev132873"] # The forwarding extras are mutually exclusive. uv also collects motrix-rl's # member-local conflicts when resolving this workspace. conflicts = [[{ extra = "cuda" }, { extra = "rocm" }]] @@ -95,7 +96,16 @@ conflicts = [[{ extra = "cuda" }, { extra = "rocm" }]] name = "pypi" url = "https://pypi.org/simple" +# Development builds are published separately from public PyPI. Restrict this +# index to MotrixSim so unrelated dependencies retain their existing sources. +[[tool.uv.index]] +name = "motphys" +url = "https://pypi.motphys.com/simple" +explicit = true + [tool.uv.sources] +motrixsim = { index = "motphys" } +motrixsim-core = { index = "motphys" } motrixlab = { workspace = true } motrix-robots = { workspace = true } motrix-env-core = { workspace = true } diff --git a/uv.lock b/uv.lock index 4573fb56..6c4740e8 100644 --- a/uv.lock +++ b/uv.lock @@ -119,6 +119,7 @@ members = [ "motrix-unitree-native", "motrixlab", ] +constraints = [{ name = "motrixsim-core", specifier = "===0.10.2.dev132873", index = "https://pypi.motphys.com/simple" }] [[package]] name = "absl-py" @@ -1509,14 +1510,14 @@ version = "0.4.0" source = { editable = "motrix_env_motrixsim" } dependencies = [ { name = "motrix-env-core" }, - { name = "motrixsim" }, + { name = "motrixsim-core" }, { name = "numpy" }, ] [package.metadata] requires-dist = [ { name = "motrix-env-core", editable = "motrix_env_core" }, - { name = "motrixsim", specifier = "==0.10.1" }, + { name = "motrixsim-core", specifier = "===0.10.2.dev132873", index = "https://pypi.motphys.com/simple" }, { name = "numpy", specifier = ">=1.26" }, ] @@ -1845,7 +1846,7 @@ dependencies = [ { name = "motrix-env-motrixsim" }, { name = "motrix-rl" }, { name = "motrix-rl-builtin" }, - { name = "motrixsim" }, + { name = "motrixsim-core" }, { name = "numpy" }, { name = "omegaconf" }, ] @@ -1859,47 +1860,32 @@ requires-dist = [ { name = "motrix-env-motrixsim", editable = "motrix_env_motrixsim" }, { name = "motrix-rl", editable = "motrix_rl" }, { name = "motrix-rl-builtin", editable = "motrix_rl_builtin" }, - { name = "motrixsim", specifier = "==0.10.1" }, + { name = "motrixsim-core", specifier = "===0.10.2.dev132873", index = "https://pypi.motphys.com/simple" }, { name = "numpy", specifier = ">=1.26" }, { name = "omegaconf", specifier = ">=2.3,<2.4" }, ] -[[package]] -name = "motrixsim" -version = "0.10.1" -source = { registry = "https://pypi.org/simple" } -dependencies = [ - { name = "motrixsim-core" }, -] -wheels = [ - { url = "https://files.pythonhosted.org/packages/cd/cd/0f2ca088cea1721d9a0cca058e9b18a41b5645ce8dcf8a857086e7667fec/motrixsim-0.10.1-py3-none-any.whl", hash = "sha256:9c27362c40b9a72c3e7ac80f556cc0effd4c30ab3afb93764c81e1f3745402b0", size = 2018, upload-time = "2026-09-18T16:25:05.939Z" }, -] - [[package]] name = "motrixsim-core" -version = "0.10.1" -source = { registry = "https://pypi.org/simple" } +version = "0.10.2.dev132873" +source = { registry = "https://pypi.motphys.com/simple" } dependencies = [ { name = "absl-py" }, { name = "numpy" }, ] wheels = [ - { url = "https://files.pythonhosted.org/packages/4f/13/5aacc370c2aa951a34587549625d4776c84d30d0ef1f1da555ef6a51923c/motrixsim_core-0.10.1-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:d12fba015f1ce890e0a07daa6c9d24730805dfb27916f3603779186294358150", size = 58730585, upload-time = "2026-09-18T16:26:14.933Z" }, - { url = "https://files.pythonhosted.org/packages/07/3d/c4d41be2de374a73aa1c1cde2a8df5b1225b204a83e4ab7f52fe095a69c7/motrixsim_core-0.10.1-cp310-cp310-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:acce59249576d724160756383632bc8af522beac337d6b41ccea0b521f972841", size = 69500031, upload-time = "2026-09-18T16:29:59.37Z" }, - { url = "https://files.pythonhosted.org/packages/fc/b4/186e83b3d89b3818aaccefd62690b77d6914370097fd5645dbc1f4d3d92a/motrixsim_core-0.10.1-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:1a16c78a2a9a5a574de38af0cf3831b12c3f754eba2be8474046434884a14b34", size = 66696962, upload-time = "2026-09-18T16:29:41.349Z" }, - { url = "https://files.pythonhosted.org/packages/17/be/95c65ea4484597c7575f4d44f6cd006bd6212d079c09a9335675749aa2c3/motrixsim_core-0.10.1-cp310-cp310-win_amd64.whl", hash = "sha256:da609700990970bd304e07cb8e9072ed4e98c986e07da103aac6b8f3507bba43", size = 50656529, upload-time = "2026-09-18T16:28:49.685Z" }, - { url = "https://files.pythonhosted.org/packages/73/5a/85b89afdb2e1c3adc6ff6f1d10874088739087b8e7ecf42bef00699d7180/motrixsim_core-0.10.1-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:3149c4073b456a6cb8e2dbce39d3a1b4d70b95da7c11bae7cfc8b8e1fdc35386", size = 58731259, upload-time = "2026-09-18T16:27:58.5Z" }, - { url = "https://files.pythonhosted.org/packages/4f/83/900092f8a9e7c8b4ff88ff7bf86c2bc8e2643105bbb0e2f85839645b0f3e/motrixsim_core-0.10.1-cp311-cp311-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:078dfd9f618474e1708fb86694faf1122373b17b393b1cffb7c84daf3cf0edb4", size = 69501226, upload-time = "2026-09-18T16:27:26.247Z" }, - { url = "https://files.pythonhosted.org/packages/39/2d/6eb9fb28a9a3117724f8cb948ea372fdd2f89d6b1b2f4662be54629a90bd/motrixsim_core-0.10.1-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:7c6175bc165025efeaeb34b4c76163793b7184a5fea1d24b480a842c257e58e9", size = 66701167, upload-time = "2026-09-18T16:30:17.576Z" }, - { url = "https://files.pythonhosted.org/packages/d0/71/817d5721250f6242263307a5a1149b710aef6af1d25399f9320aad54efb6/motrixsim_core-0.10.1-cp311-cp311-win_amd64.whl", hash = "sha256:15fd847e567677afe8713317b5a660f9d26b6a1f1157f57a7adcfa5d21af7173", size = 50653220, upload-time = "2026-09-18T16:29:05.126Z" }, - { url = "https://files.pythonhosted.org/packages/1b/95/0e298da88c4887f9e7d3ca02351fb862c098b0f8f7870a87a13a18359db3/motrixsim_core-0.10.1-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:52b02f9a28b0b4fe3cff1d2509f8a3244f042656a7927aeb70495186d925c041", size = 58697017, upload-time = "2026-09-18T16:28:32.372Z" }, - { url = "https://files.pythonhosted.org/packages/5b/c6/dc3274f0917a5554473c2c4d6c95996184139c73d6491963569fa6dbcd44/motrixsim_core-0.10.1-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:2917d0e8f52927dee5528ebc122f66e0a65f00bee7b27231aee6a4cb09278643", size = 69507661, upload-time = "2026-09-18T16:28:16.719Z" }, - { url = "https://files.pythonhosted.org/packages/ec/12/bbd8505b27aa2416d3acf987c5b059caf73d78714474934deee138ae0f57/motrixsim_core-0.10.1-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:614a08b544729f358fb8f4e1b7e9cd750e7810ec47aecc3454ff5f2c36d90cc6", size = 66723817, upload-time = "2026-09-18T16:26:50.839Z" }, - { url = "https://files.pythonhosted.org/packages/0e/41/04629d7096448caf1ea65c0b82b3fed49116296b63f0045b16a58d1bf935/motrixsim_core-0.10.1-cp312-cp312-win_amd64.whl", hash = "sha256:f8a5dcb0695afd4e31abb83ff63aeb8c82acfcf7dd567c926ae30fd9d862bbc6", size = 50665263, upload-time = "2026-09-18T16:29:22.762Z" }, - { url = "https://files.pythonhosted.org/packages/eb/7d/b8900552d9c7d81387e80862bd5edd5d86398bf6449cd5a94de8d7ed5e05/motrixsim_core-0.10.1-cp313-cp313-macosx_11_0_arm64.whl", hash = "sha256:6856d47dde407f64ddaea180a447d13144e6186e476ac23d90dfbd9a54d8df0a", size = 58697497, upload-time = "2026-09-18T16:27:08.133Z" }, - { url = "https://files.pythonhosted.org/packages/9d/27/1cba9fef3a45a6c827efa453627733aff408b763d0aae5587fe1ffe2c536/motrixsim_core-0.10.1-cp313-cp313-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:ebf2ed5f07c1e8804700383bee3936d8c91f0b56f46a2b756f5cb72896cc3e56", size = 69509267, upload-time = "2026-09-18T16:25:58.542Z" }, - { url = "https://files.pythonhosted.org/packages/38/cd/3957b90ab104b138381a6e7e9959d291ff0884ad70abeebef32d6c277efc/motrixsim_core-0.10.1-cp313-cp313-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:8ae4bff1ea808cfe42410b813eda58ddffff7e052cc0ac4fc07a0190eddb809c", size = 66721878, upload-time = "2026-09-18T16:26:34.175Z" }, - { url = "https://files.pythonhosted.org/packages/35/99/a958b8ee056fc405743cc53a4f4c5653c25d5cb733db76da643843b66bd1/motrixsim_core-0.10.1-cp313-cp313-win_amd64.whl", hash = "sha256:f85afd967b853b848a3d6fcead4e0879c933b573b4340f7281be2ce1e0949e15", size = 50662519, upload-time = "2026-09-18T16:27:40.491Z" }, + { url = "https://pypi.motphys.com/packages/motrixsim_core-0.10.2.dev132873-cp310-cp310-macosx_11_0_arm64.whl", hash = "sha256:777b9fd2f318ba5562a4394b757c79353c09ce98f8c8ace91af9b82c22b143cb" }, + { url = "https://pypi.motphys.com/packages/motrixsim_core-0.10.2.dev132873-cp310-cp310-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:77124838e3f07059d1565905edafd0de54cd7e82c34c29ae04872dcb5f71b6c4" }, + { url = "https://pypi.motphys.com/packages/motrixsim_core-0.10.2.dev132873-cp310-cp310-win_amd64.whl", hash = "sha256:f57b6fe345f0fb34a760d63c39700af19b3ad3ee196fd154a8450034e7ada2ce" }, + { url = "https://pypi.motphys.com/packages/motrixsim_core-0.10.2.dev132873-cp311-cp311-macosx_11_0_arm64.whl", hash = "sha256:be68c7c783fe70387996ff9a4513f11678e51045a965add18198855c9ef12609" }, + { url = "https://pypi.motphys.com/packages/motrixsim_core-0.10.2.dev132873-cp311-cp311-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:93f702f346e5641505702cbb70e713f6a598f30c0645bcc70b30ea7eefd36716" }, + { url = "https://pypi.motphys.com/packages/motrixsim_core-0.10.2.dev132873-cp311-cp311-win_amd64.whl", hash = "sha256:4f44f68f3670ddeff5ed181897d5dc17f9ceededa466ac838db23fcf00d93e7a" }, + { url = "https://pypi.motphys.com/packages/motrixsim_core-0.10.2.dev132873-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:bf3d91367eda7f9ebeff248c7212932a8c62bb84471d670fd792922591606f08" }, + { url = "https://pypi.motphys.com/packages/motrixsim_core-0.10.2.dev132873-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:13ae6173ebc383b68f4ef58563f5e2cfcdad0c679831ea0e3f4a6324f5b9fdb0" }, + { url = "https://pypi.motphys.com/packages/motrixsim_core-0.10.2.dev132873-cp312-cp312-win_amd64.whl", hash = "sha256:c22fda380f41eaa10d8ad5b9da05009b90a20dc32935aeb7f4d237cdba8ed332" }, + { url = "https://pypi.motphys.com/packages/motrixsim_core-0.10.2.dev132873-cp313-cp313-macosx_11_0_arm64.whl", hash = "sha256:99ebce10edcb4a11684f20e19653e38f04694b1a79f5a112677bbc4bbbc88237" }, + { url = "https://pypi.motphys.com/packages/motrixsim_core-0.10.2.dev132873-cp313-cp313-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:3bfa49345e32734ecb85e9b0a170711682385b978c5f612ec3ead08c3a670419" }, + { url = "https://pypi.motphys.com/packages/motrixsim_core-0.10.2.dev132873-cp313-cp313-win_amd64.whl", hash = "sha256:58b0275809f4ff76d687fb718f367224f2a6b2a6fff1d47c4b9f8335f7fceb31" }, ] [[package]] diff --git a/wiki/design/scene-model-config.md b/wiki/design/scene-model-config.md index 2d35e45a..cead9a4e 100644 --- a/wiki/design/scene-model-config.md +++ b/wiki/design/scene-model-config.md @@ -4,6 +4,23 @@ 本文定义 MotrixLab 的场景配置模型。`SceneCfg` 统一保存基础模型文件、asset registry、scene-level visual、system camera 和 object registry：asset 通过 `SceneAssetsCfg` 的字段名获得稳定名称，scene object 通过 `SceneObjsCfg` 的字段名获得稳定名称与组装顺序，全局视觉环境通过 `SceneVisualCfg` 配置，interactive viewer 与视频录制的默认视角通过 `SystemCameraCfg` 配置。`StandardSceneCfg` 在通用 `SceneCfg` 之上提供标准渐变天空、地面、材质、贴图、haze 与方向光，并允许用户通过 configclass 继承覆写。场景最终通过 MotrixSim MSD API 直接组装为 `World`，不通过字符串或临时 MJCF 文件拼接。 +## 感知相机与挂载 + +`SceneCfg.sensors` 使用 `SceneSensorsCfg` 字段名作为传感器名。`RaycastCameraSensorCfg` 描述物理深度相机，不是 `SystemCameraCfg` 渲染视角；配置定义在 core 的 `config.scene.sensor`，不暴露原生 simulator 类型。 + +- `site` 引用完整组装后场景中的 optical site。统一的 `SiteCfg` 是 scene object，`parent_link_name` 使用导入模型中的逻辑 link 名；编译器根据所属 `BodyCfg` 的 prefix/suffix 解析最终 link，并在所有 body 附加后挂载，因此不依赖字段声明顺序。URDF 文件内的 `sites` 也使用同一配置类型并显式提供 `name`。 +- `PinholeCameraCfg` 使用像素单位的 `(fx,fy,cx,cy)`，`from_fov` 接受角度制 FOV；`OrthographicCameraCfg` 使用两个方向的像素间距。光学轴为 +X 右、+Y 下、+Z 前，像素中心为 `(u+.5,v+.5)`。 +- 原始测量与图像预处理分离。`outputs` 启用光轴深度、射线距离、世界坐标命中点/法线；至少启用一个距离字段。near/far 限制光轴深度，近处遮挡物不会被穿透跳过。 +- 自身排除与几何白名单独立于接触响应；具体后端不支持配置时显式报错。MuJoCo compiler 支持 link site，但不支持该原生相机。 + +`RaycastCameraImageQuery(sensor=...,field=...)` 从一次原生像素交错读取建立只读视图：距离为 `(N,H,W)`，点/法线为 `(N,H,W,3)`。同一相机多个字段共享原生读取；既有 `SensorValuesQuery` 继续提供扁平布局。距离单位米，miss 为 -1，miss 点/法线为零；validity 可由距离非负得到，但不能据此判定是否已经采样。相机的宽高和投影参数直接由场景配置提供，不额外建立未被读取路径使用的 spec 对象。 + +### 当前原生生命周期边界 + +MotrixSim `0.10.2.dev132022` 在 physics step 的积分前状态采样，`update_period=0` 每物理步更新，其余周期由原生调度。读取不推进物理、不强制采样。`SceneData.reset(..., forward_kinematic=True)` 会在 reset 状态刷新 raycast camera 图像；写入关节/浮动基状态并执行带 `forward_kinematic=True` 的 write program 也会刷新图像。普通 `reset`（不请求 FK）仍保留缓存图像，直接修改 DoF 后单独调用 FK 也不会使已有 camera 帧失效或重新采样。 + +当前版本仍不提供 camera ready、帧序号、采样时间、采样位姿，也没有独立的无时间推进 refresh/invalidate API。因而 query 仍是原生缓存读取接口，不能从 `-1` 区分未就绪与全 miss，也不能把当前 site pose 当作缓存图像的采样外参。不通过图像差分或周期推算伪造帧元数据；完整的有效性和采样元数据契约仍需上游接口支持。 + ## 目标 - `SceneCfg` 能完整声明程序化场景所需的 asset 与 object。