diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 8a56ed317a..6bc9e07eab 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -181,9 +181,10 @@ topics: status: active - id: sim-visualization title: Simulation Visualization - aliases: [browser visualization, viser, sim visualization, 仿真可视化] + aliases: [browser visualization, viser, sim visualization, marker visualization, 标志物可视化, 仿真可视化] keywords: [VisualizationCfg, ViserServerCfg, SceneManifest, visualization, EntityGizmoManipulator, robot_ik_gizmo, - GizmoCfg, backpressure, capture_visualization, Newton deformable, particle set, render topology, quaternion, xyzw] + GizmoCfg, MarkerGroup, MarkerPrototypeCfg, RobotMarkers, VisualizationMarkers, robot markers, ghost robot, 机器人可视化, backpressure, capture_visualization, Newton deformable, particle set, + render topology, quaternion, xyzw] paths: [topics/sim-visualization/sim-visualization.md] source_of_truth: - embodichain/lab/visualization/protocol.py diff --git a/agent_context/topics/sensor-system/sensor-system.md b/agent_context/topics/sensor-system/sensor-system.md index 03f92cb9ef..e54de6ca77 100644 --- a/agent_context/topics/sensor-system/sensor-system.md +++ b/agent_context/topics/sensor-system/sensor-system.md @@ -134,5 +134,7 @@ quota are integration contracts. Generic image tiling belongs to | Backend contact behavior | `tests/sim/sensors/test_contact_default_e2e.py`, `test_contact_newton_e2e.py` in that directory | For missing camera output, inspect enabled fields and `ViewFlags` together. +Camera and stereo images exclude visual markers by default. Set +`include_visual_overlays=True` for an explicit preview camera. For missing contacts, inspect backend capabilities, the query filter and quota before changing buffer allocation. Pure resolver tests do not require a renderer. diff --git a/agent_context/topics/sim-visualization/runtime.md b/agent_context/topics/sim-visualization/runtime.md index 1f16ad3350..2f2c475e1b 100644 --- a/agent_context/topics/sim-visualization/runtime.md +++ b/agent_context/topics/sim-visualization/runtime.md @@ -15,12 +15,44 @@ Read this when the request needs these details. [Topic overview](sim-visualizati | `destroy()` | Stops visualization before queuing deferred simulation cleanup | During explicit `SimulationManager.update()`, each physics step increments the -visualization step/time counters and attempts a rate-limited capture. A topology -revision mismatch publishes a fresh manifest before its first matching frame. +visualization step/time counters and refreshes marker attachments after the +optional `after_substep` observer. `render_frame()` shares state publication +across camera observations, recording and rate-limited Viser capture. With +`render_final_step=False`, the final capture waits for that frame; state writes +between the last substep and the frame are included when render state is +published, including refreshed marker poses. A topology revision mismatch +publishes a fresh manifest before its first matching frame. `BaseEnv.reset()` also requests a forced capture after resetting scene state. Drawing markers and capturing visualization do not advance physics; interactive loops call `SimulationManager.update(step=1)` to process Gizmos and step the world. +Marker groups default to `scope="env"` and inherit `SimulationManager.num_envs`. +`scope="world"` creates one global batch. The new group config has no +`arena_index`. Updates use `(E,M,3)` positions/scales, `(E,M,4)` xyzw/RGBA, +and `(E,M)` type/visibility arrays; `env_ids` selects and orders the E dimension. +One selected environment accepts the `(M,...)` shorthand. Counts may differ +between environments after selected updates; `count` is the total and `counts` +contains one value per environment. Selected clear/hide/update preserves others. + +`attach(parent_uid, link_name=..., env_ids=...)` resolves prepared registered +rigid roots or articulation/robot links through public pose APIs. Stored marker +poses become parent-relative offsets. The manager refreshes these transforms +on host steps and explicit render-state synchronization; snapshots stay detached +and do not read physics. `detach(keep_world_pose=True)` preserves the displayed +pose by default. World-scope attachment is rejected rather than guessing a +source environment. Parent resolution must not retain borrowed native nodes. + +Marker groups publish a complete detached snapshot after `update()`, +`set_visibility()`, `clear()`, or `remove()`. Native groups update DexSim debug +meshes immediately. Browser groups are included as mesh overlays and may force +a frame only when the currently published manifest matches the manager's +topology revision. Marker publication never calls `prepare()` or synchronizes +physics. An unpublished host substep defers browser marker capture to +`render_frame()`, after the observer and all attachment refreshes. Paused edits +still publish immediately when no frame is pending. A dirty topology defers +the browser marker update until the next explicit host capture or simulation +update. + Manager add methods mark topology dirty for rigid objects, rigid-object groups, volume/surface deformables, robots, articulations, and `Camera` sensors. Supported `remove_asset()` branches do the same. The next simulation update refreshes the @@ -61,10 +93,11 @@ Deformable vertices are stored relative to the corresponding arena node. | `SurfaceDeformableObject` | Live Newton render-surface vertices and triangles | | `Camera` | Frustum plus optional low-frequency RGB preview | | Default ground | 1000 m × 1000 m XY grid, 1 m cells, 10 m sections | -| `SceneOverlays` | Frames, targets, trajectories, and point clouds | +| `SceneOverlays` | Frames, targets, trajectories, point clouds, and marker meshes | Lights, rigid constraints, markers/gizmos, contact sensors, and stereo cameras -are not exported as scene nodes by the current `SceneExporter`. Visual +are not exported as scene nodes by the current `SceneExporter`. Marker groups +instead travel as `MeshMarkerOverlay` entries. Visual materials and textures are not mirrored; meshes use category colors from `SceneExporter._COLORS`. @@ -151,7 +184,13 @@ GUI callbacks enqueue events; the visualization worker applies them, preserving the single-threaded Viser-handle invariant. Environment visibility affects static batches, deformables, and camera frustums. -Overlays are backend-neutral protocol objects. Point clouds larger than +Overlays are backend-neutral protocol objects. Marker groups use triangle-mesh +overlays with per-instance RGBA, pose, scale, and visibility. Built-in +prototypes are box, sphere, cylinder, capsule, cone, arrow, and frame; custom +triangle meshes are also supported. This does not add GPU instancing, and the +existing point-cloud and polyline overlay paths remain independent. + +Point clouds larger than `point_cloud_max_points` are deterministically subsampled. The ordinary `SimulationManager.capture_visualization()` helper currently supplies no overlay argument; callers needing overlays must use the active diff --git a/agent_context/topics/sim-visualization/sim-visualization.md b/agent_context/topics/sim-visualization/sim-visualization.md index 181d834a91..89500907df 100644 --- a/agent_context/topics/sim-visualization/sim-visualization.md +++ b/agent_context/topics/sim-visualization/sim-visualization.md @@ -2,7 +2,7 @@ > Topic: Simulation visualization — `SimulationManager` lifecycle, > backend-neutral scene snapshots, the Viser backend, cameras, deformables, -> interactive Gizmos, and launcher integration. +> interactive Gizmos, render-only marker groups, and launcher integration. --- @@ -18,6 +18,7 @@ | `embodichain/lab/visualization/runtime.py` | Background worker, latest-frame queues, rate limiting, health, and telemetry | | `embodichain/lab/visualization/backends/base.py` | Visualization backend contract | | `embodichain/lab/visualization/backends/viser.py` | Viser server, scene handles, GUI controls, and browser publishing | +| `embodichain/lab/visualization/markers/` | Marker prototype/group configuration, geometry generation, validation, and lifecycle | | `embodichain/lab/gym/utils/gym_utils.py` | Gym config parsing, common launcher arguments, and CLI overrides | | `embodichain/lab/gym/envs/base_env.py` | Environment scene setup, post-setup start, and forced reset capture | | `embodichain/lab/scripts/preview_asset.py` | Standalone rigid-object and articulation browser preview | @@ -55,6 +56,41 @@ The simulation owns physics and assets. Scene export is read-only. When `allow_commands=True`, Viser Gizmo callbacks enqueue pose commands that `SimulationManager` later applies on the simulation thread. +`SimulationManager.add_marker_group()` owns named render-only marker groups. +`MarkerPrototypeCfg` supplies box, sphere, cylinder, capsule, cone, arrow, +frame, or caller-provided mesh geometry; `MarkerGroup.update()` publishes +batched pose, scale, prototype, color, and visibility arrays without stepping +physics. Groups default to all sub-environments; `env_ids` selects mutations, +while `scope="world"` creates one global batch. Attach/detach follows prepared +registered roots or links through public pose reads on the simulation thread. +During `SimulationManager.update()`, attachment refresh follows the optional +`after_substep` observer and precedes recording and visualization capture, so +captures include pose changes made by that observer. +Native markers use Scene-owned pure visual objects. Creation, cloning, batched +transforms and removal go through the existing scene facade. Failed updates +preserve previously published marker state. Marker operations do not prepare or +step physics; handles are invalidated when the owning scene closes. Browser +rendering carries detached mesh snapshots in `SceneOverlays`. + +`VisualizationMarkers` adapts flat Isaac-style arrays (xyzw) with explicit +`environment_ids`. It defaults to world coordinates and environment 0. +`RobotMarkers` reads URDF/USD visuals and composes externally supplied link poses +with visual origins. URDF FK accepts all `joint_names`, including caller-resolved +mimic joints. Multiple poses can be displayed in one environment. Tasks own +motion loading, sampling and alignment. + +`RobotMarkersAppearanceCfg` selects source appearance or a base-color override, +with adjustable opacity. Source textures and material parameters are preserved; +opaque visuals use scene lighting. Transparent shading may differ by renderer. +`unlit=True` selects a uniform color. Offscreen cameras exclude visuals until +explicitly enabled. File-backed and extracted texture resources remain available +until removal. + +Batch updates read each selected environment's count directly. Viser combines +ordinary scene and marker environment IDs when creating visibility controls; +removing the last ordinary object preserves the environment's hidden state +while markers remain. The controls are rebuilt only when membership changes. + `SimulationManager` may start the Viser server during its own construction, before a standalone caller has declared any assets. That empty visualization startup must not call `prepare()` or finalize the Spawn scene. The caller's @@ -62,7 +98,7 @@ first asset-bearing `prepare()` remains the initial topology commit; the next capture observes the topology revision and republishes the manifest. All DexSim and Torch reads happen in `SceneExporter` on the simulation thread. -Protocol objects make detached CPU copies before crossing into the runtime. +Protocol objects detach mutable poses and retain shared immutable marker geometry before crossing into the runtime. Viser creation and every Viser handle mutation must happen on the single visualization worker thread; `ViserBackend._assert_update_thread()` enforces this invariant. @@ -79,7 +115,7 @@ this invariant. - Gizmo commands mutate targets only after simulation-thread validation. - Simulation/DexSim reads stay on the simulation thread. - Viser server and handle operations stay on one private worker thread. -- Cross-thread snapshots own detached contiguous CPU arrays. +- Cross-thread snapshots detach mutable state; marker geometry arrays may share validated read-only storage. - A frame is accepted only when `run_id`, `scene_revision`, node IDs, and dynamic vertex shapes match the published manifest. - Topology changes require a new manifest; pose-only changes require only a @@ -90,6 +126,9 @@ this invariant. and offsets face indices into the concatenated vertex array. Links without render meshes export empty geometry without querying mesh zero. - Sampling rates are wall-clock limits, not simulation-time guarantees. +- Marker mutation never prepares or synchronizes an unprepared scene. When + topology is dirty, browser publication waits for the next explicit host + capture or `SimulationManager.update()`. - Bind to loopback by default. Viser has no EmbodiChain authentication layer; use SSH forwarding or an authenticated gateway for remote access. @@ -100,6 +139,7 @@ this invariant. | `Visualization env_ids ... outside simulation range` | Selected IDs do not exist in the configured arenas. Validate them against `SimulationManager.num_envs`. | | Startup timeout or address-in-use error | The Viser worker did not become ready or the configured port is occupied. Select another port and inspect `visualization_health.worker_error`. | | Asset added after startup is missing | Step once, call `refresh_visualization()`, or mark topology dirty if the change bypassed manager APIs. | +| Native marker creation reports missing render-actor support | Use a DexSim build exposing Scene render-only mesh lifecycle and generic RenderBody/MaterialInst overlay properties, or run with `--viser`. Overlay routing supports Hybrid, FastRT and OfflineRT. | | Newton Viser shows only the grid after declaring a robot | Check that Viser startup did not finalize an empty Spawn scene before asset declaration and that link poses remain finite after the first update. | | A Newton articulation link is missing geometry or emits `mesh_id 0 out of range` | Export all render-body mesh segments and treat a zero-mesh link as empty geometry. Do not use the single-mesh articulation helper for Newton render bodies. | | Browser stops updating after an exporter/backend exception | `capture_visualization_safely()` latches the first error to protect simulation. Inspect health/logs, then stop and restart after fixing the cause. | @@ -120,8 +160,22 @@ Relevant tests: - `tests/visualization/test_scene_exporter.py` - `tests/visualization/test_runtime.py` - `tests/visualization/test_viser_backend.py` +- `tests/visualization/test_markers.py` +- `tests/visualization/test_native_markers.py` - `tests/sim/test_sim_manager.py` +- `tests/sim/test_marker_rendering.py` - `tests/gym/utils/test_gym_utils.py` - `tests/lab/scripts/test_preview_asset.py` - `tests/lab/scripts/test_preview_robot.py` - `tests/lab/scripts/test_preview_joint_control.py` + + +Recorded task visualization uses `examples/sim/reference_trajectory.py`: named +world link poses have axes `(T,K,L,7)`, while task objects use `VisualObject` +and paths/targets use `VisualizationMarkers`. Offset layouts translate the +robot, object, target and path together. Policy execution stays task-owned. +`RobotMarkersCfg.source` accepts the same `UsdSourceCfg` selector as scene +assets. USD singleton selection and USDZ texture extraction are shared through +`sim/spawn/_usd_assets.py`; ghost-owned extraction directories survive until +removal. Scaled marker transforms use the Scene visual batch, while rigid +VisualObject poses use the World tensor batch. diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.sensors.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.sensors.rst index 6bb45275e6..5ba915bec2 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.sensors.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.sensors.rst @@ -54,6 +54,10 @@ Camera :show-inheritance: :exclude-members: __init__, copy, replace, to_dict, validate +Camera and stereo captures exclude native markers by default. Set +``include_visual_overlays=True`` on an explicit preview camera to include +both scene-lit markers and overlays. + Attachment resolution --------------------- diff --git a/docs/source/api_reference/embodichain/embodichain.lab.visualization.rst b/docs/source/api_reference/embodichain/embodichain.lab.visualization.rst index fc3b01c714..93fda1a477 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.visualization.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.visualization.rst @@ -20,6 +20,13 @@ CLI helpers (:func:`add_viser_args_to_parser`, :func:`visualization_cfg_from_args`) wire the standard ``--viser*`` arguments into launchers. +Render-only marker groups provide reusable primitive or caller-supplied mesh +geometry for native and browser visualization. They do not create physics +bodies or advance simulation. See +:class:`embodichain.lab.visualization.markers.MarkerGroupCfg` and +:class:`embodichain.lab.visualization.markers.MarkerGroup` for creation, +batched updates, visibility, and cleanup. + Configuration ------------- @@ -35,6 +42,207 @@ Configuration :show-inheritance: :exclude-members: __init__, copy, replace, to_dict, validate +Markers +------- + +Groups default to ``scope="env"`` and use ``SimulationManager.num_envs``. +Positions are local to each sub-environment; its origin is added once during +snapshot generation. Use ``scope="world"`` for a single global batch. +There is no ``arena_index`` on ``MarkerGroupCfg``. + +``update`` accepts position/scale arrays shaped ``(E, M, 3)``, xyzw quaternion +and RGBA arrays shaped ``(E, M, 4)``, and prototype/visibility arrays shaped +``(E, M)``. With ``env_ids``, ``E`` is the number of selected environments in +that order. A single selected environment also accepts ``(M, ...)`` arrays. +Changing a selected environment's marker count resets its omitted fields; +other environments retain their state. ``count`` is the total instance count, +and ``counts`` reports counts in environment order. + +.. code-block:: python + + markers = sim.add_marker_group(MarkerGroupCfg( + name="targets", prototypes={"sphere": MarkerPrototypeCfg(shape="sphere")} + )) + markers.update(translations=positions) # (sim.num_envs, M, 3) + markers.update(translations=selected_positions, env_ids=[1, 3]) # (2, M, 3) + markers.set_visibility(False, env_ids=[1]) + markers.clear(env_ids=[3]) + +Attach to a registered rigid object or articulation root by UID, or to a robot/ +articulation link using ``link_name``. The target must already be prepared. +Existing marker poses become parent-relative offsets; subsequent host updates +and explicit render-state synchronization refresh the transforms without +marker operations stepping physics. Attachment currently requires env scope. + +.. code-block:: python + + markers.attach("robot", link_name="tool0") + markers.attach("box", env_ids=[1]) + markers.detach(env_ids=[1]) # Keep the current world pose by default. + +Prototype colors and per-instance overrides are sRGB RGBA in [0, 1]. Frame +markers retain RGB axis colors while following alpha. Mesh prototypes may carry +UV coordinates and a uint8 RGB/RGBA texture, with repeat, clamp or mirror +sampling. Geometry is copied and validated once per prototype, then shared as +read-only arrays by its instance snapshots. + +Native markers use Scene-owned visual objects, shared geometry and independent +material state. Batch updates preserve selected-environment ordering without +stepping physics. Offscreen cameras exclude these visuals by default; opt in +with ``SpawnedCamera.set_visual_overlay_enabled(True)`` for diagnostic capture. +Appearance depends on the renderer. Transparent overlap and lighting can differ +between native and browser visualization. + +.. autoclass:: embodichain.lab.visualization.markers.MarkerPrototypeCfg + :members: + :undoc-members: + :exclude-members: __init__, copy, replace, to_dict, validate + +.. autoclass:: embodichain.lab.visualization.markers.MarkerGroupCfg + :members: + :undoc-members: + :exclude-members: __init__, copy, replace, to_dict, validate + +.. autoclass:: embodichain.lab.visualization.markers.MarkerGroup + :members: + :undoc-members: + +Visualization markers and robot markers +--------------------------------------- + +``VisualizationMarkers`` adapts flat Isaac-style arrays to ``MarkerGroup``. +Its orientations are **xyzw**, matching Isaac Lab 3.0 and EmbodiChain. Marker +names determine prototype indices. ``environment_ids`` explicitly assigns each +instance to an environment; omitted IDs place every instance in environment 0. +The default coordinate space is world. Use ``coordinate_space="env"`` to add +environment origins. The adapter never infers environment membership from count. + +``RobotMarkers`` reads URDF visuals or one selected USD articulation/visual +object. Multiple visuals per link, visual origins, mesh scales and material +parts are preserved. ``visualize_link_poses`` accepts ``(E, L, 7)`` xyz+xyzw or +``(E, L, 4, 4)`` matrices; inserting ``K`` after ``E`` displays K references in +each environment. ``link_names`` gives the required link order. ``env_ids`` +selects rows to update while other environments retain their references. + +USD sources can use ``source=UsdSourceCfg(file_path=..., source_prim_path=...)``, +sharing source selection and package texture resolution with ``VisualObject``. +Use either ``source`` or ``asset_path``/``prim_path``. URDF inputs use +``asset_path``. A ghost owns extracted package textures until removal; ordinary +USD texture files are referenced directly. + +USD materials require runtime descriptors with explicit color-space and +texture-sampling support. Older runtimes raise an upgrade error before material +conversion; color spaces and sampler transforms are not guessed. URDF loading +does not require these USD material interfaces. + +URDF sources also provide ``visualize_qpos`` and ``joint_names``. Callers supply +every joint in that order, including mimic joints; FK does not expand mimic +relations. USD sources use external link poses. Motion loading, time sampling, +root alignment and target selection remain in the task. + +``RobotMarkersAppearanceCfg(mode="override")`` replaces the base-color factor while +retaining source textures, roughness, metallic, normal maps and other imported +material parameters. ``mode="asset"`` retains the original factor and multiplies +source alpha by ``opacity``. Texture RGB still modulates the new factor: dark +painted details remain dark when the factor is green. + +Opaque robot markers preserve imported materials and respond to scene lighting. +Normals, material UV coordinates and extracted texture files remain available +for the visual's lifetime. Transparency is adjustable, but translucent shading +can be simpler than opaque shading and does not promise the same result across +renderers. ``RobotMarkersAppearanceCfg(unlit=True)`` selects a uniform color; +general marker prototypes are unlit by default. + +``set_appearance`` changes the ghost's independent material state without editing +the source asset. ``set_visibility`` retains instances; ``remove`` releases them. + +.. code-block:: python + + from embodichain.lab.visualization.markers import ( + RobotMarkersAppearanceCfg, RobotMarkers, RobotMarkersCfg, + ) + + sim.prepare() + ghost = RobotMarkers(RobotMarkersCfg( + name="reference", asset_path=robot_urdf, + coordinate_space="env", + appearance=RobotMarkersAppearanceCfg(mode="asset", opacity=0.4), + ), sim) + ghost.visualize_qpos(reference_qpos, root_poses=reference_root_poses) + with sim.render_frame(): + pass + ghost.remove() + +The executable example ``examples/sim/robot_markers.py`` loads the Panda dataset +by default. It accepts ``--asset`` to select another URDF, as well as +``--layers``, ``--mode``, ``--opacity`` and ``--viser``. Its generated reference +motion does not advance physics. ``--headless --output-dir`` saves native images +and separates creation, update/submission and camera-render timings. + +Recorded task trajectories +-------------------------- + +The trajectory example defaults to ``--camera-mode follow_xy``. It fits the +whole clip once and follows the first trajectory's root in world X/Y, keeping +world height, orbit orientation and distance fixed. Use ``--camera-mode fixed`` +for a stationary camera. Live mouse orbit adjustments are preserved while +tracking. This follows ``PolicyViewerCameraCfg`` ground-plane tracking semantics. + +``examples/sim/reference_trajectory.py`` displays recorded policy link poses +through ``RobotMarkers``. It enables ghost visuals, path markers and optional +``VisualObject`` task cubes by default. The example does not step physics; a task +or policy evaluator owns state sampling and checkpoint execution. + +The NPZ contains ``poses`` shaped ``(T, K, L, 7)`` in world xyz+xyzw order, +``link_names`` shaped ``(L,)`` and a positive scalar ``dt`` in seconds. Each +trajectory has its own link poses. Optional fields are end-effector ``tips`` +``(T,K,3)``, task ``objects`` ``(T,K,7)``, ``targets`` ``(T,K,3)``, and body-frame +``command``/``velocity`` ``(T,3)``. Unequal source durations should be sampled onto +one time grid by the task; holding the final state is suitable for completed +manipulation episodes. + +``--layout overlap`` keeps common robot origins. ``--layout offset --spacing 1.1`` +adds a Y offset to each complete trajectory, its object, target and path. This +preserves the spatial relationship within each task. ``--original-first`` uses +source appearance for the first trajectory; other trajectories use colored +translucent materials with ``--opacity``. ``--live`` opens a bounded native +viewer, while the default writes H.264 video, PNG frames and capture metadata to +``--output``. ``--seconds`` and ``--fps`` set duration and sampling rate. + +Robot markers and task objects use the existing scene ownership and pose APIs. +Regular task objects participate in camera images; robot markers require +explicit camera visualization opt-in. + +.. autoclass:: embodichain.lab.visualization.markers.VisualizationMarkersCfg + :members: + :exclude-members: __init__, copy, replace, to_dict, validate + +.. autoclass:: embodichain.lab.visualization.markers.VisualizationMarkers + :members: + :exclude-members: __init__, copy, replace, to_dict, validate + +.. autoclass:: embodichain.lab.visualization.markers.RobotMarkersAppearanceCfg + :members: + :exclude-members: __init__, copy, replace, to_dict, validate + +.. autoclass:: embodichain.lab.visualization.markers.RobotMarkersCfg + :members: + :exclude-members: __init__, copy, replace, to_dict, validate + +.. autoclass:: embodichain.lab.visualization.markers.RobotMarkers + :members: + :exclude-members: __init__, copy, replace, to_dict, validate + +Implementation modules expose the same contracts: + +.. autosummary:: + + markers.visualization_markers.VisualizationMarkers + markers.visualization_markers.VisualizationMarkersCfg + markers.robot_markers.RobotMarkersAppearanceCfg + markers.robot_markers.RobotMarkersCfg + markers.robot_markers.RobotMarkers + Runtime ------- @@ -154,6 +362,10 @@ Overlays and Camera Preview :members: :undoc-members: +.. autoclass:: MeshMarkerOverlay + :members: + :undoc-members: + .. autoclass:: CameraSpec :members: :undoc-members: diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 78019d860a..58d68d14e6 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -4061,6 +4061,7 @@ embodichain.lab.visualization.protocol JointControlSpec JointControlState MeshGeometry + MeshMarkerOverlay PickCommand PointCloudOverlay SceneFrame @@ -4074,6 +4075,39 @@ embodichain.lab.visualization.protocol estimate_manifest_bytes pose_to_position_wxyz +embodichain.lab.visualization.markers +------------------------------------ + +Render-only marker groups share validated geometry and instance state between +native simulation windows and browser visualization. + +.. currentmodule:: embodichain.lab.visualization.markers + +.. autosummary:: + + MarkerGroupCfg + MarkerPrototypeCfg + MarkerGroup + +embodichain.lab.visualization.markers.cfg +---------------------------------------- + +.. currentmodule:: embodichain.lab.visualization.markers.cfg + +.. autosummary:: + + MarkerPrototypeCfg + MarkerGroupCfg + +embodichain.lab.visualization.markers.group +------------------------------------------ + +.. currentmodule:: embodichain.lab.visualization.markers.group + +.. autosummary:: + + MarkerGroup + embodichain.lab.visualization.runtime ------------------------------------- diff --git a/embodichain/lab/sim/cfg/viewer.py b/embodichain/lab/sim/cfg/viewer.py index 35710cda49..f28eff7c19 100644 --- a/embodichain/lab/sim/cfg/viewer.py +++ b/embodichain/lab/sim/cfg/viewer.py @@ -60,7 +60,7 @@ class MarkerCfg: """Type of corner/joint visualization for axis markers (e.g., SPHERE, CUBE, etc.).""" arena_index: int = -1 - """Index of the arena where the marker should be placed. -1 means all arenas.""" + """Index of the arena where the marker should be placed. -1 selects world coordinates in the global environment.""" @configclass diff --git a/embodichain/lab/sim/sensors/camera.py b/embodichain/lab/sim/sensors/camera.py index 19c0264cee..fd3821e87b 100644 --- a/embodichain/lab/sim/sensors/camera.py +++ b/embodichain/lab/sim/sensors/camera.py @@ -27,6 +27,7 @@ from typing import TYPE_CHECKING, Any, List, Literal, Sequence, Tuple from embodichain.lab.sim.sensors import BaseSensor, SensorCfg +from embodichain.lab.visualization.markers._native import _MARKER_RENDER_CATEGORY from embodichain.lab.sim._render_selection import ( _normalize_render_rows, _select_render_values, @@ -79,6 +80,9 @@ def transformation(self) -> torch.Tensor: visualization_role: Literal["sensor", "record"] = "sensor" """Role used to group this camera in live visualization previews.""" + include_visual_overlays: bool = False + """Include native visual overlays and scene-lit markers in captured images.""" + # Camera parameters width: int = 640 height: int = 480 @@ -252,6 +256,8 @@ def _build_sensor_from_config( self._create_camera_views(config, (config.intrinsics,)) self._entities = [handle.native for handle in self._camera_handles] + self._configure_visual_overlays(self._entities, config.include_visual_overlays) + # Define a mapping of data types to their respective shapes and dtypes buffer_specs = { "color": ( @@ -287,6 +293,15 @@ def _build_sensor_from_config( self.cfg: CameraCfg = config + def _configure_visual_overlays( + self, views: Sequence[dr.CameraView], include: bool + ) -> None: + if include: + self._frame_buffer.set_visual_overlay_enabled(True) + else: + for view in views: + view.set_excluded_render_categories(_MARKER_RENDER_CATEGORY) + def set_ground_plane_visibility( self, visible: bool, env_ids: Sequence[int] | None = None ) -> None: @@ -295,6 +310,7 @@ def set_ground_plane_visibility( This changes every image output, shadow and reflection consistently. It preserves collisions and other cameras. ``reset`` restores the configured policy; topology reattachment preserves the current policy. + Marker filtering retains the configured ``include_visual_overlays`` policy. Args: visible: Whether this camera permits rendering the default ground. @@ -338,6 +354,8 @@ def set_ground_plane_visibility( raise RuntimeError("Camera owner is unavailable") owner._require_ground_plane_filtering() excluded = 0 if visible else owner._GROUND_RENDER_CATEGORY + if not self.cfg.include_visual_overlays: + excluded |= _MARKER_RENDER_CATEGORY self._capture_metadata = None for index in changed: self._entities[index].set_excluded_render_categories(excluded) diff --git a/embodichain/lab/sim/sensors/stereo.py b/embodichain/lab/sim/sensors/stereo.py index e4d909ca63..9aa29513bd 100644 --- a/embodichain/lab/sim/sensors/stereo.py +++ b/embodichain/lab/sim/sensors/stereo.py @@ -202,6 +202,9 @@ def _build_sensor_from_config( self._create_camera_views(config, (config.intrinsics, config.intrinsics_right)) left_list = [h.native for h in self._camera_handles[: self.num_instances]] right_list = [h.native for h in self._camera_handles[self.num_instances :]] + self._configure_visual_overlays( + left_list + right_list, config.include_visual_overlays + ) for i in range(self.num_instances): self._entities[i] = PairCameraView( left_list[i], diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index 0ea54f59dc..71b5ed686b 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -142,6 +142,8 @@ def _is_usd_path(path: object | None) -> bool: from embodichain.lab.sim.material import _material_desc_from_cfg from embodichain.lab.sim.profiler import Profiler, ProfilerCfg from embodichain.lab.visualization.cfg import VisualizationCfg +from embodichain.lab.visualization.markers import MarkerGroup, MarkerGroupCfg +from embodichain.lab.visualization.markers._native import NativeMarkerRenderer from embodichain.utils import configclass, logger from embodichain.utils.math import ( look_at_to_pose, @@ -159,6 +161,7 @@ def _is_usd_path(path: object | None) -> bool: RuntimeStats, SceneManifest, SceneOverlays, + MeshMarkerOverlay, VisualizationRuntime, ) @@ -720,6 +723,11 @@ def _initialize_simulation( self._picker_gizmo: tuple[str, str | None] | None = None self._visual_objects: dict[str, VisualObject] = {} + + # marker management + self._marker_groups: dict[str, MarkerGroup] = {} + self._native_markers: NativeMarkerRenderer | None = None + self._rigid_objects: Dict[str, RigidObject] = dict() self._rigid_object_groups: Dict[str, RigidObjectGroup] = dict() self._deformable_objects: Dict[str, DeformableObject] = dict() @@ -1872,6 +1880,7 @@ def prepare(self) -> None: self._attach_parented_cameras() self._camera_attachment_topology_revision = topology_revision self._ready_spawn_topology_revision = topology_revision + self._refresh_marker_attachments() def _prepare_spawn_runtime(self, result: Scene) -> None: """Prepare backend runtime buffers for one Spawn topology revision.""" @@ -1966,10 +1975,12 @@ def sync_render_state(self) -> None: """Publish current physics state to render resources without stepping. Use this after direct state writes, such as an environment reset, and - before reading cameras or publishing a visualization frame. + before reading cameras or publishing a visualization frame. Native + render uploads, including marker edits, complete before this returns. Raises: - RuntimeError: If the Spawn scene has not been prepared. + RuntimeError: If the Spawn scene has not been prepared or native + render publication fails. """ result = self.spawn_result if result is None: @@ -1977,6 +1988,11 @@ def sync_render_state(self) -> None: "Render-state synchronization requires a prepared Spawn scene." ) self.physics.sync_render_state(result) + self._refresh_marker_attachments() + if self.has_native_renderer: + status = self._world._flush_render_updates() + if status < 0: + raise RuntimeError(f"Native render-state publication failed: {status}.") if getattr(self, "_render_state_published", None) is not None: self._render_state_published = True @@ -2083,10 +2099,11 @@ def update( self.physics._step(physics_dt) self._visualization_sim_step += 1 self._visualization_sim_time += physics_dt + self._pending_record_dt += physics_dt if after_substep is not None: with self.profiler.section("after_substep"): after_substep(physics_dt) - self._pending_record_dt += physics_dt + self._refresh_marker_attachments() if render_final_step or i < step - 1: with self.render_frame(capture_camera_images=i == step - 1): pass @@ -4449,6 +4466,8 @@ def remove_asset(self, uid: str) -> bool: ): self.remove_asset(sensor_cfg.uid) + for group in tuple(getattr(self, "_marker_groups", {}).values()): + group._detach_parent(uid) was_materialized = scene.builder.is_finalized scene.remove(uid) visual = getattr(self, "_visual_objects", {}).get(uid) @@ -4476,6 +4495,161 @@ def remove_asset(self, uid: str) -> bool: self.notify_visualization_topology_changed() return True + def add_marker_group(self, cfg: MarkerGroupCfg) -> MarkerGroup: + """Create an empty render-only marker group with a stable unique name. + + Args: + cfg: Prototypes and environment/world coordinate scope. Environment + groups use all ``num_envs`` environments by default. Use + ``group.update(...)`` to populate instances. Viser groups do + not allocate native geometry. Native groups require DexSim's + ``RenderBody`` and ``MaterialInst`` overlay properties. + + Returns: + Mutable marker group owned by this simulation manager. + + Raises: + ValueError: If the name is in use or configuration is invalid. + RuntimeError: If the native engine lacks debug overlay support. + """ + if cfg.name in self._marker_groups or cfg.name in self._markers: + raise ValueError(f"Marker {cfg.name!r} already exists.") + cfg.validate() + group = MarkerGroup( + cfg, + num_envs=self.num_envs, + origins=self.arena_offsets if cfg.scope == "env" else None, + pose_resolver=self._resolve_marker_parent_poses, + on_change=self._publish_marker_group, + on_remove=self._remove_marker_group, + ) + if ( + self.sim_config.visualization.backend != "viser" + and self._native_markers is None + and self.has_native_renderer + ): + self._native_markers = NativeMarkerRenderer( + self._spawn_scene.builder.result, + # The 0.6.0rc1 OptiX offscreen compositor drops raster overlays. + use_scene_for_overlays=self.sim_config.render_cfg.renderer + in {"fast-rt", "rt"}, + ) + self._marker_groups[group.name] = group + return group + + def _resolve_marker_parent_poses( + self, parent: str, link_name: str | None, env_ids: list[int] + ) -> object: + """Read live domain poses without retaining native nodes or preparing Spawn.""" + asset = None + articulated = False + for registry_name in ("_rigid_objects", "_robots", "_articulations"): + registry = getattr(self, registry_name, {}) + if parent in registry: + asset = registry[parent] + articulated = registry_name != "_rigid_objects" + break + if asset is None: + raise KeyError( + f"Marker parent {parent!r} is not a registered rigid object, robot or articulation." + ) + if self.spawn_result is None or not asset.is_spawn_bound: + raise RuntimeError( + f"Marker parent {parent!r} requires a prepared and bound scene; call prepare() explicitly." + ) + if link_name is not None: + if not articulated: + raise ValueError("link_name requires a robot or articulation parent.") + if link_name not in asset.link_names: + raise ValueError(f"Marker parent {parent!r} has no link {link_name!r}.") + return asset.get_link_pose(link_name, env_ids=env_ids) + return asset.get_local_pose()[env_ids] + + def _refresh_marker_attachments(self) -> None: + """Refresh native overlays; browser capture keeps the host's cadence.""" + for group in tuple(getattr(self, "_marker_groups", {}).values()): + try: + group._refresh_attachments(on_change=self._publish_native_marker_group) + except Exception as error: + # Automatic visual refresh must not interrupt physics/accounting. + # Group publication rolls back, allowing a later host sync to retry. + logger.log_warning( + f"Marker attachment refresh failed for {group.name!r}: {error!r}" + ) + + def get_marker_group(self, name: str) -> MarkerGroup: + """Return a registered marker group. + + Args: + name: Stable group name. + + Returns: + The existing group; unknown names raise ``KeyError``. + """ + return self._marker_groups[name] + + def get_marker_overlays(self) -> tuple[MeshMarkerOverlay, ...]: + """Capture detached group geometry and world poses for visualization. + + Returns: + Complete marker mesh snapshots without stepping physics. + """ + return tuple( + mesh + for group in getattr(self, "_marker_groups", {}).values() + for mesh in group.snapshot() + ) + + def _publish_native_marker_group(self, group: MarkerGroup) -> None: + if self._native_markers is not None: + self._native_markers.publish(group.name, group.snapshot()) + + def _publish_marker_group(self, group: MarkerGroup) -> None: + self._publish_native_marker_group(group) + self._capture_marker_groups(raise_errors=True) + + def _remove_marker_group(self, group: MarkerGroup) -> None: + if self._native_markers is not None: + self._native_markers.remove(group.name) + self._marker_groups.pop(group.name, None) + self._capture_marker_groups() + + def _capture_marker_groups(self, *, raise_errors: bool = False) -> None: + # Marker publication must not prepare pending declarations or synchronize + # an unprepared physics scene. Use only the already-published manifest; + # a dirty topology is refreshed by the next explicit host capture/update. + # Unpublished substeps belong to render_frame(), after every observer + # and attachment refresh; a marker edit must not force an early frame. + runtime = self._visualization_runtime + if ( + runtime is None + or getattr(self, "_pending_record_dt", 0.0) > 0 + or self._visualization_error_reported + or self._visualization_manifest_topology_revision + != self._visualization_topology_revision + ): + return + try: + runtime.capture( + sim_step=self._visualization_sim_step, + sim_time=self._visualization_sim_time, + overlays=self._visualization_overlays, + force=True, + capture_camera_images=False, + ) + except Exception as error: + logger.log_warning(f"Viser marker update failed: {error!r}") + self._visualization_error_reported = True + if raise_errors: + raise + + def clear_markers(self) -> None: + """Remove all new marker groups and legacy axes from this simulation.""" + for group in tuple(getattr(self, "_marker_groups", {}).values()): + group.remove() + for name in tuple(self._markers): + self.remove_marker(name) + def draw_marker( self, cfg: MarkerCfg, @@ -4535,11 +4709,13 @@ def draw_marker( ) return False - original_name = cfg.name + original_name = ( + f"{cfg.name}_{cfg.arena_index}" if cfg.arena_index >= 0 else cfg.name + ) name = original_name count = 0 - while name in self._markers: + while name in self._markers or name in getattr(self, "_marker_groups", {}): count += 1 name = f"{original_name}_{count}" if count > 0: @@ -4552,9 +4728,6 @@ def draw_marker( logger.log_warning(f"No marker poses provided.") return None - if cfg.arena_index >= 0: - name = f"{name}_{cfg.arena_index}" - env = self.get_env(cfg.arena_index) # Create markers based on marker type @@ -4602,6 +4775,9 @@ def remove_marker(self, name: str) -> bool: Returns: bool: True if the marker was removed successfully, False otherwise. """ + if name in getattr(self, "_marker_groups", {}): + self._marker_groups[name].remove() + return True if name not in self._markers: logger.log_warning(f"Marker {name} not found.") return False @@ -5439,6 +5615,12 @@ def destroy(self, exit_process: bool | None = None) -> None: def _deferred_destroy(self) -> None: """Destroy all simulated assets and release resources.""" + # Release caller-retained group callbacks and native meshes before arenas. + for group in tuple(getattr(self, "_marker_groups", {}).values()): + group.remove() + if getattr(self, "_native_markers", None) is not None: + self._native_markers.close() + self._native_markers = None # Clean up all gizmos before destroying the simulation for uid in list(self._gizmos.keys()): self.disable_gizmo(uid) diff --git a/embodichain/lab/sim/spawn/_usd_assets.py b/embodichain/lab/sim/spawn/_usd_assets.py index a830e3e875..4240bd4369 100644 --- a/embodichain/lab/sim/spawn/_usd_assets.py +++ b/embodichain/lab/sim/spawn/_usd_assets.py @@ -33,7 +33,8 @@ class _UsdTextureFiles: """Keep package texture files available through scene topology rebuilds.""" - def __init__(self) -> None: + def __init__(self, directory: Path | None = None) -> None: + self._external_directory = directory self._directory: TemporaryDirectory[str] | None = None self._paths: dict[tuple[str, int, int], str] = {} @@ -65,13 +66,14 @@ def _resolve_path(self, asset_path: str) -> str: asset = Ar.GetResolver().OpenAsset(Ar.ResolvedPath(asset_path)) if asset is None: raise FileNotFoundError(f"Cannot read USD package texture: {asset_path}") - if self._directory is None: + if self._external_directory is None and self._directory is None: self._directory = TemporaryDirectory(prefix="embodichain-usdz-textures-") _, inner = Ar.SplitPackageRelativePathInner(asset_path) # Use a generated filename, never an archive member path, so package # names cannot escape the scene's private temporary directory. digest = sha256(repr(key).encode("utf-8")).hexdigest() - destination = Path(self._directory.name) / f"{digest}{Path(inner).suffix}" + directory = self._external_directory or Path(self._directory.name) + destination = directory / f"{digest}{Path(inner).suffix}" destination.write_bytes(asset.GetBuffer()) self._paths[key] = str(destination) return str(destination) @@ -81,3 +83,37 @@ def close(self) -> None: if self._directory is not None: self._directory.cleanup() self._directory = None + + +def _parse_singleton( + path: object, + collection: str | tuple[str, ...], + label: str, + *, + prim_path: str | None = None, +): + if path is None: + raise ValueError(f"A USD path is required for the {label}.") + + from dexsim.kit.usd import parse_usd + + scene = parse_usd(str(path)) + collections = (collection,) if isinstance(collection, str) else collection + candidates = [item for name in collections for item in getattr(scene, name)] + if prim_path is not None: + candidates = [ + item + for item in candidates + if item.usd is not None and item.usd.prim_path == prim_path + ] + if len(candidates) != 1: + found = [ + (item.name, None if item.usd is None else item.usd.prim_path) + for item in candidates + ] + selection = "" if prim_path is None else f" at prim path {prim_path!r}" + raise ValueError( + f"Expected exactly one {label} in USD file {path!r}{selection}, found " + f"{len(candidates)}: {found}." + ) + return scene, candidates[0] diff --git a/embodichain/lab/sim/spawn/usd.py b/embodichain/lab/sim/spawn/usd.py index 3c1980aa07..f00b7dcd7c 100644 --- a/embodichain/lab/sim/spawn/usd.py +++ b/embodichain/lab/sim/spawn/usd.py @@ -17,6 +17,8 @@ from __future__ import annotations +from ._usd_assets import _parse_singleton + import os from dataclasses import fields, replace from typing import TypeVar @@ -257,35 +259,6 @@ def articulation_desc_from_usd( return desc, materials -def _parse_singleton( - path: object, collection: str, label: str, *, prim_path: str | None = None -): - if path is None: - raise ValueError(f"A USD path is required for the {label}.") - - from dexsim.kit.usd import parse_usd - - scene = parse_usd(str(path)) - candidates = getattr(scene, collection) - if prim_path is not None: - candidates = [ - item - for item in candidates - if item.usd is not None and item.usd.prim_path == prim_path - ] - if len(candidates) != 1: - found = [ - (item.name, None if item.usd is None else item.usd.prim_path) - for item in candidates - ] - selection = "" if prim_path is None else f" at prim path {prim_path!r}" - raise ValueError( - f"Expected exactly one {label} in USD file {path!r}{selection}, found " - f"{len(candidates)}: {found}." - ) - return scene, candidates[0] - - def _namespace_materials( renders: list[RenderDesc], materials: dict[str, MaterialDesc], diff --git a/embodichain/lab/visualization/__init__.py b/embodichain/lab/visualization/__init__.py index e66b1729b0..112751c018 100644 --- a/embodichain/lab/visualization/__init__.py +++ b/embodichain/lab/visualization/__init__.py @@ -37,6 +37,7 @@ JointControlSpec, JointControlState, MeshGeometry, + MeshMarkerOverlay, PickCommand, PointCloudOverlay, SceneFrame, @@ -76,6 +77,7 @@ "JointControlState", "LatestFrameQueue", "MeshGeometry", + "MeshMarkerOverlay", "PickCommand", "PointCloudOverlay", "RuntimeHealth", diff --git a/embodichain/lab/visualization/backends/viser.py b/embodichain/lab/visualization/backends/viser.py index 0900a89d75..d4cc219794 100644 --- a/embodichain/lab/visualization/backends/viser.py +++ b/embodichain/lab/visualization/backends/viser.py @@ -37,6 +37,7 @@ JointControlSpec, JointControlState, MeshGeometry, + MeshMarkerOverlay, PickCommand, PointCloudOverlay, SceneFrame, @@ -81,6 +82,20 @@ class _JointControlHandle: spec: JointControlSpec +@dataclass +class _MeshOverlayHandle: + handle: object + vertices: np.ndarray + faces: np.ndarray + scale: np.ndarray + env_id: int | None + uv_coords: np.ndarray | None = None + texture: np.ndarray | None = None + color: tuple = () + texture_wrap: tuple[str, str] = ("repeat", "repeat") + unlit: bool = True + + @dataclass(frozen=True) class _GuiEvent: category: str @@ -172,8 +187,11 @@ def __init__( self._camera_preview_group_folders: dict[str, object] = {} self._camera_image_handles: dict[str, object] = {} self._overlay_handles: dict[tuple[str, str], object] = {} + self._mesh_overlay_handles: dict[tuple[str, str], _MeshOverlayHandle] = {} self._overlay_base_visibility: dict[tuple[str, str], bool] = {} self._env_visibility: dict[int, bool] = {} + self._manifest: SceneManifest | None = None + self._marker_env_ids: set[int] = set() self._show_all_envs = True self._selected_scene_env: int | None = None self._overlay_visibility: dict[str, bool] = { @@ -181,6 +199,7 @@ def __init__( "trajectories": True, "targets": True, "point_clouds": True, + "markers": True, } self._gui_events: queue.SimpleQueue[_GuiEvent] = queue.SimpleQueue() self._gizmo_events: queue.SimpleQueue[_GizmoEvent] = queue.SimpleQueue() @@ -259,6 +278,15 @@ def _on_pick_click(event: object) -> None: self._server.scene.remove_pointer_callback() self._pointer_handler = None + def _environment_ids(self, manifest: SceneManifest) -> set[int]: + return ( + {node.env_id for node in manifest.nodes} + | {camera.env_id for camera in manifest.cameras} + | {gizmo.env_id for gizmo in manifest.gizmos} + | {control.env_id for control in manifest.joint_controls} + | self._marker_env_ids + ) + def _register_visibility_controls(self, manifest: SceneManifest) -> None: previous_env_visibility = self._env_visibility while True: @@ -283,12 +311,7 @@ def _register_visibility_controls(self, manifest: SceneManifest) -> None: "may mutate the simulation." ) self._register_replay_control() - env_ids = sorted( - {node.env_id for node in manifest.nodes} - | {camera.env_id for camera in manifest.cameras} - | {gizmo.env_id for gizmo in manifest.gizmos} - | {control.env_id for control in manifest.joint_controls} - ) + env_ids = sorted(self._environment_ids(manifest)) if self._selected_scene_env not in env_ids: self._selected_scene_env = env_ids[0] if env_ids else None if len(env_ids) > self._INDIVIDUAL_ENV_CONTROL_LIMIT: @@ -356,6 +379,7 @@ def _(event: object, selected_env_id: int = env_id) -> None: ("trajectories", "Trajectories"), ("targets", "Targets"), ("point_clouds", "Point clouds"), + ("markers", "Markers"), ): checkbox = self._server.gui.add_checkbox( label, initial_value=self._overlay_visibility[category] @@ -1130,6 +1154,7 @@ def publish_manifest(self, manifest: SceneManifest) -> None: for handle in self._overlay_handles.values(): handle.remove() self._overlay_handles.clear() + self._mesh_overlay_handles.clear() self._overlay_base_visibility.clear() geometry_by_id = { geometry.geometry_id: geometry for geometry in manifest.geometries @@ -1277,6 +1302,7 @@ def publish_manifest(self, manifest: SceneManifest) -> None: self._run_id = manifest.run_id self._scene_revision = manifest.scene_revision + self._manifest = manifest self._register_visibility_controls(manifest) for batch in self._mesh_batches.values(): self._apply_mesh_visibility(batch) @@ -1368,9 +1394,7 @@ def _apply_gui_events(self) -> None: for dynamic_mesh in self._dynamic_meshes.values(): self._apply_dynamic_mesh_visibility(dynamic_mesh) for key, handle in self._overlay_handles.items(): - handle.visible = self._overlay_base_visibility.get( - key, True - ) and self._overlay_visibility.get(key[0], True) + self._apply_overlay_visibility(key, handle) self._apply_camera_visibility() self._apply_gizmo_visibility() if camera_previews_changed: @@ -1511,6 +1535,147 @@ def _update_point_cloud( handle.point_size = overlay.point_size handle.visible = overlay.visible and self._overlay_visibility[category] + def _apply_overlay_visibility(self, key: tuple[str, str], handle: object) -> None: + mesh = self._mesh_overlay_handles.get(key) + env_visible = ( + True + if mesh is None or mesh.env_id is None + else self._env_visibility.get(mesh.env_id, True) + ) + handle.visible = ( + self._overlay_base_visibility.get(key, True) + and self._overlay_visibility.get(key[0], True) + and env_visible + ) + + @staticmethod + def _mesh_marker_color( + color: tuple[float, float, float, float], + ) -> tuple[int, int, int]: + return tuple(int(component * 255.0) for component in color[:3]) + + def _update_mesh_marker( + self, overlay: MeshMarkerOverlay, active: set[tuple[str, str]] + ) -> None: + category = "markers" + key = (category, overlay.overlay_id) + active.add(key) + self._overlay_base_visibility[key] = overlay.visible + mesh = self._mesh_overlay_handles.get(key) + geometry_changed = mesh is None or not ( + ( + mesh.vertices is overlay.vertices + or np.array_equal(mesh.vertices, overlay.vertices) + ) + and ( + mesh.faces is overlay.faces or np.array_equal(mesh.faces, overlay.faces) + ) + and np.array_equal(mesh.scale, overlay.scale) + and ( + mesh.uv_coords is overlay.uv_coords + or np.array_equal(mesh.uv_coords, overlay.uv_coords) + ) + and ( + mesh.texture is overlay.texture + or np.array_equal(mesh.texture, overlay.texture) + ) + and (overlay.texture is None or mesh.color == overlay.color) + and mesh.texture_wrap == overlay.texture_wrap + and mesh.unlit == overlay.unlit + ) + if geometry_changed: + if mesh is not None: + mesh.handle.remove() + path_component = safe_path_component(overlay.overlay_id) + if overlay.texture is not None: + import trimesh + from PIL import Image + + factor = np.array(overlay.color) + factor[:3] = np.where( + factor[:3] <= 0.04045, + factor[:3] / 12.92, + ((factor[:3] + 0.055) / 1.055) ** 2.4, + ) + material = trimesh.visual.material.PBRMaterial( + baseColorFactor=np.round(factor * 255).astype(np.uint8), + baseColorTexture=Image.fromarray(overlay.texture), + metallicFactor=0.0, + roughnessFactor=1.0, + alphaMode="BLEND", + doubleSided=True, + ) + triangle_mesh = trimesh.Trimesh( + vertices=overlay.vertices * overlay.scale, + faces=overlay.faces, + process=False, + visual=trimesh.visual.TextureVisuals( + uv=overlay.uv_coords, material=material + ), + ) + + def configure_material(tree: dict) -> None: + if overlay.unlit: + tree.setdefault("extensionsUsed", []).append( + "KHR_materials_unlit" + ) + for item in tree.get("materials", []): + item.setdefault("extensions", {})[ + "KHR_materials_unlit" + ] = {} + wraps = {"repeat": 10497, "clamp": 33071, "mirror": 33648} + tree["samplers"] = [ + { + "wrapS": wraps[overlay.texture_wrap[0]], + "wrapT": wraps[overlay.texture_wrap[1]], + } + ] + for texture in tree.get("textures", []): + texture["sampler"] = 0 + + data = trimesh.exchange.gltf.export_glb( + triangle_mesh, tree_postprocessor=configure_material + ) + handle = self._server.scene.add_glb( + f"/overlays/{category}/{path_component}", + data, + cast_shadow=False, + receive_shadow=False, + ) + else: + handle = self._server.scene.add_mesh_simple( + f"/overlays/{category}/{path_component}", + vertices=overlay.vertices * overlay.scale, + faces=overlay.faces, + color=self._mesh_marker_color(overlay.color), + opacity=overlay.color[3], + side="double", + cast_shadow=False, + receive_shadow=False, + ) + mesh = _MeshOverlayHandle( + handle=handle, + vertices=overlay.vertices, + faces=overlay.faces, + scale=overlay.scale, + env_id=overlay.env_id, + uv_coords=overlay.uv_coords, + texture=overlay.texture, + color=overlay.color, + texture_wrap=overlay.texture_wrap, + unlit=overlay.unlit, + ) + self._mesh_overlay_handles[key] = mesh + self._overlay_handles[key] = handle + else: + mesh.env_id = overlay.env_id + if overlay.texture is None: + mesh.handle.color = self._mesh_marker_color(overlay.color) + mesh.handle.opacity = overlay.color[3] + mesh.handle.position = overlay.position + mesh.handle.wxyz = overlay.wxyz + self._apply_overlay_visibility(key, mesh.handle) + def publish_frame(self, frame: SceneFrame) -> bool: """Publish a dynamic scene frame. @@ -1606,6 +1771,20 @@ def publish_frame(self, frame: SceneFrame) -> bool: if pending_sequence is not None: self._joint_control_pending_sequences.pop(control_id, None) + self._marker_env_ids = { + mesh.env_id for mesh in frame.overlays.meshes if mesh.env_id is not None + } + if self._manifest is not None and self._environment_ids(self._manifest) != set( + self._env_visibility + ): + self._register_visibility_controls(self._manifest) + for batch in self._mesh_batches.values(): + self._apply_mesh_visibility(batch) + for dynamic_mesh in self._dynamic_meshes.values(): + self._apply_dynamic_mesh_visibility(dynamic_mesh) + self._apply_camera_visibility() + self._apply_gizmo_visibility() + active_overlays: set[tuple[str, str]] = set() for overlay in frame.overlays.frames: self._update_frame_overlay("frames", overlay, active_overlays) @@ -1615,10 +1794,13 @@ def publish_frame(self, frame: SceneFrame) -> bool: self._update_trajectory(overlay, active_overlays) for overlay in frame.overlays.point_clouds: self._update_point_cloud(overlay, active_overlays) + for overlay in frame.overlays.meshes: + self._update_mesh_marker(overlay, active_overlays) stale = set(self._overlay_handles) - active_overlays for key in stale: self._overlay_handles.pop(key).remove() + self._mesh_overlay_handles.pop(key, None) self._overlay_base_visibility.pop(key, None) return True @@ -1729,7 +1911,11 @@ def stop(self) -> None: self._camera_preview_group_folders.clear() self._camera_image_handles.clear() self._overlay_handles.clear() + self._mesh_overlay_handles.clear() self._overlay_base_visibility.clear() + self._manifest = None + self._marker_env_ids.clear() + self._env_visibility.clear() while True: try: self._gizmo_events.get_nowait() diff --git a/embodichain/lab/visualization/markers/__init__.py b/embodichain/lab/visualization/markers/__init__.py new file mode 100644 index 0000000000..2f65927085 --- /dev/null +++ b/embodichain/lab/visualization/markers/__init__.py @@ -0,0 +1,35 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Render-only marker prototypes and mutable instance groups.""" + +from __future__ import annotations + +from .cfg import MarkerGroupCfg, MarkerPrototypeCfg +from .group import MarkerGroup +from .visualization_markers import VisualizationMarkers, VisualizationMarkersCfg +from .robot_markers import RobotMarkers, RobotMarkersCfg, RobotMarkersAppearanceCfg + +__all__ = [ + "RobotMarkers", + "RobotMarkersCfg", + "RobotMarkersAppearanceCfg", + "MarkerGroupCfg", + "MarkerPrototypeCfg", + "MarkerGroup", + "VisualizationMarkers", + "VisualizationMarkersCfg", +] diff --git a/embodichain/lab/visualization/markers/_geometry.py b/embodichain/lab/visualization/markers/_geometry.py new file mode 100644 index 0000000000..e45e4a6ce5 --- /dev/null +++ b/embodichain/lab/visualization/markers/_geometry.py @@ -0,0 +1,124 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import numpy as np + +from .cfg import MarkerPrototypeCfg + + +def _revolve(profile: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + angles = np.arange(24) * (2 * np.pi / 24) + radius, height = profile.T + vertices = np.stack( + ( + radius[:, None] * np.cos(angles), + radius[:, None] * np.sin(angles), + np.broadcast_to(height[:, None], (len(height), 24)), + ), + axis=-1, + ).reshape(-1, 3) + faces = [] + for row in range(len(profile) - 1): + for col in range(24): + a, b = row * 24 + col, row * 24 + (col + 1) % 24 + faces.extend(((a, b, a + 24), (b, b + 24, a + 24))) + faces = np.asarray(faces, dtype=np.uint32) + triangles = vertices[faces] + valid = ( + np.linalg.norm( + np.cross( + triangles[:, 1] - triangles[:, 0], triangles[:, 2] - triangles[:, 0] + ), + axis=1, + ) + > 1e-10 + ) + return vertices.astype(np.float32), faces[valid] + + +def geometry_parts( + cfg: MarkerPrototypeCfg, +) -> tuple[tuple[np.ndarray, np.ndarray, tuple[float, float, float] | None], ...]: + """Generate local triangles once per prototype; RGB override denotes frame axes.""" + shape = cfg.shape + if shape == "mesh": + return ((cfg.vertices, cfg.faces, None),) + if shape == "box": + vertices = ( + np.array( + [ + [-1, -1, -1], + [1, -1, -1], + [1, 1, -1], + [-1, 1, -1], + [-1, -1, 1], + [1, -1, 1], + [1, 1, 1], + [-1, 1, 1], + ], + dtype=np.float32, + ) + * 0.5 + ) + faces = np.array( + [ + [0, 2, 1], + [0, 3, 2], + [4, 5, 6], + [4, 6, 7], + [0, 1, 5], + [0, 5, 4], + [1, 2, 6], + [1, 6, 5], + [2, 3, 7], + [2, 7, 6], + [3, 0, 4], + [3, 4, 7], + ], + dtype=np.uint32, + ) + else: + if shape in ("sphere", "capsule"): + theta = np.linspace(-np.pi / 2, np.pi / 2, 17) + profile = np.stack((0.5 * np.cos(theta), 0.5 * np.sin(theta)), axis=1) + if shape == "capsule": + # Duplicate the equator to join the two hemispheres with a cylinder. + profile = np.concatenate( + (profile[:9] - [0, 0.5], profile[8:] + [0, 0.5]) + ) + elif shape == "cylinder": + profile = np.array([[0, -0.5], [0.5, -0.5], [0.5, 0.5], [0, 0.5]]) + elif shape == "cone": + profile = np.array([[0, -0.5], [0.5, -0.5], [0, 0.5]]) + else: # arrow or frame + profile = np.array( + [[0, 0], [0.025, 0], [0.025, 0.75], [0.075, 0.75], [0, 1]] + ) + vertices, faces = _revolve(profile) + if shape in ("arrow", "frame"): + vertices = vertices[:, [2, 0, 1]] + if shape == "frame": + return tuple( + (vertices[:, order], faces, color) + for order, color in ( + ([0, 1, 2], (1.0, 0.0, 0.0)), + ([2, 0, 1], (0.0, 1.0, 0.0)), + ([1, 2, 0], (0.0, 0.0, 1.0)), + ) + ) + return ((vertices, faces, None),) diff --git a/embodichain/lab/visualization/markers/_native.py b/embodichain/lab/visualization/markers/_native.py new file mode 100644 index 0000000000..49dfd66940 --- /dev/null +++ b/embodichain/lab/visualization/markers/_native.py @@ -0,0 +1,390 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from dataclasses import dataclass, replace +from importlib import import_module +from typing import TYPE_CHECKING +from uuid import uuid4 +from tempfile import TemporaryDirectory +from pathlib import Path + +import numpy as np +import json + +from ..protocol import MeshMarkerOverlay + +if TYPE_CHECKING: + from dexsim.scene import Scene + from dexsim.scene.objects import SpawnedRigidBody + + +# Reserved for scene-rendered markers so sensor cameras can exclude their geometry. +_MARKER_RENDER_CATEGORY = 1 << 31 + + +@dataclass +class _Entry: + handle: SpawnedRigidBody + snapshot: MeshMarkerOverlay + + +def _pose(marker: MeshMarkerOverlay) -> np.ndarray: + w, x, y, z = marker.wxyz + pose = np.eye(4, dtype=np.float32) + pose[:3, :3] = [ + [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], + [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], + [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], + ] + pose[:3, 3] = marker.position + return pose + + +def _transforms(markers: tuple[MeshMarkerOverlay, ...]) -> np.ndarray: + """Pack one group as scaled world matrices for the Scene visual batch.""" + from scipy.spatial.transform import Rotation + + matrices = np.broadcast_to(np.eye(4, dtype=np.float32), (len(markers), 4, 4)).copy() + if markers: + quaternions = np.asarray([marker.wxyz for marker in markers]) + matrices[:, :3, :3] = Rotation.from_quat( + quaternions[:, [1, 2, 3, 0]] + ).as_matrix() + matrices[:, :3, :3] *= np.asarray([marker.scale for marker in markers])[ + :, None, : + ] + matrices[:, :3, 3] = [marker.position for marker in markers] + return matrices + + +class NativeMarkerRenderer: + """Own Scene render-only mesh handles on the simulation thread. + + Removal invalidates the selected handles. Materials remain independent + between instances, and this consumer retains no material wrappers + between updates. + + Args: + scene: Prepared DexSim Scene owning render-only mesh objects. + Creating markers does not prepare or rebuild physics. + use_scene_for_overlays: Draw overlay materials in the scene and + isolate them with the camera's render-category filter. + """ + + def __init__(self, scene: Scene, *, use_scene_for_overlays: bool = False) -> None: + try: + spawn = import_module("dexsim.spawn") + required = ( + (getattr(spawn, "RenderDesc", None), "render_mode"), + (getattr(spawn, "MeshObjectDesc", None), "render_categories"), + (getattr(spawn, "MaterialDesc", None), "unlit"), + (getattr(spawn, "MaterialDesc", None), "alpha_mode"), + (getattr(spawn, "MaterialDesc", None), "depth_write"), + ) + if ( + not hasattr(spawn, "MeshObjectDesc") + or any(not hasattr(owner, name) for owner, name in required) + or not callable(getattr(scene, "add_mesh_object", None)) + or not callable(getattr(scene, "remove_mesh_object", None)) + ): + raise ImportError("Missing Scene mesh-object or overlay properties") + except ImportError as exc: + raise RuntimeError( + "Native marker groups require Scene mesh-object overlay support. " + "Install a DexSim build with render-only overlay support or use Viser." + ) from exc + self._spawn = spawn + self._scene = scene + self._use_scene_for_overlays = use_scene_for_overlays + self._groups: dict[str, dict[str, _Entry]] = {} + self._texture_directory = TemporaryDirectory( + prefix="embodichain-marker-textures-" + ) + self._texture_paths: dict[int, tuple[object, str]] = {} + self._batches: dict[str, tuple[tuple, object]] = {} + + def _create( + self, marker: MeshMarkerOverlay, template: _Entry | None = None + ) -> SpawnedRigidBody: + """Declare one render-only object through its lifetime-owning Scene.""" + name = f"__embodichain_marker_{uuid4().hex}" + if template is not None and hasattr(self._scene, "clone_mesh_object"): + return self._scene.clone_mesh_object(template.handle.path, name) + scene_lighting = self._scene_lit(marker) + use_scene = scene_lighting or self._use_scene_for_overlays + parameters = ( + json.loads(marker.material) + if scene_lighting and marker.material is not None + else {} + ) + # Source descriptors preserve lobes, maps, samplers and color space. + # The marker's display color uses sRGB regardless of the source space. + parameters.pop("name", None) + parameters.pop("base_color", None) + textured = marker.texture is not None + if parameters.get("color_space") == "linear" or textured: + color = np.asarray(marker.color) + color = color.copy() + color[:3] = np.where( + color[:3] <= 0.04045, + color[:3] / 12.92, + ((color[:3] + 0.055) / 1.055) ** 2.4, + ) + else: + color = marker.color + texture_options = {} + if marker.texture is not None and not ( + scene_lighting and parameters.get("base_color_map") + ): + from PIL import Image + + key = id(marker.texture) + if key not in self._texture_paths: + path = str(Path(self._texture_directory.name) / f"{uuid4().hex}.png") + Image.fromarray(marker.texture).save(path) + self._texture_paths[key] = (marker.texture, path) + texture_options["base_color_map"] = self._texture_paths[key][1] + texture_options["opacity_map"] = self._texture_paths[key][1] + texture_options["texture_settings"] = { + "base_color_map": self._spawn.MaterialTextureDesc( + color_space="sRGB", + wrap_s=marker.texture_wrap[0], + wrap_t=marker.texture_wrap[1], + ), + "opacity_map": self._spawn.MaterialTextureDesc( + channel="a", + color_space="raw", + wrap_s=marker.texture_wrap[0], + wrap_t=marker.texture_wrap[1], + scale=(1.0, 1.0, 1.0, color[3]), + ), + } + parameters.update(texture_options) + if "texture_settings" in parameters: + parameters["texture_settings"] = { + key: ( + self._spawn.MaterialTextureDesc(**value) + if isinstance(value, dict) + else value + ) + for key, value in parameters["texture_settings"].items() + } + if textured: + # Hybrid replaces the material factor with the sampled base color. + # Apply tint during texture lowering and keep the factor neutral. + settings = parameters.setdefault("texture_settings", {}) + base = settings.get( + "base_color_map", self._spawn.MaterialTextureDesc(color_space="sRGB") + ) + tint = np.array([*color[:3], 1.0]) + settings["base_color_map"] = replace( + base, + scale=tuple(np.asarray(base.scale) * tint), + bias=tuple(np.asarray(base.bias) * tint), + ) + color = (1.0, 1.0, 1.0, 1.0) + parameters.update(unlit=marker.unlit or (use_scene and not scene_lighting)) + if not scene_lighting: + parameters.update(alpha_mode="blend", depth_write=False, double_sided=True) + material = self._spawn.MaterialDesc( + name=f"{name}_material", + base_color=tuple(color), + **parameters, + ) + render = self._spawn.RenderDesc.from_geometry( + self._spawn.GeometryDesc.mesh( + vertices=np.ascontiguousarray(marker.vertices, dtype=np.float32), + triangles=np.ascontiguousarray(marker.faces, dtype=np.int32), + normals=marker.normals, + **( + { + "uv_coords": ( + marker.material_uv_coords + if scene_lighting and marker.material_uv_coords is not None + else marker.uv_coords + ) + } + if marker.uv_coords is not None + else {} + ), + ), + render_mode="scene" if use_scene else "overlay", + cast_shadow=False, + pickable=False, + material=material, + ) + desc = self._spawn.MeshObjectDesc( + name=name, + renders=[render], + render_categories=_MARKER_RENDER_CATEGORY if use_scene else 0, + physics=None, + per_env=False, + ) + return self._scene.add_mesh_object(desc, arena_name="default") + + def _discard(self, handle: SpawnedRigidBody) -> None: + # Scene.close() invalidates the selected handles. + # Never inspect a borrowed native actor to decide how to remove it. + if handle.is_valid: + self._scene.remove_mesh_object(handle.path) + + @staticmethod + def _scene_lit(marker: MeshMarkerOverlay) -> bool: + return marker.scene_lighting and not marker.unlit and marker.color[3] == 1.0 + + @staticmethod + def _same_geometry(left: MeshMarkerOverlay, right: MeshMarkerOverlay) -> bool: + return ( + left.unlit == right.unlit + and NativeMarkerRenderer._scene_lit(left) + == NativeMarkerRenderer._scene_lit(right) + and (left.texture is None or left.color == right.color) + and left.material == right.material + and all( + getattr(left, key) is getattr(right, key) + or np.array_equal(getattr(left, key), getattr(right, key)) + for key in ( + "vertices", + "faces", + "uv_coords", + "texture", + "texture_wrap", + "normals", + "material_uv_coords", + ) + ) + ) + + @staticmethod + def _apply( + handle: SpawnedRigidBody, + marker: MeshMarkerOverlay, + previous: MeshMarkerOverlay | None = None, + *, + transform: bool = True, + ) -> None: + if transform: + handle.set_world_pose(_pose(marker)) + handle.set_scale(*(float(value) for value in marker.scale)) + if marker.texture is None and ( + previous is None or previous.color != marker.color + ): + handle.get_material().set_base_color(marker.color) + if previous is None or previous.visible != marker.visible: + handle.set_visible(marker.visible) + + def publish(self, name: str, markers: tuple[MeshMarkerOverlay, ...]) -> None: + """Apply a complete group, staging replacements before releasing old meshes. + + Args: + name: Stable group name. + markers: Complete detached group snapshots in world coordinates. + """ + previous = self._groups.get(name, {}) + pending: dict[str, _Entry] = {} + created = [] + touched = [] + batch_enabled = hasattr(self._scene, "create_visual_object_batch") + templates = {} + for entry in previous.values(): + templates.setdefault(id(entry.snapshot.vertices), entry) + try: + for marker in markers: + entry = previous.get(marker.overlay_id) + if entry is not None and self._same_geometry(entry.snapshot, marker): + handle = entry.handle + touched.append(entry) + else: + template = templates.get(id(marker.vertices)) + if template is not None and not self._same_geometry( + template.snapshot, marker + ): + template = None + handle = self._create(marker, template) + created.append(handle) + self._apply( + handle, + marker, + ( + entry.snapshot + if entry is not None and entry.handle is handle + else None + ), + transform=not batch_enabled, + ) + pending[marker.overlay_id] = _Entry(handle, marker) + templates.setdefault(id(marker.vertices), pending[marker.overlay_id]) + if batch_enabled and pending: + transforms = _transforms(markers) + handles = tuple(entry.handle for entry in pending.values()) + cached = self._batches.get(name) + if ( + cached is None + or len(cached[0]) != len(handles) + or any(a is not b for a, b in zip(cached[0], handles)) + ): + cached = (handles, self._scene.create_visual_object_batch(handles)) + cached[1].set_world_transforms(transforms) + self._batches[name] = cached + elif not pending: + self._batches.pop(name, None) + except Exception: + try: + for handle in created: + self._discard(handle) + for entry in touched: + self._apply(entry.handle, entry.snapshot) + finally: + self._prune_textures() + raise + for key, entry in previous.items(): + if key not in pending or pending[key].handle is not entry.handle: + self._discard(entry.handle) + self._groups[name] = pending + if created or len(previous) != len(pending): + self._prune_textures() + + def _prune_textures(self) -> None: + """Release host image copies and staging files after native upload.""" + active = { + id(entry.snapshot.texture) + for group in self._groups.values() + for entry in group.values() + if entry.snapshot.texture is not None + } + for key in self._texture_paths.keys() - active: + _, path = self._texture_paths.pop(key) + Path(path).unlink(missing_ok=True) + + def remove(self, name: str) -> None: + """Release all native objects in one group. + + Args: + name: Group to release; absent groups are ignored. + """ + self._batches.pop(name, None) + for entry in self._groups.pop(name, {}).values(): + self._discard(entry.handle) + self._prune_textures() + + def close(self) -> None: + """Release marker handles; also safe after their Scene has closed.""" + for name in tuple(self._groups): + self.remove(name) + self._texture_paths.clear() + self._texture_directory.cleanup() diff --git a/embodichain/lab/visualization/markers/_robot_marker_assets.py b/embodichain/lab/visualization/markers/_robot_marker_assets.py new file mode 100644 index 0000000000..144e0e1103 --- /dev/null +++ b/embodichain/lab/visualization/markers/_robot_marker_assets.py @@ -0,0 +1,399 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Decode robot visual assets without creating simulation objects.""" + +from __future__ import annotations + +from dataclasses import asdict, dataclass, fields, replace +from uuid import uuid4 +from pathlib import Path +import xml.etree.ElementTree as ET + +import numpy as np +from PIL import Image +from scipy.spatial.transform import Rotation +import trimesh + +from .cfg import MarkerPrototypeCfg + + +@dataclass(frozen=True) +class RobotMarkerVisual: + link_name: str + local_pose: np.ndarray + prototype: MarkerPrototypeCfg + + +def _numbers(text: str, count: int) -> np.ndarray: + values = np.asarray([float(value) for value in text.split()]) + if values.shape != (count,) or not np.isfinite(values).all(): + raise ValueError(f"Expected {count} finite numbers, received {text!r}.") + return values + + +def _asset_path(root: Path, value: str) -> Path: + if value.startswith("file://"): + value = value[7:] + if value.startswith("package://"): + relative = Path(value[10:]) + candidates = [ + root / relative, + root / Path(*relative.parts[1:]), + root.parent / Path(*relative.parts[1:]), + ] + else: + candidates = [root / value] + for candidate in candidates: + if candidate.is_file(): + return candidate.resolve() + raise FileNotFoundError(f"Cannot resolve visual asset {value!r} from {root}.") + + +def _image(path: Path | str) -> np.ndarray: + with Image.open(path) as image: + return np.asarray(image.convert("RGBA")).copy() + + +def _mesh_parts( + path: Path, texture_directory: Path | None = None +) -> list[MarkerPrototypeCfg]: + scene = trimesh.load_scene(path, process=False) + parts = [] + for node in scene.graph.nodes_geometry: + transform, geometry_name = scene.graph[node] + mesh = scene.geometry[geometry_name] + if not isinstance(mesh, trimesh.Trimesh): + raise ValueError(f"Expected triangle geometry in {path}: {geometry_name}") + mesh = mesh.copy() + mesh.apply_transform(transform) + visual = mesh.visual + material = getattr(visual, "material", None) + if isinstance(material, trimesh.visual.material.MultiMaterial): + for index in np.unique(visual.face_materials): + faces = mesh.faces[visual.face_materials == index] + part = trimesh.Trimesh( + vertices=mesh.vertices, + faces=faces, + process=False, + visual=trimesh.visual.TextureVisuals( + uv=visual.uv, material=material.materials[index] + ), + ) + parts.append(_mesh_prototype(part, texture_directory)) + else: + parts.append(_mesh_prototype(mesh, texture_directory)) + if not parts: + raise ValueError(f"No triangle visuals in {path}.") + return parts + + +def _mesh_prototype( + mesh: trimesh.Trimesh, texture_directory: Path | None = None +) -> MarkerPrototypeCfg: + color_visual = mesh.visual.kind in {"vertex", "face"} + if color_visual: + mesh = mesh.copy() + mesh.visual = mesh.visual.to_texture() + material = getattr(mesh.visual, "material", None) + image = getattr(material, "baseColorTexture", None) + if image is None: + image = getattr(material, "image", None) + color = getattr(material, "baseColorFactor", None) + if color is None: + color = getattr(material, "diffuse", None) + if color is None or color_visual: + color = np.array([255, 255, 255, 255]) + color = np.asarray(color, dtype=float) / 255.0 + if isinstance(material, trimesh.visual.material.PBRMaterial) and not color_visual: + color[:3] = _linear_to_srgb(color[:3]) + uv = getattr(mesh.visual, "uv", None) + parameters = {"metallic": 0.0, "roughness": 1.0} + if isinstance(material, trimesh.visual.material.PBRMaterial): + parameters.update( + color_space="linear", + metallic=( + material.metallicFactor if material.metallicFactor is not None else 1.0 + ), + roughness=( + material.roughnessFactor + if material.roughnessFactor is not None + else 1.0 + ), + double_sided=material.doubleSided, + alpha_mode=material.alphaMode, + alpha_cutoff=( + material.alphaCutoff if material.alphaCutoff is not None else 0.5 + ), + ) + if material.emissiveFactor is not None: + parameters["emissive"] = (*material.emissiveFactor.tolist(), 1.0) + for source, target in ( + ("baseColorTexture", "base_color_map"), + ("normalTexture", "normal_map"), + ("emissiveTexture", "emissive_map"), + ("occlusionTexture", "ao_map"), + ("metallicRoughnessTexture", "metallic_roughness_map"), + ): + texture = getattr(material, source, None) + if texture is not None and texture_directory is not None: + target_path = texture_directory / f"{uuid4().hex}.png" + texture.save(target_path) + parameters[target] = str(target_path) + elif material is not None: + shininess = getattr(material, "kwargs", {}).get("Ns", 0.0) + parameters["roughness"] = float(np.sqrt(2.0 / (float(shininess) + 2.0))) + return MarkerPrototypeCfg( + shape="mesh", + vertices=np.asarray(mesh.vertices), + normals=np.asarray(mesh.vertex_normals), + material=parameters, + faces=np.asarray(mesh.faces), + color=tuple(color), + uv_coords=uv, + texture=None if image is None else np.asarray(image.convert("RGBA")).copy(), + ) + + +def _linear_to_srgb(color: np.ndarray) -> np.ndarray: + """Encode linear RGB for the marker protocol's sRGB material contract.""" + return np.where( + color <= 0.0031308, + color * 12.92, + 1.055 * np.maximum(color, 0) ** (1 / 2.4) - 0.055, + ) + + +def _validate_usd_material(material: object) -> None: + """Reject runtimes that cannot preserve USD color and sampler semantics.""" + if not all( + hasattr(material, field) for field in ("color_space", "texture_settings") + ): + raise RuntimeError( + "USD robot materials require a runtime with explicit color-space " + "and texture-sampling support. Upgrade the visualization runtime " + "before loading this USD asset." + ) + + +def _usd_texture(prototype: MarkerPrototypeCfg, material: object) -> None: + """Reuse USD texture lowering and bake its coordinates into marker UVs.""" + from dexsim.spawn import MaterialTextureDesc + from dexsim.spawn.adapters.material_texture import prepare_material_textures + + # The browser/overlay texture contains base color, its multiplier and opacity. + settings = { + name: material.texture_settings.get( + name, MaterialTextureDesc(channel="a" if name == "opacity_map" else "rgb") + ) + for name in ("base_color_map", "base_color_multiplier_map", "opacity_map") + if getattr(material, name) is not None + } + if not settings: + return + unused_maps = { + field.name: None + for field in fields(material) + if field.name.endswith("_map") and field.name not in settings + } + data, sampling = prepare_material_textures( + replace(material, texture_settings=settings, **unused_maps) + )["base_color_map"] + if prototype.uv_coords is None: + raise ValueError( + "Textured USD robot marker requires authored mesh UV coordinates." + ) + uv = np.asarray(prototype.uv_coords, dtype=float).copy() + angle = np.deg2rad(sampling.uv_rotation) + rotation = np.array( + [[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]] + ) + uv = (uv * sampling.uv_scale) @ rotation.T + sampling.uv_translation + # The native decoder returns bottom-up linear pixels. The marker protocol + # stores top-down sRGB images; alpha remains linear coverage. + data = data[::-1].copy() + wraps = [sampling.wrap_s, sampling.wrap_t] + for coordinate, axis in ((0, 1), (1, 0)): + if wraps[coordinate] != "black": + continue + size = data.shape[axis] + shape = list(data.shape) + shape[axis] += 2 + padded = np.empty(shape, dtype=data.dtype) + padded[:] = sampling.border_color + interior = [slice(None)] * 3 + interior[axis] = slice(1, -1) + padded[tuple(interior)] = data + data = padded + uv[:, coordinate] = (uv[:, coordinate] * size + 1) / (size + 2) + wraps[coordinate] = "clamp" + data[..., :3] = _linear_to_srgb(data[..., :3]) + prototype.texture = np.rint(np.clip(data, 0, 1) * 255).astype(np.uint8) + prototype.uv_coords = uv + prototype.texture_wrap = tuple(wraps) + + +def load_urdf_visuals( + path: Path, texture_directory: Path | None = None +) -> list[RobotMarkerVisual]: + root = ET.parse(path).getroot() + materials = {item.attrib["name"]: item for item in root.findall("material")} + meshes = {} + result = [] + for link in root.findall("link"): + name = link.attrib["name"] + for visual in link.findall("visual"): + pose = np.eye(4) + origin = visual.find("origin") + if origin is not None: + pose[:3, 3] = _numbers(origin.get("xyz", "0 0 0"), 3) + pose[:3, :3] = Rotation.from_euler( + "xyz", _numbers(origin.get("rpy", "0 0 0"), 3) + ).as_matrix() + geometry = visual.find("geometry") + if geometry is None or len(geometry) != 1: + raise ValueError(f"Expected one geometry in visual of link {name!r}.") + geometry = geometry[0] + if geometry.tag == "mesh": + mesh_path = _asset_path(path.parent, geometry.attrib["filename"]) + if mesh_path not in meshes: + meshes[mesh_path] = _mesh_parts(mesh_path, texture_directory) + import copy + + prototypes = [copy.copy(part) for part in meshes[mesh_path]] + scale = _numbers(geometry.get("scale", "1 1 1"), 3) + elif geometry.tag == "box": + prototypes = [MarkerPrototypeCfg(shape="box", color=(1, 1, 1, 1))] + scale = _numbers(geometry.attrib["size"], 3) + elif geometry.tag == "sphere": + prototypes = [MarkerPrototypeCfg(shape="sphere", color=(1, 1, 1, 1))] + scale = np.repeat(2 * float(geometry.attrib["radius"]), 3) + elif geometry.tag == "cylinder": + prototypes = [MarkerPrototypeCfg(shape="cylinder", color=(1, 1, 1, 1))] + scale = np.array( + [2 * float(geometry.attrib["radius"])] * 2 + + [float(geometry.attrib["length"])] + ) + else: + raise ValueError(f"Unsupported URDF visual geometry: {geometry.tag}") + if not np.isfinite(scale).all() or np.any(scale <= 0): + raise ValueError("URDF visual scale must be positive and finite.") + material = visual.find("material") + if material is not None and len(material) == 0: + material = materials.get(material.get("name")) + for prototype in prototypes: + prototype.scale = tuple(scale) + if material is not None: + color = material.find("color") + texture = material.find("texture") + if color is not None: + prototype.color = tuple(_numbers(color.attrib["rgba"], 4)) + if texture is not None: + texture_path = _asset_path( + path.parent, texture.attrib["filename"] + ) + prototype.texture = _image(texture_path) + prototype.material = dict(prototype.material or {}) + prototype.material["base_color_map"] = str(texture_path) + settings = dict(prototype.material.get("texture_settings", {})) + settings.pop("base_color_map", None) + prototype.material["texture_settings"] = settings + result.append(RobotMarkerVisual(name, pose.copy(), prototype)) + if not result: + raise ValueError(f"No URDF visual geometry in {path}.") + return result + + +def load_usd_visuals( + path: Path, prim_path: str | None, texture_directory: Path | None = None +) -> list[RobotMarkerVisual]: + from dexsim.spawn.adapters.common import generated_mesh + from embodichain.lab.sim.spawn._usd_assets import _UsdTextureFiles, _parse_singleton + + scene, obj = _parse_singleton( + path, + ("articulations", "mesh_objects"), + "robot marker articulation or visual object", + prim_path=prim_path, + ) + links = ( + [(link.name, link.visuals) for link in obj.links] + if hasattr(obj, "links") + else [(obj.name, obj.renders)] + ) + files = _UsdTextureFiles(texture_directory) + result = [] + try: + materials = files.resolve(scene.materials) + for link_name, renders in links: + for render in renders: + material = ( + files.resolve({"inline": render.material})["inline"] + if render.material is not None + else materials.get(render.material_ref) + ) + if render.file_path is not None: + prototypes = _mesh_parts(Path(render.file_path), texture_directory) + for prototype in prototypes: + prototype.vertices = trimesh.transform_points( + prototype.vertices, render.local_transform + ) + normal_transform = np.linalg.inv( + render.local_transform[:3, :3] + ).T + prototype.normals = prototype.normals @ normal_transform.T + lengths = np.linalg.norm( + prototype.normals, axis=1, keepdims=True + ) + prototype.normals /= np.where(lengths > 1e-8, lengths, 1.0) + else: + vertices, faces, normals, uv = generated_mesh(render) + prototypes = [ + MarkerPrototypeCfg( + shape="mesh", + vertices=vertices, + normals=normals if np.size(normals) else None, + faces=np.asarray(faces).reshape(-1, 3), + uv_coords=uv if np.size(uv) else None, + color=(1, 1, 1, 1), + ) + ] + for prototype in prototypes: + if material is not None: + _validate_usd_material(material) + if material.base_color is not None: + color = np.array(material.base_color, dtype=float) + if material.color_space == "linear": + color[:3] = _linear_to_srgb(color[:3]) + prototype.color = tuple(color) + # Preserve original UVs for native PBR. The browser/transparent + # texture path below bakes the base-color sampler into UVs. + parameters = asdict(material) + for key in ("name", "usd", "import_warnings", "base_color"): + parameters.pop(key, None) + prototype.material = parameters + prototype.material_uv_coords = ( + None + if prototype.uv_coords is None + else np.array(prototype.uv_coords, copy=True) + ) + _usd_texture(prototype, material) + result.append(RobotMarkerVisual(link_name, np.eye(4), prototype)) + finally: + files.close() + if not result: + raise ValueError(f"No USD visual geometry in {path}.") + return result diff --git a/embodichain/lab/visualization/markers/cfg.py b/embodichain/lab/visualization/markers/cfg.py new file mode 100644 index 0000000000..1329eda547 --- /dev/null +++ b/embodichain/lab/visualization/markers/cfg.py @@ -0,0 +1,71 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from dataclasses import MISSING +from typing import Literal + +from embodichain.utils import configclass + +__all__ = ["MarkerPrototypeCfg", "MarkerGroupCfg"] + + +@configclass +class MarkerPrototypeCfg: + """Reusable render-only geometry and its default RGBA appearance. + + Box, sphere, cylinder and cone have unit bounding dimensions. Capsule has + unit diameter and total height two. Arrow points along +X from zero to one; + frame contains three such arrows, colored RGB. Frame RGB stays fixed while + its alpha follows ``color``. Mesh vertices use caller-defined local units. + ``scale`` multiplies instance scale before rotation. All dimensions use meters. + """ + + shape: Literal[ + "box", "sphere", "cylinder", "capsule", "cone", "arrow", "frame", "mesh" + ] = "box" + scale: tuple[float, float, float] = (1.0, 1.0, 1.0) + color: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 1.0) + vertices: object | None = None + faces: object | None = None + uv_coords: object | None = None + texture: object | None = None + """Optional uint8 RGB/RGBA image; requires one UV coordinate per vertex.""" + texture_wrap: tuple[str, str] = ("repeat", "repeat") + """Horizontal and vertical sampling: repeat, clamp, or mirror.""" + normals: object | None = None + material_uv_coords: object | None = None + material: dict[str, object] | None = None + """Optional native PBR material parameters, following MaterialDesc fields.""" + scene_lighting: bool = False + """Use ordinary scene shading for opaque native instances.""" + unlit: bool = True + """Disable native surface lighting. Reference ghosts default to lit.""" + + +@configclass +class MarkerGroupCfg: + """Named prototypes shared by isolated environment batches. + + ``scope="env"`` uses all manager environments and environment-local poses. + ``scope="world"`` uses one world-space batch. Dictionary order defines + prototype indices. A standalone group defaults to one environment. + """ + + name: str = MISSING + prototypes: dict[str, MarkerPrototypeCfg] = MISSING + scope: Literal["env", "world"] = "env" diff --git a/embodichain/lab/visualization/markers/group.py b/embodichain/lab/visualization/markers/group.py new file mode 100644 index 0000000000..c499d2d5be --- /dev/null +++ b/embodichain/lab/visualization/markers/group.py @@ -0,0 +1,618 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import copy +from collections.abc import Callable +from tempfile import TemporaryDirectory + +import numpy as np +from scipy.spatial.transform import Rotation + +from .._utils import to_numpy_array +from ..protocol import MeshMarkerOverlay +from .cfg import MarkerGroupCfg +from ._geometry import geometry_parts + +__all__ = ["MarkerGroup"] + + +class MarkerGroup: + """Validated marker state shared by native and browser visualization. + + Args: + cfg: Name, prototypes and coordinate-system selection. + origin: World translation for a standalone single environment. + origins: World translations shaped (num_envs, 3). + num_envs: Number of environment batches; world scope always uses one. + pose_resolver: Owner callback returning environment-local parent poses + shaped (E, 7), ordered xyz then xyzw, without preparing physics. + on_change: Owner callback for publishing a complete state after validation. + on_remove: Owner callback for releasing render resources. + """ + + def __init__( + self, + cfg: MarkerGroupCfg, + *, + origin: object = (0, 0, 0), + origins: object | None = None, + num_envs: int = 1, + pose_resolver: Callable[[str, str | None, list[int]], object] | None = None, + on_change: Callable[[MarkerGroup], None] | None = None, + on_remove: Callable[[MarkerGroup], None] | None = None, + ) -> None: + cfg.validate() + self._cfg = copy.deepcopy(cfg) + if not isinstance(cfg.name, str) or not cfg.name: + raise ValueError("Marker group name must be a non-empty string.") + if cfg.scope not in {"env", "world"}: + raise ValueError("scope must be 'env' or 'world'.") + if isinstance(num_envs, bool) or not isinstance(num_envs, int) or num_envs < 1: + raise ValueError("num_envs must be a positive integer.") + if not isinstance(cfg.prototypes, dict) or not cfg.prototypes: + raise ValueError("Marker prototypes must be a non-empty dictionary.") + self._num_envs = num_envs if cfg.scope == "env" else 1 + if cfg.scope == "world": + self._origins = np.zeros((1, 3), dtype=np.float64) + else: + self._origins = to_numpy_array( + np.tile(origin, (num_envs, 1)) if origins is None else origins, + np.float64, + ) + if ( + self._origins.shape != (self.num_envs, 3) + or not np.isfinite(self._origins).all() + ): + raise ValueError("origins must be finite with shape (num_envs, 3).") + self._parts = self._compile_parts(self._cfg) + self._on_change = on_change + self._on_remove = on_remove + self._visible = np.ones(self.num_envs, dtype=bool) + self._pose_resolver = pose_resolver + self._attachments: dict[int, tuple[str, str | None]] = {} + self._parent_poses: dict[int, np.ndarray] = {} + self._removed = False + self._source_textures: TemporaryDirectory | None = None + self._states = [self._defaults(0) for _ in range(self.num_envs)] + + @staticmethod + def _compile_parts(cfg: MarkerGroupCfg) -> list: + result = [] + for key, prototype in cfg.prototypes.items(): + if not isinstance(key, str) or not key: + raise ValueError("Prototype names must be non-empty strings.") + prototype.validate() + if prototype.shape not in { + "box", + "sphere", + "cylinder", + "capsule", + "cone", + "arrow", + "frame", + "mesh", + }: + raise ValueError(f"Unsupported marker shape: {prototype.shape!r}") + if prototype.shape != "mesh" and ( + prototype.vertices is not None or prototype.faces is not None + ): + raise ValueError( + "vertices and faces are only valid for mesh prototypes." + ) + parts = geometry_parts(prototype) + # Reuse the transport boundary's geometry/material validation. + compiled = [] + for vertices, faces, rgb in parts: + template = MeshMarkerOverlay( + overlay_id="validation", + vertices=vertices, + faces=faces, + position=np.zeros(3), + wxyz=np.array([1, 0, 0, 0]), + scale=prototype.scale, + color=prototype.color, + uv_coords=prototype.uv_coords, + texture=prototype.texture, + texture_wrap=prototype.texture_wrap, + unlit=prototype.unlit, + scene_lighting=prototype.scene_lighting, + normals=prototype.normals, + material=prototype.material, + material_uv_coords=prototype.material_uv_coords, + ) + compiled.append((template._geometry, rgb)) + result.append(compiled) + return result + + @property + def name(self) -> str: + """Stable owner registry key.""" + return self._cfg.name + + @property + def count(self) -> int: + """Total logical instances across all environments, including hidden ones.""" + return sum(self.counts) + + @property + def counts(self) -> tuple[int, ...]: + """Logical instance counts in environment order (one for world scope).""" + return tuple(len(state["translations"]) for state in self._states) + + @property + def scope(self) -> str: + """Coordinate scope, either ``env`` or ``world``.""" + return self._cfg.scope + + @property + def num_envs(self) -> int: + """Number of independently updateable batches.""" + return self._num_envs + + def _env_ids(self, env_ids: object | None) -> list[int]: + if env_ids is None: + return list(range(self.num_envs)) + # Do not coerce strings, floats, or booleans into integer selections. + if np.isscalar(env_ids) or getattr(env_ids, "ndim", None) == 0: + raise ValueError("env_ids must be a one-dimensional integer selection.") + if isinstance(env_ids, (list, tuple)) and any( + isinstance(value, (bool, np.bool_)) for value in env_ids + ): + raise ValueError("env_ids cannot contain booleans.") + values = to_numpy_array(env_ids, None) + if values.ndim != 1 or (values.size and values.dtype.kind not in "iu"): + raise ValueError("env_ids must be a one-dimensional integer selection.") + ids = values.astype(np.int64).tolist() + if len(set(ids)) != len(ids) or any(i < 0 or i >= self.num_envs for i in ids): + raise ValueError("env_ids must be unique and in bounds.") + return ids + + def _defaults(self, count: int) -> dict[str, np.ndarray | None]: + return dict( + translations=np.zeros((count, 3), dtype=np.float32), + orientations_xyzw=np.tile([0.0, 0.0, 0.0, 1.0], (count, 1)), + scales=np.ones((count, 3), dtype=np.float32), + prototype_indices=np.zeros(count, dtype=np.int64), + colors=None, + visible=np.ones(count, dtype=bool), + ) + + def _check_live(self) -> None: + if self._removed: + raise RuntimeError(f"Marker group {self.name!r} has been removed.") + + def update( + self, + *, + translations: object | None = None, + orientations_xyzw: object | None = None, + scales: object | None = None, + prototype_indices: object | None = None, + colors: object | None = None, + visible: object | None = None, + env_ids: object | None = None, + ) -> None: + """Publish validated arrays without preparing or stepping physics. + + Arrays use (E, M, 3) translations/scales, (E, M, 4) xyzw rotations/RGBA + colors, and (E, M) prototype indices/visibility. E is the selected + environment count. Single-environment selections also accept (M, ...). + Quaternions are normalized; scales must be positive and RGBA in [0, 1]. + Translations set each selected environment's count. A changed count + resets omitted fields in that environment; unchanged counts retain them. + Attached translations and orientations are offsets in the parent frame. + Empty selections are validated no-ops; arrays are never broadcast. + + Args: + translations: Finite environment-local positions, or attached offsets. + orientations_xyzw: Nonzero quaternions in xyzw order. + scales: Positive instance scales, multiplied by prototype scales. + prototype_indices: Integer indices into configured prototypes. + colors: Optional RGBA overrides in [0, 1]. + visible: Per-instance boolean visibility. + env_ids: Unique environment indices, or None for every environment. + """ + self._check_live() + ids = self._env_ids(env_ids) + values = dict( + translations=translations, + orientations_xyzw=orientations_xyzw, + scales=scales, + prototype_indices=prototype_indices, + colors=colors, + visible=visible, + ) + if all(value is None for value in values.values()): + raise ValueError("At least one marker update array is required.") + batches = {} + for key, value in values.items(): + if value is None: + continue + array = to_numpy_array(value, np.float64) + width = { + "translations": 3, + "orientations_xyzw": 4, + "scales": 3, + "colors": 4, + }.get(key) + ndim = 3 if width else 2 + if len(ids) == 1 and array.ndim == ndim - 1: + array = array[None] + if ( + array.ndim != ndim + or array.shape[0] != len(ids) + or (width and array.shape[-1] != width) + ): + raise ValueError( + f"{key} must have shape (E, M{', ' + str(width) if width else ''})." + ) + batches[key] = array + states = self._states.copy() + for row, env_id in enumerate(ids): + states[env_id] = self._updated_state( + env_id, {key: value[row] for key, value in batches.items()} + ) + if ids: + self._commit( + on_change=self._on_change, + states=states, + parent_poses=self._resolve_attachments(), + ) + + def _updated_state( + self, env_id: int, arrays: dict[str, np.ndarray] + ) -> dict[str, np.ndarray | None]: + previous = self._states[env_id] + previous_count = len(previous["translations"]) + count = ( + len(arrays["translations"]) if "translations" in arrays else previous_count + ) + state = self._defaults(count) if count != previous_count else previous.copy() + for key, array in arrays.items(): + width = { + "translations": 3, + "orientations_xyzw": 4, + "scales": 3, + "colors": 4, + }.get(key) + shape = (count, width) if width else (count,) + if array.shape != shape or not np.isfinite(array).all(): + raise ValueError(f"{key} must be finite with shape {shape}.") + if np.any(np.abs(array) > np.finfo(np.float32).max): + raise ValueError(f"{key} must be representable as float32.") + if key == "scales" and np.any(array <= 0): + raise ValueError("scales must be positive.") + if key == "colors" and (np.any(array < 0) or np.any(array > 1)): + raise ValueError("colors must be RGBA in [0, 1].") + if key == "prototype_indices": + if ( + np.any(array != np.floor(array)) + or np.any(array < 0) + or np.any(array >= len(self._parts)) + ): + raise ValueError( + "prototype_indices must reference configured prototypes." + ) + array = array.astype(np.int64) + elif key == "visible": + if np.any((array != 0) & (array != 1)): + raise ValueError("visible must contain booleans.") + array = array.astype(bool) + elif key == "orientations_xyzw": + norms = np.linalg.norm(array, axis=1, keepdims=True) + if np.any(norms <= 1e-12): + raise ValueError( + "orientations_xyzw must contain nonzero quaternions." + ) + array = array / norms + state[key] = array + prototype_scales = np.asarray( + [prototype.scale for prototype in self._cfg.prototypes.values()], + dtype=np.float64, + ) + world_positions = state["translations"] + self._origins[env_id] + combined_scales = state["scales"] * prototype_scales[state["prototype_indices"]] + if np.any(np.abs(world_positions) > np.finfo(np.float32).max): + raise ValueError("World positions must be representable as float32.") + if np.any(combined_scales > np.finfo(np.float32).max) or np.any( + combined_scales.astype(np.float32) <= 0 + ): + raise ValueError("Combined scales must be positive float32 values.") + return state + + def _commit( + self, + *, + on_change: Callable[[MarkerGroup], None] | None, + **changes: object, + ) -> None: + if "cfg" in changes: + changes["parts"] = self._compile_parts(changes["cfg"]) + previous = {key: getattr(self, "_" + key) for key in changes} + changed_envs = set() + if "states" in changes: + changed_envs.update( + env_id + for env_id, (old, new) in enumerate( + zip(self._states, changes["states"]) + ) + if old is not new + ) + if "parent_poses" in changes: + old_poses, new_poses = self._parent_poses, changes["parent_poses"] + changed_envs.update( + env_id + for env_id in old_poses.keys() | new_poses.keys() + if env_id not in old_poses + or env_id not in new_poses + or not np.array_equal(old_poses[env_id], new_poses[env_id]) + ) + for key, value in changes.items(): + setattr(self, "_" + key, value) + try: + # Geometry and styles are already validated. Check only changed + # transforms here; render snapshots belong to the actual consumer. + for env_id in changed_envs: + positions, _ = self._poses(env_id) + if not np.isfinite(positions).all() or np.any( + np.abs(positions) > np.finfo(np.float32).max + ): + raise ValueError("World positions must be finite float32 values.") + if on_change is not None: + on_change(self) + except Exception: + for key, value in previous.items(): + setattr(self, "_" + key, value) + raise + + def _poses(self, env_id: int) -> tuple[np.ndarray, np.ndarray]: + state = self._states[env_id] + xyz = state["translations"] + xyzw = state["orientations_xyzw"] + if env_id in self._parent_poses and len(xyz): + parent = self._parent_poses[env_id] + rotation = Rotation.from_quat(parent[3:]) + xyz = rotation.apply(xyz) + parent[:3] + xyzw = (rotation * Rotation.from_quat(xyzw)).as_quat() + return xyz + self._origins[env_id], xyzw + + def snapshot(self) -> tuple[MeshMarkerOverlay, ...]: + """Return detached world-space snapshots without querying parent assets. + + Returns: + Mesh parts with stable environment/instance IDs and wxyz rotations. + """ + if self._removed: + return () + result = [] + prototypes = tuple(self._cfg.prototypes.values()) + for env_id, state in enumerate(self._states): + positions, orientations = self._poses(env_id) + if not np.isfinite(positions).all() or np.any( + np.abs(positions) > np.finfo(np.float32).max + ): + raise ValueError("World positions must be finite float32 values.") + for index, prototype_index in enumerate(state["prototype_indices"]): + prototype = prototypes[prototype_index] + rgba = ( + prototype.color + if state["colors"] is None + else state["colors"][index] + ) + for part_index, (geometry, rgb) in enumerate( + self._parts[prototype_index] + ): + color = ( + tuple(float(v) for v in rgba) + if rgb is None + else (*rgb, float(rgba[3])) + ) + result.append( + MeshMarkerOverlay( + overlay_id=f"group:{self.name}:{env_id}:{index}:{part_index}", + vertices=geometry.vertices, + faces=geometry.faces, + position=positions[index], + wxyz=orientations[index][[3, 0, 1, 2]], + scale=state["scales"][index] * prototype.scale, + color=color, + unlit=prototype.unlit, + scene_lighting=prototype.scene_lighting, + _shared_geometry=geometry, + visible=bool( + self._visible[env_id] and state["visible"][index] + ), + env_id=env_id if self.scope == "env" else None, + ) + ) + return tuple(result) + + def set_visibility(self, visible: bool, *, env_ids: object | None = None) -> None: + """Show or hide selected environments, preserving instance visibility. + + Args: + visible: Whether selected environment batches should be displayed. + env_ids: Unique environment indices, or None for every environment. + """ + self._check_live() + ids = self._env_ids(env_ids) + if not isinstance(visible, (bool, np.bool_)): + raise ValueError("visible must be a boolean.") + values = self._visible.copy() + values[ids] = visible + if ids: + self._commit(on_change=self._on_change, visible=values) + + def clear(self, *, env_ids: object | None = None) -> None: + """Remove selected instances, retaining prototypes and attachment state. + + Args: + env_ids: Unique environment indices, or None for every environment. + """ + self._check_live() + ids = self._env_ids(env_ids) + states = self._states.copy() + for env_id in ids: + states[env_id] = self._defaults(0) + if ids: + self._commit(on_change=self._on_change, states=states) + + def attach( + self, + parent: str, + *, + link_name: str | None = None, + env_ids: object | None = None, + ) -> None: + """Interpret existing selected poses as offsets from a live registered asset. + + The owner resolves prepared roots or articulation links without preparing + physics. World scope is unsupported without a source environment selector. + + Args: + parent: Registered rigid-object, robot or articulation UID. + link_name: Articulation/robot link name, or None for the asset root. + env_ids: Unique environment indices, or None for every environment. + """ + self._check_live() + ids = self._env_ids(env_ids) + if self.scope == "world": + raise ValueError("Attachment is unsupported for world-scope groups.") + if ( + not isinstance(parent, str) + or not parent + or ( + link_name is not None + and (not isinstance(link_name, str) or not link_name) + ) + ): + raise ValueError("parent and optional link_name must be non-empty strings.") + attachments = self._attachments.copy() + for env_id in ids: + attachments[env_id] = (parent, link_name) + if ids: + self._commit( + on_change=self._on_change, + attachments=attachments, + parent_poses=self._resolve_attachments(attachments), + ) + + def _resolve_attachments( + self, attachments: dict[int, tuple[str, str | None]] | None = None + ) -> dict[int, np.ndarray]: + attachments = self._attachments if attachments is None else attachments + if attachments and self._pose_resolver is None: + raise RuntimeError("Attachment requires an owner-supplied pose resolver.") + result = {} + for target in dict.fromkeys(attachments.values()): + ids = [env_id for env_id, value in attachments.items() if value == target] + poses = to_numpy_array(self._pose_resolver(*target, ids), np.float64) + if poses.shape != (len(ids), 7) or not np.isfinite(poses).all(): + raise ValueError("Parent poses must be finite with shape (E, 7).") + norms = np.linalg.norm(poses[:, 3:], axis=1, keepdims=True) + if np.any(norms <= 1e-12) or not np.isfinite(norms).all(): + raise ValueError( + "Parent orientations must be nonzero finite quaternions." + ) + poses[:, 3:] /= norms + result.update(zip(ids, poses)) + return result + + def _refresh_attachments( + self, *, on_change: Callable[[MarkerGroup], None] | None = None + ) -> None: + """Refresh changed poses using the host's automatic publication policy.""" + if not self._attachments: + return + poses = self._resolve_attachments() + if all( + np.array_equal(pose, self._parent_poses.get(env_id)) + for env_id, pose in poses.items() + ): + return + self._commit(on_change=on_change, parent_poses=poses) + + def detach( + self, *, env_ids: object | None = None, keep_world_pose: bool = True + ) -> None: + """Detach selected environments, preserving their live world poses by default. + + With ``keep_world_pose=False``, stored offsets become environment-local + poses. Neither option advances physics. + + Args: + env_ids: Unique environment indices, or None for every environment. + keep_world_pose: Preserve current live world poses when True. + """ + self._check_live() + ids = self._env_ids(env_ids) + if not isinstance(keep_world_pose, (bool, np.bool_)): + raise ValueError("keep_world_pose must be a boolean.") + attachments = self._attachments.copy() + poses = ( + self._resolve_attachments() + if keep_world_pose and ids + else self._parent_poses.copy() + ) + states = self._states.copy() + for env_id in ids: + if env_id not in attachments: + continue + if keep_world_pose: + state = states[env_id].copy() + xyz, xyzw = state["translations"], state["orientations_xyzw"] + if len(xyz): + rotation = Rotation.from_quat(poses[env_id][3:]) + state["translations"] = rotation.apply(xyz) + poses[env_id][:3] + state["orientations_xyzw"] = ( + rotation * Rotation.from_quat(xyzw) + ).as_quat() + states[env_id] = state + del attachments[env_id] + poses.pop(env_id, None) + if ids: + self._commit( + on_change=self._on_change, + states=states, + attachments=attachments, + parent_poses=poses, + ) + + def _detach_parent(self, parent: str) -> None: + ids = [ + env_id + for env_id, target in self._attachments.items() + if target[0] == parent + ] + if ids: + self.detach(env_ids=ids) + + def remove(self) -> None: + """Release the entire group once; further updates raise RuntimeError.""" + if self._removed: + return + if self._on_remove is not None: + self._on_remove(self) + self._removed = True + self._states = [self._defaults(0) for _ in range(self.num_envs)] + self._attachments.clear() + self._parent_poses.clear() + self._pose_resolver = self._on_change = self._on_remove = None + if self._source_textures is not None: + self._source_textures.cleanup() + self._source_textures = None diff --git a/embodichain/lab/visualization/markers/robot_markers.py b/embodichain/lab/visualization/markers/robot_markers.py new file mode 100644 index 0000000000..575ef96009 --- /dev/null +++ b/embodichain/lab/visualization/markers/robot_markers.py @@ -0,0 +1,361 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Articulated reference visuals composed from externally supplied link poses.""" + +from __future__ import annotations + +import copy +from dataclasses import MISSING +from pathlib import Path +from tempfile import TemporaryDirectory +from typing import TYPE_CHECKING, Literal +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.spatial.transform import Rotation + +from embodichain.utils import configclass +from embodichain.lab.sim.cfg.source import UsdSourceCfg, _usd_source_from_value +from .._utils import to_numpy_array +from .cfg import MarkerGroupCfg +from ._robot_marker_assets import load_urdf_visuals, load_usd_visuals + +if TYPE_CHECKING: + from embodichain.lab.sim import SimulationManager + +__all__ = ["RobotMarkersAppearanceCfg", "RobotMarkersCfg", "RobotMarkers"] + + +@configclass +class RobotMarkersAppearanceCfg: + """Independent material appearance and opacity. + + ``override`` replaces the base-color factor while retaining source textures + and material parameters. ``asset`` preserves the original factor. Opacity + controls coverage. Opaque visuals use scene lighting; translucent appearance + may differ across renderers. Offscreen cameras exclude visuals until + visualization is enabled. + """ + + mode: Literal["override", "asset"] = "override" + color: tuple[float, float, float] = (0.2, 0.85, 0.45) + opacity: float = 0.4 + unlit: bool = False + """Use uniform native color without surface lighting when True.""" + + +@configclass +class RobotMarkersCfg: + """Robot marker source and reference coordinate convention.""" + + name: str = MISSING + asset_path: str | None = None + source: UsdSourceCfg | None = None + """USD source selector shared with VisualObject, RigidObject and Robot.""" + prim_path: str | None = None + appearance: RobotMarkersAppearanceCfg = RobotMarkersAppearanceCfg() + coordinate_space: Literal["world", "env"] = "world" + + +def _appearance(value: RobotMarkersAppearanceCfg) -> RobotMarkersAppearanceCfg: + value.validate() + if value.mode not in {"override", "asset"}: + raise ValueError("Robot marker mode must be 'override' or 'asset'.") + color = np.asarray(value.color, dtype=float) + if ( + color.shape != (3,) + or not np.isfinite(color).all() + or np.any((color < 0) | (color > 1)) + ): + raise ValueError("Robot marker color must be RGB in [0, 1].") + if not np.isfinite(value.opacity) or not 0 <= value.opacity <= 1: + raise ValueError("Robot marker opacity must be in [0, 1].") + if not isinstance(value.unlit, bool): + raise ValueError("Robot marker unlit must be a boolean.") + return copy.deepcopy(value) + + +def _matrices(poses: object) -> np.ndarray: + value = to_numpy_array(poses, np.float64) + if value.shape[-1:] == (7,): + if not np.isfinite(value).all(): + raise ValueError("Link poses must be finite.") + quaternions = value[..., 3:] + if np.any(np.linalg.norm(quaternions, axis=-1) <= 1e-12): + raise ValueError("Link xyzw quaternions must be nonzero.") + result = np.broadcast_to(np.eye(4), (*value.shape[:-1], 4, 4)).copy() + if value.size == 0: + return result + result[..., :3, :3] = ( + Rotation.from_quat(quaternions.reshape(-1, 4)) + .as_matrix() + .reshape(*value.shape[:-1], 3, 3) + ) + result[..., :3, 3] = value[..., :3] + return result + if value.shape[-2:] != (4, 4) or not np.isfinite(value).all(): + raise ValueError("Poses must be finite matrices or xyz + xyzw arrays.") + rotations = value[..., :3, :3] + if ( + not np.allclose(value[..., 3, :], [0, 0, 0, 1], atol=1e-6, rtol=0) + or not np.allclose( + np.swapaxes(rotations, -1, -2) @ rotations, np.eye(3), atol=1e-5, rtol=0 + ) + or np.any(np.linalg.det(rotations) <= 0) + ): + raise ValueError( + "Link poses must be rigid transforms without scale or reflection." + ) + return value + + +class RobotMarkers: + """Display robot poses as markers without creating physical articulations. + + A marker group owns the link visuals for all environments and reference + layers. Callers supply link poses or URDF joint positions; tasks own motion + generation and playback. + + Args: + cfg: URDF or USD source, appearance and coordinate convention. + sim: Owner of the marker resources. Prepare native scenes explicitly. + """ + + def __init__(self, cfg: RobotMarkersCfg, sim: SimulationManager) -> None: + cfg.validate() + if cfg.coordinate_space not in {"world", "env"}: + raise ValueError("coordinate_space must be 'world' or 'env'.") + self.cfg = copy.deepcopy(cfg) + self.cfg.appearance = _appearance(cfg.appearance) + source = _usd_source_from_value(cfg.source) + if source is not None: + source.validate() + if cfg.asset_path is not None or cfg.prim_path is not None: + raise ValueError("Use source or asset_path/prim_path, not both.") + asset_path, prim_path = source.file_path, source.source_prim_path + else: + asset_path, prim_path = cfg.asset_path, cfg.prim_path + if asset_path is None: + raise ValueError("Robot markers require asset_path or source.") + path = Path(asset_path).expanduser().resolve() + if not path.is_file(): + raise FileNotFoundError(path) + self._urdf_path = path if path.suffix.lower() == ".urdf" else None + texture_files = TemporaryDirectory( + prefix="embodichain-robot-markers-materials-" + ) + if self._urdf_path is not None: + self._visuals = load_urdf_visuals(path, Path(texture_files.name)) + elif path.suffix.lower() in {".usd", ".usda", ".usdc", ".usdz"}: + self._visuals = load_usd_visuals(path, prim_path, Path(texture_files.name)) + else: + raise ValueError("Robot marker source must be URDF or USD.") + self._link_names = tuple( + dict.fromkeys(item.link_name for item in self._visuals) + ) + self._link_indices = np.array( + [self._link_names.index(item.link_name) for item in self._visuals] + ) + self._local_poses = np.stack([item.local_pose for item in self._visuals]) + self._group = sim.add_marker_group( + MarkerGroupCfg( + name=cfg.name, + prototypes=self._prototypes(self.cfg.appearance), + scope="env", + ) + ) + self._group._source_textures = texture_files + self._chain = None + + def _prototypes(self, appearance: RobotMarkersAppearanceCfg) -> dict: + prototypes = {} + for index, visual in enumerate(self._visuals): + prototype = copy.copy(visual.prototype) + prototype.unlit = appearance.unlit + prototype.scene_lighting = True + if appearance.mode == "override": + prototype.color = (*appearance.color, appearance.opacity) + else: + prototype.color = ( + *prototype.color[:3], + prototype.color[3] * appearance.opacity, + ) + prototypes[str(index)] = prototype + return prototypes + + @property + def link_names(self) -> tuple[str, ...]: + """Ordered visual link names expected by visualize_link_poses().""" + return self._link_names + + @property + def num_visuals(self) -> int: + """Number of material-separated visual parts per robot.""" + return len(self._visuals) + + @property + def joint_names(self) -> tuple[str, ...]: + """URDF joint order for visualize_qpos(), including mimic joints.""" + return tuple(self._kinematic_chain().get_joint_parameter_names()) + + def _kinematic_chain(self): + if self._urdf_path is None: + raise ValueError( + "Joint-space visualization requires a URDF source; use link poses for USD." + ) + if self._chain is None: + import pytorch_kinematics as pk + + data = self._urdf_path.read_bytes() + root = ET.fromstring(data) + if not root.findall("joint"): + links = root.findall("link") + if len(links) != 1: + raise ValueError("A zero-joint URDF must contain exactly one link.") + name = links[0].attrib["name"] + self._chain = pk.Chain(pk.Frame(name, link=pk.Link(name))) + else: + self._chain = pk.build_chain_from_urdf(data) + return self._chain + + def visualize_link_poses( + self, poses: object, *, env_ids: object | None = None + ) -> None: + """Compose reference link poses with each visual's local transform. + + Args: + poses: Array (E, L, 7) in xyz + xyzw order or (E, L, 4, 4). + Insert an instance dimension K after E to display multiple + references per environment. L follows link_names. Task alignment + and sampling remain with the caller; mesh scale applies once. + env_ids: Unique environments to update; other environments retain state. + """ + ids = self._group._env_ids(env_ids) + matrices = _matrices(poses) + if matrices.ndim == 4: + matrices = matrices[:, None] + if ( + matrices.ndim != 5 + or matrices.shape[0] != len(ids) + or matrices.shape[2:] != (len(self.link_names), 4, 4) + ): + raise ValueError( + "Link pose shape must match selected environments and link_names." + ) + if not ids: + return + count = matrices.shape[1] * self.num_visuals + if count == 0: + self._group.clear(env_ids=ids) + return + visual_poses = matrices[:, :, self._link_indices] @ self._local_poses + translations = visual_poses[..., :3, 3].reshape(len(ids), count, 3) + if self.cfg.coordinate_space == "world": + translations = translations - self._group._origins[ids, None, :] + rotations = Rotation.from_matrix( + visual_poses[..., :3, :3].reshape(-1, 3, 3) + ).as_quat() + self._group.update( + translations=translations, + orientations_xyzw=rotations.reshape(len(ids), count, 4), + prototype_indices=np.tile( + np.arange(self.num_visuals), (len(ids), matrices.shape[1]) + ), + env_ids=ids, + ) + + def visualize_qpos( + self, + qpos: object, + *, + root_poses: object | None = None, + env_ids: object | None = None, + ) -> None: + """Compute URDF FK and publish reference poses in one marker update. + + Args: + qpos: Joint positions (E, J), or (E, K, J) for K references per + environment, in joint_names order. + root_poses: Optional root transforms with the same leading E / E,K + dimensions as qpos, ending in 7 or (4,4). Identity when omitted. + env_ids: Environments to update, retaining other environments. + """ + import torch + + q = ( + qpos + if isinstance(qpos, torch.Tensor) + else torch.as_tensor(qpos, dtype=torch.float32) + ) + if not q.is_floating_point(): + q = q.float() + ids = self._group._env_ids(env_ids) + chain = self._kinematic_chain() + if ( + q.ndim not in (2, 3) + or q.shape[0] != len(ids) + or q.shape[-1] != len(self.joint_names) + or not bool(torch.isfinite(q).all()) + ): + raise ValueError("qpos must be finite with shape (E, J) or (E, K, J).") + if not ids: + return + if q.ndim == 3 and q.shape[1] == 0: + self._group.clear(env_ids=ids) + return + chain = chain.to(dtype=q.dtype, device=q.device) + self._chain = chain + with torch.no_grad(): + batch_size = int(np.prod(q.shape[:-1])) + fk = chain.forward_kinematics(q.reshape(batch_size, q.shape[-1])) + matrices = torch.stack( + [fk[name].get_matrix() for name in self.link_names], dim=1 + ) + matrices = matrices.reshape(*q.shape[:-1], len(self.link_names), 4, 4) + matrices = to_numpy_array(matrices, np.float64) + if root_poses is not None: + roots = _matrices(root_poses) + if roots.shape != (*q.shape[:-1], 4, 4): + raise ValueError("root_poses must match selected environments.") + matrices = roots[..., None, :, :] @ matrices + self.visualize_link_poses(matrices, env_ids=ids) + + def set_appearance(self, appearance: RobotMarkersAppearanceCfg) -> None: + """Change color mode or opacity without changing the source asset. + + Args: + appearance: Validated appearance, applied atomically to all parts. + """ + appearance = _appearance(appearance) + self._group._check_live() + cfg = copy.copy(self._group._cfg) + cfg.prototypes = self._prototypes(appearance) + self._group._commit(on_change=self._group._on_change, cfg=cfg) + self.cfg.appearance = appearance + + def set_visibility(self, visible: bool, *, env_ids: object | None = None) -> None: + """Set selected robot markers' visibility while retaining their reference poses. + + Args: + visible: Whether to display the selected robot markers. + env_ids: Optional environment selection. + """ + self._group.set_visibility(visible, env_ids=env_ids) + + def remove(self) -> None: + """Release the robot markers' render resources; repeated removal is safe.""" + self._group.remove() diff --git a/embodichain/lab/visualization/markers/visualization_markers.py b/embodichain/lab/visualization/markers/visualization_markers.py new file mode 100644 index 0000000000..34b87c2a8a --- /dev/null +++ b/embodichain/lab/visualization/markers/visualization_markers.py @@ -0,0 +1,186 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Isaac-style flat marker updates backed by the shared marker group.""" + +from __future__ import annotations + +from dataclasses import MISSING +from typing import TYPE_CHECKING, Literal + +import numpy as np + +from embodichain.utils import configclass +from .._utils import to_numpy_array +from .cfg import MarkerGroupCfg, MarkerPrototypeCfg + +if TYPE_CHECKING: + from embodichain.lab.sim import SimulationManager + +__all__ = ["VisualizationMarkers", "VisualizationMarkersCfg"] + + +@configclass +class VisualizationMarkersCfg: + """Flat marker configuration using xyzw quaternions. + + ``prim_path`` is a unique registry name, not a USD scene path. Dictionary + order defines marker indices. World coordinates are the default; ``env`` + adds the selected environment's origin once. No environment layout is + inferred from the number of instances. + """ + + prim_path: str = MISSING + markers: dict[str, MarkerPrototypeCfg] = MISSING + coordinate_space: Literal["world", "env"] = "world" + + +class VisualizationMarkers: + """Adapt flat Isaac-style arrays to a manager-owned MarkerGroup. + + Args: + cfg: Prototypes and coordinate convention. + sim: Simulation owning the marker resources. Native rendering requires + an explicitly prepared scene. + """ + + def __init__(self, cfg: VisualizationMarkersCfg, sim: SimulationManager) -> None: + cfg.validate() + if cfg.coordinate_space not in {"world", "env"}: + raise ValueError("coordinate_space must be 'world' or 'env'.") + self._coordinate_space = cfg.coordinate_space + self._group = sim.add_marker_group( + MarkerGroupCfg(name=cfg.prim_path, prototypes=cfg.markers) + ) + self._environment_ids = np.empty(0, dtype=np.int64) + self._visible = True + + @property + def count(self) -> int: + """Number of logical instances, including hidden instances.""" + return self._group.count + + @property + def num_prototypes(self) -> int: + """Number of configured marker prototypes.""" + return len(self._group._cfg.prototypes) + + def is_visible(self) -> bool: + """Return the visibility requested through this facade.""" + return self._visible + + def set_visibility(self, visible: bool) -> None: + """Set visibility while retaining instance state. + + Args: + visible: Whether the group is displayed. + """ + self._group.set_visibility(visible) + self._visible = bool(visible) + + def visualize( + self, + translations: object | None = None, + orientations: object | None = None, + scales: object | None = None, + marker_indices: object | None = None, + environment_ids: object | None = None, + ) -> None: + """Update a complete flat instance selection atomically. + + Omitted arrays retain previous values when the count is unchanged. + Translations determine a new count and reset omitted fields on resize. + Instances may be interleaved across environments. Omitted environment + IDs retain membership, or select environment zero after a resize. + Hidden groups accept updates and show the latest state when enabled. + + Args: + translations: Finite positions shaped (N, 3). + orientations: Nonzero xyzw quaternions shaped (N, 4). + scales: Positive scales shaped (N, 3). + marker_indices: Prototype indices shaped (N,). + environment_ids: Integer environment IDs shaped (N,). Changing + membership requires translations in the same call. + """ + self._group._check_live() + provided = { + key: to_numpy_array(value, np.float64) + for key, value in { + "translations": translations, + "orientations_xyzw": orientations, + "scales": scales, + "prototype_indices": marker_indices, + }.items() + if value is not None + } + if not provided and environment_ids is None: + return + count = self.count + if "translations" in provided: + if provided["translations"].ndim != 2: + raise ValueError("translations must have shape (N, 3).") + count = len(provided["translations"]) + resized = count != self.count + if environment_ids is None: + ids = ( + np.zeros(count, dtype=np.int64) + if resized + else self._environment_ids.copy() + ) + else: + raw = to_numpy_array(environment_ids, None) + if raw.shape != (count,) or (count and raw.dtype.kind not in "iu"): + raise ValueError( + "environment_ids must be an integer array shaped (N,)." + ) + if np.any(raw < 0) or np.any(raw >= self._group.num_envs): + raise ValueError("environment_ids are out of bounds.") + ids = raw.astype(np.int64) + if translations is None and not np.array_equal(ids, self._environment_ids): + raise ValueError("Changing environment_ids requires translations.") + arrays = self._group._defaults(count) + if not resized and count: + # Restore caller ordering from the authoritative per-environment state. + for env_id, state in enumerate(self._group._states): + rows = self._environment_ids == env_id + for key in arrays: + if state[key] is not None: + arrays[key][rows] = state[key] + for key, array in provided.items(): + width = {"translations": 3, "orientations_xyzw": 4, "scales": 3}.get(key) + expected = (count, width) if width else (count,) + if array.shape != expected: + raise ValueError(f"{key} must have shape {expected}.") + arrays[key] = array + if translations is not None and self._coordinate_space == "world": + arrays["translations"] = arrays["translations"] - self._group._origins[ids] + states = [ + self._group._updated_state( + env_id, + { + key: array[ids == env_id] + for key, array in arrays.items() + if array is not None + }, + ) + for env_id in range(self._group.num_envs) + ] + self._group._commit(on_change=self._group._on_change, states=states) + self._environment_ids = ids + + def remove(self) -> None: + """Release all marker resources; repeated removal is safe.""" + self._group.remove() diff --git a/embodichain/lab/visualization/protocol.py b/embodichain/lab/visualization/protocol.py index 7eaf6d7de4..9d819daee8 100644 --- a/embodichain/lab/visualization/protocol.py +++ b/embodichain/lab/visualization/protocol.py @@ -16,9 +16,10 @@ from __future__ import annotations -from dataclasses import dataclass, field +from dataclasses import InitVar, dataclass, field from datetime import datetime, timezone from typing import Literal, Protocol +import json import numpy as np @@ -39,6 +40,7 @@ "JointControlSpec", "JointControlState", "MeshGeometry", + "MeshMarkerOverlay", "PickCommand", "PointCloudOverlay", "SceneFrame", @@ -53,7 +55,7 @@ "pose_to_position_wxyz", ] -SCHEMA_VERSION = 5 +SCHEMA_VERSION = 7 Color = tuple[int, int, int] @@ -637,6 +639,195 @@ def __post_init__(self) -> None: object.__setattr__(self, "colors", colors) +@dataclass(frozen=True) +class _MeshMarkerGeometry: + """Validated, detached, immutable geometry shared by marker instances.""" + + vertices: np.ndarray + faces: np.ndarray + uv_coords: np.ndarray | None + texture: np.ndarray | None + texture_wrap: tuple[str, str] = ("repeat", "repeat") + normals: np.ndarray | None = None + material: object | None = None + material_uv_coords: np.ndarray | None = None + + def __post_init__(self) -> None: + vertices = _array(self.vertices, np.float32) + if self.normals is not None: + normals = _array(self.normals, np.float32) + if normals.shape != vertices.shape or not np.isfinite(normals).all(): + raise ValueError("Mesh normals must be finite and match vertices.") + normals.setflags(write=False) + object.__setattr__(self, "normals", normals) + if self.material_uv_coords is not None: + material_uv = _array(self.material_uv_coords, np.float32) + if ( + material_uv.shape != (len(vertices), 2) + or not np.isfinite(material_uv).all() + ): + raise ValueError("Material UV coordinates must be finite Vx2.") + material_uv.setflags(write=False) + object.__setattr__(self, "material_uv_coords", material_uv) + if self.material is not None: + material = ( + json.loads(self.material) + if isinstance(self.material, str) + else self.material + ) + if not isinstance(material, dict): + raise ValueError("Mesh material must contain descriptor parameters.") + object.__setattr__( + self, "material", json.dumps(material, sort_keys=True, allow_nan=False) + ) + face_values = _array(self.faces, np.float64) + if vertices.ndim != 2 or vertices.shape[1] != 3: + raise ValueError( + f"vertices must have shape (V, 3), received {vertices.shape}." + ) + if face_values.size == 0: + raise ValueError("Mesh marker geometry must contain at least one triangle.") + if face_values.ndim != 2 or face_values.shape[1] != 3: + raise ValueError( + f"faces must have shape (F, 3), received {face_values.shape}." + ) + if not np.all(np.isfinite(vertices)): + raise ValueError("Mesh marker vertices must be finite.") + if ( + not np.all(np.isfinite(face_values)) + or np.any(face_values < 0.0) + or np.any(face_values > np.iinfo(np.uint32).max) + or np.any(face_values != np.floor(face_values)) + ): + raise ValueError( + "Mesh marker faces must contain finite non-negative integers " + "representable as uint32." + ) + faces = np.ascontiguousarray(face_values, dtype=np.uint32) + if faces.size and (vertices.shape[0] == 0 or np.max(faces) >= len(vertices)): + raise ValueError("Mesh marker faces contain out-of-bounds vertex indices.") + uv = None if self.uv_coords is None else _array(self.uv_coords, np.float32) + texture = None if self.texture is None else _array(self.texture, None) + if len(self.texture_wrap) != 2 or any( + value not in {"repeat", "clamp", "mirror"} for value in self.texture_wrap + ): + raise ValueError( + "texture_wrap must contain two repeat, clamp, or mirror modes." + ) + object.__setattr__(self, "texture_wrap", tuple(self.texture_wrap)) + if uv is not None and ( + uv.shape != (len(vertices), 2) or not np.isfinite(uv).all() + ): + raise ValueError("uv_coords must be finite with shape (V, 2).") + if texture is not None: + if ( + uv is None + or texture.dtype != np.uint8 + or texture.ndim != 3 + or texture.shape[2] not in (3, 4) + or min(texture.shape[:2]) == 0 + ): + raise ValueError( + "texture requires UVs and a nonempty uint8 RGB/RGBA image." + ) + for name, value in ( + ("vertices", vertices), + ("faces", faces), + ("uv_coords", uv), + ("texture", texture), + ): + if value is not None: + value.setflags(write=False) + object.__setattr__(self, name, value) + + +@dataclass(frozen=True) +class MeshMarkerOverlay: + """Triangle mesh marker with detached poses and shared immutable geometry. + + Color RGB and texture RGB use sRGB; alpha is linear coverage. + """ + + overlay_id: str + vertices: np.ndarray + faces: np.ndarray + position: np.ndarray + wxyz: np.ndarray + scale: np.ndarray = field(default_factory=lambda: np.ones((3,), dtype=np.float32)) + color: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 1.0) + visible: bool = True + env_id: int | None = None + uv_coords: np.ndarray | None = None + texture: np.ndarray | None = None + texture_wrap: tuple[str, str] = ("repeat", "repeat") + unlit: bool = True + scene_lighting: bool = False + normals: np.ndarray | None = None + material: object | None = None + material_uv_coords: np.ndarray | None = None + + _shared_geometry: InitVar[_MeshMarkerGeometry | None] = field( + default=None, kw_only=True + ) + + def __post_init__(self, _shared_geometry: _MeshMarkerGeometry | None) -> None: + if not isinstance(self.overlay_id, str) or not self.overlay_id: + raise ValueError("Mesh marker overlay_id must be a non-empty string.") + if not isinstance(self.unlit, bool): + raise ValueError("Mesh marker unlit must be a boolean.") + if not isinstance(self.scene_lighting, bool): + raise ValueError("Mesh marker scene_lighting must be a boolean.") + geometry = _shared_geometry or _MeshMarkerGeometry( + self.vertices, + self.faces, + self.uv_coords, + self.texture, + self.texture_wrap, + self.normals, + self.material, + self.material_uv_coords, + ) + position, wxyz = _normalize_position_wxyz(self.position, self.wxyz) + scale = _array(self.scale, np.float32) + color = tuple(float(component) for component in self.color) + if not np.all(np.isfinite(position)) or not np.all(np.isfinite(wxyz)): + raise ValueError("Mesh marker pose arrays must be finite.") + if scale.shape != (3,): + raise ValueError(f"scale must have shape (3,), got {scale.shape}.") + if not np.all(np.isfinite(scale)) or np.any(scale <= 0.0): + raise ValueError("Mesh marker scale values must be finite and positive.") + if len(color) != 4 or not all( + np.isfinite(component) and 0.0 <= component <= 1.0 for component in color + ): + raise ValueError( + "Mesh marker color must be an RGBA tuple with values in [0, 1]." + ) + if self.env_id is not None and ( + isinstance(self.env_id, (bool, np.bool_)) + or not isinstance(self.env_id, (int, np.integer)) + or self.env_id < 0 + ): + raise ValueError( + "Mesh marker env_id must be a non-negative integer or None." + ) + object.__setattr__(self, "_geometry", geometry) + for name in ( + "vertices", + "faces", + "uv_coords", + "texture", + "texture_wrap", + "normals", + "material", + "material_uv_coords", + ): + object.__setattr__(self, name, getattr(geometry, name)) + object.__setattr__(self, "position", position) + object.__setattr__(self, "wxyz", wxyz) + object.__setattr__(self, "scale", scale) + object.__setattr__(self, "color", color) + + @dataclass(frozen=True) class SceneOverlays: """All optional overlays attached to a dynamic frame.""" @@ -645,6 +836,12 @@ class SceneOverlays: trajectories: tuple[TrajectoryOverlay, ...] = field(default_factory=tuple) targets: tuple[TargetOverlay, ...] = field(default_factory=tuple) point_clouds: tuple[PointCloudOverlay, ...] = field(default_factory=tuple) + meshes: tuple[MeshMarkerOverlay, ...] = field(default_factory=tuple) + + def __post_init__(self) -> None: + mesh_ids = [mesh.overlay_id for mesh in self.meshes] + if len(mesh_ids) != len(set(mesh_ids)): + raise ValueError("SceneOverlays contains duplicate mesh overlay IDs.") @dataclass(frozen=True) @@ -742,7 +939,7 @@ def estimate_camera_image_frame_bytes(frame: CameraImageFrame) -> int: def estimate_frame_bytes(frame: SceneFrame) -> int: - """Estimate NumPy payload bytes retained by a frame.""" + """Estimate NumPy payload bytes, counting shared marker geometry once.""" total = ( frame.positions.nbytes + frame.wxyz.nbytes @@ -755,6 +952,20 @@ def estimate_frame_bytes(frame: SceneFrame) -> int: total += overlay.points.nbytes for overlay in frame.overlays.point_clouds: total += overlay.points.nbytes + overlay.colors.nbytes + geometry_arrays = set() + for overlay in frame.overlays.meshes: + total += overlay.position.nbytes + overlay.wxyz.nbytes + overlay.scale.nbytes + for array in ( + overlay.vertices, + overlay.faces, + overlay.uv_coords, + overlay.texture, + overlay.normals, + overlay.material_uv_coords, + ): + if array is not None and id(array) not in geometry_arrays: + total += array.nbytes + geometry_arrays.add(id(array)) for overlay in (*frame.overlays.frames, *frame.overlays.targets): total += overlay.position.nbytes + overlay.wxyz.nbytes for gizmo in frame.gizmos: diff --git a/embodichain/lab/visualization/scene_exporter.py b/embodichain/lab/visualization/scene_exporter.py index 1eb8ddaca8..7f0e827825 100644 --- a/embodichain/lab/visualization/scene_exporter.py +++ b/embodichain/lab/visualization/scene_exporter.py @@ -18,7 +18,7 @@ import hashlib import uuid -from dataclasses import dataclass +from dataclasses import dataclass, replace from time import perf_counter from typing import TYPE_CHECKING from urllib.parse import quote @@ -39,6 +39,7 @@ JointControlSpec, JointControlState, MeshGeometry, + MeshMarkerOverlay, PointCloudOverlay, SceneFrame, SceneManifest, @@ -772,6 +773,12 @@ def _prepare_overlays(self, overlays: SceneOverlays | None) -> SceneOverlays: else None ) marker_frames = self._capture_axis_marker_overlays(reserved_frame_ids) + reserved_mesh_ids = ( + {mesh.overlay_id for mesh in overlays.meshes} + if overlays is not None + else set() + ) + marker_meshes = self._capture_marker_mesh_overlays(reserved_mesh_ids) overlays = overlays or SceneOverlays() fluid_points = [] for uid, fluid in getattr(self._sim, "_fluid_objects", {}).items(): @@ -824,8 +831,37 @@ def _prepare_overlays(self, overlays: SceneOverlays | None) -> SceneOverlays: trajectories=overlays.trajectories, targets=overlays.targets, point_clouds=tuple(point_clouds), + meshes=marker_meshes + overlays.meshes, ) + def _capture_marker_mesh_overlays( + self, reserved_mesh_ids: set[str] + ) -> tuple[MeshMarkerOverlay, ...]: + """Capture world-space marker meshes supplied by the simulation.""" + get_marker_overlays = getattr(self._sim, "get_marker_overlays", None) + if get_marker_overlays is None: + return () + + meshes: list[MeshMarkerOverlay] = [] + used_ids = set(reserved_mesh_ids) + selected_env_ids = set(self._env_ids) + for marker in get_marker_overlays(): + if marker.env_id is not None and marker.env_id not in selected_env_ids: + continue + base_id = marker.overlay_id + overlay_id = base_id + suffix = 1 + while overlay_id in used_ids: + overlay_id = f"{base_id}#{suffix}" + suffix += 1 + used_ids.add(overlay_id) + meshes.append( + marker + if overlay_id == base_id + else replace(marker, overlay_id=overlay_id) + ) + return tuple(meshes) + def capture( self, *, diff --git a/examples/sim/reference_trajectory.py b/examples/sim/reference_trajectory.py new file mode 100644 index 0000000000..5f58f5b606 --- /dev/null +++ b/examples/sim/reference_trajectory.py @@ -0,0 +1,463 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Play named robot link trajectories with ghosts, paths and visual task objects. + +Input NPZ fields: poses (T,K,L,7), link_names (L,), dt (scalar). Poses use +world xyz+xyzw. Optional fields are tips (T,K,3), objects (T,K,7), targets +(T,K,3), command (T,3) and velocity (T,3). No physics steps are executed. +Camera tracking and velocity arrows use the asset's named root. A fixed view +can replay visual links without the root; velocity arrows are omitted in that case. +""" + +from __future__ import annotations + +import argparse +import json +from pathlib import Path +import subprocess +import time +import warnings +import xml.etree.ElementTree as ET + +import numpy as np +from PIL import Image +from scipy.spatial.transform import Rotation +import torch + +from dexsim import spawn +from embodichain.learning.rl.policy_evaluation.camera import PolicyViewerCameraCfg +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import DenoisingCfg, RenderCfg, VisualObjectCfg +from embodichain.lab.sim.material import VisualMaterialCfg +from embodichain.lab.sim.shapes import CubeCfg +from embodichain.lab.visualization import ( + add_viser_args_to_parser, + visualization_cfg_from_args, +) +from embodichain.lab.visualization.markers import ( + RobotMarkersAppearanceCfg, + MarkerPrototypeCfg, + RobotMarkers, + RobotMarkersCfg, + VisualizationMarkers, + VisualizationMarkersCfg, +) + +__all__ = ["main"] +COLORS = [(0.15, 0.8, 0.4), (0.2, 0.55, 1.0), (1.0, 0.55, 0.15)] + + +def _root_link_index(asset_path: str, names: tuple[str, ...]) -> int | None: + """Find the asset root, which may be absent from a visual-only recording.""" + path = Path(asset_path).expanduser().resolve() + if path.suffix.lower() == ".urdf": + robot = ET.parse(path).getroot() + children = { + joint.find("child").attrib["link"] for joint in robot.findall("joint") + } + roots = [ + link.attrib["name"] + for link in robot.findall("link") + if link.attrib["name"] not in children + ] + if len(roots) != 1: + raise ValueError("Robot asset must contain exactly one root link.") + root_name = roots[0] + else: + from embodichain.lab.sim.spawn._usd_assets import _parse_singleton + + _, obj = _parse_singleton( + path, + ("articulations", "mesh_objects"), + "robot marker articulation or visual object", + ) + root_name = obj.root_link_name if hasattr(obj, "links") else obj.name + if not names or len(set(names)) != len(names): + raise ValueError("Recorded link_names must be nonempty and unique.") + return names.index(root_name) if root_name in names else None + + +def _camera_path( + poses: np.ndarray, offsets: np.ndarray, mode: str, root_index: int | None = 0 +) -> tuple[np.ndarray, np.ndarray]: + """Fit the whole clip once, then track XY at fixed height and distance.""" + if root_index is None: + if mode == "follow_xy": + raise ValueError( + "follow_xy requires the recorded robot root; use --camera-mode fixed " + "for recordings containing only visual links." + ) + root_index = 0 + anchors = np.zeros((len(poses), 3), dtype=np.float64) + if mode == "follow_xy": + anchors[:, :2] = poses[:, 0, root_index, :2] + else: + anchors[:, :2] = poses[0, 0, root_index, :2] + relative = poses[..., :3] + offsets[None, :, None] - anchors[:, None, None] + lower = relative.min(axis=(0, 1, 2)) + upper = relative.max(axis=(0, 1, 2)) + center = (lower + upper) / 2 + spread = max(0.8, float(np.max(upper - lower))) + cfg = PolicyViewerCameraCfg( + eye_offset=tuple(np.array([1.25, -1.7, 0.9]) * spread), + target_height=float(center[2]), + ) + targets = anchors + center + targets[:, 2] = cfg.target_height + # Apply the preset heading once, as in the policy viewer's reset. + yaw = Rotation.from_quat(poses[0, 0, root_index, 3:]).as_euler("xyz")[2] + offset = Rotation.from_euler("z", yaw).apply(cfg.eye_offset) + return targets + offset, targets + + +def main() -> None: + """Render one recorded task trajectory with explicit visualization defaults.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--asset", required=True) + parser.add_argument("--trajectory", type=Path, required=True) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--layout", choices=["overlap", "offset"], default="overlap") + parser.add_argument("--spacing", type=float, default=1.1) + parser.add_argument("--opacity", type=float, default=0.4) + parser.add_argument("--original-first", action="store_true") + parser.add_argument("--seconds", type=float, default=12) + parser.add_argument("--fps", type=int, default=25) + parser.add_argument("--live", action="store_true") + parser.add_argument( + "--camera-mode", + choices=["follow_xy", "fixed"], + default="follow_xy", + help="Follow root XY at fixed world height, or use one fixed view.", + ) + add_viser_args_to_parser(parser) + args = parser.parse_args() + if args.seconds <= 0 or args.fps <= 0 or args.spacing < 0: + parser.error("seconds/fps must be positive and spacing nonnegative") + data = np.load(args.trajectory, allow_pickle=False) + poses = np.asarray(data["poses"], dtype=np.float32) + names = tuple(str(v) for v in data["link_names"]) + if poses.ndim != 4 or poses.shape[-2:] != (len(names), 7): + parser.error("poses must be (T,K,L,7), matching link_names") + if not np.isfinite(poses).all() or not len(poses) or not poses.shape[1]: + parser.error("poses must be finite and nonempty") + if np.any(np.linalg.norm(poses[..., 3:], axis=-1) < 1e-8): + parser.error("pose quaternions must be nonzero") + dt = float(data["dt"]) + if not np.isfinite(dt) or dt <= 0: + parser.error("dt must be positive and finite") + count = poses.shape[1] + offsets = np.zeros((count, 3)) + if args.layout == "offset": + offsets[:, 1] = (np.arange(count) - (count - 1) / 2) * args.spacing + try: + root_index = _root_link_index(args.asset, names) + camera_eyes, camera_targets = _camera_path( + poses, offsets, args.camera_mode, root_index + ) + except ValueError as exc: + parser.error(str(exc)) + vector_fields = [ + (prototype, field) + for prototype, field in enumerate(("command", "velocity")) + if field in data + ] + if root_index is None and vector_fields: + warnings.warn( + "Recorded robot root is absent; command and velocity arrows are omitted. " + "Include the root in poses and link_names to display them.", + stacklevel=2, + ) + vector_fields = [] + args.output.mkdir(parents=True, exist_ok=True) + torch.set_num_threads(8) + sim = SimulationManager( + SimulationManagerCfg( + visualization=visualization_cfg_from_args(args), + headless=not args.live, + device="cpu", + num_envs=1, + width=960, + height=720, + enable_entity_gizmo=False, + startup_summary="off", + render_cfg=RenderCfg( + renderer="hybrid", + spp=1, + denoising=DenoisingCfg(window="dlss", offscreen="dlss"), + ), + ) + ) + items = [] + objects = [] + encoder = None + try: + for k in range(count): + if "objects" in data: + obj = sim.add_visual_object( + VisualObjectCfg( + uid=f"task_object_{k}", shape=CubeCfg(size=(0.04, 0.04, 0.06)) + ) + ) + objects.append(obj) + sim.prepare() + for k, obj in enumerate(objects): + obj.set_visual_material( + sim.create_visual_material( + VisualMaterialCfg( + uid=f"object_material_{k}", + base_color=[*COLORS[k % len(COLORS)], 1.0], + ) + ) + ) + scene = sim._spawn_scene.builder.result + for name, position, intensity in [ + ("key", [2, -3, 4], 45), + ("fill", [-2, 2, 3], 15), + ]: + matrix = np.eye(4, dtype=np.float32) + matrix[:3, 3] = position + scene.add_light( + spawn.LightDesc( + name=name, light_type="POINT", pose=matrix, intensity=intensity + ) + ) + for k in range(count): + original = args.original_first and k == 0 + item = RobotMarkers( + RobotMarkersCfg( + name=f"trajectory_{k}", + asset_path=args.asset, + appearance=RobotMarkersAppearanceCfg( + mode="asset" if original else "override", + color=COLORS[(k - int(args.original_first)) % len(COLORS)], + opacity=1.0 if original else args.opacity, + ), + ), + sim, + ) + items.append((item, [names.index(name) for name in item.link_names])) + marker_cfg = { + str(k): MarkerPrototypeCfg( + shape="sphere", + scale=(0.015, 0.015, 0.015), + color=(*COLORS[k % len(COLORS)], 0.85), + ) + for k in range(count) + } + paths = VisualizationMarkers( + VisualizationMarkersCfg(prim_path="trajectory_paths", markers=marker_cfg), + sim, + ) + if "tips" in data: + tips = np.asarray(data["tips"]) + sample = tips[:: max(1, len(tips) // 100)] + offsets[None] + paths.visualize( + sample.reshape(-1, 3), + marker_indices=np.tile(np.arange(count), len(sample)), + ) + targets = VisualizationMarkers( + VisualizationMarkersCfg( + prim_path="task_targets", + markers={ + "target": MarkerPrototypeCfg(shape="frame", scale=(0.1, 0.1, 0.1)) + }, + ), + sim, + ) + arrows = VisualizationMarkers( + VisualizationMarkersCfg( + prim_path="velocity", + markers={ + "command": MarkerPrototypeCfg( + shape="arrow", color=(0.1, 0.9, 0.3, 1) + ), + "measured": MarkerPrototypeCfg( + shape="arrow", color=(0.2, 0.55, 1, 1) + ), + }, + ), + sim, + ) + camera = scene.add_camera( + spawn.CameraDesc(name="trajectory_capture", resolution=(960, 720)) + ) + camera.open_camera() + camera.set_visual_overlay_enabled(True) + native_window = args.live and sim.open_window() + if not args.live: + encoder = subprocess.Popen( + [ + "ffmpeg", + "-y", + "-hide_banner", + "-loglevel", + "error", + "-f", + "rawvideo", + "-pixel_format", + "rgb24", + "-video_size", + "960x720", + "-framerate", + str(args.fps), + "-i", + "pipe:0", + "-an", + "-c:v", + "libx264", + "-threads", + "4", + "-preset", + "fast", + "-crf", + "18", + "-pix_fmt", + "yuv420p", + "-movflags", + "+faststart", + str(args.output / "video.mp4"), + ], + stdin=subprocess.PIPE, + ) + started = time.monotonic() + frames = round(args.seconds * args.fps) + rendered_frames = 0 + for frame in range(frames): + if args.live and time.monotonic() - started >= args.seconds: + break + index = min(round(frame / args.fps / dt), len(poses) - 1) + if args.live: + index = int((time.monotonic() - started) / dt) % len(poses) + for k, (item, selected) in enumerate(items): + row = poses[index, k, selected].copy() + row[:, :3] += offsets[k] + item.visualize_link_poses(row[None]) + if objects: + pose = np.asarray(data["objects"][index, k]).copy() + pose[:3] += offsets[k] + objects[k].set_local_pose(torch.as_tensor(pose[None])) + if "targets" in data: + targets.visualize(np.asarray(data["targets"][index]) + offsets) + if vector_fields: + root = poses[index, 0, root_index] + vectors = np.stack( + [data[field][index] for _, field in vector_fields] + ).copy() + vectors[:, 2] = 0 + vectors = Rotation.from_quat(root[3:]).apply(vectors) + angle = np.arctan2(vectors[:, 1], vectors[:, 0]) + origins = np.tile(root[:3] + offsets[0], (len(vector_fields), 1)) + origins[:, 2] += [0.2 + 0.1 * i for i, _ in vector_fields] + scales = np.tile([1.0, 0.35, 0.35], (len(vector_fields), 1)) + scales[:, 0] = np.maximum(0.02, np.linalg.norm(vectors, axis=-1)) + arrows.visualize( + origins, + Rotation.from_euler("z", angle[:, None]).as_quat(), + scales, + [i for i, _ in vector_fields], + ) + eye, center = camera_eyes[index], camera_targets[index] + camera.look_at(eye.tolist(), center.tolist(), [0, 0, 1]) + if native_window: + window = sim.get_world().get_windows() + if frame == 0: + window.set_look_at(eye.tolist(), center.tolist(), [0, 0, 1]) + else: + window.translate_camera_orbit(center - previous_center) + previous_center = center.copy() + sim.get_world()._flush_render_updates() + capture_frame = not args.live or frame == 0 + with sim.render_frame( + force_visualization=True, capture_camera_images=False + ): + if capture_frame: + camera.render() + if capture_frame: + rgb = np.ascontiguousarray( + camera.get_rgb_map()[..., :3], dtype=np.uint8 + ) + if frame in (0, frames // 2, frames - 1): + Image.fromarray(rgb).save(args.output / f"frame-{frame:04}.png") + if frame == frames // 2: + Image.fromarray(rgb).save(args.output / "poster.png") + if encoder is not None: + encoder.stdin.write(rgb.tobytes()) + if args.live and frame == 0: + # Start the viewing period after the first complete frame. + started = time.monotonic() + rendered_frames += 1 + if frame % args.fps == 0: + print(f"frame {frame}/{frames}", flush=True) + if args.live: + time.sleep( + max(0.0, started + (frame + 1) / args.fps - time.monotonic()) + ) + if encoder is not None: + encoder.stdin.close() + if encoder.wait() != 0: + raise RuntimeError("Video encoder failed") + np.savez( + args.output / "camera-poses.npz", + eye=camera_eyes, + target=camera_targets, + dt=dt, + mode=args.camera_mode, + ) + (args.output / "capture.json").write_text( + json.dumps( + { + "camera_mode": args.camera_mode, + "camera_eye_z_range": float(np.ptp(camera_eyes[:, 2])), + "camera_target_z_range": float(np.ptp(camera_targets[:, 2])), + "camera_offset_range": np.ptp( + camera_eyes - camera_targets, axis=0 + ).tolist(), + "asset": args.asset, + "trajectory": str(args.trajectory), + "layout": args.layout, + "spacing": args.spacing, + "opacity": args.opacity, + "original_first": args.original_first, + "frames": rendered_frames, + "fps": args.fps, + "seconds": ( + time.monotonic() - started + if args.live + else rendered_frames / args.fps + ), + "live": args.live, + "physics_steps": sim._visualization_sim_step, + "links": len(names), + "trajectories": count, + "elapsed_seconds": time.monotonic() - started, + }, + indent=2, + ) + ) + finally: + if args.live and sim.is_window_opened: + sim.close_window() + if encoder is not None and encoder.poll() is None: + encoder.kill() + encoder.wait() + for item, _ in items: + item.remove() + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +if __name__ == "__main__": + main() diff --git a/examples/sim/robot_markers.py b/examples/sim/robot_markers.py new file mode 100644 index 0000000000..9abe7248d8 --- /dev/null +++ b/examples/sim/robot_markers.py @@ -0,0 +1,227 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Animate multiple render-only robot references from one URDF asset. + +The default asset is Franka Panda. Use --asset PATH to select another URDF. +Use --layers 4 --mode asset --opacity 0.4. Add --viser for +browser visualization, or --headless --output-dir PATH to save native frames. +The sinusoidal reference is generated by this example; RobotMarkers only +performs FK and visual placement. Physics does not advance. +""" + +from __future__ import annotations + +import argparse +from pathlib import Path + +from embodichain.cli.sim import add_sim_args_to_parser + + +def build_parser() -> argparse.ArgumentParser: + """Create the demo CLI without initializing the simulator.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--asset", type=Path, help="Robot URDF path; defaults to the Panda dataset." + ) + parser.add_argument( + "--layers", type=int, default=1, help="References per environment." + ) + parser.add_argument("--mode", choices=("asset", "override"), default="override") + parser.add_argument("--opacity", type=float, default=0.4) + parser.add_argument("--root-height", type=float, default=0.85) + parser.add_argument("--steps", type=int, default=300) + parser.add_argument( + "--warmup", + type=int, + default=5, + help="Initial frames excluded from timing statistics.", + ) + parser.add_argument("--fps", type=float, default=30.0) + parser.add_argument("--output-dir", type=Path) + parser.add_argument("--resolution", type=int, nargs=2, default=(640, 480)) + add_sim_args_to_parser(parser) + return parser + + +if __name__ == "__main__": + _cli_args = build_parser().parse_args() + +import json +import time +import numpy as np + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import DenoisingCfg, RenderCfg, physics_cfg_for_backend +from embodichain.lab.visualization import visualization_cfg_from_args +from embodichain.lab.visualization.markers import ( + RobotMarkersAppearanceCfg, + RobotMarkers, + RobotMarkersCfg, +) + + +def _run(sim: SimulationManager, args: argparse.Namespace) -> dict: + from dexsim import spawn + from PIL import Image + + sim.prepare() + sim.set_ground_plane_visibility(False) + offsets = sim.arena_offsets.detach().cpu().numpy() + center = (offsets.min(axis=0) + offsets.max(axis=0)) * 0.5 + center[2] += 0.8 + distance = 2.5 + float(np.linalg.norm(np.ptp(offsets[:, :2], axis=0))) * 1.5 + eye = center + np.array([0.6, -0.8, 0.28]) * distance + if not args.headless and sim.open_window(): + sim.get_world().get_windows().set_look_at( + eye, center, np.array([0.0, 0.0, 1.0]) + ) + camera = None + if args.output_dir is not None: + args.output_dir.mkdir(parents=True, exist_ok=True) + camera = sim._spawn_scene.builder.result.add_camera( + spawn.CameraDesc( + name="reference_preview", resolution=tuple(args.resolution) + ) + ) + camera.open_camera() + camera.look_at(eye.tolist(), center.tolist(), [0, 0, 1]) + camera.set_visual_overlay_enabled(True) + + started = time.perf_counter() + ghost = RobotMarkers( + RobotMarkersCfg( + name="reference", + asset_path=str(args.asset), + coordinate_space="env", + appearance=RobotMarkersAppearanceCfg(mode=args.mode, opacity=args.opacity), + ), + sim, + ) + q = np.zeros((sim.num_envs, args.layers, len(ghost.joint_names)), np.float32) + roots = np.tile(np.eye(4), (sim.num_envs, args.layers, 1, 1)) + roots[..., 2, 3] = args.root_height + roots[..., 0, 3] = np.arange(args.layers) * 0.12 + ghost.visualize_qpos(q, root_poses=roots) + sim.get_world()._flush_render_updates() + create_seconds = time.perf_counter() - started + update_samples, render_samples = [], [] + next_frame = time.perf_counter() + for step in range(args.warmup + args.steps): + phase = step * 0.04 + np.arange(args.layers)[None, :, None] * 0.3 + reference = q + 0.08 * np.sin(phase + np.arange(q.shape[-1]) * 0.4) + started = time.perf_counter() + ghost.visualize_qpos(reference, root_poses=roots) + sim.get_world()._flush_render_updates() + update_seconds = time.perf_counter() - started + started = time.perf_counter() + with sim.render_frame(force_visualization=True, capture_camera_images=False): + if camera is not None: + camera.render() + render_seconds = time.perf_counter() - started + if step >= args.warmup: + update_samples.append(update_seconds) + render_samples.append(render_seconds) + if camera is not None and step in {args.warmup, args.warmup + args.steps - 1}: + Image.fromarray(np.array(camera.get_rgb_map(), copy=True)).save( + args.output_dir / f"{args.mode}-{step:04d}.png" + ) + if args.fps > 0: + next_frame += 1 / args.fps + time.sleep(max(0, next_frame - time.perf_counter())) + result = dict( + environments=sim.num_envs, + layers=args.layers, + robots=sim.num_envs * args.layers, + visuals_per_robot=ghost.num_visuals, + joints=list(ghost.joint_names), + renderer=args.renderer, + appearance=args.mode, + opacity=args.opacity, + resolution=list(args.resolution), + render_scope=( + "synchronous_camera" if camera is not None else "frame_publication" + ), + warmup=args.warmup, + repeats=args.steps, + create_seconds=create_seconds, + # Pose publication and camera rendering are timed separately. + update_seconds=update_samples, + render_seconds=render_samples, + median_update_ms=float(np.median(update_samples) * 1000), + median_render_ms=float(np.median(render_samples) * 1000), + physics_steps=sim._visualization_sim_step, + ) + ghost.remove() + return result + + +def main(args: argparse.Namespace | None = None) -> None: + """Run the bounded reference visualization and write optional measurements. + + Args: + args: Parsed CLI options, or None to read the command line. + """ + args = build_parser().parse_args() if args is None else args + if args.layers < 1 or args.steps < 1 or args.num_envs < 1: + raise ValueError("layers, steps and num_envs must be positive.") + if args.warmup < 0: + raise ValueError("warmup must be nonnegative.") + if not np.isfinite(args.fps) or args.fps < 0: + raise ValueError("fps must be finite and nonnegative; zero disables pacing.") + if not np.isfinite(args.root_height): + raise ValueError("root-height must be finite.") + if not np.isfinite(args.opacity) or not 0 <= args.opacity <= 1: + raise ValueError("opacity must be finite and in [0, 1].") + if min(args.resolution) < 1: + raise ValueError("resolution dimensions must be positive.") + if args.output_dir is not None and args.renderer == "no-render": + raise ValueError("Native frame capture requires a native renderer.") + if args.asset is None: + from embodichain.data import get_data_path + + args.asset = Path(get_data_path("Franka/Panda/Panda.urdf")) + sim = SimulationManager( + SimulationManagerCfg( + headless=args.headless, + device=args.device, + num_envs=args.num_envs, + arena_space=args.arena_space, + enable_entity_gizmo=False, + startup_summary="off", + width=args.resolution[0], + height=args.resolution[1], + render_cfg=RenderCfg( + renderer=args.renderer, + denoising=DenoisingCfg(window="off", offscreen="off"), + ), + physics_cfg=physics_cfg_for_backend(args.physics), + visualization=visualization_cfg_from_args(args), + ) + ) + try: + result = _run(sim, args) + if args.output_dir is not None: + (args.output_dir / "measurements.json").write_text( + json.dumps(result, indent=2) + "\n" + ) + print(json.dumps(result, indent=2)) + finally: + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +if __name__ == "__main__": + main(_cli_args) diff --git a/examples/sim/visualization_markers.py b/examples/sim/visualization_markers.py new file mode 100644 index 0000000000..848a5e2fd1 --- /dev/null +++ b/examples/sim/visualization_markers.py @@ -0,0 +1,197 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Animate markers in every sub-environment and update selected environments. + +Run with --viser --num_envs 4 to compare selected environment visibility and +clear/repopulation against unaffected environments. The world origin is drawn +once using a separate world-scope group. +""" + +from __future__ import annotations + +import argparse + +from embodichain.cli.sim import add_sim_args_to_parser + + +def build_parser() -> argparse.ArgumentParser: + """Build the command-line parser without initializing simulation.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--steps", + type=int, + default=900, + help="Number of bounded animation steps to run.", + ) + parser.add_argument( + "--fps", + type=float, + default=30.0, + help="Maximum host update rate used to pace the demonstration.", + ) + add_sim_args_to_parser(parser) + return parser + + +if __name__ == "__main__": + _cli_args = build_parser().parse_args() + + +import math +import time + +import numpy as np + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import RenderCfg, physics_cfg_for_backend +from embodichain.lab.visualization import visualization_cfg_from_args +from embodichain.lab.visualization.markers import ( + MarkerGroupCfg, + MarkerPrototypeCfg, +) + +_SHAPES = ("box", "sphere", "cylinder", "capsule", "cone", "arrow", "frame") +_COLORS = ( + (0.95, 0.20, 0.20, 0.85), + (0.95, 0.55, 0.10, 0.85), + (0.95, 0.90, 0.15, 0.85), + (0.25, 0.80, 0.30, 0.85), + (0.15, 0.65, 0.95, 0.85), + (0.35, 0.30, 0.95, 0.85), + (0.85, 0.25, 0.80, 0.85), +) + + +def _translations(phase: float) -> np.ndarray: + """Return seven animated positions in one horizontal row.""" + x = np.linspace(-1.5, 1.5, len(_SHAPES), dtype=np.float32) + z = 0.45 + 0.12 * np.sin(phase + np.arange(len(_SHAPES))) + return np.column_stack((x, np.zeros_like(x), z)).astype(np.float32) + + +def main(args: argparse.Namespace | None = None) -> None: + """Run the bounded marker lifecycle demonstration.""" + if args is None: + args = build_parser().parse_args() + if args.steps < 1: + raise ValueError("--steps must be at least 1.") + if not math.isfinite(args.fps) or args.fps <= 0.0: + raise ValueError("--fps must be a positive finite number.") + + sim = SimulationManager( + SimulationManagerCfg( + headless=args.headless, + device=args.device, + num_envs=args.num_envs, + arena_space=args.arena_space, + render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=physics_cfg_for_backend(args.physics), + visualization=visualization_cfg_from_args(args), + ) + ) + try: + marker = sim.add_marker_group( + MarkerGroupCfg( + name="built_in_markers", + prototypes={ + shape: MarkerPrototypeCfg( + shape=shape, + scale=(0.20, 0.20, 0.20), + color=color, + ) + for shape, color in zip(_SHAPES, _COLORS, strict=True) + }, + ) + ) + sim.prepare() + if sim.is_use_gpu_physics: + sim.init_gpu_physics() + if not args.headless: + sim.open_window() + + world_frame = sim.add_marker_group( + MarkerGroupCfg( + name="world_origin", + scope="world", + prototypes={ + "frame": MarkerPrototypeCfg(shape="frame", scale=(0.4, 0.4, 0.4)), + }, + ) + ) + world_frame.update(translations=[[0, 0, 0.02]]) + prototype_indices = np.broadcast_to( + np.arange(len(_SHAPES), dtype=np.int64), (sim.num_envs, len(_SHAPES)) + ).copy() + colors = np.broadcast_to( + np.asarray(_COLORS, dtype=np.float32), (sim.num_envs, len(_SHAPES), 4) + ).copy() + + def positions(phase: float) -> np.ndarray: + return np.stack( + [_translations(phase + env * 0.25) for env in range(sim.num_envs)] + ) + + selected_envs = np.arange(0, sim.num_envs, 2) + other_envs = np.setdiff1d(np.arange(sim.num_envs), selected_envs) + marker.update( + translations=positions(0.0), + prototype_indices=prototype_indices, + colors=colors, + ) + lifecycle_duration = max(1, args.steps // 10) + hide_step = args.steps // 4 + show_step = min(hide_step + lifecycle_duration, args.steps - 1) + clear_step = args.steps // 2 + repopulate_step = min(clear_step + lifecycle_duration, args.steps - 1) + populated = True + frame_period = 1.0 / args.fps + next_frame = time.perf_counter() + for step in range(args.steps): + phase = step * 2.0 * math.pi / max(args.steps, 1) + if step == hide_step: + marker.set_visibility(False, env_ids=selected_envs) + elif step == show_step: + marker.set_visibility(True, env_ids=selected_envs) + if step == clear_step: + marker.clear(env_ids=selected_envs) + populated = False + elif step == repopulate_step: + marker.update( + translations=positions(phase)[selected_envs], + prototype_indices=prototype_indices[selected_envs], + colors=colors[selected_envs], + env_ids=selected_envs, + ) + populated = True + elif populated: + marker.update(translations=positions(phase)) + if not populated and len(other_envs): + marker.update( + translations=positions(phase)[other_envs], env_ids=other_envs + ) + + # Marker updates are render-only; physics advances explicitly here. + sim.update(step=1) + next_frame += frame_period + time.sleep(max(0.0, next_frame - time.perf_counter())) + marker.remove() + world_frame.remove() + finally: + sim.destroy() + + +if __name__ == "__main__": + main(_cli_args) diff --git a/scripts/benchmark/visualization/markers.py b/scripts/benchmark/visualization/markers.py new file mode 100644 index 0000000000..b829507722 --- /dev/null +++ b/scripts/benchmark/visualization/markers.py @@ -0,0 +1,244 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Measure marker batch state updates and detached CPU snapshot generation. + +Run: python -m scripts.benchmark.visualization.markers --repeats 5 +This does not measure native GPU rendering, Viser encoding/network transport, +or GPU instancing. Array bytes count distinct NumPy snapshot arrays, including +shared geometry once. Python objects and native/GPU allocations are excluded. +""" + +from __future__ import annotations + +import argparse +from collections.abc import Callable +from datetime import datetime, timezone +from pathlib import Path +import time + +import numpy as np +import psutil +import torch + +from embodichain.lab.visualization.markers import ( + MarkerGroup, + MarkerGroupCfg, + MarkerPrototypeCfg, +) + + +def _measure(operation: Callable[[], object], repeats: int) -> dict[str, float]: + """Measure a warmed CPU operation and process/Torch memory deltas.""" + operation() + process = psutil.Process() + gpu = torch.cuda.is_available() + if gpu: + torch.cuda.reset_peak_memory_stats() + cpu_before = process.memory_info().rss + gpu_before = torch.cuda.memory_allocated() if gpu else 0 + samples = [] + for _ in range(repeats): + start = time.perf_counter() + operation() + samples.append((time.perf_counter() - start) * 1000) + return dict( + cost_time_ms=float(np.median(samples)), + p95_ms=float(np.percentile(samples, 95)), + cpu_delta_mb=(process.memory_info().rss - cpu_before) / 2**20, + gpu_delta_mb=((torch.cuda.memory_allocated() if gpu else 0) - gpu_before) + / 2**20, + peak_gpu_mb=(torch.cuda.max_memory_allocated() if gpu else 0) / 2**20, + ) + + +def _table(headers: list[str], rows: list[dict[str, object]]) -> str: + """Write a compact Markdown table with comparable numeric precision.""" + + def display(value: object) -> str: + return f"{value:.4f}" if isinstance(value, float) else str(value) + + return "\n".join( + [ + "| " + " | ".join(headers) + " |", + "| " + " | ".join("---" for _ in headers) + " |", + *( + "| " + " | ".join(display(row[key]) for key in headers) + " |" + for row in rows + ), + ] + ) + + +def run_all_benchmarks(args: argparse.Namespace) -> Path: + """Measure environment/instance scaling and write one report.""" + performance, metrics = [], [] + for environments in args.envs: + for markers in args.markers: + origins = np.zeros((environments, 3), dtype=np.float32) + origins[:, 0] = np.arange(environments) * 5 + group = MarkerGroup( + MarkerGroupCfg( + name="benchmark", prototypes={"box": MarkerPrototypeCfg()} + ), + num_envs=environments, + origins=origins, + ) + positions = np.zeros((environments, markers, 3), dtype=np.float32) + group.update(translations=positions) + selected = np.arange(0, environments, 4) + moved = positions[selected].copy() + moved[..., 2] = 1 + operations = { + "full_update": lambda: group.update(translations=positions), + "selected_update": lambda: group.update( + translations=moved, env_ids=selected + ), + "snapshot": group.snapshot, + } + for name, operation in operations.items(): + group.update(translations=positions) + measurement = _measure(operation, args.repeats) + snapshots = group.snapshot() + valid = len(snapshots) == environments * markers + for snapshot in snapshots: + env = snapshot.env_id + expected_z = ( + 1.0 if name == "selected_update" and env in selected else 0.0 + ) + valid = valid and bool( + np.allclose( + snapshot.position, origins[env] + [0, 0, expected_z] + ) + ) + if not valid: + raise AssertionError(f"Incorrect marker snapshots for {name}") + arrays = { + id(value): value + for item in snapshots + for value in ( + item.vertices, + item.faces, + item.position, + item.wxyz, + item.scale, + ) + } + size = sum(value.nbytes for value in arrays.values()) + case = dict(envs=environments, markers_per_env=markers, impl=name) + performance.append({**case, **measurement}) + metrics.append( + { + **case, + "success_rate": 1.0, + "logical_instances": len(snapshots), + "selected_envs": ( + len(selected) if name == "selected_update" else environments + ), + "snapshot_array_bytes": size, + } + ) + print( + f"envs={environments:3d} markers={markers:4d} {name:16s}: " + f"{measurement['cost_time_ms']:9.3f} ms | CPU delta " + f"{measurement['cpu_delta_mb']:+.2f} MB | " + f"Torch GPU delta {measurement['gpu_delta_mb']:+.2f} MB" + ) + group.remove() + leaderboard = [] + for rank, operation in enumerate(("full_update", "selected_update", "snapshot"), 1): + rows = [row for row in performance if row["impl"] == operation] + leaderboard.append( + dict( + rank=rank, + algorithm=operation, + success_rate=1.0, + median_cost_time_ms=float( + np.median([row["cost_time_ms"] for row in rows]) + ), + ) + ) + stamp = datetime.now(timezone.utc).strftime("%Y%m%d_%H%M%S") + report = args.output or Path(f"outputs/benchmarks/markers_{stamp}.md") + report.parent.mkdir(parents=True, exist_ok=True) + report.write_text( + "\n\n".join( + [ + "# Marker CPU batch benchmark", + f"UTC: {stamp}; repetitions: {args.repeats}; median and p95 of warmed calls.", + "Measures CPU state validation/update and detached mesh snapshots only. " + "No native rendering or Viser client is active. GPU columns cover Torch " + "allocations only (this workload allocates no GPU tensors), not native renderer VRAM. " + "RSS deltas are allocator-sensitive, not peak live allocation. " + "Snapshot array bytes count distinct NumPy arrays, including shared " + "geometry once; metadata, encoding and wire overhead are excluded. " + "The three operations do different work; leaderboard ranks are tied by " + "correctness and are not a speed ranking of interchangeable algorithms.", + "## Time & Memory", + _table( + [ + "envs", + "markers_per_env", + "impl", + "cost_time_ms", + "p95_ms", + "cpu_delta_mb", + "gpu_delta_mb", + "peak_gpu_mb", + ], + performance, + ), + "## Success & Other Metrics", + _table( + [ + "envs", + "markers_per_env", + "impl", + "success_rate", + "logical_instances", + "selected_envs", + "snapshot_array_bytes", + ], + metrics, + ), + "## Leaderboard", + _table( + ["rank", "algorithm", "success_rate", "median_cost_time_ms"], + leaderboard, + ), + ] + ) + + "\n" + ) + print(f"Markdown report saved: {report}") + return report + + +def main() -> None: + """Parse positive benchmark sizes and repetitions.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--envs", type=int, nargs="+", default=[1, 16, 64]) + parser.add_argument("--markers", type=int, nargs="+", default=[1, 16, 128]) + parser.add_argument("--repeats", type=int, default=5) + parser.add_argument("--output", type=Path) + args = parser.parse_args() + if args.repeats < 1 or any(size < 1 for size in args.envs + args.markers): + parser.error("sizes and repeats must be positive") + run_all_benchmarks(args) + + +if __name__ == "__main__": + main() diff --git a/tests/sim/sensors/test_camera.py b/tests/sim/sensors/test_camera.py index 5ebc0e706e..3c25172894 100644 --- a/tests/sim/sensors/test_camera.py +++ b/tests/sim/sensors/test_camera.py @@ -597,29 +597,45 @@ def test_batched_camera_depth_matches_known_geometry(stereo): @pytest.mark.no_sim -def test_ground_visibility_preflight_and_selected_reset() -> None: +@pytest.mark.parametrize("stereo", [False, True]) +@pytest.mark.parametrize("include_overlays", [False, True]) +def test_ground_visibility_preflight_and_selected_reset( + stereo, include_overlays +) -> None: """Invalid batches cannot partially change camera policy or enqueue writes.""" - camera = object.__new__(Camera) + camera = object.__new__(StereoCamera if stereo else Camera) owner = MagicMock() owner._GROUND_RENDER_CATEGORY = 1 camera._owner_ref = lambda: owner camera._is_destroyed = False camera.num_instances = 4 - camera._entities = [MagicMock() for _ in range(4)] + views = [[MagicMock() for _ in range(2 if stereo else 1)] for _ in range(4)] + camera._entities = [ + PairCameraView(*pair, np.eye(4)) if stereo else pair[0] for pair in views + ] camera._ground_plane_visibility = [True] * 4 camera._capture_metadata = None - camera.cfg = CameraCfg() + camera.cfg = CameraCfg(include_visual_overlays=include_overlays) camera._reset_pose = MagicMock() for ids in ([0, 4], [0, 0], [1.5]): with pytest.raises(ValueError): camera.set_ground_plane_visibility(False, ids) - for entity in camera._entities: - entity.set_excluded_render_categories.assert_not_called() + for pair in views: + for view in pair: + view.set_excluded_render_categories.assert_not_called() camera.set_ground_plane_visibility(False, torch.tensor([1, 3])) assert camera._ground_plane_visibility == [True, False, True, False] camera.reset([1]) assert camera._ground_plane_visibility == [True, True, True, False] - assert camera._entities[3].set_excluded_render_categories.call_args.args == (1,) + # Ground uses bit 0; default sensor images must also exclude marker bit 31. + marker_mask = 0 if include_overlays else 1 << 31 + for view in views[1]: + assert view.set_excluded_render_categories.call_args.args == (marker_mask,) + for view in views[3]: + assert view.set_excluded_render_categories.call_args.args == (1 | marker_mask,) + for index in (0, 2): + for view in views[index]: + view.set_excluded_render_categories.assert_not_called() camera._is_destroyed = True with pytest.raises(RuntimeError, match="destroyed"): camera.set_ground_plane_visibility(True) diff --git a/tests/sim/test_marker_rendering.py b/tests/sim/test_marker_rendering.py new file mode 100644 index 0000000000..b341cc973b --- /dev/null +++ b/tests/sim/test_marker_rendering.py @@ -0,0 +1,510 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import numpy as np +import pytest + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import RenderCfg +from embodichain.lab.visualization.markers import MarkerGroupCfg, MarkerPrototypeCfg + +pytestmark = [pytest.mark.gpu, pytest.mark.requires_sim] + + +def _require_render_actor_capability(): + from dexsim import engine, spawn + from dexsim.scene import Scene + + required = ( + (Scene, "add_mesh_object"), + (Scene, "remove_mesh_object"), + (getattr(spawn, "RenderDesc", None), "render_mode"), + (getattr(spawn, "MaterialDesc", None), "unlit"), + (getattr(spawn, "MaterialDesc", None), "alpha_mode"), + (getattr(spawn, "MaterialDesc", None), "depth_write"), + (getattr(engine, "RenderBody", None), "set_raytrace_visible"), + (getattr(engine, "RenderBody", None), "set_pickable"), + (getattr(engine, "RenderBody", None), "build"), + (getattr(engine, "MaterialInst", None), "set_unlit"), + (getattr(engine, "MaterialInst", None), "set_alpha_mode"), + (getattr(engine, "AlphaMode", None), "BLEND"), + ) + if any(not hasattr(owner, name) for owner, name in required): + pytest.skip("Requires DexSim Scene mesh lifecycle and overlay capabilities") + + +def test_native_marker_removal_preserves_retained_material(): + _require_render_actor_capability() + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + sim_device="cpu", + enable_entity_gizmo=False, + render_cfg=RenderCfg(renderer="hybrid"), + startup_summary="off", + ) + ) + try: + # Keep borrowed native references inside the helper's lifetime, before + # the World is destroyed. + _exercise_repeated_marker_lifecycle(sim) + finally: + sim.destroy(exit_process=False) + del sim + SimulationManager.flush_cleanup_queue() + + +def _exercise_repeated_marker_lifecycle(sim): + scene = sim._spawn_scene.builder.result + arena = sim.get_env() + retained_material = None + try: + baseline_paths = set(scene.handles) + baseline_count = arena.get_actor_num() + revision = sim._prepared_spawn_topology_revision + group = sim.add_marker_group( + MarkerGroupCfg( + name="material_collection", + prototypes={ + "box": MarkerPrototypeCfg(shape="box"), + "sphere": MarkerPrototypeCfg(shape="sphere"), + }, + ) + ) + for _ in range(3): + group.update(translations=[[0, 0, 1], [1, 0, 1]], prototype_indices=[0, 0]) + handles = { + path: scene.handles[path] + for path in set(scene.handles) - baseline_paths + } + assert len(handles) == 2 + assert arena.get_actor_num() == baseline_count + 2 + retained_material = next(iter(handles.values())).get_material() + color = [0.25, 0.5, 0.75, 0.5] + group.update( + translations=[[0, 1, 1], [1, 1, 1]], + scales=[[2, 2, 2], [2, 2, 2]], + colors=[color, color], + ) + group.set_visibility(False) + assert set(scene.handles) == baseline_paths | handles.keys() + for path, handle in handles.items(): + assert scene.handles[path] is handle + assert handle.is_valid + assert not handle.is_visible() + np.testing.assert_allclose(handle.get_world_pose()[:3, 3][1:], [1, 1]) + np.testing.assert_allclose(handle.get_scale(), [2, 2, 2]) + np.testing.assert_allclose(retained_material.get_base_color(), color[:3]) + + group.update(prototype_indices=[1, 1]) + replacements = [ + scene.handles[path] for path in set(scene.handles) - baseline_paths + ] + assert len(replacements) == 2 + assert arena.get_actor_num() == baseline_count + 2 + assert all( + not handle.is_valid and handle.native() is None + for handle in handles.values() + ) + assert all(handle.is_valid for handle in replacements) + assert set(scene.handles).isdisjoint(handles) + + group.clear() + assert arena.get_actor_num() == baseline_count + assert set(scene.handles) == baseline_paths + assert all( + not handle.is_valid and handle.native() is None + for handle in replacements + ) + # A separately retained material remains usable after marker removal. + retained_material.set_base_color([1, 0, 0, 1]) + np.testing.assert_allclose(retained_material.get_base_color(), [1, 0, 0]) + del retained_material + group.clear() + assert arena.get_actor_num() == baseline_count + group.remove() + group.remove() + assert sim.get_marker_overlays() == () + assert sim._prepared_spawn_topology_revision == revision + assert sim._visualization_sim_step == 0 + + finally: + # Pytest retains failed frames; drop native wrappers before World teardown. + retained_material = None + arena = None + + +def test_marker_group_native_rendering_and_sensor_isolation(): + _require_render_actor_capability() + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + sim_device="cpu", + enable_entity_gizmo=False, + render_cfg=RenderCfg(renderer="hybrid"), + startup_summary="off", + ) + ) + try: + _exercise_marker_rendering(sim) + finally: + sim.destroy(exit_process=False) + del sim + SimulationManager.flush_cleanup_queue() + + +def _exercise_marker_rendering(sim): + import dexsim + + arena = sim.get_env() + world = sim.get_world() + camera_group = world.create_camera_group([96, 96], 1, True) + camera = arena.create_camera( + "markers_contract", 96, 96, True, dexsim.render.ViewFlags.ALL, camera_group + ) + camera.open_camera() + camera.look_at([0, 0, 3], [0, 0, 1], [0, 1, 0]) + + sim.prepare() + + def render(): + sim.sync_render_state() + camera.render() + return np.array(camera.get_rgb_map()[..., :3], copy=True) + + baseline = render() + count = arena.get_actor_num() + revision = sim._prepared_spawn_topology_revision + step = sim._visualization_sim_step + group = sim.add_marker_group( + MarkerGroupCfg( + name="native", + prototypes={ + "box": MarkerPrototypeCfg( + shape="box", scale=(0.5, 0.5, 0.5), color=(1, 0, 0, 1) + ), + "sphere": MarkerPrototypeCfg( + shape="sphere", scale=(0.5, 0.5, 0.5), color=(0, 1, 0, 0.5) + ), + }, + ) + ) + group.update(translations=[[0, 0, 1]]) + # The lit scene has temporal RT/DLSS noise. Detect the marker's saturated + # color instead of incorrectly requiring identical consecutive beauty frames. + sensor = render().astype(float) + background = baseline.astype(float) + red_mask = lambda image: (image[..., 0] > image[..., 1] + 50) + assert ( + np.count_nonzero(red_mask(sensor)) <= np.count_nonzero(red_mask(background)) + 2 + ) + camera_group.set_visual_overlay_enabled(True) + red = render() + assert ( + np.count_nonzero(red[..., 0].astype(float) > red[..., 1].astype(float) + 50) + > 20 + ) + group.update(prototype_indices=[1]) + green = render() + assert ( + np.count_nonzero(green[..., 1].astype(float) > green[..., 0].astype(float) + 20) + > 20 + ) + group.update(translations=[[2, 0, 1]]) + moved = render().astype(float) + assert np.count_nonzero(moved[..., 1] > moved[..., 0] + 20) < 5 + for offset in (0.1, 0.2, 0.3): + group.update(translations=[[offset, 0, 1]]) + assert arena.get_actor_num() == count + 1 + group.clear() + assert arena.get_actor_num() == count + assert sim._prepared_spawn_topology_revision == revision + assert sim._visualization_sim_step == step + group.remove() + assert sim.get_marker_overlays() == () + + +def test_environment_batched_native_attachment_follows_bound_articulation(tmp_path): + from embodichain.lab.sim.cfg import ArticulationCfg + + _require_render_actor_capability() + source = tmp_path / "marker_parent.urdf" + source.write_text(""" + + + + """) + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + sim_device="cpu", + num_envs=2, + enable_entity_gizmo=False, + render_cfg=RenderCfg(renderer="hybrid"), + startup_summary="off", + ) + ) + try: + target = sim.add_articulation( + ArticulationCfg(uid="parent", fpath=str(source), init_pos=(0.0, 0.0, 2.0)) + ) + group = sim.add_marker_group( + MarkerGroupCfg( + name="batched", + prototypes={"box": MarkerPrototypeCfg(scale=(0.1, 0.1, 0.1))}, + ) + ) + group.update(translations=[[[1, 0, 0]], [[2, 0, 0]]]) + with pytest.raises(RuntimeError, match="prepared"): + group.attach("parent") + sim.prepare() + group.attach("parent", env_ids=[0]) + group.attach("parent", link_name="tool", env_ids=[1]) + origins = sim.arena_offsets.cpu().numpy() + np.testing.assert_allclose( + group.snapshot()[0].position, origins[0] + [1, 0, 2], atol=1e-5 + ) + np.testing.assert_allclose( + group.snapshot()[1].position, origins[1] + [2, 0, 3], atol=1e-5 + ) + count = sim.get_env().get_actor_num() + step = sim._visualization_sim_step + # Selected environment growth must leave the first environment untouched. + group.update(env_ids=[1], translations=[[3, 0, 0], [4, 0, 0]]) + assert group.counts == (1, 2) + assert sim.get_env().get_actor_num() == count + 1 + group.set_visibility(False, env_ids=[0]) + assert [m.visible for m in group.snapshot()] == [False, True, True] + group.detach(env_ids=[1]) + np.testing.assert_allclose( + group.snapshot()[1].position, origins[1] + [3, 0, 3], atol=1e-5 + ) + sim.sync_render_state() + assert sim._visualization_sim_step == step + assert sim.remove_asset("parent") + sim.sync_render_state() + np.testing.assert_allclose( + group.snapshot()[0].position, origins[0] + [1, 0, 2], atol=1e-5 + ) + group.clear(env_ids=[1]) + assert group.counts == (1, 0) + finally: + sim.destroy(exit_process=False) + del sim + SimulationManager.flush_cleanup_queue() + + +def test_native_environment_batches_selected_mutations_without_preparation(): + _require_render_actor_capability() + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + sim_device="cpu", + num_envs=2, + enable_entity_gizmo=False, + render_cfg=RenderCfg(renderer="hybrid"), + startup_summary="off", + ) + ) + try: + arena = sim.get_env() + count = arena.get_actor_num() + revision = sim._prepared_spawn_topology_revision + group = sim.add_marker_group( + MarkerGroupCfg( + name="native_batches", + prototypes={"box": MarkerPrototypeCfg(scale=(0.1, 0.1, 0.1))}, + ) + ) + group.update( + translations=[[[1, 0, 1]], [[2, 0, 1]]], + colors=[[[1, 0, 0, 0.5]], [[0, 1, 0, 1]]], + ) + assert arena.get_actor_num() == count + 2 + origins = sim.arena_offsets.cpu().numpy() + np.testing.assert_allclose(group.snapshot()[0].position, origins[0] + [1, 0, 1]) + np.testing.assert_allclose(group.snapshot()[1].position, origins[1] + [2, 0, 1]) + group.update(env_ids=[1], translations=[[3, 0, 1], [4, 0, 1]]) + assert group.counts == (1, 2) + assert arena.get_actor_num() == count + 3 + group.set_visibility(False, env_ids=[0]) + assert [m.visible for m in group.snapshot()] == [False, True, True] + group.clear(env_ids=[1]) + assert arena.get_actor_num() == count + 1 + assert group.counts == (1, 0) + assert sim._prepared_spawn_topology_revision == revision + assert sim._visualization_sim_step == 0 + group.remove() + assert arena.get_actor_num() == count + finally: + sim.destroy(exit_process=False) + del sim + SimulationManager.flush_cleanup_queue() + + +@pytest.mark.parametrize("stereo", [False, True]) +def test_robot_markers_textures_opacity_and_material_switch( + tmp_path, request: pytest.FixtureRequest, stereo: bool +) -> None: + from PIL import Image + import trimesh + from embodichain.lab.sim.cfg import DenoisingCfg + + pixels = np.full((8, 8, 4), [255, 0, 0, 255], dtype=np.uint8) + pixels[:, 4:] = [0, 0, 255, 255] + pixels[4:, 4:] = [255, 255, 255, 255] + mesh = trimesh.Trimesh( + vertices=[[-0.6, -0.6, 0], [0.6, -0.6, 0], [0.6, 0.6, 0], [-0.6, 0.6, 0]], + faces=[[0, 1, 2], [0, 2, 3]], + process=False, + visual=trimesh.visual.TextureVisuals( + uv=[[0, 0], [1, 0], [1, 1], [0, 1]], + material=trimesh.visual.material.PBRMaterial( + baseColorTexture=Image.fromarray(pixels), + baseColorFactor=[255] * 4, + metallicFactor=0.0, + roughnessFactor=0.5, + ), + ), + ) + mesh.export(tmp_path / "mesh.glb") + path = tmp_path / "ghost.urdf" + path.write_text( + '' + '' + "" + ) + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + device="cpu", + enable_entity_gizmo=False, + render_cfg=RenderCfg( + renderer=request.config.getoption("--renderer"), + denoising=DenoisingCfg(window="off", offscreen="off"), + ), + startup_summary="off", + ) + ) + try: + _exercise_textured_ghost(sim, path, stereo) + finally: + sim.destroy(exit_process=False) + del sim + SimulationManager.flush_cleanup_queue() + + +def _exercise_textured_ghost(sim, path, stereo): + from embodichain.lab.sim.sensors import CameraCfg, StereoCameraCfg + from embodichain.lab.visualization.markers import ( + RobotMarkersAppearanceCfg, + RobotMarkers, + RobotMarkersCfg, + ) + + sim.prepare() + sim.set_ground_plane_visibility(False) + scene = sim._spawn_scene.builder.result + + def make_camera(uid, include_overlays): + intrinsics = (105.0, 105.0, 64.0, 64.0) + config_type = StereoCameraCfg if stereo else CameraCfg + return sim.add_sensor( + config_type( + uid=uid, + width=128, + height=128, + intrinsics=intrinsics, + extrinsics=CameraCfg.ExtrinsicsCfg( + eye=(0, 0, 3), target=(0, 0, 1), up=(0, 1, 0) + ), + include_visual_overlays=include_overlays, + **({"intrinsics_right": intrinsics} if stereo else {}), + ) + ) + + sensor = make_camera("sensor", False) + preview = make_camera("preview", True) + + def render(camera): + sim.get_world()._flush_render_updates() + camera.update() + data = camera.get_data() + keys = ("color", "color_right") if stereo else ("color",) + return np.stack([data[key][0, ..., :3].cpu().numpy() for key in keys]).astype( + float + ) + + def colored(image, channel): + others = [i for i in range(3) if i != channel] + return np.count_nonzero( + (image[..., channel] > 100) + & (image[..., channel] > image[..., others].max(-1) + 40), + axis=(1, 2), + ) + + baseline = render(sensor) + before = set(scene.handles) + ghost = RobotMarkers( + RobotMarkersCfg( + name="texture", + asset_path=str(path), + appearance=RobotMarkersAppearanceCfg(mode="asset", opacity=1), + ), + sim, + ) + poses = np.eye(4)[None, None] + poses[..., 2, 3] = 1 + ghost.visualize_link_poses(poses) + assert np.all(colored(render(sensor), 0) <= colored(baseline, 0) + 2) + opaque = render(preview) + assert np.all(colored(opaque, 0) > 100) and np.all(colored(opaque, 2) > 100) + for ground_visible in (False, True): + sensor.set_ground_plane_visibility(ground_visible) + preview.set_ground_plane_visibility(ground_visible) + assert np.all(colored(render(sensor), 0) <= colored(baseline, 0) + 2) + visible = render(preview) + assert np.all(colored(visible, 0) > 100) and np.all(colored(visible, 2) > 100) + ghost.set_appearance( + RobotMarkersAppearanceCfg(mode="override", color=(0, 1, 0), opacity=1) + ) + opaque_green = render(preview) + assert np.all(colored(opaque_green, 1) > 100) + assert np.all(colored(opaque_green, 0) == 0) and np.all( + colored(opaque_green, 2) == 0 + ) + ghost.set_appearance(RobotMarkersAppearanceCfg(mode="asset", opacity=1)) + restored = render(preview) + assert np.all(colored(restored, 0) > 100) and np.all(colored(restored, 2) > 100) + ghost.set_appearance(RobotMarkersAppearanceCfg(mode="asset", opacity=0)) + invisible = render(preview) + assert np.all(colored(invisible, 0) <= colored(baseline, 0) + 2) + assert np.all(colored(invisible, 2) <= colored(baseline, 2) + 2) + ghost.set_appearance( + RobotMarkersAppearanceCfg(mode="override", color=(0, 1, 0), opacity=0.6) + ) + green = render(preview) + assert np.all(colored(green, 1) > 100) and np.all(colored(green, 0) == 0) + ghost.set_appearance(RobotMarkersAppearanceCfg(mode="asset", opacity=0.6)) + translucent = render(preview) + assert np.all(colored(translucent, 0) > 100) and np.all( + colored(translucent, 2) > 100 + ) + assert np.all(colored(render(sensor), 0) <= colored(baseline, 0) + 2) + ghost.remove() + assert set(scene.handles) == before diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index a5665fa0c3..07a6ab7ce9 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -317,6 +317,9 @@ def thread_rt(self) -> FakeThreadRuntime: def update(self, physics_dt: float) -> None: self.physics_updates.append(physics_dt) + def _flush_render_updates(self) -> int: + return 0 + def enable_entity_gizmo(self, config: object | None = None) -> object: self.entity_gizmo_configs.append(config) if self.entity_gizmo is None: @@ -455,6 +458,7 @@ def _make_visualization_sim_manager() -> ( physics_dt=0.01, physics_cfg=DefaultPhysicsCfg(), visualization=SimpleNamespace(backend="viser"), + render_cfg=SimpleNamespace(renderer="hybrid"), ) sim.device = SimpleNamespace(type="cpu") sim.profiler = Profiler(None, torch.device("cpu")) @@ -2485,25 +2489,47 @@ def test_prepare_retries_render_state_sync_without_recommit() -> None: sim._world.update.assert_not_called() -def test_sync_render_state_publishes_current_state_without_stepping() -> None: - """Explicit publication must refresh render state without advancing physics.""" +@pytest.mark.parametrize("renderer", ["hybrid", "no-render"]) +def test_sync_render_state_publishes_current_state_without_stepping(renderer) -> None: + """Explicit publication makes staged states visible without a physics step.""" result = MagicMock() spawn_scene = MagicMock() spawn_scene.builder.is_finalized = True spawn_scene.builder.result = result - sync_render_state = MagicMock() - + staged, visible = {}, {} sim = object.__new__(SimulationManager) + sim.sim_config = SimpleNamespace(render_cfg=RenderCfg(renderer=renderer)) sim._spawn_scene = spawn_scene sim._world = MagicMock() - sim.physics = SimpleNamespace(sync_render_state=sync_render_state) + sim._render_state_published = False + sim.physics = SimpleNamespace( + sync_render_state=lambda _result: staged.update(robot=3) + ) + sim._refresh_marker_attachments = lambda: staged.update(marker=7) + + def publish(): + visible.update(staged) + return 0 + sim._world._flush_render_updates.side_effect = publish sim.sync_render_state() - sync_render_state.assert_called_once_with(result) + assert visible == ({"robot": 3, "marker": 7} if renderer == "hybrid" else {}) + assert sim._render_state_published is True sim._world.update.assert_not_called() +def test_failed_native_render_publication_remains_retryable() -> None: + sim, _, _, _, _ = _make_render_frame_sim() + sim._world._flush_render_updates = MagicMock(side_effect=[-1, 0]) + sim._render_state_published = False + with pytest.raises(RuntimeError, match="Native render-state publication failed"): + sim.sync_render_state() + assert sim._render_state_published is False + sim.sync_render_state() + assert sim._render_state_published is True + + def test_add_camera_uses_owning_manager_render_context( monkeypatch: pytest.MonkeyPatch, ) -> None: @@ -3350,6 +3376,351 @@ def translate(cfg, **kwargs): assert operations == [] +@pytest.mark.parametrize( + ("renderer", "backend"), + [("hybrid", "viser"), ("no-render", "none"), ("no-render", "viser")], +) +def test_marker_group_lifecycle_and_environment_offsets_do_not_step_physics( + renderer, backend +): + from embodichain.lab.visualization.markers import MarkerGroupCfg, MarkerPrototypeCfg + + sim, runtime = _make_visualization_sim_manager() + sim._marker_groups = {} + sim._native_markers = None + sim._markers = {} + sim.sim_config.visualization.backend = backend + sim.sim_config.render_cfg = SimpleNamespace(renderer=renderer) + sim.sim_config.num_envs = 1 + sim.arena_offsets = torch.tensor([[10.0, 20.0, 30.0]]) + group = sim.add_marker_group( + MarkerGroupCfg( + name="goal", + prototypes={"sphere": MarkerPrototypeCfg(shape="sphere")}, + ) + ) + assert sim.get_marker_group("goal") is group + assert sim._native_markers is None + group.update(translations=[[1, 2, 3]]) + np.testing.assert_allclose(sim.get_marker_overlays()[0].position, [11, 22, 33]) + assert sim._world.physics_updates == [] + assert sim._visualization_sim_step == 0 + with pytest.raises(ValueError, match="already exists"): + sim.add_marker_group( + MarkerGroupCfg(name="goal", prototypes={"box": MarkerPrototypeCfg()}) + ) + group.clear() + assert sim.get_marker_overlays() == () + group.update(translations=[[0, 0, 0]]) + assert sim.remove_marker("goal") + assert sim.get_marker_overlays() == () + with pytest.raises(RuntimeError, match="removed"): + group.update(translations=[[0, 0, 0]]) + + +def test_legacy_marker_arena_names_are_unique_before_registration(): + sim, _ = _make_visualization_sim_manager() + sim._markers = {} + sim._arenas = [MagicMock()] + cfg = MarkerCfg(name="target", axis_xpos=np.eye(4), arena_index=0) + sim.draw_marker(cfg) + sim.draw_marker(cfg) + assert set(sim._markers) == {"target_0", "target_0_1"} + + +def test_marker_publication_before_prepare_only_uses_existing_manifest(): + from embodichain.lab.visualization.markers import MarkerGroupCfg, MarkerPrototypeCfg + + sim, runtime = _make_visualization_sim_manager() + sim.sim_config.num_envs = 1 + sim._marker_groups, sim._markers, sim._native_markers = {}, {}, None + sim.arena_offsets = torch.zeros((1, 3)) + sim._spawn_scene = MagicMock() + sim._rigid_objects = {"declared_but_not_prepared": object()} + sim._visualization_manifest_topology_revision = sim._visualization_topology_revision + sim.start_visualization = MagicMock(side_effect=AssertionError("must not prepare")) + sim.sync_render_state = MagicMock(side_effect=AssertionError("must not sync")) + group = sim.add_marker_group( + MarkerGroupCfg(name="only_markers", prototypes={"box": MarkerPrototypeCfg()}) + ) + group.update(translations=[[0, 0, 0]]) + assert len(runtime.capture_calls) == 1 + assert runtime.capture_calls[-1]["capture_camera_images"] is False + assert not sim._visualization_error_reported + sim.prepare.assert_not_called() + sim.start_visualization.assert_not_called() + sim.sync_render_state.assert_not_called() + # Declarations may invalidate captured sources. Wait for explicit host prepare. + sim._visualization_topology_revision += 1 + group.update(translations=[[1, 0, 0]]) + assert len(runtime.capture_calls) == 1 + assert sim.get_marker_overlays()[0].position[0] == 1 + sim.prepare.assert_not_called() + + +def _make_attachment_sim(): + from embodichain.lab.visualization.markers import MarkerGroupCfg, MarkerPrototypeCfg + + sim, runtime = _make_visualization_sim_manager() + sim.sim_config.num_envs = 2 + sim.arena_offsets = torch.tensor([[10.0, 0, 0], [0, 20.0, 0]]) + sim._marker_groups, sim._markers, sim._native_markers = {}, {}, None + sim._rigid_objects, sim._robots, sim._articulations = {}, {}, {} + sim._sensors, sim._deformable_objects, sim._rigid_object_groups = {}, {}, {} + sim._lights = {} + sim._spawn_scene = MagicMock() + sim._spawn_scene.builder.is_finalized = True + sim._spawn_scene.__contains__.side_effect = lambda uid: uid in sim._rigid_objects + sim.physics.sync_render_state = lambda result: None + group = sim.add_marker_group( + MarkerGroupCfg(name="attached", prototypes={"box": MarkerPrototypeCfg()}) + ) + group.update(translations=[[[1, 0, 0]], [[2, 0, 0]]]) + return sim, group + + +def test_marker_manager_default_batches_root_and_link_attachment_without_prepare(): + sim, group = _make_attachment_sim() + root = torch.tensor([[2.0, 0, 0, 0, 0, 0, 1], [3.0, 0, 0, 0, 0, 0, 1]]) + tool = torch.tensor([[0.0, 4, 0, 0, 0, 0, 1], [0.0, 5, 0, 0, 0, 0, 1]]) + sim._robots["robot"] = SimpleNamespace( + is_spawn_bound=True, + link_names=["tool"], + get_local_pose=lambda: root, + get_link_pose=lambda name, env_ids: tool[env_ids], + ) + assert group.num_envs == sim.num_envs == 2 + group.attach("robot", env_ids=[0]) + group.attach("robot", link_name="tool", env_ids=[1]) + np.testing.assert_allclose( + [m.position for m in sim.get_marker_overlays()], [[13, 0, 0], [2, 25, 0]] + ) + root[0, 0] = 4 + tool[1, 1] = 7 + SimulationManager.sync_render_state(sim) + np.testing.assert_allclose( + [m.position for m in sim.get_marker_overlays()], [[15, 0, 0], [2, 27, 0]] + ) + sim.prepare.assert_not_called() + assert sim._world.physics_updates == [] + + +def test_marker_attachment_update_precedes_recording_and_uses_live_replacement(): + sim, group = _make_attachment_sim() + pose = torch.tensor([[2.0, 0, 0, 0, 0, 0, 1], [3.0, 0, 0, 0, 0, 0, 1]]) + sim._rigid_objects["target"] = SimpleNamespace( + is_spawn_bound=True, get_local_pose=lambda: pose + ) + group.attach("target") + # Replacing the registry facade must not leave borrowed parent/native handles. + sim._rigid_objects["target"] = SimpleNamespace( + is_spawn_bound=True, + get_local_pose=lambda: pose + torch.tensor([5.0, 0, 0, 0, 0, 0, 0]), + ) + recorded = [] + sim._window_record_state = SimpleNamespace(capture_from_sim_update=True) + sim._step_window_record_from_sim_update = lambda state, dt: recorded.append( + [m.position.copy() for m in sim.get_marker_overlays()] + ) + sim.update_gizmos = lambda: None + sim.capture_visualization_safely = lambda **kw: None + sim._log_scene_summary = lambda: None + sim.update(step=1) + np.testing.assert_allclose(recorded[0], [[18, 0, 0], [10, 20, 0]]) + assert len(sim._world.physics_updates) == 1 + + +def test_marker_attachment_removal_detaches_before_destroying_parent(): + sim, group = _make_attachment_sim() + alive = True + + def pose(): + assert alive, "read destroyed parent" + return torch.tensor([[2.0, 0, 0, 0, 0, 0, 1], [3.0, 0, 0, 0, 0, 0, 1]]) + + def remove(uid): + nonlocal alive + alive = False + + sim._rigid_objects["target"] = SimpleNamespace( + is_spawn_bound=True, get_local_pose=pose + ) + sim._spawn_scene.remove.side_effect = remove + group.attach("target") + assert sim.remove_asset("target") + SimulationManager.sync_render_state(sim) + group.update(env_ids=[0], colors=[[0, 1, 0, 0.5]]) + np.testing.assert_allclose( + [m.position for m in sim.get_marker_overlays()], [[13, 0, 0], [5, 20, 0]] + ) + + +@pytest.mark.parametrize( + "target,link,error", + [ + ("missing", None, KeyError), + ("unbound", None, RuntimeError), + ("rigid", "tool", ValueError), + ("robot", "missing", ValueError), + ], +) +def test_marker_attach_rejects_missing_unprepared_and_invalid_links( + target, link, error +): + sim, group = _make_attachment_sim() + sim._rigid_objects = { + "unbound": SimpleNamespace(is_spawn_bound=False), + "rigid": SimpleNamespace(is_spawn_bound=True), + } + sim._robots["robot"] = SimpleNamespace(is_spawn_bound=True, link_names=["tool"]) + with pytest.raises(error): + group.attach(target, link_name=link) + np.testing.assert_allclose( + [m.position for m in group.snapshot()], [[11, 0, 0], [2, 20, 0]] + ) + sim.prepare.assert_not_called() + assert sim._world.physics_updates == [] + + +def test_marker_publication_failure_rolls_back_selected_environment_state(): + sim, group = _make_attachment_sim() + sim._visualization_manifest_topology_revision = sim._visualization_topology_revision + + def fail_capture(**kwargs): + raise RuntimeError("snapshot capture failed") + + sim._visualization_runtime.capture = fail_capture + with pytest.raises(RuntimeError, match="snapshot capture failed"): + group.update(env_ids=[1], translations=[[9, 0, 0], [8, 0, 0]]) + assert group.counts == (1, 1) + np.testing.assert_allclose( + [m.position for m in group.snapshot()], [[11, 0, 0], [2, 20, 0]] + ) + assert sim._visualization_error_reported + + +def _attach_groups_for_host_update(group_count): + from embodichain.lab.visualization.markers import MarkerGroupCfg, MarkerPrototypeCfg + + sim, group = _make_attachment_sim() + pose = torch.tensor([[2.0, 0, 0, 0, 0, 0, 1], [3.0, 0, 0, 0, 0, 0, 1]]) + sim._rigid_objects["target"] = SimpleNamespace( + is_spawn_bound=True, get_local_pose=lambda: pose + ) + groups = [group] + for index in range(1, group_count): + extra = sim.add_marker_group( + MarkerGroupCfg( + name=f"attached_{index}", prototypes={"box": MarkerPrototypeCfg()} + ) + ) + extra.update(translations=[[[1, 0, 0]], [[2, 0, 0]]]) + groups.append(extra) + for attached in groups: + attached.attach("target") + sim._visualization_manifest_topology_revision = sim._visualization_topology_revision + runtime = sim._visualization_runtime + sim.start_visualization = lambda: runtime + sim.sync_render_state = lambda: SimulationManager.sync_render_state(sim) + sim.update_gizmos = lambda: None + sim._log_scene_summary = lambda: None + runtime.capture_calls.clear() + return sim, groups, pose + + +def test_automatic_attachment_capture_failure_does_not_interrupt_physics_or_recording(): + sim, groups, pose = _attach_groups_for_host_update(2) + pose[:, 0] += 1 + attempts = [] + + def fail_capture(**kwargs): + attempts.append(kwargs) + raise RuntimeError("worker capture failed") + + sim._visualization_runtime.capture = fail_capture + recorded = [] + sim._window_record_state = SimpleNamespace(capture_from_sim_update=True) + sim._step_window_record_from_sim_update = lambda state, dt: recorded.append( + (sim._visualization_sim_step, groups[0].snapshot()[0].position.copy()) + ) + sim.update(step=2) + assert len(sim._world.physics_updates) == sim._visualization_sim_step == 2 + assert sim._visualization_sim_time == pytest.approx(0.02) + assert [step for step, _ in recorded] == [1, 2] + for _, position in recorded: + np.testing.assert_allclose(position, [14, 0, 0]) + assert len(attempts) == 1 and attempts[0]["force"] is False + assert sim._visualization_error_reported + + +def test_automatic_attachment_native_publication_only_when_parent_changes(): + sim, groups, pose = _attach_groups_for_host_update(1) + published = [] + sim._native_markers = SimpleNamespace( + publish=lambda name, markers: published.append((name, markers)) + ) + SimulationManager.sync_render_state(sim) + assert published == [] + pose[:, 0] += 1 + SimulationManager.sync_render_state(sim) + SimulationManager.sync_render_state(sim) + assert len(published) == 1 + np.testing.assert_allclose(published[0][1][0].position, [14, 0, 0]) + assert sim._visualization_runtime.capture_calls == [] + + +def test_automatic_attachment_native_failure_preserves_step_accounting(): + sim, groups, pose = _attach_groups_for_host_update(1) + sim.sim_config.visualization.backend = "none" + pose[:, 0] += 1 + + def fail_publish(name, markers): + raise RuntimeError("native marker upload failed") + + sim._native_markers = SimpleNamespace(publish=fail_publish) + sim.update(step=2) + assert len(sim._world.physics_updates) == sim._visualization_sim_step == 2 + assert sim._visualization_sim_time == pytest.approx(0.02) + # Failed native publication keeps the previous detached transforms. + np.testing.assert_allclose(groups[0].snapshot()[0].position, [13, 0, 0]) + + +@pytest.mark.parametrize( + "renderer_name, use_scene", [("hybrid", False), ("fast-rt", True), ("rt", True)] +) +def test_native_markers_receive_prepared_scene_without_preparing_physics( + monkeypatch, renderer_name: str, use_scene: bool +) -> None: + from embodichain.lab.visualization.markers import MarkerGroupCfg, MarkerPrototypeCfg + + sim, _ = _make_visualization_sim_manager() + sim.sim_config.num_envs = 1 + sim.sim_config.visualization.backend = "native" + sim.sim_config.render_cfg.renderer = renderer_name + sim._marker_groups, sim._markers, sim._native_markers = {}, {}, None + sim.arena_offsets = torch.zeros((1, 3)) + scene = object() + sim._spawn_scene = SimpleNamespace( + builder=SimpleNamespace(backend="newton", is_finalized=False, result=scene) + ) + sim.get_env = MagicMock(side_effect=AssertionError("raw Arena is not the owner")) + renderer = SimpleNamespace(publish=MagicMock(), remove=MagicMock()) + factory = MagicMock(return_value=renderer) + monkeypatch.setattr(sim_manager_module, "NativeMarkerRenderer", factory) + + group = sim.add_marker_group( + MarkerGroupCfg(name="goal", prototypes={"box": MarkerPrototypeCfg()}) + ) + group.update(translations=[[0, 0, 0]]) + + factory.assert_called_once_with(scene, use_scene_for_overlays=use_scene) + renderer.publish.assert_called_once() + sim.get_env.assert_not_called() + sim.prepare.assert_not_called() + sim.sync_render_state.assert_not_called() + assert sim._world.physics_updates == [] + + @pytest.mark.parametrize("steps", [0, 1, 4]) def test_after_substep_observer_preserves_manager_and_camera_boundaries( steps: int, @@ -3426,6 +3797,126 @@ def test_process_no_render_is_detected_after_world_cleanup(monkeypatch) -> None: assert SimulationManager._get_process_renderer() == "no-render" +def test_after_substep_marker_refresh_precedes_visualization_capture() -> None: + sim, groups, pose = _attach_groups_for_host_update(3) + runtime = sim._visualization_runtime + events: list[str] = [] + captured_positions: list[np.ndarray] = [] + published_positions: list[np.ndarray] = [] + world_update = sim._world.update + capture = sim.capture_visualization_safely + + def update_world(dt: float) -> None: + world_update(dt) + events.append("physics") + + def after_substep(dt: float) -> None: + events.append("observer") + pose[:, 0] += 1 + + def publish(name: str, markers: object) -> None: + events.append("markers") + published_positions.append(np.array([marker.position for marker in markers])) + + def capture_state(**kwargs: object) -> None: + events.append("capture") + captured_positions.append( + np.array([marker.position for marker in groups[0].snapshot()]) + ) + capture(**kwargs) + + sim._world.update = update_world + sim._native_markers = SimpleNamespace(publish=publish) + sim.capture_visualization_safely = capture_state + + sim.update(step=2, after_substep=after_substep) + + assert ( + events + == ["physics", "observer", "markers", "markers", "markers", "capture"] * 2 + ) + # The fixture's parent poses, marker offsets, and Arena offsets produce + # these world positions after the observer moves each parent by one unit. + expected = [[[14, 0, 0], [6, 20, 0]], [[15, 0, 0], [7, 20, 0]]] + np.testing.assert_allclose(published_positions, np.repeat(expected, 3, axis=0)) + np.testing.assert_allclose(captured_positions, expected) + assert [call["capture_camera_images"] for call in runtime.capture_calls] == [ + False, + True, + ] + assert [(call["sim_step"], call["force"]) for call in runtime.capture_calls] == [ + (1, False), + (2, False), + ] + groups[0].update(env_ids=[0], translations=[[2, 0, 0]]) + assert len(runtime.capture_calls) == 3 + assert runtime.capture_calls[-1]["force"] is True + + +def test_deferred_final_frame_refreshes_markers_after_interval_state_writes() -> None: + sim, groups, pose = _attach_groups_for_host_update(1) + runtime = sim._visualization_runtime + rendered, recorded, captured = [], [], [] + + def marker_positions(): + return np.array([marker.position for marker in groups[0].snapshot()]) + + sim._world.render_camera_group = lambda _: rendered.append(marker_positions()) + sim._window_record_state = SimpleNamespace(capture_from_sim_update=True) + sim._step_window_record_from_sim_update = lambda _state, _dt: recorded.append( + marker_positions() + ) + capture = sim.capture_visualization_safely + + def capture_markers(**kwargs: object) -> None: + capture(**kwargs) + captured.append(marker_positions()) + + sim.capture_visualization_safely = capture_markers + + def after_substep(_dt: float) -> None: + pose[:, 0] += 1 + + sim.update(step=1, after_substep=after_substep, render_final_step=False) + assert runtime.capture_calls == [] + assert rendered == recorded == [] + np.testing.assert_allclose(marker_positions(), [[14, 0, 0], [6, 20, 0]]) + + # Gym interval events may change the parent after the final physics substep. + pose[:, 0] += 5 + with sim.render_frame(): + sim.render_camera_group([0]) + + expected = [[19, 0, 0], [11, 20, 0]] + np.testing.assert_allclose(rendered, [expected]) + np.testing.assert_allclose(recorded, [expected]) + np.testing.assert_allclose(captured, [expected]) + assert len(runtime.capture_calls) == 1 + assert len(sim._world.physics_updates) == 1 + + +def test_marker_update_in_observer_waits_for_deferred_final_frame() -> None: + sim, groups, pose = _attach_groups_for_host_update(2) + runtime = sim._visualization_runtime + + def after_substep(_dt: float) -> None: + pose[:, 0] += 1 + groups[1].set_visibility(False) + + sim.update(step=1, after_substep=after_substep, render_final_step=False) + + assert runtime.capture_calls == [] + assert all(not marker.visible for marker in groups[1].snapshot()) + np.testing.assert_allclose( + [marker.position for marker in groups[0].snapshot()], + [[14, 0, 0], [6, 20, 0]], + ) + with sim.render_frame(): + pass + assert len(runtime.capture_calls) == 1 + assert runtime.capture_calls[0]["sim_step"] == 1 + + def test_reset_fluid_selects_rows_and_honors_exclusions(): sim = object.__new__(SimulationManager) sim.physics = SimpleNamespace(name="default") diff --git a/tests/sim/test_sim_manager_cfg.py b/tests/sim/test_sim_manager_cfg.py index 7641c0c0f7..4e30acea55 100644 --- a/tests/sim/test_sim_manager_cfg.py +++ b/tests/sim/test_sim_manager_cfg.py @@ -265,6 +265,7 @@ def update(self, _physics_dt: float) -> None: _visualization_sim_step=0, _visualization_sim_time=0.0, _pending_record_dt=0.0, + _refresh_marker_attachments=lambda: None, _window_record_state=None, render_frame=lambda **_kwargs: nullcontext(), _log_scene_summary=lambda: None, diff --git a/tests/visualization/test_markers.py b/tests/visualization/test_markers.py new file mode 100644 index 0000000000..2b6b799407 --- /dev/null +++ b/tests/visualization/test_markers.py @@ -0,0 +1,453 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import numpy as np +import pytest + +from embodichain.lab.visualization.markers import ( + MarkerGroup, + MarkerGroupCfg, + MarkerPrototypeCfg, +) + + +def make_group(**kwargs): + return MarkerGroup( + MarkerGroupCfg( + name="targets", + prototypes={ + "red": MarkerPrototypeCfg(shape="box", color=(1, 0, 0, 0.4)), + "blue": MarkerPrototypeCfg( + shape="sphere", scale=(0.2, 0.2, 0.2), color=(0, 0, 1, 1) + ), + }, + **kwargs, + ) + ) + + +def test_update_preserves_other_fields_and_snapshots_own_arrays(): + group = make_group() + positions = np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]]) + group.update(translations=positions, prototype_indices=[1, 0]) + positions[:] = 99 + before = group.snapshot() + group.update(colors=[[0, 1, 0, 0.25], [1, 1, 0, 0.5]]) + after = group.snapshot() + assert len(after) == group.count == 2 + np.testing.assert_allclose(after[0].position, [1, 2, 3]) + np.testing.assert_allclose(after[0].scale, [0.2, 0.2, 0.2]) + assert after[0].color == (0, 1, 0, 0.25) + assert before[0].color == (0, 0, 1, 1) + after[0].position[:] = 9 + np.testing.assert_allclose(group.snapshot()[0].position, [1, 2, 3]) + + +@pytest.mark.parametrize( + "bad", + [ + {"translations": [[float("nan"), 0, 0]]}, + {"orientations_xyzw": [[0, 0, 0, 0]]}, + {"scales": [[1, -1, 1]]}, + {"prototype_indices": [2]}, + {"prototype_indices": [0.5]}, + {"colors": [[1, 0, 0, 2]]}, + {"visible": [2]}, + {"orientations_xyzw": [[0, 0, 0, 1], [0, 0, 0, 1]]}, + ], +) +def test_invalid_update_leaves_state_unchanged(bad): + group = make_group() + group.update(translations=[[1, 2, 3]]) + before = group.snapshot()[0] + with pytest.raises(ValueError): + group.update(**bad) + after = group.snapshot()[0] + np.testing.assert_array_equal(after.position, before.position) + np.testing.assert_array_equal(after.wxyz, before.wxyz) + assert after.color == before.color + + +def test_count_changes_reset_omitted_attributes_and_clear_retains_group(): + group = make_group() + group.update(translations=[[0, 0, 0]], prototype_indices=[1], visible=[False]) + group.update(translations=[[1, 0, 0], [2, 0, 0]]) + assert all(s.visible for s in group.snapshot()) + assert all(s.color == (1, 0, 0, 0.4) for s in group.snapshot()) + group.clear() + assert group.count == 0 and group.snapshot() == () + group.update(translations=[[3, 0, 0]]) + assert group.count == 1 + + +def test_group_visibility_does_not_discard_updates_or_per_instance_visibility(): + group = make_group() + group.update(translations=[[0, 0, 0], [1, 0, 0]], visible=[True, False]) + group.set_visibility(False) + group.update(translations=[[2, 0, 0], [3, 0, 0]]) + assert not any(s.visible for s in group.snapshot()) + group.set_visibility(True) + assert [s.visible for s in group.snapshot()] == [True, False] + np.testing.assert_allclose(group.snapshot()[0].position, [2, 0, 0]) + + +def test_quaternion_order_and_arena_translation_applied_once(): + group = MarkerGroup( + MarkerGroupCfg(name="env", prototypes={"box": MarkerPrototypeCfg()}), + origin=[10, 20, 30], + ) + quat = np.array([1.0, 2.0, 3.0, 4.0]) + group.update(translations=[[1, 2, 3]], orientations_xyzw=[quat]) + marker = group.snapshot()[0] + np.testing.assert_allclose(marker.position, [11, 22, 33]) + np.testing.assert_allclose(marker.wxyz, quat[[3, 0, 1, 2]] / np.linalg.norm(quat)) + assert marker.env_id == 0 + + +def test_removed_group_cannot_be_reused_and_callback_releases_once(): + removed = [] + group = MarkerGroup( + MarkerGroupCfg(name="gone", prototypes={"box": MarkerPrototypeCfg()}), + on_remove=lambda g: removed.append(g.name), + ) + group.remove() + group.remove() + assert removed == ["gone"] + with pytest.raises(RuntimeError, match="removed"): + group.update(translations=[[0, 0, 0]]) + + +@pytest.mark.parametrize( + "shape", ["box", "sphere", "cylinder", "capsule", "cone", "arrow", "frame"] +) +def test_builtin_geometries_are_finite_valid_triangles(shape): + group = MarkerGroup( + MarkerGroupCfg(name=shape, prototypes={shape: MarkerPrototypeCfg(shape=shape)}) + ) + group.update(translations=[[0, 0, 0]]) + meshes = group.snapshot() + assert len(meshes) == (3 if shape == "frame" else 1) + for mesh in meshes: + assert len(mesh.vertices) > 3 and len(mesh.faces) > 0 + assert np.isfinite(mesh.vertices).all() + assert mesh.faces.max() < len(mesh.vertices) + if shape == "frame": + assert [s.color[:3] for s in meshes] == [(1, 0, 0), (0, 1, 0), (0, 0, 1)] + + +def test_custom_mesh_and_config_are_detached_from_caller(): + vertices = np.array([[0.0, 0, 0], [1, 0, 0], [0, 1, 0]]) + cfg = MarkerGroupCfg( + name="triangle", + prototypes={ + "mesh": MarkerPrototypeCfg( + shape="mesh", vertices=vertices, faces=[[0, 1, 2]] + ) + }, + ) + group = MarkerGroup(cfg) + vertices[:] = 99 + cfg.prototypes["mesh"].color = (0, 0, 0, 0) + group.update(translations=[[0, 0, 0]]) + assert group.snapshot()[0].vertices.max() == 1 + assert group.snapshot()[0].color == (1, 0, 0, 1) + + +def test_failed_publication_rolls_back_logical_state(): + def fail(group): + raise RuntimeError("renderer rejected update") + + group = MarkerGroup( + MarkerGroupCfg(name="failure", prototypes={"box": MarkerPrototypeCfg()}), + on_change=fail, + ) + with pytest.raises(RuntimeError, match="renderer rejected"): + group.update(translations=[[0, 0, 0]]) + assert group.count == 0 + + +@pytest.mark.parametrize( + "values", + [ + {"translations": [[1e40, 0, 0]]}, + {"scales": [[1e40, 1, 1]]}, + {"orientations_xyzw": [[1e300, 0, 0, 1]]}, + ], +) +def test_update_rejects_values_not_representable_by_render_snapshots(values): + group = make_group() + group.update(translations=[[0, 0, 0]]) + with pytest.raises(ValueError): + group.update(**values) + np.testing.assert_allclose(group.snapshot()[0].position, [0, 0, 0]) + + +def test_scale_product_is_validated_before_committing_state(): + group = MarkerGroup( + MarkerGroupCfg( + name="scaled", prototypes={"box": MarkerPrototypeCfg(scale=(1e30, 1, 1))} + ) + ) + group.update(translations=[[0, 0, 0]]) + with pytest.raises(ValueError): + group.update(scales=[[1e30, 1, 1]]) + np.testing.assert_allclose(group.snapshot()[0].scale, [1e30, 1, 1]) + + +def batched_group(**kwargs): + return MarkerGroup( + MarkerGroupCfg(name="batch", prototypes={"box": MarkerPrototypeCfg()}), + num_envs=2, + origins=[[10, 0, 0], [0, 20, 0]], + **kwargs, + ) + + +def test_environment_batches_preserve_unselected_counts_styles_and_origins(): + group = batched_group() + group.update( + translations=[[[1, 0, 0]], [[2, 0, 0]]], + colors=[[[1, 0, 0, 0.2]], [[0, 1, 0, 0.4]]], + ) + assert group.count == 2 and group.counts == (1, 1) + original_ids = [m.overlay_id for m in group.snapshot()] + group.update(env_ids=[1], translations=[[3, 0, 0], [4, 0, 0]]) + assert group.counts == (1, 2) + meshes = group.snapshot() + np.testing.assert_allclose( + [m.position for m in meshes], [[11, 0, 0], [3, 20, 0], [4, 20, 0]] + ) + assert meshes[0].color == (1, 0, 0, 0.2) + assert meshes[1].color == (1, 0, 0, 1) + assert [m.overlay_id for m in meshes[:2]] == original_ids + group.set_visibility(False, env_ids=[1]) + group.update(env_ids=[1], translations=[[5, 0, 0]]) + assert [m.visible for m in group.snapshot()] == [True, False] + group.clear(env_ids=[0]) + assert group.counts == (0, 1) + group.update(env_ids=[0], translations=[[1, 0, 0]]) + assert group.snapshot()[0].visible + + +@pytest.mark.parametrize("env_ids", [[0, 0], [2], [-1], [0.5], [True], [[0]], ["0"]]) +def test_invalid_environment_selection_is_atomic(env_ids): + group = batched_group() + group.update(translations=[[[1, 0, 0]], [[2, 0, 0]]]) + with pytest.raises(ValueError): + group.update(env_ids=env_ids, translations=[[9, 0, 0]]) + np.testing.assert_allclose( + [m.position for m in group.snapshot()], [[11, 0, 0], [2, 20, 0]] + ) + + +def test_batch_shape_validation_and_empty_selection(): + group = batched_group() + with pytest.raises(ValueError): + group.update(translations=[[1, 0, 0], [2, 0, 0]]) + group.update(env_ids=[], translations=np.empty((0, 3, 3))) + assert group.counts == (0, 0) + with pytest.raises(ValueError): + group.update(env_ids=[], translations=[[1, 0, 0]]) + group.update(translations=[[[0, 0, 0]], [[1, 0, 0]]]) + with pytest.raises(ValueError): + group.update( + translations=[[[9, 0, 0]], [[8, 0, 0]]], + colors=[[[1, 0, 0, 0.5]], [[0, 0, 0, 2]]], + ) + np.testing.assert_allclose( + [m.position for m in group.snapshot()], [[10, 0, 0], [1, 20, 0]] + ) + + +def test_attachment_rotation_live_refresh_and_detach_world_pose(): + angle = np.sqrt(0.5) + poses = np.array([[2, 0, 0, 0, 0, angle, angle], [0, 3, 0, 0, 0, 0, 1.0]]) + calls = [] + + def resolve(parent, link_name, env_ids): + calls.append((parent, link_name, tuple(env_ids))) + return poses[env_ids] + + group = batched_group(pose_resolver=resolve) + group.update(translations=[[[1, 0, 0]], [[2, 0, 0]]]) + group.attach("robot", link_name="tool", env_ids=[0]) + assert calls[-1] == ("robot", "tool", (0,)) + np.testing.assert_allclose(group.snapshot()[0].position, [12, 1, 0], atol=1e-6) + np.testing.assert_allclose(group.snapshot()[0].wxyz, [angle, 0, 0, angle]) + poses[0, 0] = 4 + group._refresh_attachments() + np.testing.assert_allclose(group.snapshot()[0].position, [14, 1, 0], atol=1e-6) + group.detach(env_ids=[0]) + poses[0, 0] = 9 + group._refresh_attachments() + np.testing.assert_allclose(group.snapshot()[0].position, [14, 1, 0], atol=1e-6) + np.testing.assert_allclose(group.snapshot()[1].position, [2, 20, 0]) + + +def test_world_scope_single_batch_rejects_attachment(): + group = MarkerGroup( + MarkerGroupCfg( + name="world", scope="world", prototypes={"box": MarkerPrototypeCfg()} + ), + num_envs=2, + origins=[[10, 0, 0], [0, 20, 0]], + ) + group.update(translations=[[1, 2, 3]]) + assert group.counts == (1,) + assert group.snapshot()[0].env_id is None + np.testing.assert_allclose(group.snapshot()[0].position, [1, 2, 3]) + with pytest.raises(ValueError, match="world"): + group.attach("robot") + + +def test_attachment_and_selected_updates_roll_back_on_publication_failure(): + fail = False + + def publish(group): + if fail: + raise RuntimeError("publish failed") + + group = batched_group( + on_change=publish, + pose_resolver=lambda parent, link, ids: np.tile( + [5, 0, 0, 0, 0, 0, 1], (len(ids), 1) + ), + ) + group.update(translations=[[[1, 0, 0]], [[2, 0, 0]]]) + fail = True + for action in ( + lambda: group.attach("parent"), + lambda: group.set_visibility(False, env_ids=[1]), + lambda: group.clear(env_ids=[1]), + ): + with pytest.raises(RuntimeError, match="publish failed"): + action() + assert group.counts == (1, 1) + np.testing.assert_allclose( + [m.position for m in group.snapshot()], [[11, 0, 0], [2, 20, 0]] + ) + assert all(m.visible for m in group.snapshot()) + + +@pytest.mark.parametrize("env_ids", [0, True, [False, 1], [0, True], np.array(0)]) +def test_environment_selection_rejects_scalars_and_mixed_booleans(env_ids): + group = batched_group() + with pytest.raises(ValueError): + group.clear(env_ids=env_ids) + + +def test_detach_without_preserving_world_pose_and_failed_detach_are_atomic(): + fail = False + + def publish(group): + if fail: + raise RuntimeError("publish failed") + + group = batched_group( + on_change=publish, + pose_resolver=lambda parent, link, ids: np.tile( + [5, 0, 0, 0, 0, 0, 1], (len(ids), 1) + ), + ) + group.update(translations=[[[1, 0, 0]], [[2, 0, 0]]]) + group.attach("parent") + fail = True + with pytest.raises(RuntimeError, match="publish failed"): + group.detach(env_ids=[0]) + np.testing.assert_allclose(group.snapshot()[0].position, [16, 0, 0]) + fail = False + group.detach(env_ids=[0], keep_world_pose=False) + np.testing.assert_allclose(group.snapshot()[0].position, [11, 0, 0]) + np.testing.assert_allclose(group.snapshot()[1].position, [7, 20, 0]) + + +def test_snapshot_never_resolves_live_attachments(): + calls = [] + + def resolver(parent, link, ids): + calls.append(tuple(ids)) + return np.tile([5, 0, 0, 0, 0, 0, 1], (len(ids), 1)) + + group = batched_group(pose_resolver=resolver) + group.update(translations=[[[1, 0, 0]], [[2, 0, 0]]]) + group.attach("parent") + calls.clear() + group.snapshot() + group.snapshot() + assert calls == [] + + +def test_updates_validate_transforms_without_constructing_render_geometry(monkeypatch): + import embodichain.lab.visualization.markers.group as group_module + + group = batched_group( + pose_resolver=lambda parent, link, ids: np.tile( + [5, 0, 0, 0, 0, 0, 1], (len(ids), 1) + ) + ) + + def unexpected_snapshot(**kwargs): + raise AssertionError( + "Updates without a publisher must not allocate render geometry" + ) + + with monkeypatch.context() as patch: + patch.setattr(group_module, "MeshMarkerOverlay", unexpected_snapshot) + group.update(translations=[[[1, 0, 0]], [[2, 0, 0]]]) + group.attach("parent", env_ids=[1]) + group._refresh_attachments() + group.detach(env_ids=[1]) + np.testing.assert_allclose( + [m.position for m in group.snapshot()], [[11, 0, 0], [7, 20, 0]] + ) + + +def test_composed_attachment_overflow_is_rejected_before_publication(): + published = [] + group = batched_group( + on_change=lambda group: published.append(group.count), + pose_resolver=lambda parent, link, ids: np.tile( + [3e38, 0, 0, 0, 0, 0, 1], (len(ids), 1) + ), + ) + group.update(translations=[[[3e38, 0, 0]], [[0, 0, 0]]]) + with pytest.raises(ValueError, match="World positions"): + group.attach("parent", env_ids=[0]) + assert published == [2] + np.testing.assert_allclose(group.snapshot()[0].position, [3e38, 0, 0], rtol=1e-6) + + +def test_batch_update_does_not_rescan_all_environment_counts(monkeypatch): + num_envs = 64 + group = MarkerGroup( + MarkerGroupCfg(name="batch", prototypes={"box": MarkerPrototypeCfg()}), + num_envs=num_envs, + ) + group.update(translations=np.zeros((num_envs, 1, 3))) + original_counts = MarkerGroup.counts.fget + scans = [] + + def counted_counts(self): + scans.append(self.num_envs) + return original_counts(self) + + monkeypatch.setattr(MarkerGroup, "counts", property(counted_counts)) + group.update(visible=np.zeros((num_envs, 1), dtype=bool)) + assert len(scans) <= 1, "One batch must not scan every environment per row." + assert not any(marker.visible for marker in group.snapshot()) + np.testing.assert_array_equal(original_counts(group), np.ones(num_envs)) diff --git a/tests/visualization/test_native_markers.py b/tests/visualization/test_native_markers.py new file mode 100644 index 0000000000..e481f6a73d --- /dev/null +++ b/tests/visualization/test_native_markers.py @@ -0,0 +1,531 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from dataclasses import replace +from pathlib import Path +from types import SimpleNamespace +import weakref + +import numpy as np +import pytest + +from dexsim import spawn +from dexsim.scene.objects import SpawnedRigidBody +from embodichain.lab.visualization.protocol import MeshMarkerOverlay +from embodichain.lab.visualization.markers import _native +from embodichain.lab.visualization.markers._native import NativeMarkerRenderer + + +@pytest.fixture(autouse=True) +def sdk_descriptors(monkeypatch): + sdk = SimpleNamespace( + GeometryDesc=spawn.GeometryDesc, + RenderDesc=spawn.RenderDesc, + MaterialDesc=spawn.MaterialDesc, + MaterialTextureDesc=spawn.MaterialTextureDesc, + MeshObjectDesc=spawn.MeshObjectDesc, + ) + + def import_sdk(name): + assert name == "dexsim.spawn" + return sdk + + monkeypatch.setattr(_native, "import_module", import_sdk) + return sdk + + +class NativeActor: + """Render-state storage behind the real stable SpawnedRigidBody facade.""" + + def __init__(self, desc): + self.color = tuple(desc.renders[0].material.base_color) + self.visible = True + self.pose = np.eye(4) + self.scale = (1, 1, 1) + + def set_world_pose(self, pose, sync=True): + self.pose = pose.copy() + + def set_scale(self, x, y, z, sync=True): + self.scale = (x, y, z) + + def set_visible(self, visible): + self.visible = visible + + def get_material(self): + return self + + def set_base_color(self, color): + self.color = tuple(color) + + +class Scene: + """Fake Scene allocation/removal while retaining real stable handles.""" + + def __init__(self): + self.handles = {} + self.created = 0 + self.fail = False + self.closed = False + self.removed = [] + + def add_mesh_object(self, desc, *, arena_name="default"): + assert arena_name == "default" + assert isinstance(desc, spawn.MeshObjectDesc) + for render in desc.renders: + if render.render_mode not in ("scene", "overlay"): + raise ValueError("render_mode must be scene or overlay") + if self.fail: + raise RuntimeError("upload failed") + self.created += 1 + assert desc.name not in self.handles + handle = SpawnedRigidBody( + path=desc.name, + object_desc=desc, + _render_actor=NativeActor(desc), + _physics_binding=None, + _arena_index=0, + ) + handle._result_ref = weakref.ref(self) + self.handles[handle.path] = handle + return handle + + def remove_mesh_object(self, path): + assert not self.closed, "must not remove resources after Scene.close()" + self.removed.append(path) + self.handles.pop(path).discard() + + def close(self): + self.closed = True + for handle in self.handles.values(): + handle.discard() + self.handles.clear() + + +def marker(): + return MeshMarkerOverlay( + "marker", + np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]]), + np.array([[0, 1, 2]]), + np.array([1, 2, 3]), + np.array([0.5, 0.5, 0.5, 0.5]), + ) + + +def test_updates_reuse_scene_handles_and_keep_world_pose_scale_and_alpha(): + scene = Scene() + renderer = NativeMarkerRenderer(scene) + renderer.publish("g", (marker(),)) + handle = next(iter(scene.handles.values())) + native = handle.native() + np.testing.assert_allclose(native.pose[:3, 3], [1, 2, 3]) + np.testing.assert_allclose(native.pose[:3, :3], [[0, 0, 1], [1, 0, 0], [0, 1, 0]]) + renderer.publish( + "g", + ( + replace( + marker(), color=(0, 1, 0, 0.2), scale=np.array([2, 3, 4]), visible=False + ), + ), + ) + assert scene.created == 1 + assert native.color == (0, 1, 0, 0.2) and native.scale == (2, 3, 4) + np.testing.assert_array_equal(handle.desc.scale, [2, 3, 4]) + assert not native.visible + path = handle.path + renderer.remove("g") + assert scene.removed == [path] + assert scene.handles == {} and not handle.is_valid + renderer.close() + + +def test_mesh_descriptor_requests_overlay_without_physics(): + scene = Scene() + renderer = NativeMarkerRenderer(scene) + snapshot = replace(marker(), color=(0.2, 0.4, 0.6, 0.3)) + renderer.publish("g", (snapshot,)) + handle = next(iter(scene.handles.values())) + desc = handle.desc + assert desc.physics is None and desc.collisions == [] and not desc.per_env + (render,) = desc.renders + assert isinstance(render, spawn.RenderDesc) + np.testing.assert_array_equal(render.vertices, snapshot.vertices) + np.testing.assert_array_equal(render.triangles, snapshot.faces) + assert render.render_mode == "overlay" + assert render.cast_shadow is False and render.pickable is False + material = render.material + assert isinstance(material, spawn.MaterialDesc) + assert material.base_color == snapshot.color + assert material.unlit and material.alpha_mode == "blend" + assert material.depth_write is False and material.double_sided + + +def test_failed_geometry_replacement_retains_previous_scene_handles(): + scene = Scene() + renderer = NativeMarkerRenderer(scene) + initial = replace( + marker(), + texture=np.zeros((2, 2, 3), dtype=np.uint8), + uv_coords=np.array([[0, 0], [1, 0], [0, 1]], dtype=np.float32), + ) + renderer.publish("g", (initial,)) + previous = dict(scene.handles) + committed_path = Path( + next(iter(previous.values())).desc.renders[0].material.base_color_map + ) + scene.fail = True + for value in range(1, 4): + replacement = replace( + initial, texture=np.full((2, 2, 3), value, dtype=np.uint8) + ) + with pytest.raises(RuntimeError, match="upload failed"): + renderer.publish("g", (replacement,)) + assert list(committed_path.parent.glob("*.png")) == [committed_path] + assert len(renderer._texture_paths) == 1 + assert scene.handles == previous + renderer.close() + assert scene.handles == {} + + +def test_scene_paths_are_unique_across_groups_environments_and_renderers(): + scene = Scene() + first = NativeMarkerRenderer(scene) + second = NativeMarkerRenderer(scene) + first.publish("g", (marker(), replace(marker(), overlay_id="marker/env1"))) + first.publish("h", (marker(),)) + second.publish("g", (marker(),)) + assert len(scene.handles) == 4 + first.remove("g") + assert len(scene.handles) == 2 + second.close() + first.close() + assert scene.handles == {} + + +def test_texture_files_remain_owned_after_group_removal_and_replacement(): + from PIL import Image + + scene = Scene() + renderer = NativeMarkerRenderer(scene) + images = [np.full((2, 2, 3), value, dtype=np.uint8) for value in (20, 80, 140)] + for group, pixels in zip(("a", "b"), images): + renderer.publish( + group, + ( + replace( + marker(), + texture=pixels, + uv_coords=np.array([[0, 0], [1, 0], [0, 1]], dtype=np.float32), + ), + ), + ) + renderer.remove("a") + renderer.publish( + "c", + ( + replace( + marker(), + texture=images[2], + uv_coords=np.array([[0, 0], [1, 0], [0, 1]], dtype=np.float32), + ), + ), + ) + paths = [ + Path(handle.desc.renders[0].material.base_color_map) + for handle in scene.handles.values() + ] + assert len(set(paths)) == 2 + for path, pixels in zip(paths, images[1:]): + with Image.open(path) as image: + np.testing.assert_array_equal(np.asarray(image), pixels) + renderer.remove("b") + assert not paths[0].exists() and paths[1].is_file() + renderer.close() + assert not paths[1].exists() + + +def test_geometry_replacement_releases_old_handle_after_success(): + scene = Scene() + renderer = NativeMarkerRenderer(scene) + renderer.publish("g", (marker(),)) + old_handle = next(iter(scene.handles.values())) + renderer.publish("g", (replace(marker(), vertices=marker().vertices * 2),)) + assert scene.created == 2 and len(scene.handles) == 1 + assert old_handle.path not in scene.handles and not old_handle.is_valid + renderer.close() + assert scene.handles == {} + + +def test_later_creation_failure_rolls_back_updates_and_new_handles(monkeypatch): + scene = Scene() + renderer = NativeMarkerRenderer(scene) + renderer.publish("g", (marker(),)) + original = next(iter(scene.handles.values())) + native = original.native() + before = dict(scene.handles) + original_create = scene.add_mesh_object + + def create(*args, **kwargs): + if scene.created == 2: + raise RuntimeError("upload failed") + return original_create(*args, **kwargs) + + monkeypatch.setattr(scene, "add_mesh_object", create) + with pytest.raises(RuntimeError, match="upload failed"): + renderer.publish( + "g", + ( + replace(marker(), position=np.array([9, 8, 7]), color=(1, 0, 0, 0.2)), + replace(marker(), overlay_id="new"), + replace(marker(), overlay_id="failed"), + ), + ) + assert scene.handles == before + np.testing.assert_allclose(native.pose[:3, 3], [1, 2, 3]) + assert native.color == marker().color + + +def test_apply_failure_removes_staged_handle_and_restores_previous_state(monkeypatch): + scene = Scene() + renderer = NativeMarkerRenderer(scene) + renderer.publish("g", (marker(),)) + original = next(iter(scene.handles.values())) + native = original.native() + before = dict(scene.handles) + set_visible = NativeActor.set_visible + + def fail_new_handle(handle, visible): + if handle is not native: + raise RuntimeError("visibility update failed") + set_visible(handle, visible) + + monkeypatch.setattr(NativeActor, "set_visible", fail_new_handle) + with pytest.raises(RuntimeError, match="visibility update failed"): + renderer.publish( + "g", + ( + replace(marker(), scale=np.array([3, 2, 1]), visible=False), + replace(marker(), overlay_id="new"), + ), + ) + assert scene.handles == before + assert native.visible and native.scale == (1, 1, 1) + np.testing.assert_array_equal(original.desc.scale, [1, 1, 1]) + + +def test_remove_and_close_are_idempotent_after_scene_close(): + scene = Scene() + renderer = NativeMarkerRenderer(scene) + renderer.publish("g", (marker(),)) + renderer.publish("h", (marker(),)) + scene.close() + renderer.remove("g") + renderer.remove("g") + renderer.close() + renderer.close() + assert scene.removed == [] + assert renderer._groups == {} + + +def test_missing_scene_capability_fails_before_allocation(monkeypatch): + scene = Scene() + monkeypatch.setattr(scene, "add_mesh_object", None) + with pytest.raises(RuntimeError, match="Install a DexSim build"): + NativeMarkerRenderer(scene) + assert not scene.handles + + +def test_missing_spawn_module_fails_with_installation_guidance(monkeypatch): + def missing_sdk(name): + raise ModuleNotFoundError(name) + + monkeypatch.setattr(_native, "import_module", missing_sdk) + with pytest.raises(RuntimeError, match="Install a DexSim build"): + NativeMarkerRenderer(Scene()) + + +def test_updates_follow_stable_handle_after_native_rebind(): + scene = Scene() + renderer = NativeMarkerRenderer(scene) + renderer.publish("g", (marker(),)) + handle = next(iter(scene.handles.values())) + old_native = handle.native() + replacement = SpawnedRigidBody( + path=handle.path, + object_desc=handle.desc, + _render_actor=NativeActor(handle.desc), + _physics_binding=None, + _arena_index=0, + ) + handle.rebind(replacement) + renderer.publish("g", (replace(marker(), position=np.array([4, 5, 6])),)) + assert scene.created == 1 + assert next(iter(scene.handles.values())) is handle + np.testing.assert_array_equal(handle.native().pose[:3, 3], [4, 5, 6]) + np.testing.assert_array_equal(old_native.pose[:3, 3], [1, 2, 3]) + renderer.close() + assert not handle.is_valid + + +@pytest.mark.parametrize( + "owner, field_name", + [ + ("MaterialDesc", "unlit"), + (None, "MeshObjectDesc"), + ], +) +def test_missing_descriptor_capability_fails_before_allocation( + monkeypatch, sdk_descriptors, owner, field_name +): + target = sdk_descriptors if owner is None else getattr(sdk_descriptors, owner) + monkeypatch.delattr(target, field_name) + scene = Scene() + with pytest.raises(RuntimeError, match="Install a DexSim build"): + NativeMarkerRenderer(scene) + assert not scene.handles + + +def test_scene_lighting_preserves_material_and_switches_at_transparency(): + scene = Scene() + renderer = NativeMarkerRenderer(scene) + snapshot = replace( + marker(), + unlit=False, + scene_lighting=True, + color=(0.15, 0.8, 0.4, 1), + material={"metallic": 0.6, "roughness": 0.23}, + ) + renderer.publish("g", (snapshot,)) + opaque = next(iter(scene.handles.values())) + render = opaque.desc.renders[0] + assert render.render_mode == "scene" + assert render.material.metallic == 0.6 + assert render.material.roughness == 0.23 + renderer.publish("g", (replace(snapshot, color=(0.15, 0.8, 0.4, 0.4)),)) + transparent = next(iter(scene.handles.values())) + assert not opaque.is_valid + assert transparent.desc.renders[0].render_mode == "overlay" + assert transparent.desc.renders[0].material.alpha_mode == "blend" + np.testing.assert_allclose(transparent.native().pose[:3, 3], snapshot.position) + renderer.publish("g", (replace(snapshot, unlit=True),)) + unlit = next(iter(scene.handles.values())) + assert not transparent.is_valid + assert unlit.desc.renders[0].material.unlit is True + np.testing.assert_allclose(unlit.native().pose[:3, 3], snapshot.position) + renderer.publish("g", (replace(snapshot, unlit=True, color=(0, 1, 0, 0.4)),)) + assert next(iter(scene.handles.values())) is unlit + renderer.close() + + +def test_packed_transforms_preserve_order_scale_and_detached_storage(): + from embodichain.lab.visualization.markers._native import _transforms + + first = replace(marker(), scale=np.array([2, 3, 4])) + second = replace( + marker(), position=np.array([-1, -2, -3]), wxyz=np.array([1, 0, 0, 0]) + ) + matrices = _transforms((first, second)) + np.testing.assert_allclose(matrices[0, :3, :3], [[0, 0, 4], [2, 0, 0], [0, 3, 0]]) + np.testing.assert_allclose(matrices[1, :3, :3], np.eye(3)) + np.testing.assert_allclose(matrices[:, :3, 3], [[1, 2, 3], [-1, -2, -3]]) + matrices[:] = 0 + np.testing.assert_array_equal(first.position, [1, 2, 3]) + assert _transforms(()).shape == (0, 4, 4) + + +@pytest.mark.parametrize("source_map", [None, "/source/base-color.png"]) +def test_scene_lighting_keeps_texture_with_native_uv_coordinates(source_map): + from PIL import Image + + scene = Scene() + renderer = NativeMarkerRenderer(scene) + pixels = np.full((2, 2, 3), [40, 90, 190], dtype=np.uint8) + native_uv = np.array([[0, 0], [2, 0], [0, 2]], dtype=np.float32) + snapshot = replace( + marker(), + unlit=False, + scene_lighting=True, + texture=pixels, + uv_coords=native_uv / 2, + material_uv_coords=native_uv, + material={"base_color_map": source_map} if source_map else None, + ) + try: + renderer.publish("g", (snapshot,)) + render = next(iter(scene.handles.values())).desc.renders[0] + np.testing.assert_array_equal(render.uv_coords, native_uv) + if source_map: + assert render.material.base_color_map == source_map + else: + assert render.material.base_color_map is not None + with Image.open(render.material.base_color_map) as image: + np.testing.assert_array_equal(np.asarray(image), pixels) + finally: + renderer.close() + + +@pytest.mark.parametrize("source_map", [False, True]) +def test_opaque_texture_tint_is_baked_and_updates_without_double_color( + tmp_path, source_map +): + from PIL import Image + from dexsim.spawn.adapters.material_texture import prepare_material_textures + + pixels = np.full((2, 2, 4), 255, dtype=np.uint8) + path = tmp_path / "base.png" + Image.fromarray(pixels).save(path) + material = ( + { + "base_color_map": str(path), + "texture_settings": { + "base_color_map": { + "color_space": "sRGB", + "scale": (0.5, 1, 1, 1), + "bias": (0.25, 0, 0, 0), + } + }, + } + if source_map + else None + ) + snapshot = replace( + marker(), + unlit=False, + scene_lighting=True, + texture=pixels, + color=(0, 1, 0, 1), + uv_coords=np.zeros((3, 2)), + material=material, + ) + scene = Scene() + renderer = NativeMarkerRenderer(scene) + try: + for color, expected in [ + ((0, 1, 0, 1), (0, 1, 0, 1)), + ((1, 0, 0, 1), (0.75 if source_map else 1, 0, 0, 1)), + ]: + renderer.publish("g", (replace(snapshot, color=color),)) + handle = next(iter(scene.handles.values())) + lowered, _ = prepare_material_textures(handle.desc.renders[0].material)[ + "base_color_map" + ] + np.testing.assert_allclose(lowered[0, 0], expected) + np.testing.assert_array_equal(handle.get_material().color, [1, 1, 1, 1]) + assert len(scene.handles) == 1 + finally: + renderer.close() diff --git a/tests/visualization/test_protocol.py b/tests/visualization/test_protocol.py index 19a161d903..ee33d737b5 100644 --- a/tests/visualization/test_protocol.py +++ b/tests/visualization/test_protocol.py @@ -29,12 +29,170 @@ JointControlCommand, JointControlSpec, JointControlState, + MeshMarkerOverlay, MeshGeometry, + SceneFrame, SceneManifest, + SceneOverlays, + estimate_frame_bytes, pose_to_position_wxyz, ) +def test_mesh_marker_overlay_owns_validated_arrays_and_counts_bytes() -> None: + vertices = np.array( + [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]], + dtype=np.float64, + ) + faces = np.array([[0, 1, 2]], dtype=np.int64) + position = np.array([1.0, 2.0, 3.0], dtype=np.float64) + overlay = MeshMarkerOverlay( + "goal", + vertices, + faces, + position, + np.array([2.0, 0.0, 0.0, 0.0]), + scale=np.array([2.0, 3.0, 4.0]), + color=(0.2, 0.4, 0.6, 0.5), + env_id=1, + ) + vertices[0, 0] = 99.0 + faces[0, 0] = 2 + position[0] = 99.0 + + assert overlay.vertices.dtype == np.float32 + assert overlay.faces.dtype == np.uint32 + np.testing.assert_allclose(overlay.vertices[0], [0.0, 0.0, 0.0]) + np.testing.assert_array_equal(overlay.faces, [[0, 1, 2]]) + np.testing.assert_allclose(overlay.position, [1.0, 2.0, 3.0]) + np.testing.assert_allclose(overlay.wxyz, [1.0, 0.0, 0.0, 0.0]) + frame = SceneFrame( + "run", + 1, + 1, + 1, + 0.01, + (), + np.empty((0, 3), dtype=np.float32), + np.empty((0, 4), dtype=np.float32), + np.empty((0,), dtype=np.bool_), + overlays=SceneOverlays(meshes=(overlay,)), + ) + assert estimate_frame_bytes(frame) == sum( + array.nbytes + for array in ( + overlay.vertices, + overlay.faces, + overlay.position, + overlay.wxyz, + overlay.scale, + ) + ) + + +@pytest.mark.parametrize( + ("overrides", "message"), + [ + ({"vertices": [[0.0, np.nan, 0.0]]}, "finite"), + ({"faces": [[0, 1, 3]]}, "vertex indices"), + ({"faces": [[0, 1, -1]]}, "non-negative integers"), + ({"faces": [[0, 1, 1.5]]}, "non-negative integers"), + ({"faces": [[0, 1, 2**32]]}, "non-negative integers"), + ({"faces": []}, "at least one triangle"), + ({"overlay_id": ""}, "non-empty string"), + ({"env_id": True}, "non-negative integer"), + ({"scale": [1.0, 0.0, 1.0]}, "positive"), + ({"color": (1.1, 0.0, 0.0, 1.0)}, "RGBA"), + ({"uv_coords": [[0, 0]]}, "uv_coords"), + ({"uv_coords": [[float("nan"), 0]] * 3}, "uv_coords"), + ({"texture": np.zeros((2, 2, 4), np.uint8)}, "requires UVs"), + ({"uv_coords": np.zeros((3, 2)), "texture": np.zeros((2, 2, 4))}, "uint8"), + ( + {"uv_coords": np.zeros((3, 2)), "texture": np.zeros((0, 2, 4), np.uint8)}, + "nonempty", + ), + ], +) +def test_mesh_marker_overlay_rejects_invalid_geometry_and_style( + overrides: dict[str, object], message: str +) -> None: + values: dict[str, object] = { + "overlay_id": "goal", + "vertices": [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]], + "faces": [[0, 1, 2]], + "position": [0.0, 0.0, 0.0], + "wxyz": [1.0, 0.0, 0.0, 0.0], + } + values.update(overrides) + + with pytest.raises(ValueError, match=message): + MeshMarkerOverlay(**values) # type: ignore[arg-type] + + +def test_textured_geometry_is_detached_shared_and_counted_once(): + from embodichain.lab.visualization.markers import ( + MarkerGroup, + MarkerGroupCfg, + MarkerPrototypeCfg, + ) + + image = np.full((2, 2, 4), 255, np.uint8) + uv = np.array([[0, 0], [1, 0], [0, 1]], np.float32) + group = MarkerGroup( + MarkerGroupCfg( + name="texture", + prototypes={ + "mesh": MarkerPrototypeCfg( + shape="mesh", + vertices=[[0, 0, 0], [1, 0, 0], [0, 1, 0]], + faces=[[0, 1, 2]], + uv_coords=uv, + texture=image, + normals=[[0, 0, 1]] * 3, + material_uv_coords=uv * 0.5, + ) + }, + ) + ) + group.update(translations=[[0, 0, 0], [1, 0, 0]]) + first, second = group.snapshot() + image[:] = 0 + uv[:] = 3 + assert first.texture is second.texture and first.vertices is second.vertices + assert first.texture.min() == 255 + np.testing.assert_array_equal(first.uv_coords, [[0, 0], [1, 0], [0, 1]]) + for array in (first.vertices, first.faces, first.texture, first.uv_coords): + assert not array.flags.writeable + frame = SceneFrame( + "run", + 1, + 1, + 1, + 0.01, + (), + np.empty((0, 3), np.float32), + np.empty((0, 4), np.float32), + np.empty(0, bool), + overlays=SceneOverlays(meshes=(first, second)), + ) + assert estimate_frame_bytes(frame) == ( + sum( + array.nbytes + for array in ( + first.vertices, + first.faces, + first.uv_coords, + first.texture, + first.normals, + first.material_uv_coords, + ) + ) + + 2 * (first.position.nbytes + first.wxyz.nbytes + first.scale.nbytes) + ) + group.update(translations=[[5, 0, 0], [6, 0, 0]]) + np.testing.assert_array_equal(first.position, [0, 0, 0]) + + def test_pose_conversion_converts_embodichain_xyzw_to_protocol_wxyz() -> None: pose = np.array([1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 4.0], dtype=np.float32) diff --git a/tests/visualization/test_reference_trajectory_example.py b/tests/visualization/test_reference_trajectory_example.py new file mode 100644 index 0000000000..f554c0ccf3 --- /dev/null +++ b/tests/visualization/test_reference_trajectory_example.py @@ -0,0 +1,88 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Camera behavior for recorded reference trajectories.""" + +from __future__ import annotations + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation +from examples.sim.reference_trajectory import _camera_path, _root_link_index + + +def test_follow_camera_ignores_vertical_motion_and_articulation_extent(): + poses = np.zeros((4, 2, 3, 7)) + poses[..., 6] = 1 + poses[:, :, :, 0] = np.arange(4)[:, None, None] + poses[:, :, :, 1] = np.arange(4)[:, None, None] * 0.5 + poses[:, :, :, 2] = np.array([0.2, 1.3, 0.4, 1.0])[:, None, None] + poses[:, :, 2, 0] += np.array([0.1, 1.0, 0.2, 0.8])[:, None] + offsets = np.array([[0, -0.5, 0], [0, 0.5, 0]]) + eye, target = _camera_path(poses, offsets, "follow_xy") + np.testing.assert_allclose(np.diff(target[:, :2], axis=0), [[1, 0.5]] * 3) + np.testing.assert_allclose(eye[:, 2], eye[0, 2]) + np.testing.assert_allclose(target[:, 2], target[0, 2]) + np.testing.assert_allclose(eye - target, np.tile(eye[0] - target[0], (4, 1))) + + +def test_fixed_camera_retains_one_view_over_translating_clip(): + poses = np.zeros((3, 1, 2, 7)) + poses[..., 6] = 1 + poses[:, :, :, :3] = np.arange(3)[:, None, None, None] + eye, target = _camera_path(poses, np.zeros((1, 3)), "fixed") + np.testing.assert_allclose(eye, np.tile(eye[0], (3, 1))) + np.testing.assert_allclose(target, np.tile(target[0], (3, 1))) + + +@pytest.mark.parametrize("mode", ["follow_xy", "fixed"]) +def test_camera_uses_named_root_when_recording_and_asset_link_orders_differ( + tmp_path, mode +): + asset = tmp_path / "robot.urdf" + asset.write_text( + '' + '' + ) + names = ("tip", "root") + poses = np.zeros((3, 1, 2, 7)) + poses[..., 6] = 1 + poses[:, 0, 1, 0] = [0, 1, 2] + poses[:, 0, 0, 0] = [10, 20, 40] + poses[:, 0, 1, 3:] = Rotation.from_euler("z", 90, degrees=True).as_quat() + root_index = _root_link_index(str(asset), names) + assert root_index == 1 + expected = _camera_path(poses[:, :, [1, 0]], np.zeros((1, 3)), mode) + actual = _camera_path(poses, np.zeros((1, 3)), mode, root_index) + np.testing.assert_allclose(actual, expected) + + +def test_fixed_view_accepts_visual_links_without_nonvisual_root(tmp_path): + asset = tmp_path / "robot.urdf" + asset.write_text( + '' + '' + '' + ) + root_index = _root_link_index(str(asset), ("body",)) + assert root_index is None + poses = np.zeros((3, 1, 1, 7)) + poses[..., 6] = 1 + poses[:, 0, 0, 0] = [0, 1, 2] + eye, target = _camera_path(poses, np.zeros((1, 3)), "fixed", root_index) + np.testing.assert_allclose(eye, np.tile(eye[0], (3, 1))) + np.testing.assert_allclose(target, np.tile(target[0], (3, 1))) + with pytest.raises(ValueError, match="--camera-mode fixed"): + _camera_path(poses, np.zeros((1, 3)), "follow_xy", root_index) diff --git a/tests/visualization/test_robot_markers.py b/tests/visualization/test_robot_markers.py new file mode 100644 index 0000000000..3cd37bdc55 --- /dev/null +++ b/tests/visualization/test_robot_markers.py @@ -0,0 +1,553 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import numpy as np +import pytest +import json +from pathlib import Path +from scipy.spatial.transform import Rotation + +from embodichain.lab.visualization.markers import ( + MarkerGroup, + MarkerPrototypeCfg, + RobotMarkers, + RobotMarkersCfg, + RobotMarkersAppearanceCfg, +) + + +class _Sim: + def add_marker_group(self, cfg): + self.group = MarkerGroup(cfg, num_envs=2, origins=[[0, 0, 0], [10, 0, 0]]) + return self.group + + +@pytest.fixture +def usd_material_runtime(): + from dexsim.spawn import MaterialDesc + + fields = MaterialDesc.__dataclass_fields__ + if not {"color_space", "texture_settings"} <= fields.keys(): + pytest.skip("Runtime lacks USD color-space and texture-sampling support.") + + +def test_usd_material_rejects_runtime_without_sampler_semantics(): + from types import SimpleNamespace + from embodichain.lab.visualization.markers._robot_marker_assets import ( + _validate_usd_material, + ) + + with pytest.raises(RuntimeError, match="Upgrade the visualization runtime"): + _validate_usd_material(SimpleNamespace(base_color=(1, 1, 1, 1))) + + +@pytest.fixture +def robot_path(tmp_path): + path = tmp_path / "robot.urdf" + path.write_text(""" + + + + + + + + + """) + return path + + +def _ghost(path, mode="asset", space="env"): + sim = _Sim() + ghost = RobotMarkers( + RobotMarkersCfg( + name="ghost", + asset_path=str(path), + coordinate_space=space, + appearance=RobotMarkersAppearanceCfg(mode=mode, opacity=0.5), + ), + sim, + ) + return ghost, sim.group + + +def test_visual_origin_scale_material_and_partial_environments(robot_path): + ghost, group = _ghost(robot_path) + assert ghost.link_names == ("base", "tip") and ghost.num_visuals == 3 + poses = np.tile(np.eye(4), (2, 2, 1, 1)) + poses[0, 0, :3, :3] = Rotation.from_euler("z", 90, degrees=True).as_matrix() + ghost.visualize_link_poses(poses) + snapshots = group.snapshot() + np.testing.assert_allclose(snapshots[0].position, [0, 1, 0], atol=1e-7) + np.testing.assert_allclose(snapshots[0].scale, [2, 3, 4]) + np.testing.assert_allclose(snapshots[0].color, [1, 0, 0, 0.4]) + np.testing.assert_allclose(snapshots[3].position, [11, 0, 0]) + assert abs(snapshots[0].wxyz[3]) == pytest.approx(1) + ghost.visualize_link_poses(np.tile(np.eye(4), (1, 2, 1, 1)), env_ids=[0]) + np.testing.assert_allclose(group.snapshot()[3].position, [11, 0, 0]) + + +def test_world_poses_are_not_offset_twice(robot_path): + ghost, group = _ghost(robot_path, space="world") + ghost.visualize_link_poses(np.tile(np.eye(4), (2, 2, 1, 1))) + np.testing.assert_allclose(group.snapshot()[3].position, [1, 0, 0]) + + +def test_appearance_toggle_preserves_source_and_geometry(robot_path): + ghost, group = _ghost(robot_path) + ghost.visualize_qpos([[0], [0]]) + assert all(not snapshot.unlit for snapshot in group.snapshot()) + original = tuple(ghost._visuals[0].prototype.color) + ghost.set_appearance( + RobotMarkersAppearanceCfg(mode="override", color=(0, 1, 0), opacity=0) + ) + np.testing.assert_allclose(group.snapshot()[0].color, [0, 1, 0, 0]) + ghost.set_appearance(RobotMarkersAppearanceCfg(mode="asset", opacity=1)) + np.testing.assert_allclose(group.snapshot()[0].color, original) + assert ghost._visuals[0].prototype.color == original + assert all(not snapshot.unlit for snapshot in group.snapshot()) + ghost.set_appearance(RobotMarkersAppearanceCfg(mode="asset", unlit=True)) + assert all(snapshot.unlit for snapshot in group.snapshot()) + ghost.set_appearance(RobotMarkersAppearanceCfg(mode="asset", opacity=0.2)) + assert all(not snapshot.unlit for snapshot in group.snapshot()) + ghost.set_visibility(False, env_ids=[1]) + assert all(not item.visible for item in group.snapshot()[3:]) + assert all(item.visible for item in group.snapshot()[:3]) + ghost.remove() + ghost.remove() + assert group.snapshot() == () + + +def test_reference_joint_fk_uses_named_link_order(robot_path): + ghost, group = _ghost(robot_path) + assert ghost.joint_names == ("hinge",) + roots = np.tile([0, 0, 0, 0, 0, 0, 1], (2, 1)).astype(float) + roots[:, 0] = 2 + ghost.visualize_qpos([[0], [np.pi / 2]], root_poses=roots) + snapshots = group.snapshot() + np.testing.assert_allclose(snapshots[2].position, [2, 0, 1.5]) + np.testing.assert_allclose(snapshots[5].position, [12, 0, 1.5]) + np.testing.assert_allclose( + snapshots[5].wxyz, [np.sqrt(0.5), 0, 0, np.sqrt(0.5)], atol=1e-6 + ) + + +def test_single_link_joint_space_preserves_roots_and_visual_origins(tmp_path): + path = tmp_path / "static.urdf" + path.write_text( + '' + '' + ) + ghost, group = _ghost(path) + assert ghost.joint_names == () + ghost.visualize_qpos(np.empty((2, 0))) + np.testing.assert_allclose( + [s.position for s in group.snapshot()], [[1, 0, 0], [11, 0, 0]] + ) + roots = np.tile(np.eye(4), (1, 2, 1, 1)) + roots[0, 0, :3, :3] = Rotation.from_euler("z", 90, degrees=True).as_matrix() + roots[0, :, 1, 3] = [2, 4] + ghost.visualize_qpos(np.empty((1, 2, 0)), root_poses=roots, env_ids=[1]) + np.testing.assert_allclose( + [s.position for s in group.snapshot()], + [[1, 0, 0], [10, 3, 0], [11, 4, 0]], + atol=1e-6, + ) + ghost.visualize_qpos(np.empty((1, 0, 0)), env_ids=[1]) + assert group.counts == (1, 0) + + +@pytest.mark.parametrize( + "appearance", + [ + RobotMarkersAppearanceCfg(mode="bad"), + RobotMarkersAppearanceCfg(opacity=-1), + RobotMarkersAppearanceCfg(opacity=float("nan")), + RobotMarkersAppearanceCfg(color=(1, 2, 3)), + ], +) +def test_invalid_appearance_keeps_previous_state(robot_path, appearance): + ghost, group = _ghost(robot_path) + ghost.visualize_qpos([[0], [0]]) + before = group.snapshot()[0].color + with pytest.raises(ValueError): + ghost.set_appearance(appearance) + assert group.snapshot()[0].color == before + + +def test_invalid_pose_keeps_previous_state(robot_path): + ghost, group = _ghost(robot_path) + ghost.visualize_qpos([[0], [0]]) + poses = np.tile(np.eye(4), (2, 2, 1, 1)) + poses[0, 0, 0, 0] = 2 + with pytest.raises(ValueError, match="rigid"): + ghost.visualize_link_poses(poses) + np.testing.assert_allclose(group.snapshot()[0].position, [1, 0, 0]) + + +def test_missing_mesh_reports_source_error(tmp_path): + path = tmp_path / "broken.urdf" + path.write_text( + '' + ) + with pytest.raises(FileNotFoundError, match="missing.stl"): + _ghost(path) + + +def test_empty_environment_pose_update_is_a_noop(robot_path): + ghost, group = _ghost(robot_path) + ghost.visualize_qpos([[0], [0]]) + before = group.snapshot() + ghost.visualize_link_poses(np.empty((0, 2, 7)), env_ids=[]) + np.testing.assert_array_equal(group.snapshot()[0].position, before[0].position) + + +def test_multiple_references_share_prototypes_and_resize_selected_environments( + robot_path, +): + ghost, group = _ghost(robot_path) + roots = np.tile(np.eye(4), (2, 3, 1, 1)) + roots[:, :, 1, 3] = [0, 2, 4] + ghost.visualize_qpos(np.zeros((2, 3, 1)), root_poses=roots) + assert group.counts == (9, 9) + snapshots = group.snapshot() + assert snapshots[0].vertices is snapshots[3].vertices + np.testing.assert_allclose(snapshots[8].position, [0, 4, 1.5]) + ghost.visualize_qpos(np.zeros((1, 2, 1)), env_ids=[1]) + assert group.counts == (9, 6) + ghost.visualize_link_poses(np.empty((1, 0, 2, 7)), env_ids=[0]) + assert group.counts == (0, 6) + + +def test_urdf_texture_and_mesh_scale_survive_appearance_switch(tmp_path): + from PIL import Image + import trimesh + + pixels = np.array( + [ + [[255, 0, 0, 255], [0, 255, 0, 255]], + [[0, 0, 255, 255], [255, 255, 255, 128]], + ], + dtype=np.uint8, + ) + mesh = trimesh.Trimesh( + vertices=[[-1, -1, 0], [1, -1, 0], [1, 1, 0], [-1, 1, 0]], + faces=[[0, 1, 2], [0, 2, 3]], + process=False, + visual=trimesh.visual.TextureVisuals( + uv=[[0, 0], [1, 0], [1, 1], [0, 1]], + material=trimesh.visual.material.PBRMaterial( + baseColorTexture=Image.fromarray(pixels), + baseColorFactor=[255, 255, 255, 255], + metallicFactor=0.65, + roughnessFactor=0.27, + normalTexture=Image.fromarray( + np.full((2, 2, 3), [128, 128, 255], dtype=np.uint8) + ), + ), + ), + ) + mesh.export(tmp_path / "mesh.glb") + path = tmp_path / "textured.urdf" + path.write_text( + '' + '' + "" + ) + ghost, group = _ghost(path) + ghost.visualize_link_poses(np.tile(np.eye(4), (2, 1, 1, 1))) + snapshot = group.snapshot()[0] + np.testing.assert_array_equal(snapshot.texture, pixels) + np.testing.assert_allclose(snapshot.uv_coords, [[0, 0], [1, 0], [1, 1], [0, 1]]) + np.testing.assert_allclose(snapshot.scale, [2, 3, 4]) + before = json.loads(snapshot.material) + assert before["metallic"] == pytest.approx(0.65) + assert before["roughness"] == pytest.approx(0.27) + assert Path(before["normal_map"]).is_file() + ghost.set_appearance(RobotMarkersAppearanceCfg(mode="override", opacity=1)) + tinted = group.snapshot()[0] + assert json.loads(tinted.material) == before + np.testing.assert_allclose(tinted.normals, snapshot.normals) + np.testing.assert_array_equal(tinted.texture, pixels) + ghost.set_appearance(RobotMarkersAppearanceCfg(mode="asset")) + np.testing.assert_array_equal(group.snapshot()[0].texture, pixels) + + +def test_vertex_color_asset_is_preserved_as_texture(tmp_path): + import trimesh + from embodichain.lab.visualization.markers._robot_marker_assets import _mesh_parts + + mesh = trimesh.creation.box() + mesh.visual.vertex_colors = np.tile([255, 0, 0, 255], (len(mesh.vertices), 1)) + path = tmp_path / "colored.glb" + mesh.export(path) + (prototype,) = _mesh_parts(path) + np.testing.assert_allclose(prototype.color, [1, 1, 1, 1]) + np.testing.assert_array_equal(prototype.texture[0, 0], [255, 0, 0, 255]) + assert prototype.uv_coords.shape == (len(prototype.vertices), 2) + + +def test_gltf_linear_color_factor_is_encoded_for_marker_protocol(): + import trimesh + from embodichain.lab.visualization.markers._robot_marker_assets import ( + _mesh_prototype, + ) + + mesh = trimesh.creation.box() + mesh.visual = trimesh.visual.TextureVisuals( + material=trimesh.visual.material.PBRMaterial(baseColorFactor=[64, 64, 64, 128]) + ) + prototype = _mesh_prototype(mesh) + np.testing.assert_allclose( + prototype.color, [0.5380651] * 3 + [128 / 255], atol=1e-6 + ) + + +@pytest.mark.parametrize("packaged", [False, True]) +def test_usd_visual_origin_material_and_packaged_texture( + tmp_path, packaged, usd_material_runtime +): + from PIL import Image + from pxr import Gf, Sdf, Usd, UsdGeom, UsdShade, UsdUtils + + pixels = np.full((2, 2, 4), [255, 0, 0, 255], dtype=np.uint8) + Image.fromarray(pixels).save(tmp_path / "texture.png") + path = tmp_path / "visual.usda" + stage = Usd.Stage.CreateNew(str(path)) + UsdGeom.SetStageMetersPerUnit(stage, 1.0) + root = UsdGeom.Xform.Define(stage, "/Visual") + root.GetPrim().CreateAttribute("MeshObject", Sdf.ValueTypeNames.Token) + stage.SetDefaultPrim(root.GetPrim()) + mesh = UsdGeom.Mesh.Define(stage, "/Visual/mesh") + mesh.CreatePointsAttr([(-1, -1, 0), (1, -1, 0), (0, 1, 0)]) + mesh.CreateFaceVertexCountsAttr([3]) + mesh.CreateFaceVertexIndicesAttr([0, 1, 2]) + mesh.AddTranslateOp().Set(Gf.Vec3d(1, 2, 3)) + mesh.AddScaleOp().Set(Gf.Vec3f(2, 3, 4)) + UsdGeom.PrimvarsAPI(mesh).CreatePrimvar( + "st", Sdf.ValueTypeNames.TexCoord2fArray, UsdGeom.Tokens.vertex + ).Set([(0, 0), (1, 0), (0.5, 1)]) + material = UsdShade.Material.Define(stage, "/Material") + shader = UsdShade.Shader.Define(stage, "/Material/Shader") + shader.CreateIdAttr("UsdPreviewSurface") + shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(0.25)) + shader.CreateInput("opacity", Sdf.ValueTypeNames.Float).Set(0.8) + shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(0.27) + shader.CreateInput("metallic", Sdf.ValueTypeNames.Float).Set(0.65) + material.CreateSurfaceOutput().ConnectToSource(shader.ConnectableAPI(), "surface") + UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim()).Bind(material) + stage.GetRootLayer().Save() + ghost, group = _ghost(path) + ghost.visualize_link_poses(np.tile(np.eye(4), (2, 1, 1, 1))) + snapshot = group.snapshot()[0] + np.testing.assert_allclose(snapshot.vertices, [[-1, -1, 3], [3, -1, 3], [1, 5, 3]]) + np.testing.assert_allclose(snapshot.color, [0.5370987] * 3 + [0.4], atol=1e-6) + ghost.remove() + texture = UsdShade.Shader.Define(stage, "/Material/Texture") + texture.CreateIdAttr("UsdUVTexture") + texture.CreateInput("file", Sdf.ValueTypeNames.Asset).Set( + Sdf.AssetPath("texture.png") + ) + texture.CreateOutput("rgb", Sdf.ValueTypeNames.Float3) + shader.GetInput("diffuseColor").ConnectToSource(texture.ConnectableAPI(), "rgb") + stage.GetRootLayer().Save() + if packaged: + package = tmp_path / "visual.usdz" + assert UsdUtils.CreateNewUsdzPackage(Sdf.AssetPath(str(path)), str(package)) + path = package + ghost, group = _ghost(path) + ghost.visualize_link_poses(np.tile(np.eye(4), (2, 1, 1, 1))) + snapshot = group.snapshot()[0] + # USD's default useMetadata resolves to a black border. It is baked into + # padded pixels so the native and GLB samplers can both clamp correctly. + np.testing.assert_array_equal(snapshot.texture[1:-1, 1:-1], pixels) + np.testing.assert_array_equal(snapshot.texture[0, 0], [0, 0, 0, 255]) + np.testing.assert_allclose( + snapshot.uv_coords, [[0.25, 0.25], [0.75, 0.25], [0.5, 0.75]] + ) + assert snapshot.texture_wrap == ("clamp", "clamp") + parameters = json.loads(snapshot.material) + assert parameters["roughness"] == pytest.approx(0.27) + assert parameters["metallic"] == pytest.approx(0.65) + texture_path = Path(parameters["base_color_map"]) + assert texture_path.is_file() + np.testing.assert_allclose(snapshot.material_uv_coords, [[0, 0], [1, 0], [0.5, 1]]) + if packaged: + del ghost + assert texture_path.is_file() + group.remove() + assert not texture_path.exists() + + +@pytest.mark.parametrize("multiplier", [False, True]) +def test_usd_texture_sampling_preserves_uv_transform_and_opacity( + tmp_path, usd_material_runtime, multiplier +): + from PIL import Image + from dexsim.spawn import MaterialDesc, MaterialTextureDesc + from embodichain.lab.visualization.markers._robot_marker_assets import _usd_texture + + path = tmp_path / "sampling.png" + Image.fromarray(np.full((2, 2, 4), [128, 128, 128, 64], np.uint8)).save(path) + base = MaterialTextureDesc( + color_space="raw", + uv_scale=(2, 3), + uv_translation=(0.1, 0.2), + uv_rotation=90, + wrap_s="mirror", + wrap_t="clamp", + ) + alpha = MaterialTextureDesc( + channel="a", + color_space="raw", + uv_scale=(2, 3), + uv_translation=(0.1, 0.2), + uv_rotation=90, + wrap_s="mirror", + wrap_t="clamp", + ) + material = MaterialDesc( + name="source", + base_color_map=str(path), + opacity_map=str(path), + texture_settings={"base_color_map": base, "opacity_map": alpha}, + ) + if multiplier: + from dataclasses import replace + + material.base_color_multiplier_map = str(path) + material.emissive_map = str(path) + material.emissive_multiplier_map = str(path) + material.texture_settings.update( + base_color_multiplier_map=replace(base, scale=(0.5, 0.5, 0.5, 1)), + emissive_map=base, + emissive_multiplier_map=base, + ) + prototype = MarkerPrototypeCfg( + shape="mesh", uv_coords=np.array([[1.0, 0.0], [0.0, 1.0]]) + ) + _usd_texture(prototype, material) + np.testing.assert_allclose( + prototype.uv_coords, [[0.1, 2.2], [-2.9, 0.2]], atol=1e-6 + ) + expected_rgb = 99 if multiplier else 188 + np.testing.assert_array_equal(prototype.texture[0, 0], [expected_rgb] * 3 + [64]) + assert prototype.texture_wrap == ("mirror", "clamp") + alpha.uv_scale = (1, 1) + with pytest.raises(ValueError, match="matching UV"): + _usd_texture(prototype, material) + + +def test_omnipbr_usd_bakes_ambient_occlusion_into_marker_color( + tmp_path, usd_material_runtime +): + from PIL import Image + from pxr import Sdf, Usd, UsdGeom, UsdShade + + Image.fromarray(np.full((2, 2, 3), 255, np.uint8)).save(tmp_path / "base.png") + Image.fromarray(np.full((2, 2, 3), 128, np.uint8)).save(tmp_path / "ao.png") + path = tmp_path / "omnipbr.usda" + stage = Usd.Stage.CreateNew(str(path)) + UsdGeom.SetStageMetersPerUnit(stage, 1) + root = UsdGeom.Xform.Define(stage, "/Visual") + root.GetPrim().CreateAttribute("MeshObject", Sdf.ValueTypeNames.Token) + stage.SetDefaultPrim(root.GetPrim()) + mesh = UsdGeom.Mesh.Define(stage, "/Visual/mesh") + mesh.CreatePointsAttr([(-1, -1, 0), (1, -1, 0), (0, 1, 0)]) + mesh.CreateFaceVertexCountsAttr([3]) + mesh.CreateFaceVertexIndicesAttr([0, 1, 2]) + UsdGeom.PrimvarsAPI(mesh).CreatePrimvar( + "st", Sdf.ValueTypeNames.TexCoord2fArray, UsdGeom.Tokens.vertex + ).Set([(0, 0), (1, 0), (0.5, 1)]) + material = UsdShade.Material.Define(stage, "/Material") + shader = UsdShade.Shader.Define(stage, "/Material/Shader") + shader.CreateIdAttr("OmniPBR") + shader.CreateInput("diffuse_texture", Sdf.ValueTypeNames.Asset).Set( + Sdf.AssetPath("base.png") + ) + shader.CreateInput("ao_texture", Sdf.ValueTypeNames.Asset).Set( + Sdf.AssetPath("ao.png") + ) + shader.CreateInput("ao_to_diffuse", Sdf.ValueTypeNames.Float).Set(1) + shader.CreateInput("texture_scale", Sdf.ValueTypeNames.Float2).Set((2, 3)) + material.CreateSurfaceOutput().ConnectToSource(shader.ConnectableAPI(), "surface") + UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim()).Bind(material) + stage.GetRootLayer().Save() + ghost, group = _ghost(path) + ghost.visualize_link_poses(np.tile(np.eye(4), (2, 1, 1, 1))) + snapshot = group.snapshot()[0] + np.testing.assert_array_equal(snapshot.texture[0, 0], [188, 188, 188, 255]) + np.testing.assert_allclose(snapshot.uv_coords, [[0, 0], [2, 0], [1, 3]]) + parameters = json.loads(snapshot.material) + assert parameters["base_color_multiplier_map"].endswith("ao.png") + assert parameters["texture_settings"]["base_color_multiplier_map"]["uv_scale"] == [ + 2, + 3, + ] + ghost.remove() + + +def test_shared_usd_source_selects_one_visual_and_rejects_ambiguous_address(tmp_path): + from embodichain.lab.sim.cfg.source import UsdSourceCfg + from pxr import Sdf, Usd, UsdGeom + + path = tmp_path / "objects.usda" + stage = Usd.Stage.CreateNew(str(path)) + for name in ("A", "B"): + root = UsdGeom.Xform.Define(stage, f"/{name}") + root.GetPrim().CreateAttribute("MeshObject", Sdf.ValueTypeNames.Token) + UsdGeom.Cube.Define(stage, f"/{name}/mesh") + stage.GetRootLayer().Save() + source = UsdSourceCfg(file_path=str(path), source_prim_path="/B") + ghost = RobotMarkers(RobotMarkersCfg(name="selected", source=source), _Sim()) + assert ghost.link_names == ("B",) + ghost.remove() + with pytest.raises(ValueError, match="exactly one"): + RobotMarkers(RobotMarkersCfg(name="ambiguous", asset_path=str(path)), _Sim()) + with pytest.raises(ValueError, match="not both"): + RobotMarkers( + RobotMarkersCfg(name="duplicate", source=source, asset_path=str(path)), + _Sim(), + ) + + +def test_usd_mesh_without_authored_uv_retains_material(tmp_path, usd_material_runtime): + from pxr import Gf, Sdf, Usd, UsdGeom, UsdShade + + path = tmp_path / "untextured.usda" + stage = Usd.Stage.CreateNew(str(path)) + root = UsdGeom.Xform.Define(stage, "/Visual") + root.GetPrim().CreateAttribute("MeshObject", Sdf.ValueTypeNames.Token) + mesh = UsdGeom.Mesh.Define(stage, "/Visual/mesh") + mesh.CreatePointsAttr([(0, 0, 0), (1, 0, 0), (0, 1, 0)]) + mesh.CreateFaceVertexCountsAttr([3]) + mesh.CreateFaceVertexIndicesAttr([0, 1, 2]) + material = UsdShade.Material.Define(stage, "/Material") + shader = UsdShade.Shader.Define(stage, "/Material/Shader") + shader.CreateIdAttr("UsdPreviewSurface") + shader.CreateInput("diffuseColor", Sdf.ValueTypeNames.Color3f).Set(Gf.Vec3f(0.25)) + shader.CreateInput("roughness", Sdf.ValueTypeNames.Float).Set(0.3) + material.CreateSurfaceOutput().ConnectToSource(shader.ConnectableAPI(), "surface") + UsdShade.MaterialBindingAPI.Apply(mesh.GetPrim()).Bind(material) + stage.GetRootLayer().Save() + ghost, group = _ghost(path) + ghost.visualize_link_poses(np.tile(np.eye(4), (2, 1, 1, 1))) + snapshot = group.snapshot()[0] + assert snapshot.material_uv_coords is None + assert json.loads(snapshot.material)["roughness"] == pytest.approx(0.3) + ghost.remove() diff --git a/tests/visualization/test_scene_exporter.py b/tests/visualization/test_scene_exporter.py index 36513129c8..6aafc2c541 100644 --- a/tests/visualization/test_scene_exporter.py +++ b/tests/visualization/test_scene_exporter.py @@ -24,6 +24,7 @@ FrameOverlay, JointControlSpec, JointControlState, + MeshMarkerOverlay, PointCloudOverlay, SceneExporter, SceneOverlays, @@ -338,6 +339,32 @@ def is_visible(self) -> bool: return self._visible +def _marker_overlay(overlay_id: str, env_id: int | None) -> MeshMarkerOverlay: + return MeshMarkerOverlay( + overlay_id=overlay_id, + vertices=np.array( + [[0.0, 0.0, 0.0], [0.1, 0.0, 0.0], [0.0, 0.1, 0.0]], + dtype=np.float32, + ), + faces=np.array([[0, 1, 2]], dtype=np.uint32), + position=np.array([2.25, 0.0, 0.5], dtype=np.float32), + wxyz=np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32), + env_id=env_id, + ) + + +class _MeshMarkerSimulation(_EmptySimulation): + num_envs = 2 + arena_offsets = np.array([[0.0, 0.0, 0.0], [2.0, 0.0, 0.0]], dtype=np.float32) + + def get_marker_overlays(self) -> tuple[MeshMarkerOverlay, ...]: + return ( + _marker_overlay("shared", 1), + _marker_overlay("filtered", 0), + _marker_overlay("world", None), + ) + + class _AxisMarkerSimulation(_EmptySimulation): def __init__(self) -> None: pose = np.eye(4, dtype=np.float32) @@ -615,6 +642,28 @@ def test_axis_marker_id_does_not_collide_with_caller_frame() -> None: np.testing.assert_allclose(frames[1].position, caller_frame.position) +def test_mesh_markers_are_filtered_preserved_and_not_offset_twice() -> None: + exporter = SceneExporter( + _MeshMarkerSimulation(), + VisualizationCfg(backend="viser", env_ids=[1]), + run_id="mesh-marker-run", + ) + caller = _marker_overlay("shared", 1) + + exporter.build_manifest() + result = exporter.capture( + sim_step=1, + sim_time=0.01, + overlays=SceneOverlays(meshes=(caller,)), + ) + + meshes = result.frame.overlays.meshes + assert [mesh.overlay_id for mesh in meshes] == ["shared#1", "world", "shared"] + assert [mesh.env_id for mesh in meshes] == [1, None, 1] + np.testing.assert_allclose(meshes[0].position, [2.25, 0.0, 0.5]) + assert meshes[-1] is caller + + def test_camera_frustum_pose_and_low_frequency_rgb_are_exported() -> None: simulation = _CameraSimulation() exporter = SceneExporter( @@ -730,12 +779,13 @@ def test_rigid_groups_and_deformable_meshes_are_exported() -> None: ) -def test_fluid_point_overlay_is_detached_and_display_filtered(): +def test_fluid_point_overlay_is_detached_and_display_filtered_with_markers(): import torch from types import SimpleNamespace from embodichain.lab.sim.cfg import FluidObjectCfg, FluidRenderCfg sim = _EmptySimulation() + sim.get_marker_overlays = lambda: (_marker_overlay("ghost", 0),) original = torch.tensor([[[0.0, 0, -1], [0.0, 0, 1]]]) sim._fluid_objects = { "water": SimpleNamespace( @@ -746,6 +796,7 @@ def test_fluid_point_overlay_is_detached_and_display_filtered(): } exporter = SceneExporter(sim, VisualizationCfg(backend="viser", env_ids=[0])) overlays = exporter._prepare_overlays(None) + assert [mesh.overlay_id for mesh in overlays.meshes] == ["ghost"] assert len(overlays.point_clouds) == 1 np.testing.assert_array_equal(overlays.point_clouds[0].points, [[0, 0, 1]]) torch.testing.assert_close(original, torch.tensor([[[0.0, 0, -1], [0.0, 0, 1]]])) diff --git a/tests/visualization/test_viser_backend.py b/tests/visualization/test_viser_backend.py index 743dfc65a0..ec65abe09e 100644 --- a/tests/visualization/test_viser_backend.py +++ b/tests/visualization/test_viser_backend.py @@ -32,6 +32,7 @@ GizmoState, JointControlSpec, JointControlState, + MeshMarkerOverlay, MeshGeometry, SceneFrame, SceneManifest, @@ -256,6 +257,13 @@ def add_mesh_simple(self, name: str, **kwargs: object) -> _Handle: self.dynamic_mesh_handles.append(handle) return handle + def add_glb(self, name: str, glb_data: bytes, **kwargs: object) -> _Handle: + handle = _Handle( + name=name, glb_data=glb_data, removed=False, visible=True, **kwargs + ) + self.dynamic_mesh_handles.append(handle) + return handle + def add_camera_frustum(self, name: str, **kwargs: object) -> _Handle: handle = _Handle(name=name, removed=False, **kwargs) self.camera_handles.append(handle) @@ -382,6 +390,189 @@ def test_viser_backend_renders_axis_marker_frame_overlay() -> None: backend.stop() +def _mesh_marker( + *, + position: tuple[float, float, float] = (0.0, 0.0, 0.0), + scale: tuple[float, float, float] = (1.0, 1.0, 1.0), + color: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 1.0), + env_id: int | None = 0, +) -> MeshMarkerOverlay: + return MeshMarkerOverlay( + overlay_id="goal", + vertices=np.array( + [[0.0, 0.0, 0.0], [1.0, 0.0, 0.0], [0.0, 1.0, 0.0]], + dtype=np.float32, + ), + faces=np.array([[0, 1, 2]], dtype=np.uint32), + position=np.asarray(position, dtype=np.float32), + wxyz=np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float32), + scale=np.asarray(scale, dtype=np.float32), + color=color, + env_id=env_id, + ) + + +def _empty_frame_with_mesh( + overlay: MeshMarkerOverlay | None, sequence: int +) -> SceneFrame: + return SceneFrame( + run_id="run", + scene_revision=1, + sequence=sequence, + sim_step=sequence, + sim_time=sequence * 0.01, + node_ids=(), + positions=np.empty((0, 3), dtype=np.float32), + wxyz=np.empty((0, 4), dtype=np.float32), + visible=np.empty((0,), dtype=np.bool_), + overlays=SceneOverlays(meshes=() if overlay is None else (overlay,)), + ) + + +def test_viser_backend_reuses_mesh_marker_handles_until_geometry_changes() -> None: + server = _Server() + backend = ViserBackend(ViserServerCfg(port=8765), server_factory=lambda **_: server) + initial = _mesh_marker(color=(1.0, 0.0, 0.0, 0.4)) + + backend.start() + backend.publish_manifest(SceneManifest("run", 1, (), ())) + assert backend.publish_frame(_empty_frame_with_mesh(initial, 1)) + handle = server.scene.dynamic_mesh_handles[-1] + np.testing.assert_allclose(handle.vertices, initial.vertices) + assert handle.color == (255, 0, 0) + assert handle.opacity == 0.4 + assert handle.side == "double" + assert handle.cast_shadow is False + assert handle.receive_shadow is False + + styled = _mesh_marker(position=(1.0, 2.0, 3.0), color=(0.0, 1.0, 0.0, 0.7)) + assert backend.publish_frame(_empty_frame_with_mesh(styled, 2)) + assert server.scene.dynamic_mesh_handles[-1] is handle + np.testing.assert_allclose(handle.position, [1.0, 2.0, 3.0]) + assert handle.color == (0, 255, 0) + assert handle.opacity == 0.7 + + scaled = _mesh_marker(scale=(2.0, 3.0, 4.0)) + assert backend.publish_frame(_empty_frame_with_mesh(scaled, 3)) + replacement = server.scene.dynamic_mesh_handles[-1] + assert replacement is not handle + assert handle.removed + np.testing.assert_allclose( + replacement.vertices, + scaled.vertices * np.array([2.0, 3.0, 4.0], dtype=np.float32), + ) + + assert backend.publish_frame(_empty_frame_with_mesh(None, 4)) + assert replacement.removed + backend.stop() + + +def test_viser_textured_markers_export_unlit_glb_and_preserve_pose_updates() -> None: + import io + import json + import struct + import trimesh + + server = _Server() + backend = ViserBackend(ViserServerCfg(port=8765), server_factory=lambda **_: server) + texture = np.full((2, 2, 4), [255, 128, 0, 255], dtype=np.uint8) + marker = replace( + _mesh_marker(color=(0.5, 0.5, 0.5, 0.4)), + texture=texture, + uv_coords=np.array([[0, 0], [1, 0], [0, 1]]), + texture_wrap=("mirror", "clamp"), + ) + backend.start() + backend.publish_manifest(SceneManifest("run", 1, (), ())) + backend.publish_frame(_empty_frame_with_mesh(marker, 1)) + handle = server.scene.dynamic_mesh_handles[-1] + json_size = struct.unpack_from(" ( + None +): + server = _Server() + backend = ViserBackend(ViserServerCfg(port=8765), server_factory=lambda **_: server) + geometry = MeshGeometry( + "env-geometry", + np.empty((0, 3), dtype=np.float32), + np.empty((0, 3), dtype=np.uint32), + ) + node = SceneNode( + node_id="env:0/rigid:empty", + path="/envs/0/rigid_objects/empty", + parent_id="env:0", + env_id=0, + kind="rigid_object", + geometry_id=geometry.geometry_id, + ) + frame = replace( + _empty_frame_with_mesh(_mesh_marker(), 1), + node_ids=(node.node_id,), + positions=np.zeros((1, 3), dtype=np.float32), + wxyz=np.array([[1.0, 0.0, 0.0, 0.0]], dtype=np.float32), + visible=np.ones((1,), dtype=np.bool_), + ) + + backend.start() + backend.publish_manifest(SceneManifest("run", 1, (node,), (geometry,))) + assert backend.publish_frame(frame) + handle = server.scene.dynamic_mesh_handles[-1] + assert handle.visible + + server.gui.checkboxes["Environment 0"].callback( + SimpleNamespace(target=SimpleNamespace(value=False)) + ) + backend.poll() + assert not handle.visible + server.gui.checkboxes["Markers"].callback( + SimpleNamespace(target=SimpleNamespace(value=False)) + ) + backend.poll() + server.gui.checkboxes["Environment 0"].callback( + SimpleNamespace(target=SimpleNamespace(value=True)) + ) + backend.poll() + assert not handle.visible + backend.stop() + + def test_viser_backend_uploads_static_mesh_once_and_updates_only_poses() -> None: server = _Server() backend = ViserBackend(ViserServerCfg(port=8765), server_factory=lambda **_: server) @@ -1301,3 +1492,145 @@ def test_viser_backend_disambiguates_compact_joint_labels() -> None: "arm-0": "arm [0] (°)", "arm-1": "arm [1] (°)", } + + +def test_marker_only_environment_gets_controls_and_keeps_hidden_state(): + server = _Server() + backend = ViserBackend(ViserServerCfg(port=8765), server_factory=lambda **_: server) + backend.start() + backend.publish_manifest(SceneManifest("run", 1, (), ())) + frame = _empty_frame_with_mesh(_mesh_marker(env_id=7), 1) + assert backend.publish_frame(frame) + handle = server.scene.dynamic_mesh_handles[-1] + assert "Environment 7" in server.gui.checkboxes + checkbox = server.gui.checkboxes["Environment 7"] + checkbox.callback(SimpleNamespace(target=SimpleNamespace(value=False))) + backend.poll() + assert not handle.visible + assert backend.publish_frame(replace(frame, sequence=2)) + assert server.gui.checkboxes["Environment 7"] is checkbox + assert not handle.visible + backend.stop() + + +def test_marker_environment_stays_hidden_after_last_scene_node_is_removed(): + server = _Server() + backend = ViserBackend(ViserServerCfg(port=8765), server_factory=lambda **_: server) + geometry = MeshGeometry( + "env-geometry", + np.empty((0, 3), dtype=np.float32), + np.empty((0, 3), dtype=np.uint32), + ) + node = SceneNode( + node_id="env:0/rigid:empty", + path="/envs/0/rigid_objects/empty", + parent_id="env:0", + env_id=0, + kind="rigid_object", + geometry_id=geometry.geometry_id, + ) + backend.start() + backend.publish_manifest(SceneManifest("run", 1, (node,), (geometry,))) + frame = replace( + _empty_frame_with_mesh(_mesh_marker(), 1), + node_ids=(node.node_id,), + positions=np.zeros((1, 3), dtype=np.float32), + wxyz=np.array([[1, 0, 0, 0]], dtype=np.float32), + visible=np.ones(1, dtype=bool), + ) + assert backend.publish_frame(frame) + handle = server.scene.dynamic_mesh_handles[-1] + server.gui.checkboxes["Environment 0"].callback( + SimpleNamespace(target=SimpleNamespace(value=False)) + ) + backend.poll() + backend.publish_manifest(SceneManifest("run", 2, (), ())) + assert "Environment 0" in server.gui.checkboxes + assert backend.publish_frame( + replace(_empty_frame_with_mesh(_mesh_marker(), 2), scene_revision=2) + ) + assert not handle.visible + assert server.gui.checkboxes["Environment 0"].value is False + backend.stop() + + +def test_marker_only_many_environments_use_selection_and_world_markers_stay_visible(): + server = _Server() + backend = ViserBackend(ViserServerCfg(port=8765), server_factory=lambda **_: server) + backend.start() + backend.publish_manifest(SceneManifest("run", 1, (), ())) + count = backend._INDIVIDUAL_ENV_CONTROL_LIMIT + 1 + markers = tuple( + replace(_mesh_marker(env_id=i), overlay_id=f"marker-{i}") for i in range(count) + ) + world_marker = replace(_mesh_marker(env_id=None), overlay_id="world-marker") + frame = replace( + _empty_frame_with_mesh(None, 1), + overlays=SceneOverlays(meshes=(*markers, world_marker)), + ) + assert backend.publish_frame(frame) + server.gui.checkboxes["Show all environments"].callback( + SimpleNamespace(target=SimpleNamespace(value=False)) + ) + server.gui.dropdowns["Selected environment"].callback( + SimpleNamespace(target=SimpleNamespace(value="3")) + ) + backend.poll() + handles = server.scene.dynamic_mesh_handles[-count - 1 :] + assert [h.visible for h in handles] == [i == 3 for i in range(count)] + [True] + assert backend.publish_frame(replace(frame, sequence=2)) + assert [h.visible for h in handles] == [i == 3 for i in range(count)] + [True] + assert backend.publish_frame(_empty_frame_with_mesh(world_marker, 3)) + assert not backend._env_visibility + assert server.scene.dynamic_mesh_handles[-1].visible + backend.stop() + + +def test_marker_removal_reselects_and_shows_existing_scene_mesh(): + server = _Server() + backend = ViserBackend(ViserServerCfg(port=8765), server_factory=lambda **_: server) + geometry = MeshGeometry( + "static-geometry", + np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0]], dtype=np.float32), + np.array([[0, 1, 2]], dtype=np.uint32), + ) + node = SceneNode( + "static", "/envs/0/static", "env:0", 0, "rigid_object", geometry.geometry_id + ) + markers = tuple( + replace(_mesh_marker(env_id=i), overlay_id=f"marker-{i}") + for i in range(1, backend._INDIVIDUAL_ENV_CONTROL_LIMIT + 2) + ) + frame = replace( + _empty_frame_with_mesh(None, 1), + node_ids=(node.node_id,), + positions=np.zeros((1, 3), dtype=np.float32), + wxyz=np.array([[1, 0, 0, 0]], dtype=np.float32), + visible=np.ones(1, dtype=bool), + overlays=SceneOverlays(meshes=markers), + ) + backend.start() + backend.publish_manifest(SceneManifest("run", 1, (node,), (geometry,))) + assert backend.publish_frame(frame) + handle = server.scene.mesh_handles[0] + server.gui.checkboxes["Show all environments"].callback( + SimpleNamespace(target=SimpleNamespace(value=False)) + ) + server.gui.dropdowns["Selected environment"].callback( + SimpleNamespace(target=SimpleNamespace(value="3")) + ) + backend.poll() + np.testing.assert_array_equal(handle.batched_opacities, [0]) + for sequence in (2, 3): + assert backend.publish_frame( + replace( + frame, + sequence=sequence, + overlays=SceneOverlays( + meshes=tuple(m for m in markers if m.env_id != 3) + ), + ) + ) + assert server.gui.dropdowns["Selected environment"].value == "0" + np.testing.assert_array_equal(handle.batched_opacities, [1]) + backend.stop() diff --git a/tests/visualization/test_visualization_markers.py b/tests/visualization/test_visualization_markers.py new file mode 100644 index 0000000000..a294d256b5 --- /dev/null +++ b/tests/visualization/test_visualization_markers.py @@ -0,0 +1,114 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + + +import numpy as np +import pytest + +from embodichain.lab.visualization.markers import ( + MarkerGroup, + MarkerPrototypeCfg, + VisualizationMarkers, + VisualizationMarkersCfg, +) + + +class _Sim: + def add_marker_group(self, cfg): + self.group = MarkerGroup( + cfg, num_envs=3, origins=[[0, 0, 0], [10, 0, 0], [20, 0, 0]] + ) + return self.group + + +def _markers(space="world"): + sim = _Sim() + marker = VisualizationMarkers( + VisualizationMarkersCfg( + prim_path="test", + coordinate_space=space, + markers={ + "box": MarkerPrototypeCfg(), + "sphere": MarkerPrototypeCfg(shape="sphere"), + }, + ), + sim, + ) + return marker, sim.group + + +@pytest.mark.parametrize( + "space,expected", [("world", [1, 2, 3]), ("env", [11, 22, 13])] +) +def test_flat_interleaved_environments_and_xyzw(space, expected): + marker, group = _markers(space) + marker.visualize( + [[1, 0, 0], [2, 0, 0], [3, 0, 0]], + orientations=[[0, 0, 1, 0]] * 3, + marker_indices=[0, 1, 0], + environment_ids=[1, 2, 1], + ) + assert group.counts == (0, 2, 1) + snaps = group.snapshot() + np.testing.assert_allclose( + [s.position[0] for s in snaps], np.array(expected)[[0, 2, 1]] + ) + np.testing.assert_allclose(snaps[0].wxyz, [0, 0, 0, 1]) + assert [s.env_id for s in snaps] == [1, 1, 2] + marker.visualize(scales=[[2, 2, 2], [3, 3, 3], [4, 4, 4]]) + np.testing.assert_allclose([s.scale[0] for s in group.snapshot()], [2, 4, 3]) + marker.visualize(marker_indices=[1, 0, 1]) + np.testing.assert_allclose( + [s.position[0] for s in group.snapshot()], np.array(expected)[[0, 2, 1]] + ) + + +def test_hidden_resize_clear_and_no_environment_inference(): + marker, group = _markers() + marker.set_visibility(False) + marker.visualize(np.zeros((6, 3))) + assert group.counts == (6, 0, 0) + assert not any(s.visible for s in group.snapshot()) + marker.set_visibility(True) + assert all(s.visible for s in group.snapshot()) + marker.visualize(np.empty((0, 3))) + assert marker.count == 0 and group.snapshot() == () + marker.remove() + marker.remove() + with pytest.raises(RuntimeError): + marker.visualize([[0, 0, 0]]) + + +@pytest.mark.parametrize( + "kwargs", + [ + {"translations": [[float("nan"), 0, 0]]}, + {"translations": [[0, 0]]}, + {"environment_ids": [3]}, + {"environment_ids": [True]}, + {"environment_ids": [1.0]}, + {"environment_ids": [1]}, + ], +) +def test_failed_flat_update_preserves_previous_state(kwargs): + marker, group = _markers() + marker.visualize([[1, 2, 3]]) + with pytest.raises(ValueError): + marker.visualize(**kwargs) + np.testing.assert_allclose(group.snapshot()[0].position, [1, 2, 3]) + assert marker.count == 1