diff --git a/CHANGELOG.md b/CHANGELOG.md index f838232d..7bdcbbc8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -54,6 +54,15 @@ All notable changes to GeneLab are recorded here. time, so robots whose actuator parameters live in Python config — rather than in an MJCF file — can use the same named knobs without hand-translating to PD gains. +- **Camera debug helpers.** Added + `InteractiveScene.draw_camera_frustums(camera_names=None, color=...)` + and `InteractiveScene.draw_camera_trajectory(positions, radius, color)` + wrapping Genesis 1.0's `scene.draw_debug_frustum` / + `scene.draw_debug_trajectory`. The frustum helper resolves names + against the scene's `CameraSensor` set; the trajectory helper passes + positions through verbatim. Added a public `gs_camera` property on + `CameraSensor` so the scene helper can reach the Genesis handle + without poking private state. ### Changed diff --git a/docs/concepts/scene.en.md b/docs/concepts/scene.en.md index 4e454efb..36a1bf9e 100644 --- a/docs/concepts/scene.en.md +++ b/docs/concepts/scene.en.md @@ -31,6 +31,23 @@ Genesis APIs and Isaac Lab-style task code use different vocabulary. Wrappers is difference: MDP terms read stable GeneLab properties while the backend integration handles Genesis details. +## Camera debug helpers + +`InteractiveScene` exposes two thin wrappers around Genesis 1.0's debug-draw +API for inspecting attached cameras: + +- `draw_camera_frustums(camera_names=None, color=(1, 1, 1, 0.3))` draws the view + frustum of every (or named) `CameraSensor` on the scene. Returns the number of + frustums drawn. Raises if a name does not resolve to a `CameraSensor`. +- `draw_camera_trajectory(positions, radius=0.002, color=(1, 0.5, 0, 0.8))` + draws a polyline through `positions` (one row per world-frame point). The + caller owns the buffer — typically a recorded camera path — so the scene + keeps no history. + +Both helpers must be called after `InteractiveScene.build()`; calling pre-build +raises `RuntimeError`. Use them from a play-time hook to visualise where the +RGB-D sensors are looking and how their pose evolved over a rollout. + ## Where to continue - [Module Map](../reference/module-map.md) diff --git a/docs/concepts/scene.zh.md b/docs/concepts/scene.zh.md index a945bf9b..3d531cd5 100644 --- a/docs/concepts/scene.zh.md +++ b/docs/concepts/scene.zh.md @@ -27,6 +27,22 @@ entity 表访问，例如用 `env.articulations["robot"].data` 读取 `RobotStat Genesis API 和 Isaac Lab 风格任务代码使用的词汇不同。wrapper 隔离这种差异：MDP term 读取稳定的 GeneLab 属性，后端集成负责 Genesis 细节。 +## 相机调试辅助 + +`InteractiveScene` 提供两个对 Genesis 1.0 debug-draw API 的薄封装，用于检查 +挂载在场景上的相机： + +- `draw_camera_frustums(camera_names=None, color=(1, 1, 1, 0.3))` 绘制场景上 + 每一个（或指定名字的）`CameraSensor` 的视锥。返回实际绘制的视锥数。指定 + 的名字若没有对应 `CameraSensor` 会抛错。 +- `draw_camera_trajectory(positions, radius=0.002, color=(1, 0.5, 0, 0.8))` + 在 `positions`（每行一个世界系坐标点）上画一条折线。调用方持有 buffer —— + 通常是录制下来的相机路径 —— 所以 scene 不维护任何历史。 + +两个辅助方法都必须在 `InteractiveScene.build()` 之后调用；build 之前调用会抛 +`RuntimeError`。一般是从 play 阶段的 hook 里调用，用来可视化 RGB-D 传感器 +所看的视野以及一次 rollout 中位姿的变化。 + ## 继续阅读 - [模块地图](../reference/module-map.md) diff --git a/src/genelab/scene/interactive_scene.py b/src/genelab/scene/interactive_scene.py index 1bfe5412..b1a423e6 100644 --- a/src/genelab/scene/interactive_scene.py +++ b/src/genelab/scene/interactive_scene.py @@ -27,6 +27,9 @@ from genelab.sensor import Sensor from genelab.terrains import TerrainImporter +if TYPE_CHECKING: + from genelab.sensor.camera import CameraSensor + _pyrender_save_patch_applied = False @@ -340,6 +343,70 @@ def reset(self, env_ids: torch.Tensor) -> None: for art in self.articulations.values(): art.reset(env_ids) + def draw_camera_frustums( + self, + *, + camera_names: tuple[str, ...] | None = None, + color: tuple[float, float, float, float] = (1.0, 1.0, 1.0, 0.3), + ) -> int: + """Draw the view frustum of one or more :class:`~genelab.sensor.CameraSensor` s. + + Wraps Genesis 1.0's ``scene.draw_debug_frustum(camera, color=...)``. When + ``camera_names`` is ``None`` every ``CameraSensor`` on the scene is drawn; + otherwise only the named ones (a name that doesn't resolve to a + ``CameraSensor`` raises). Returns the number of frustums drawn so callers + can detect mis-named selections without inspecting the viewer state. + + Cameras allocate their Genesis-side handle in + :meth:`~genelab.sensor.CameraSensor.pre_build_genesis`; calling this before + :meth:`build` raises a ``RuntimeError``. + """ + if not self._built: + raise RuntimeError("draw_camera_frustums called before InteractiveScene.build()") + selected = self._select_camera_sensors(camera_names) + for sensor in selected: + self._gs_scene.draw_debug_frustum(sensor.gs_camera, color=color) + return len(selected) + + def draw_camera_trajectory( + self, + positions: Any, + *, + radius: float = 0.002, + color: tuple[float, float, float, float] = (1.0, 0.5, 0.0, 0.8), + ) -> None: + """Draw a polyline through ``positions`` (one row per world-frame point). + + Thin wrapper over Genesis 1.0's + ``scene.draw_debug_trajectory(poss, radius=…, color=…)``. The caller owns + the positions buffer (typically a recorded camera path or an inspection + waypoint sequence) — no history is maintained on the scene side. + """ + if not self._built: + raise RuntimeError("draw_camera_trajectory called before InteractiveScene.build()") + self._gs_scene.draw_debug_trajectory(positions, radius=radius, color=color) + + def _select_camera_sensors(self, names: tuple[str, ...] | None) -> "list[CameraSensor]": + from genelab.sensor.camera import CameraSensor + + if names is None: + return [s for s in self._sensors.values() if isinstance(s, CameraSensor)] + selected: list[CameraSensor] = [] + for n in names: + sensor = self._sensors.get(n) + if sensor is None: + raise KeyError( + f"draw_camera_frustums: no sensor named {n!r} on this scene " + f"(have: {sorted(self._sensors)})" + ) + if not isinstance(sensor, CameraSensor): + raise TypeError( + f"draw_camera_frustums: sensor {n!r} is not a CameraSensor " + f"(got {type(sensor).__name__})" + ) + selected.append(sensor) + return selected + def close(self) -> None: scene = self._gs_scene if scene is None: diff --git a/src/genelab/sensor/camera.py b/src/genelab/sensor/camera.py index 36ad5ce0..103a6ccd 100644 --- a/src/genelab/sensor/camera.py +++ b/src/genelab/sensor/camera.py @@ -151,6 +151,16 @@ def _validate_cfg(self) -> None: f"render_rgb / render_depth / render_segmentation must be True" ) + @property + def gs_camera(self) -> object | None: + """The underlying Genesis camera handle, or ``None`` before ``pre_build_genesis``. + + Exposed for scene-level debug helpers (e.g. + :meth:`~genelab.scene.InteractiveScene.draw_camera_frustums`) that need the + handle to call Genesis's ``draw_debug_frustum`` API directly. + """ + return self._cam + def _allocate_camera(self, gs_scene: object, link: object) -> object: cam = gs_scene.add_camera( # type: ignore[attr-defined] res=(self._cfg_typed.width, self._cfg_typed.height), diff --git a/tests/test_scene_camera_debug.py b/tests/test_scene_camera_debug.py new file mode 100644 index 00000000..d30aa5c6 --- /dev/null +++ b/tests/test_scene_camera_debug.py @@ -0,0 +1,149 @@ +"""Unit tests for ``InteractiveScene.draw_camera_frustums`` / ``draw_camera_trajectory``. + +The scene helpers wrap Genesis 1.0's ``scene.draw_debug_frustum(camera, color=...)`` +and ``scene.draw_debug_trajectory(poss, ...)``. The tests use a fake gs_scene that +records every debug-draw call so we can assert (a) the right camera handle is +forwarded for each selection and (b) selection errors are raised with useful +messages — no real Genesis viewer needed. +""" + +from typing import Any + +import pytest + +torch = pytest.importorskip("torch") + + +class _FakeCameraHandle: + """Stand-in for the gs.Camera handle attached by ``CameraSensor._allocate_camera``.""" + + def __init__(self, name: str) -> None: + self.name = name + + +class _FakeCameraSensor: + """Minimum subset of CameraSensor that ``_select_camera_sensors`` reads. + + Subclasses the real ``CameraSensor`` class symbol so the ``isinstance`` check + inside the helper still passes — the helper only ever touches ``gs_camera``. + """ + + def __init__(self, name: str) -> None: + self._name = name + self._cam = _FakeCameraHandle(name) + + @property + def gs_camera(self) -> Any: + return self._cam + + +class _FakeGsScene: + """Records every debug-draw call so tests can assert on them.""" + + def __init__(self) -> None: + self.frustum_calls: list[tuple[Any, tuple[float, float, float, float]]] = [] + self.trajectory_calls: list[tuple[Any, float, tuple[float, float, float, float]]] = [] + + def draw_debug_frustum(self, camera: Any, color: tuple[float, float, float, float]) -> None: + self.frustum_calls.append((camera, color)) + + def draw_debug_trajectory( + self, poss: Any, radius: float, color: tuple[float, float, float, float] + ) -> None: + self.trajectory_calls.append((poss, radius, color)) + + +def _build_scene(sensors: dict[str, Any], *, built: bool = True) -> Any: + """Construct just enough ``InteractiveScene`` surface for the debug helpers.""" + from genelab.scene.interactive_scene import InteractiveScene + + scene = InteractiveScene.__new__(InteractiveScene) + scene._gs_scene = _FakeGsScene() + scene._sensors = sensors + scene._built = built + return scene + + +def _patch_camera_sensor_isinstance(monkeypatch: pytest.MonkeyPatch) -> None: + """``_select_camera_sensors`` uses ``isinstance(sensor, CameraSensor)``; route + the import to a class the fakes inherit from so the test fakes pass the check + without needing a real Genesis env to construct a real ``CameraSensor``.""" + import genelab.sensor.camera as camera_mod + + monkeypatch.setattr(camera_mod, "CameraSensor", _FakeCameraSensor) + + +def test_draw_camera_frustums_default_draws_every_camera( + monkeypatch: pytest.MonkeyPatch, +) -> None: + _patch_camera_sensor_isinstance(monkeypatch) + sensors = {"front": _FakeCameraSensor("front"), "side": _FakeCameraSensor("side")} + scene = _build_scene(sensors) + + n = scene.draw_camera_frustums() + + assert n == 2 + drawn = [call[0].name for call in scene._gs_scene.frustum_calls] + assert sorted(drawn) == ["front", "side"] + + +def test_draw_camera_frustums_named_selection( + monkeypatch: pytest.MonkeyPatch, +) -> None: + _patch_camera_sensor_isinstance(monkeypatch) + sensors = {"front": _FakeCameraSensor("front"), "side": _FakeCameraSensor("side")} + scene = _build_scene(sensors) + + n = scene.draw_camera_frustums(camera_names=("side",), color=(0.1, 0.2, 0.3, 0.4)) + + assert n == 1 + cam, color = scene._gs_scene.frustum_calls[0] + assert cam.name == "side" + assert color == (0.1, 0.2, 0.3, 0.4) + + +def test_draw_camera_frustums_unknown_name_raises( + monkeypatch: pytest.MonkeyPatch, +) -> None: + _patch_camera_sensor_isinstance(monkeypatch) + sensors = {"front": _FakeCameraSensor("front")} + scene = _build_scene(sensors) + + with pytest.raises(KeyError, match="no sensor named 'ghost'"): + scene.draw_camera_frustums(camera_names=("ghost",)) + + +def test_draw_camera_frustums_non_camera_sensor_raises( + monkeypatch: pytest.MonkeyPatch, +) -> None: + _patch_camera_sensor_isinstance(monkeypatch) + sensors = {"front": _FakeCameraSensor("front"), "imu": object()} + scene = _build_scene(sensors) + + with pytest.raises(TypeError, match="not a CameraSensor"): + scene.draw_camera_frustums(camera_names=("imu",)) + + +def test_draw_camera_frustums_pre_build_raises() -> None: + scene = _build_scene({}, built=False) + with pytest.raises(RuntimeError, match="before InteractiveScene.build"): + scene.draw_camera_frustums() + + +def test_draw_camera_trajectory_forwards_args() -> None: + scene = _build_scene({}) + poss = torch.tensor([[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [1.0, 1.0, 0.0]]) + + scene.draw_camera_trajectory(poss, radius=0.005, color=(0.0, 1.0, 0.0, 1.0)) + + assert len(scene._gs_scene.trajectory_calls) == 1 + forwarded, radius, color = scene._gs_scene.trajectory_calls[0] + assert torch.equal(forwarded, poss) + assert radius == pytest.approx(0.005) + assert color == (0.0, 1.0, 0.0, 1.0) + + +def test_draw_camera_trajectory_pre_build_raises() -> None: + scene = _build_scene({}, built=False) + with pytest.raises(RuntimeError, match="before InteractiveScene.build"): + scene.draw_camera_trajectory([[0.0, 0.0, 0.0]])