diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 550d70b338..5787019196 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -180,7 +180,7 @@ topics: aliases: [browser visualization, viser, sim visualization, marker visualization, 标志物可视化, 仿真可视化] keywords: [VisualizationCfg, ViserServerCfg, SceneManifest, visualization, EntityGizmoManipulator, robot_ik_gizmo, GizmoCfg, MarkerGroup, MarkerPrototypeCfg, RobotMarkers, VisualizationMarkers, robot markers, ghost robot, 机器人可视化, backpressure, capture_visualization, Newton deformable, particle set, - render topology, quaternion, xyzw] + render topology, quaternion, xyzw, cable visualization, cable capsule] paths: [topics/sim-visualization/sim-visualization.md] source_of_truth: - embodichain/lab/visualization/protocol.py diff --git a/agent_context/topics/env-framework/env-framework.md b/agent_context/topics/env-framework/env-framework.md index d7b9505e3a..b0936f8ed9 100644 --- a/agent_context/topics/env-framework/env-framework.md +++ b/agent_context/topics/env-framework/env-framework.md @@ -23,6 +23,7 @@ managers, demonstration collection and the opt-in Task Program bridge. | Raw schema, optional history and exact historical queries | `managers/observation_manager.py`; Env dispatches capture/reset/close boundaries | | Optional measured physical objectives | `embodichain/lab/gym/envs/objectives/`; Gym owns observation/reset hooks independently of expert acceptance | | Controller-ready commands | `embodichain/lab/gym/envs/types.py`: `ControllerAction` | +| Cable scene declarations and raw recording | `gym_utils.py` → `EmbodiedEnv._setup_interactive_objects()`; `trajectory_state.py` → `observations.get_entity_state` | | Demo segment execution / outcomes / collection qualification | `embodichain/lab/gym/envs/demo.py` | | Configured Task Program registration | `embodichain/lab/gym/envs/task_program/registration.py` | | Timing instrumentation | `embodichain/lab/gym/utils/profiler.py` | diff --git a/agent_context/topics/env-framework/execution.md b/agent_context/topics/env-framework/execution.md index 2d502f7d08..c6c5e65e94 100644 --- a/agent_context/topics/env-framework/execution.md +++ b/agent_context/topics/env-framework/execution.md @@ -303,7 +303,8 @@ not orchestrate multiple experiments or qualify kinematic playback. cursors keep their accepted-versus-diagnostic semantics; there is no independent history instance or write lifecycle. Standard state fields use observation functors; the host does not perform a separate physics read in its write hook. Full robot state, - articulations and selected rigid objects retain their existing action schema. + articulations, selected rigid objects and cable segments share that acquisition + and retain their existing action schema. `cfg.trajectory_uids` restricts non-robot replay fields. Raw channels, T+1 timing and lifetime queries are owned by the [recording contract](env-framework.md#expert-trajectory-contract). @@ -347,6 +348,21 @@ not orchestrate multiple experiments or qualify kinematic playback. keeps its own cursor and layout. Internally allocated expert policy observations include the terminal T+1 slot in the backing store; the public T-transition view and existing dataset action/annotation alignment stay compatible. +- **Cable observations**: `states.cables[uid].segment_state_w` and the matching + raw observation field share the T+1 rollout storage. `get_entity_state` describes + world segment position, XYZW orientation, COM linear velocity and angular + velocity, preserving each cable's segment topology and row validity. The field + records observations, not solver history. `ReplayWrapper` accepts dynamic + replay only when every recorded initial state matches the full reset; it + rejects scrubbing, partial reset and broadcasting world-space cable states. + Cable artifacts store `meta.arena_origins_w` in exported row order. Replay + translates captured reset positions into the source world frame for comparison, + allowing a per-environment export to replay at a different arena origin. + Raw observations, orientations and velocities retain their original meaning. + Older files without origin metadata require matching world placement. + `trajectory_state.restore_trajectory_state()` rejects cable entries before + any write. Use action replay to validate a new episode, not a visible-state + restore as a history-preserving checkpoint. - **Fixed action layout**: an ActionManager-backed trajectory uses the batched policy action space; a controller/expert trajectory uses the private `ExpertActionSpec` layout. One trajectory cannot mix these schemas. diff --git a/agent_context/topics/manager-functor/manager-functor.md b/agent_context/topics/manager-functor/manager-functor.md index 95beff4991..0e5adbcfa8 100644 --- a/agent_context/topics/manager-functor/manager-functor.md +++ b/agent_context/topics/manager-functor/manager-functor.md @@ -104,8 +104,12 @@ matching their source functions/parameters, including callable strings and omitted default arguments. Env only supplies the declarations. Recording-only fields never enlarge policy observations or policy rollout allocation. -`observations.get_entity_state` derives native UID, quantity, arena/joint frame, -representation, shape, dtype and joint order from its source. Immutable +`observations.get_entity_state` derives native UID, quantity, reference frame, +representation, shape, dtype and joint order from its source. Cable +`segment_state_w` uses world coordinates and a segment axis; its descriptor +includes COM linear velocity and angular velocity alongside position and XYZW +orientation. Recording declares these fields through `trajectory_observation_cfgs`, +so cable observations share the ordinary acquisition and history lifecycle. Immutable schema descriptors are reused by semantic value; no simulation handles enter the bounded cache. Pose acquisition uses the resolved UID without constructing another `SceneEntityCfg` during each step. Explicit boundaries diff --git a/agent_context/topics/sim-visualization/configuration.md b/agent_context/topics/sim-visualization/configuration.md index 5d535d012b..7907ce5aee 100644 --- a/agent_context/topics/sim-visualization/configuration.md +++ b/agent_context/topics/sim-visualization/configuration.md @@ -19,7 +19,7 @@ are nested under `visualization.viser_server`; there is no top-level | `point_cloud_max_points` | `100000` | Deterministic per-cloud downsampling limit | | `sensor_image_fps` | `2.0` | Maximum RGB image capture rate; `None` captures once per eligible simulation step | | `soft_body_fps` | `5.0` | Maximum soft-body and cloth vertex capture rate | -| `allow_commands` | `False` | Allow trusted Viser clients to drag configured Gizmos; common `--viser` launchers set this to `True` | +| `allow_commands` | `False` | Allow trusted Viser clients to drag configured Gizmos; common `--viser` launchers enable commands when the deployment does not specify a policy | | `viser_server` | `ViserServerCfg()` | Server bind configuration | ### `ViserServerCfg` @@ -76,7 +76,10 @@ manifest when manager APIs marked the topology dirty. `merge_args_with_gym_config()` makes `--viser` set `headless=True`, changes the backend to `"viser"`, and applies the CLI sampling/server overrides. A -file-based config may also contain a `visualization` section. The old nested +file-based config may also contain a `visualization` section. An explicit +`visualization.allow_commands=False` remains read-only when `--viser` enables +the viewer, so automated deployments can reserve robot drive targets for their +expert. The old nested key `visualization.server` is accepted and normalized to `visualization.viser_server`. @@ -87,9 +90,9 @@ limiting. `visualization_cfg_from_args()` only builds visualization config. `SimulationManagerCfg` makes the simulation headless whenever the resulting -backend is `"viser"`. The common `--viser` option also enables Gizmo commands; -there is no separate `--viser-gizmo` option. Programmatic configurations may -retain `allow_commands=False` for read-only deployments. +backend is `"viser"`. The common `--viser` option defaults to enabling Gizmo +commands when no file policy is present; there is no separate `--viser-gizmo` +option. Programmatic configurations retain their configured command policy. ### Asset preview diff --git a/agent_context/topics/sim-visualization/runtime.md b/agent_context/topics/sim-visualization/runtime.md index 2f2c475e1b..b862655b80 100644 --- a/agent_context/topics/sim-visualization/runtime.md +++ b/agent_context/topics/sim-visualization/runtime.md @@ -6,7 +6,7 @@ Read this when the request needs these details. [Topic overview](sim-visualizati | API | Behavior | |---|---| -| `start_visualization()` | No-op for backend `"none"`; otherwise starts or returns the active runtime and forces an initial capture | +| `start_visualization()` | No-op for backend `"none"`; prepares declared assets (including cable-only scenes), otherwise leaves an empty Spawn builder open; starts or returns the active runtime and forces an initial capture | | `notify_visualization_topology_changed()` | Increments the manager-local topology revision | | `refresh_visualization()` | Rebuilds and asynchronously publishes a manifest immediately | | `capture_visualization(force=False)` | Refreshes dirty topology, then captures a due frame | @@ -54,7 +54,7 @@ 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 +volume/surface deformables, cables, robots, articulations, and `Camera` sensors. Supported `remove_asset()` branches do the same. The next simulation update refreshes the browser automatically. Use `refresh_visualization()` when the refresh must happen before another physics step. Code that changes mesh topology outside @@ -80,6 +80,11 @@ fields already named `wxyz` remain protocol-native. Arena offsets are added to rigid, robot, articulation, and camera poses. Deformable vertices are stored relative to the corresponding arena node. +Cable `segment_pose_w` already contains world positions, including Arena +offsets; export must not add those offsets again. +Pose export gathers visible environment/component rows on the source device +before CPU conversion and quaternion normalization. Native state fetching remains +asset-owned; host conversion scales with the exported node count. ### Exported content @@ -87,6 +92,7 @@ Deformable vertices are stored relative to the corresponding arena node. |---|---| | `RigidObject` | Complete combined render mesh; multi-segment faces receive matching vertex offsets | | `RigidObjectGroup` | One node and pose per constituent object | +| `CableObject` | One capsule node per segment, shared rest geometry, and live world poses | | `Robot` | One mesh node per non-empty link | | `Articulation` | One mesh node per non-empty link | | `VolumeDeformableObject` | Live Newton render-surface vertices and triangles | @@ -101,6 +107,13 @@ instead travel as `MeshMarkerOverlay` entries. Visual materials and textures are not mirrored; meshes use category colors from `SceneExporter._COLORS`. +Cables use capsule observer geometry generated from configured endpoint +distances and radius, centered along each segment's local +Z axis. Export does +not depend on native rendering resources and does not reproduce tube skins or +custom visual meshes. Prepared cable data is sampled once per cable per frame; +segment poses update at `scene_fps` through the static-mesh batch path, without +rebuilding geometry or using the deformable-vertex channel. + ## Cameras Only sensors whose `sensor_type` is exactly `"Camera"` are added to the diff --git a/agent_context/topics/sim-visualization/sim-visualization.md b/agent_context/topics/sim-visualization/sim-visualization.md index 89500907df..a9b1121267 100644 --- a/agent_context/topics/sim-visualization/sim-visualization.md +++ b/agent_context/topics/sim-visualization/sim-visualization.md @@ -137,7 +137,8 @@ this invariant. | Symptom | Cause / action | |---|---| | `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`. | +| Startup timeout, possibly just after Viser starts listening | The worker startup budget includes Viser frontend dependency installation/build and initial manifest publication. `VisualizationRuntime.start()` waits up to 120 seconds by default and returns as soon as ready; an unusually slow build may need a longer explicit `start(timeout=...)`. Inspect startup logs before retrying. | +| Address-in-use error | 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. | diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 094c023c5b..2c32e843f2 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -56,6 +56,7 @@ VisualObjectCfg, RigidObjectGroupCfg, ArticulationCfg, + CableObjectCfg, LightCfg, ) from embodichain.lab.sim.objects import Robot @@ -307,6 +308,8 @@ class EnvLightCfg: articulation: List[ArticulationCfg] = [] + cable_object: list[CableObjectCfg] = [] + """Fixed-topology cables declared before simulation preparation.""" fluid_object: list[FluidObjectCfg] = [] """Fixed-count fluids owned by the physical environment component.""" @@ -3790,6 +3793,8 @@ def validate_resolved(cfg: RigidObjectCfg) -> None: for cfg in self.cfg.rigid_object_group: self.sim.add_rigid_object_group(cfg=cfg) + for cfg in self.cfg.cable_object: + self.sim.add_cable_object(cfg=cfg) for cfg in self.cfg.fluid_object: self.sim.add_fluid_object(cfg) @@ -4297,6 +4302,12 @@ def save_trajectory( self.get_demo_episode_metadata(int(env_id)) for env_id in env_ids ], } + if "cables" in states.keys(): + # Cable observations retain world coordinates. Keep each exported + # row's origin so action replay can validate another arena placement. + meta["arena_origins_w"] = ( + self.sim.arena_offsets[env_ids_t].detach().cpu().tolist() + ) meta.update(self._trajectory_action_metadata()) final_observations = _export_final_observations( getattr(self, "_final_acceptance_rules", ()), diff --git a/embodichain/lab/gym/envs/managers/observations.py b/embodichain/lab/gym/envs/managers/observations.py index 1bbb7efaca..6289662b20 100644 --- a/embodichain/lab/gym/envs/managers/observations.py +++ b/embodichain/lab/gym/envs/managers/observations.py @@ -84,6 +84,16 @@ def _state_descriptor( names: tuple[str, ...], ) -> ObservationDescriptor: # Only immutable schema values are cached; no environment or asset handle. + if quantity == "segment_state_w": + return ObservationDescriptor( + uid, + quantity, + "world", + "xyz_xyzw_com_lin_vel_ang_vel", + ("m", "unitless", "m/s", "rad/s"), + shape, + dtype, + ) joint = quantity in ("qpos", "qvel") pose = quantity in ("pose", "segment_start_pose") return ObservationDescriptor( @@ -132,7 +142,9 @@ def get_entity_state( env: EmbodiedEnv, obs: EnvObs, entity_cfg: SceneEntityCfg, - quantity: Literal["pose", "qpos", "qvel", "lin_vel", "ang_vel"] = "pose", + quantity: Literal[ + "pose", "qpos", "qvel", "lin_vel", "ang_vel", "segment_state_w" + ] = "pose", ) -> ObservationSample: """Acquire a described original state channel for any observation consumer. @@ -140,7 +152,9 @@ def get_entity_state( functors. Missing entities remain invalid, even though policy views retain the historical zero-fill behavior. Joint channels contain the complete asset-defined order, including mimic joints; selectors are rejected rather - than silently describing a subset as the complete state. + than silently describing a subset as the complete state. Cable + ``segment_state_w`` contains each segment's world position, XYZW quaternion, + COM linear velocity and angular velocity; it does not include solver history. Args: env: Environment owning the native scene. @@ -151,7 +165,14 @@ def get_entity_state( Returns: Raw values and explicit row validity, before policy transformations. """ - if quantity not in ("pose", "qpos", "qvel", "lin_vel", "ang_vel"): + if quantity not in ( + "pose", + "qpos", + "qvel", + "lin_vel", + "ang_vel", + "segment_state_w", + ): raise ValueError(f"Unsupported raw state quantity: {quantity!r}.") if entity_cfg.joint_names is not None or entity_cfg.joint_ids != slice(None): raise ValueError("Raw entity state requires the complete joint order.") @@ -169,11 +190,14 @@ def produce() -> ObservationSample: joint = quantity in ("qpos", "qvel") if obj is None: width = 0 if joint else 7 if quantity == "pose" else 3 - values = torch.zeros((env.num_envs, width), device=env.device) + shape = (0, 13) if quantity == "segment_state_w" else (width,) + values = torch.zeros((env.num_envs, *shape), device=env.device) valid = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) errors = (f"Entity {uid!r} is unavailable.",) * env.num_envs else: - if quantity == "pose": + if quantity == "segment_state_w": + values = obj.data.segment_state_w + elif quantity == "pose": values = obj.get_local_pose(to_matrix=False) elif joint: values = obj.get_qpos() if quantity == "qpos" else obj.get_qvel() @@ -184,9 +208,9 @@ def produce() -> ObservationSample: else: values = getattr(obj.body_data, quantity) valid = torch.isfinite(values).reshape(env.num_envs, -1).all(dim=-1) - if quantity == "pose": + if quantity in ("pose", "segment_state_w"): valid &= ( - (torch.linalg.vector_norm(values[..., 3:], dim=-1) > 0) + (torch.linalg.vector_norm(values[..., 3:7], dim=-1) > 0) .reshape(env.num_envs, -1) .all(dim=-1) ) diff --git a/embodichain/lab/gym/envs/wrapper/replay.py b/embodichain/lab/gym/envs/wrapper/replay.py index 01bc978f32..8746d4b38b 100644 --- a/embodichain/lab/gym/envs/wrapper/replay.py +++ b/embodichain/lab/gym/envs/wrapper/replay.py @@ -34,7 +34,10 @@ ) from embodichain.lab.gym.envs.types import ControllerAction from embodichain.lab.gym.utils.gym_utils import load_trajectory -from embodichain.lab.gym.utils.trajectory_state import restore_trajectory_state +from embodichain.lab.gym.utils.trajectory_state import ( + capture_trajectory_state, + restore_trajectory_state, +) from embodichain.utils import logger if TYPE_CHECKING: @@ -67,6 +70,12 @@ class ReplayWrapper(gym.Wrapper): full resets only. Stepping past exhaustion raises instead of extending objective dwell time by repeating the final command. + Cable recordings support dynamic replay from matching full-reset states + only. Their visible state omits contact/solver history, so state scrubbing, + partial resets and broadcasting world-space cable recordings are rejected. + Source arena origins, when recorded, translate the captured reset positions + into the saved frame for validation. Older files require matching placement. + For a deployment with final acceptance rules, ``final_acceptance`` reports the offline evaluation of the saved terminal capture, independently of physics replay and Gym termination flags. @@ -95,6 +104,33 @@ def __init__( self._mode = mode self._trajectory = load_trajectory(trajectory) meta = self._trajectory["meta"] + self._cable_replay = "cables" in self._trajectory["states"].keys() + self._cable_source_origins: torch.Tensor | None = None + if ( + getattr(getattr(env, "sim", None), "_cable_objects", {}) + and not self._cable_replay + ): + raise ValueError( + "Cable replay requires recorded initial states for every scene cable; include them in trajectory_uids." + ) + if self._cable_replay: + if mode != "dynamic": + raise NotImplementedError( + "Cable recordings require dynamic replay from reset." + ) + if int(meta["num_envs"]) != env.num_envs: + raise ValueError( + "World-space cable recordings require matching environment counts." + ) + if "arena_origins_w" in meta: + origins = torch.as_tensor( + meta["arena_origins_w"], dtype=torch.float32, device=env.device + ) + if origins.shape != (env.num_envs, 3) or not bool( + torch.isfinite(origins).all() + ): + raise ValueError("arena_origins_w must be finite (num_envs, 3).") + self._cable_source_origins = origins self._fluid_reader = fluid_reader if getattr(getattr(env, "cfg", None), "fluid_object", ()): if self._fluid_reader is None: @@ -249,17 +285,26 @@ def final_acceptance(self) -> tuple[dict[str, Any], ...] | None: def reset( self, *, seed: int | None = None, options: dict | None = None ) -> tuple[EnvObs, dict]: - if self._objective_replay and options is not None and "reset_ids" in options: - raise ValueError("Physical-objective replay does not support reset_ids.") + if ( + (self._objective_replay or self._cable_replay) + and options is not None + and "reset_ids" in options + ): + raise ValueError( + "Physical-objective/cable replay does not support reset_ids." + ) obs, info = self.env.reset(seed=seed, options=options) # Disable physics during restore so set_local_pose's internal update # does not integrate dynamics. - self.env.sim.enable_physics(False) - restore_trajectory_state(self.env, self._trajectory["states"][:, 0]) - if self._fluid_reader is not None: - self._fluid_reader.apply( - self.env, torch.zeros(self.env.num_envs, dtype=torch.long) - ) + if self._cable_replay: + self._validate_cable_reset_state() + else: + self.env.sim.enable_physics(False) + restore_trajectory_state(self.env, self._trajectory["states"][:, 0]) + if self._fluid_reader is not None: + self._fluid_reader.apply( + self.env, torch.zeros(self.env.num_envs, dtype=torch.long) + ) if self._mode == "dynamic": self.env.sim.enable_physics(True) self.env._replay_no_auto_reset = True @@ -286,6 +331,46 @@ def reset( self.env._record_initial_observation(frame, env_ids) return obs, info + def _validate_cable_reset_state(self) -> None: + """Validate initial observations without approximating a checkpoint.""" + initial = self._trajectory["states"][:, 0] + for category, assets in ( + ("cables", getattr(self.env.sim, "_cable_objects", {})), + ("articulations", self.env.sim._articulations), + ("rigid_objects", self.env.sim._rigid_objects), + ): + recorded = ( + set(initial[category].keys()) if category in initial.keys() else set() + ) + if not recorded.issubset(assets) or ( + category == "cables" and recorded != set(assets) + ): + raise ValueError( + f"Cable replay scene does not match recorded {category}." + ) + current = initial.clone() + capture_trajectory_state( + self.env, current, torch.arange(self.env.num_envs, device=self.env.device) + ) + if self._cable_source_origins is not None: + translation = self._cable_source_origins - self.env.sim.arena_offsets.to( + self._cable_source_origins + ) + # Compare the captured reset in the saved world's frame. The live + # scene and the recording keep their original poses and history. + for cable in current["cables"].values(): + state = cable["segment_state_w"] + state[..., :3] += translation.to(state)[:, None, :] + for key, expected in initial.items(include_nested=True, leaves_only=True): + observed = current[key].to(expected) + if observed.shape != expected.shape or not torch.allclose( + observed, expected, rtol=1e-5, atol=1e-6 + ): + raise ValueError( + f"Cable replay initial state {key!r} differs from the full reset; " + "a recorded observation cannot restore solver history." + ) + def step( self, action: Any ) -> tuple[EnvObs, torch.Tensor, torch.Tensor, torch.Tensor, dict]: @@ -341,6 +426,10 @@ def go_to_step(self, step: int) -> EnvObs: Returns: The observation at the target step. """ + if self._cable_replay: + raise NotImplementedError( + "Cable recordings do not support state scrubbing." + ) env = self.env max_step = self.control_max_step step = max(0, min(int(step), max_step)) diff --git a/embodichain/lab/gym/utils/_component_composition.py b/embodichain/lab/gym/utils/_component_composition.py index 0e0f35e656..e2ea19e8e8 100644 --- a/embodichain/lab/gym/utils/_component_composition.py +++ b/embodichain/lab/gym/utils/_component_composition.py @@ -62,6 +62,7 @@ "rigid_object", "rigid_object_group", "articulation", + "cable_object", "fluid_object", } ) diff --git a/embodichain/lab/gym/utils/gym_utils.py b/embodichain/lab/gym/utils/gym_utils.py index 528019e46c..db26f6e92d 100644 --- a/embodichain/lab/gym/utils/gym_utils.py +++ b/embodichain/lab/gym/utils/gym_utils.py @@ -689,6 +689,7 @@ def config_to_cfg( MeshCfg, RigidObjectGroupCfg, ArticulationCfg, + CableObjectCfg, LightCfg, DLSSCfg, NRDCfg, @@ -1074,6 +1075,16 @@ class ComponentCfg: resolve_asset_paths(cfg) env_cfg.articulation.append(cfg) + env_cfg.cable_object = [] + for value in config.get("cable_object", []): + cable = CableObjectCfg.from_dict(value) + if ( + isinstance(cable.visual_shape, MeshCfg) + and cable.visual_shape.fpath is not None + ): + cable.visual_shape.fpath = get_data_path(cable.visual_shape.fpath) + env_cfg.cable_object.append(cable) + env_cfg.control_parts = config["env"].get("control_parts", None) env_cfg.sim_steps_per_control = config["env"].get("sim_steps_per_control", 4) ignore_terminations = config["env"].get( @@ -1678,7 +1689,9 @@ def merge_args_with_gym_config(args: argparse.Namespace, gym_config: dict) -> di if cli_visualization.env_ids is None else list(cli_visualization.env_ids) ) - visualization["allow_commands"] = cli_visualization.allow_commands + # Enabling the viewer must preserve a deployment's explicit read-only + # policy, especially while an expert owns the robot drive targets. + visualization.setdefault("allow_commands", cli_visualization.allow_commands) legacy_server = visualization.pop("server", {}) viser_server = deepcopy(visualization.get("viser_server", legacy_server)) viser_server["host"] = cli_visualization.viser_server.host @@ -2441,6 +2454,29 @@ def _zeros(*shape: int) -> torch.Tensor: device=device, ) + cables = { + uid: obj + for uid, obj in getattr(env.sim, "_cable_objects", {}).items() + if uids is None or uid in uids + } + if cables: + states["cables"] = TensorDict( + { + uid: TensorDict( + { + "segment_state_w": _zeros( + num_envs, max_steps, obj.data.num_segments, 13 + ) + }, + batch_size=[num_envs, max_steps], + device=device, + ) + for uid, obj in cables.items() + }, + batch_size=[num_envs, max_steps], + device=device, + ) + td: dict = { "states": TensorDict(states, batch_size=[num_envs, max_steps], device=device) } diff --git a/embodichain/lab/gym/utils/trajectory_state.py b/embodichain/lab/gym/utils/trajectory_state.py index 821a0896f5..4843947512 100644 --- a/embodichain/lab/gym/utils/trajectory_state.py +++ b/embodichain/lab/gym/utils/trajectory_state.py @@ -87,7 +87,10 @@ def capture_trajectory_state( env_ids: torch.Tensor, step_ids: torch.Tensor | None = None, ) -> None: - """Capture restorable simulation state into a trajectory state view. + """Capture robot/rigid states and cable observations into a trajectory view. + + Cable segment states contain world XYZ, XYZW, COM linear velocity and + angular velocity. They omit solver history and do not define a checkpoint. Args: env: Environment containing the robot and simulation scene entities. @@ -126,6 +129,16 @@ def capture_trajectory_state( step_ids, ) + if "cables" in states.keys(): + for uid, cable in env.sim._cable_objects.items(): + if uid in states["cables"].keys(): + _assign_state( + states["cables"][uid]["segment_state_w"], + cable.data.segment_state_w, + env_ids, + step_ids, + ) + if "rigid_objects" in states.keys(): rigid_states = states["rigid_objects"] for uid, rigid_object in env.sim._rigid_objects.items(): @@ -160,10 +173,22 @@ def restore_trajectory_state( ) -> None: """Restore one trajectory timestep into an environment scene. + Cable recordings require action replay from an episode reset. A cable + entry is rejected before any robot or object write, because restoring its + visible pose and velocity cannot restore solver/contact history. + Args: env: Environment containing the robot and simulation scene entities. states: One timestep of recorded robot and scene-object states. + + Raises: + NotImplementedError: The recording contains cable observations. """ + if "cables" in states.keys(): + raise NotImplementedError( + "Recorded cable state is an observation, not a solver-history " + "checkpoint. Reset the episode and replay its commands instead." + ) robot_states = states["robot"] env.robot.set_local_pose(robot_states["root_pose"]) # Restore complete qpos, including mimic joints. Kinematic replay disables @@ -352,6 +377,9 @@ def add(prefix: str, uid: str, quantities: tuple[str, ...]) -> None: for uid in env.sim._rigid_objects: if uids is None or uid in uids: add("rigid_objects", uid, ("pose", "lin_vel", "ang_vel")) + for uid in getattr(env.sim, "_cable_objects", {}): + if uids is None or uid in uids: + add("cables", uid, ("segment_state_w",)) return result diff --git a/embodichain/lab/visualization/runtime.py b/embodichain/lab/visualization/runtime.py index 8bf02824a0..84c4d9d295 100644 --- a/embodichain/lab/visualization/runtime.py +++ b/embodichain/lab/visualization/runtime.py @@ -443,8 +443,14 @@ def _raise_worker_error(self) -> None: "Visualization update thread failed." ) from self._worker_error - def start(self, timeout: float = 10.0) -> None: - """Build the initial manifest and start the backend update thread.""" + def start(self, timeout: float = 120.0) -> None: + """Build the initial manifest and start the backend update thread. + + Args: + timeout: Maximum seconds to wait for backend readiness, including + any frontend dependency installation/build and initial manifest + publication. Returns as soon as the backend is ready. + """ if self.is_running: return manifest = self.exporter.build_manifest() @@ -461,7 +467,11 @@ def start(self, timeout: float = 10.0) -> None: self._thread.start() if not self._ready_event.wait(timeout=timeout): self.stop(timeout=timeout) - raise TimeoutError("Timed out while starting the visualization backend.") + raise TimeoutError( + f"Timed out after {timeout:g}s while starting the visualization backend. " + "Check backend startup logs; frontend installation/build may " + "require a longer start(timeout=...) budget." + ) self._raise_worker_error() self._next_capture_time = perf_counter() self._next_image_capture_time = perf_counter() diff --git a/embodichain/lab/visualization/scene_exporter.py b/embodichain/lab/visualization/scene_exporter.py index 7f0e827825..614d5d516b 100644 --- a/embodichain/lab/visualization/scene_exporter.py +++ b/embodichain/lab/visualization/scene_exporter.py @@ -85,6 +85,44 @@ def mesh_geometry_id( return f"sha256:{digest.hexdigest()}" +def _capsule_mesh(radius: float, length: float) -> tuple[np.ndarray, np.ndarray]: + """Build a centered +Z capsule; length is the distance between cap centers.""" + radial_segments = 12 + cap_segments = 4 + angles = np.arange(radial_segments) * (2.0 * np.pi / radial_segments) + latitudes = np.linspace(0.0, np.pi / 2.0, cap_segments + 1)[:-1] + ring_radii = radius * np.cos(np.concatenate((-latitudes[::-1], latitudes))) + ring_heights = np.concatenate( + ( + -length / 2.0 - radius * np.sin(latitudes[::-1]), + length / 2.0 + radius * np.sin(latitudes), + ) + ) + rings = np.empty((len(ring_heights), radial_segments, 3), dtype=np.float32) + rings[..., 0] = ring_radii[:, None] * np.cos(angles) + rings[..., 1] = ring_radii[:, None] * np.sin(angles) + rings[..., 2] = ring_heights[:, None] + vertices = np.concatenate( + ( + [[0.0, 0.0, -length / 2.0 - radius]], + rings.reshape(-1, 3), + [[0.0, 0.0, length / 2.0 + radius]], + ) + ).astype(np.float32) + faces: list[tuple[int, int, int]] = [] + for side in range(radial_segments): + next_side = (side + 1) % radial_segments + faces.append((0, 1 + next_side, 1 + side)) + for ring in range(len(ring_heights) - 1): + lower = 1 + ring * radial_segments + upper = lower + radial_segments + faces.append((lower + side, lower + next_side, upper + side)) + faces.append((lower + next_side, upper + next_side, upper + side)) + top_ring = 1 + (len(ring_heights) - 1) * radial_segments + faces.append((top_ring + side, top_ring + next_side, len(vertices) - 1)) + return vertices, np.asarray(faces, dtype=np.uint32) + + @dataclass(frozen=True) class CaptureResult: """A captured frame and the time spent copying scene data.""" @@ -106,8 +144,7 @@ class _NodeSource: node: SceneNode asset_key: tuple[str, str] asset: object - link_index: int | None = None - object_index: int | None = None + component_index: int | None = None @dataclass(frozen=True) @@ -154,6 +191,7 @@ class SceneExporter: "articulation_link": (255, 175, 70), "soft_object": (230, 120, 255), "cloth_object": (255, 105, 145), + "cable_segment": (255, 210, 85), } def __init__( @@ -240,12 +278,7 @@ def _build_pose_batches(sources: tuple[_NodeSource, ...]) -> tuple[_PoseBatch, . batches: list[_PoseBatch] = [] for asset_key, indexed_sources in grouped.items(): component_indices = [ - ( - source.link_index - if source.link_index is not None - else source.object_index - ) - for _, source in indexed_sources + source.component_index for _, source in indexed_sources ] has_components = component_indices[0] is not None if any( @@ -364,6 +397,7 @@ def build_manifest(self) -> SceneManifest: ) self._append_rigid_object_groups(sources, geometries) + self._append_cable_objects(sources, geometries) self._append_articulations( sources=sources, geometries=geometries, @@ -563,7 +597,65 @@ def _append_rigid_object_groups( node=node, asset_key=("rigid_group", uid), asset=asset, - object_index=object_index, + component_index=object_index, + ) + ) + + def _append_cable_objects( + self, + sources: list[_NodeSource], + geometries: dict[str, MeshGeometry], + ) -> None: + """Export segment capsules independently of native tube/custom visuals.""" + geometry_ids: dict[tuple[float, float], str] = {} + for uid in self._sim.get_cable_object_uid_list(): + asset = self._sim.get_cable_object(uid) + if asset is None: + continue + data = asset.data + if data is None: + raise RuntimeError( + f"Call SimulationManager.prepare() before exporting cable {uid!r}." + ) + points = _to_numpy(asset.cfg.points, np.float32, copy=False) + lengths = np.linalg.norm(np.diff(points, axis=0), axis=1) + if data.num_segments != len(lengths): + raise ValueError( + f"Cable {uid!r} has {data.num_segments} segments but " + f"its configured points describe {len(lengths)}." + ) + radius = float(asset.cfg.radius) + uid_component = safe_path_component(uid) + for segment_index, length in enumerate(lengths): + dimensions = (radius, float(length)) + if dimensions not in geometry_ids: + vertices, faces = _capsule_mesh(*dimensions) + geometry_id = self._add_geometry( + geometries, vertices, faces, self._COLORS["cable_segment"] + ) + assert geometry_id is not None + geometry_ids[dimensions] = geometry_id + for env_id in self._env_ids: + node = SceneNode( + node_id=( + f"env:{env_id}/cable:{uid_component}/" + f"segment:{segment_index}" + ), + path=( + f"/envs/{env_id}/cable_objects/{uid_component}/" + f"segments/{segment_index}" + ), + parent_id=f"env:{env_id}/cable:{uid_component}", + env_id=env_id, + kind="cable_segment", + geometry_id=geometry_ids[dimensions], + ) + sources.append( + _NodeSource( + node=node, + asset_key=("cable", uid), + asset=asset, + component_index=segment_index, ) ) @@ -723,7 +815,7 @@ def _append_articulations( node=node, asset_key=(asset_prefix, uid), asset=asset, - link_index=link_index, + component_index=link_index, ) ) @@ -904,24 +996,26 @@ def capture( ) if batch.asset_kind in {"rigid", "rigid_group", "visual"}: pose = batch.asset.get_local_pose(to_matrix=False) + elif batch.asset_kind == "cable": + data = batch.asset.data + if data is None: + raise RuntimeError( + "Call SimulationManager.prepare() before capturing cable state." + ) + pose = data.segment_pose_w else: pose = batch.asset.body_data.body_link_pose - all_positions, all_wxyz = pose_to_position_wxyz(pose) if batch.component_indices is None: - batch_positions = all_positions[batch.env_ids] - batch_wxyz = all_wxyz[batch.env_ids] + pose = pose[batch.env_ids] else: - batch_positions = all_positions[ - batch.env_ids, - batch.component_indices, - ] - batch_wxyz = all_wxyz[ - batch.env_ids, - batch.component_indices, - ] - positions[batch.destination_indices] = ( - batch_positions + self._env_offsets[batch.env_ids] - ) + pose = pose[batch.env_ids, batch.component_indices] + # Gather on the source device before CPU conversion and quaternion + # normalization, so export scales with the visible node count. + batch_positions, batch_wxyz = pose_to_position_wxyz(pose) + # Cable snapshots already contain world positions, including Arena origins. + if batch.asset_kind != "cable": + batch_positions = batch_positions + self._env_offsets[batch.env_ids] + positions[batch.destination_indices] = batch_positions wxyz[batch.destination_indices] = batch_wxyz dynamic_meshes: list[DynamicMeshUpdate] = [] diff --git a/examples/README.md b/examples/README.md index 29d45a38e8..5480107f3b 100644 --- a/examples/README.md +++ b/examples/README.md @@ -134,3 +134,10 @@ EmbodiChain defaults to DexUni when no solver is selected; legacy `auto`/`AutoSolverCfg` selections must migrate to `dexuni` or the concrete solver needed by the scene. Explicit `semi_implicit` remains required for Newton gradient mode. + +The [cable Viser demo](sim/demo/cable_viser.py) displays replicated segment +capsules in the browser: + +```bash +python -m embodichain run-example sim/demo/cable_viser --num-envs 2 --port 8080 --duration 12 +``` diff --git a/examples/sim/demo/cable_viser.py b/examples/sim/demo/cable_viser.py new file mode 100644 index 0000000000..13f6e32806 --- /dev/null +++ b/examples/sim/demo/cable_viser.py @@ -0,0 +1,115 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Preview replicated, anchored cables in Viser until Ctrl+C. + +Requires CUDA and a DexSim build with cable support. Viser displays segment +capsules without a native renderer. Gravity bends each cable; the scene resets +every six simulated seconds so newly connected browsers can see the motion. +""" + +from __future__ import annotations + +import argparse +import time + +import numpy as np + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import ( + CableObjectCfg, + CablePhysicsCfg, + NewtonPhysicsCfg, + RenderCfg, +) +from embodichain.lab.visualization import VisualizationCfg, ViserServerCfg + + +def main() -> None: + """Run a paced cable-only scene and release its resources on exit.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--port", type=int, default=8080) + parser.add_argument("--num-envs", type=int, default=2) + parser.add_argument( + "--duration", + type=float, + default=0.0, + help="Simulated seconds; 0 runs until Ctrl+C.", + ) + args = parser.parse_args() + if args.num_envs < 1 or args.duration < 0: + parser.error("--num-envs must be positive and --duration must be nonnegative") + + fps = 60 + dt = 1.0 / fps + cfg = SimulationManagerCfg( + headless=True, + device="cuda:0", + num_envs=args.num_envs, + arena_space=2.0, + physics_dt=dt, + enable_entity_gizmo=False, + render_cfg=RenderCfg(renderer="no-render"), + physics_cfg=NewtonPhysicsCfg( + solver_cfg={"solver_type": "dexuni"}, + num_substeps=8, + sync_to_renderer=False, + ), + visualization=VisualizationCfg( + backend="viser", + env_ids=None, + scene_fps=30.0, + viser_server=ViserServerCfg(host="127.0.0.1", port=args.port), + ), + ) + sim = SimulationManager(cfg) + try: + points = np.zeros((25, 3), dtype=np.float32) + points[:, 0] = np.linspace(0.0, 0.8, len(points)) + sim.add_cable_object( + CableObjectCfg( + uid="preview_cable", + points=points, + radius=0.01, + init_pos=(-0.4, 0.0, 1.2), + fixed_segments=(0,), + render_mode="capsules", + attrs=CablePhysicsCfg(bend_stiffness=0.01, twist_stiffness=0.01), + ) + ) + runtime = sim.start_visualization() + sim.capture_visualization(force=True) + print( + f"Open {runtime.endpoint} — {args.num_envs} cables; Ctrl+C to stop.", + flush=True, + ) + frame = 0 + while args.duration == 0.0 or frame * dt < args.duration: + started = time.perf_counter() + if frame and frame % (6 * fps) == 0: + sim.reset_objects_state() + sim.capture_visualization(force=True) + sim.update(step=1) + frame += 1 + time.sleep(max(0.0, dt - (time.perf_counter() - started))) + except KeyboardInterrupt: + pass + finally: + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +if __name__ == "__main__": + main() diff --git a/tests/gym/envs/test_cable_recording_cuda.py b/tests/gym/envs/test_cable_recording_cuda.py new file mode 100644 index 0000000000..81c39b5eae --- /dev/null +++ b/tests/gym/envs/test_cable_recording_cuda.py @@ -0,0 +1,261 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Real cable observations across Gym steps and selective episode resets.""" + +from __future__ import annotations + +import faulthandler +import gc +import os +from pathlib import Path +import subprocess +import sys + +import pytest + +pytestmark = [pytest.mark.subprocess_sim, pytest.mark.gpu] + + +def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( + tmp_path: Path, +) -> None: + # A fresh process preserves NoRender without the shared test engine's + # renderer initialization. No downloaded robot or scene assets are needed. + urdf = tmp_path / "slider.urdf" + urdf.write_text( + """ + + + +""", + encoding="utf-8", + ) + result = subprocess.run( + [sys.executable, str(Path(__file__).resolve()), str(urdf)], + env=os.environ.copy(), + capture_output=True, + text=True, + timeout=180, + ) + assert result.returncode == 0, result.stdout + result.stderr + + +def _run_recording(urdf: str) -> None: + import torch + + from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg + from embodichain.lab.gym.envs.managers import ObservationCfg, SceneEntityCfg + from embodichain.lab.gym.envs.managers.observations import get_entity_state + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + from embodichain.lab.sim.cfg import ( + CableObjectCfg, + NewtonPhysicsCfg, + RenderCfg, + RobotCfg, + ) + + cfg = EmbodiedEnvCfg( + num_envs=2, + max_episode_steps=4, + sim_steps_per_control=1, + ignore_terminations=True, + record_trajectory=True, + trajectory_auto_save=False, + trajectory_uids=["wire", "long_wire"], + sim_cfg=SimulationManagerCfg( + headless=True, + device="cuda:0", + arena_space=3.0, + physics_dt=1 / 120, + enable_entity_gizmo=False, + startup_summary="off", + render_cfg=RenderCfg(renderer="no-render"), + physics_cfg=NewtonPhysicsCfg( + device="cuda:0", + num_substeps=2, + solver_cfg={"solver_type": "dexuni"}, + ), + ), + robot=RobotCfg( + uid="slider", + fpath=urdf, + init_pos=(-1.0, 0.0, 1.0), + asset_physics_mode="overlay", + ), + cable_object=[ + CableObjectCfg( + uid=uid, + points=[[0.1 * index, 0, 0] for index in range(segments + 1)], + radius=0.005, + init_pos=(0.0, y, 1.0), + ) + for uid, segments, y in (("wire", 2, 0.0), ("long_wire", 3, 0.5)) + ], + observations={ + "wire": ObservationCfg( + func=get_entity_state, + name="wire_alias", + mode="add", + consumers=("policy", "recording"), + params={ + "entity_cfg": SceneEntityCfg(uid="wire"), + "quantity": "segment_state_w", + }, + ) + }, + ) + env = None + try: + env = EmbodiedEnv(cfg) + assert not env.sim.has_native_renderer + obs, _ = env.reset(seed=73) + manager = env.observation_manager + assert manager is not None and manager.has_history + cables = {uid: env.sim.get_cable_object(uid) for uid in cfg.trajectory_uids} + fields = {uid: f"cables/{uid}/segment_state_w" for uid in cables} + episodes = env.episode_context.episode_ids + expected = { + uid: [cable.data.segment_state_w.clone()] for uid, cable in cables.items() + } + torch.testing.assert_close(obs["wire_alias"], expected["wire"][0]) + assert manager.field_ids["wire_alias"] == manager.field_ids[fields["wire"]] + + action = torch.zeros((2, 1), device=env.device) + for _ in range(2): + env.step(action) + for uid, cable in cables.items(): + expected[uid].append(cable.data.segment_state_w.clone()) + assert env._traj_steps.tolist() == [2, 2] + window = manager.read_history_window( + [0, 0], + [2, 2], + fields=list(fields.values()), + episode_ids=episodes, + device=env.device, + ) + for uid, field in fields.items(): + states = torch.stack(expected[uid], dim=1) + assert states.shape == (2, 3, cables[uid].data.num_segments, 13) + assert bool((states[:, -1, :, 2] < states[:, 0, :, 2]).all()) + observed = torch.stack(window["fields"][field]["values"]) + torch.testing.assert_close(observed, states, rtol=0, atol=0) + assert bool(torch.stack(window["fields"][field]["valid"]).all()) + descriptor = manager.schema[field] + assert descriptor.coordinate_frame == "world" + assert descriptor.shape == (cables[uid].data.num_segments, 13) + assert descriptor.representation == "xyz_xyzw_com_lin_vel_ang_vel" + recorded = env._traj_buffer["states", "cables", uid, "segment_state_w"] + assert recorded.shape[1] == cfg.max_episode_steps + 1 + torch.testing.assert_close(recorded[:, :3], states, rtol=0, atol=0) + + # Reset row 0 through the public lifecycle. Row 1 must keep both its + # physical state and all three historical boundaries, including S2. + env.reset(options={"reset_ids": [0]}) + new_episodes = env.episode_context.episode_ids + assert new_episodes == (episodes[0] + 1, episodes[1]) + assert env._traj_steps.tolist() == [0, 2] + reset_state = manager.read_history( + [0, 2], fields=list(fields.values()), episode_ids=new_episodes + ) + for uid, field in fields.items(): + native = cables[uid].data.segment_state_w + torch.testing.assert_close(native[0], expected[uid][0][0], rtol=0, atol=0) + torch.testing.assert_close(native[1], expected[uid][-1][1], rtol=0, atol=0) + torch.testing.assert_close( + reset_state.values[field], native.cpu(), rtol=0, atol=0 + ) + with pytest.raises(ValueError, match="episode"): + manager.read_history( + [0], fields=[fields["wire"]], episode_ids=[episodes[0]], env_ids=[0] + ) + + env.step(action) + assert env._traj_steps.tolist() == [1, 3] + latest = manager.read_history( + [1, 3], fields=list(fields.values()), episode_ids=new_episodes + ) + retained = manager.read_history_window( + [0], + [2], + fields=list(fields.values()), + episode_ids=[episodes[1]], + env_ids=[1], + device=env.device, + ) + for uid, field in fields.items(): + torch.testing.assert_close( + latest.values[field], + cables[uid].data.segment_state_w.cpu(), + rtol=0, + atol=0, + ) + torch.testing.assert_close( + retained["fields"][field]["values"][0], + torch.stack(expected[uid], dim=1)[1], + rtol=0, + atol=0, + ) + # Commit the nonzero arena through public reset with auto-save enabled, + # then replay that actual artifact in a fresh single-environment scene. + from embodichain.lab.gym.envs.wrapper import ReplayWrapper + from embodichain.lab.gym.utils.gym_utils import load_trajectory + + env.cfg.trajectory_auto_save = True + env.cfg.trajectory_save_dir = str(Path(urdf).parent) + source_origin = env.sim.arena_offsets[1].cpu().clone() + env.reset(options={"reset_ids": [1]}) + paths = list(Path(urdf).parent.glob("traj_env1_*.pt")) + assert len(paths) == 1 + saved = load_trajectory(str(paths[0])) + assert saved["meta"]["env_ids"] == [1] + assert saved["meta"]["arena_origins_w"] == [source_origin.tolist()] + for uid, field in fields.items(): + assert saved["raw_observations"]["fields"][field]["values"].shape[1] == 4 + + env.close(exit_process=False) + env = None + manager = None + cables.clear() + gc.collect() + SimulationManager.flush_cleanup_queue() + + cfg.num_envs = 1 + cfg.sim_cfg.num_envs = 1 + cfg.record_trajectory = False + env = EmbodiedEnv(cfg) + replay = ReplayWrapper(env, saved) + replay.reset() + for _ in range(saved["meta"]["lengths"][0]): + replay.step(None) + for uid, field in fields.items(): + expected_final = latest.values[field][[1]].clone() + expected_final[..., :3] -= source_origin + observed = env.sim.get_cable_object(uid).data.segment_state_w.cpu() + observed[..., :3] -= env.sim.arena_offsets.cpu()[:, None, :] + torch.testing.assert_close(observed, expected_final, atol=2e-6, rtol=1e-5) + finally: + if env is not None: + env.close(exit_process=False) + env = None + gc.collect() + SimulationManager.flush_cleanup_queue() + + +if __name__ == "__main__": + faulthandler.enable() + faulthandler.dump_traceback_later(120) + _run_recording(sys.argv[1]) diff --git a/tests/gym/envs/test_embodied_env_cables.py b/tests/gym/envs/test_embodied_env_cables.py new file mode 100644 index 0000000000..20bdeef1e5 --- /dev/null +++ b/tests/gym/envs/test_embodied_env_cables.py @@ -0,0 +1,316 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Cable scene composition through the public Gym configuration loader.""" + +from __future__ import annotations + +from pathlib import Path +from unittest.mock import MagicMock + +import pytest +import yaml + +from embodichain.lab.gym.envs import EmbodiedEnv +from embodichain.lab.gym.utils.gym_utils import config_to_cfg +from embodichain.lab.sim.cfg import CableObjectCfg, CablePhysicsCfg + +pytestmark = pytest.mark.no_sim + + +@pytest.mark.parametrize("component", [False, True], ids=["inline", "component"]) +def test_cable_config_is_declared_before_environment_preparation( + tmp_path: Path, component: bool +) -> None: + cable = { + "uid": "wire", + "points": [[0, 0, 1], [0.2, 0, 1]], + "radius": 0.01, + "fixed_segments": [0], + "attrs": {"bend_stiffness": 0.1}, + } + config = { + "id": "EmbodiedEnv-v1", + "physics": "newton", + "env": {}, + "robot": {"uid": "robot"}, + "cable_object": [cable], + } + if component: + (tmp_path / "env.yaml").write_text( + yaml.safe_dump( + { + "environment_id": "EmbodiedEnv-v1", + "physics": "newton", + "env": {}, + "simulation": {"cable_object": [cable]}, + } + ) + ) + config = { + "id": "EmbodiedEnv-v1", + "environment": {"component": "env.yaml"}, + "robot": {"uid": "robot"}, + } + cfg = config_to_cfg(config, source_path=tmp_path / "task.yaml") + assert len(cfg.cable_object) == 1 + assert isinstance(cfg.cable_object[0], CableObjectCfg) + assert isinstance(cfg.cable_object[0].attrs, CablePhysicsCfg) + assert cfg.cable_object[0].attrs.bend_stiffness == 0.1 + + env = object.__new__(EmbodiedEnv) + env.cfg = cfg + env.sim = MagicMock() + env._setup_interactive_objects() + env.sim.add_cable_object.assert_called_once_with(cfg=cfg.cable_object[0]) + env.sim.prepare.assert_not_called() + + +@pytest.mark.parametrize("resource", ["registry", "relative"]) +@pytest.mark.parametrize("component", [False, True]) +def test_cable_visual_mesh_resolves_from_dataset_or_declaring_component( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch, resource: str, component: bool +) -> None: + from embodichain import data + + owner = tmp_path / "environment" if component else tmp_path + owner.mkdir(exist_ok=True) + mesh = owner / "wire.obj" + mesh.write_text("v 0 0 0\n") + reference = "CableAssets/wire.obj" if resource == "registry" else "./wire.obj" + resolved = [] + + def get_data_path(path: str) -> str: + resolved.append(path) + assert path == (reference if resource == "registry" else str(mesh)) + return str(mesh) + + monkeypatch.setattr(data, "get_data_path", get_data_path) + cable = { + "uid": "wire", + "points": [[0, 0, 1], [0.2, 0, 1]], + "radius": 0.01, + "visual_shape": {"fpath": reference}, + } + config = { + "id": "EmbodiedEnv-v1", + "physics": "newton", + "env": {}, + "robot": {"uid": "robot"}, + "cable_object": [cable], + } + if component: + (owner / "env.yaml").write_text( + yaml.safe_dump( + { + "environment_id": "EmbodiedEnv-v1", + "physics": "newton", + "env": {}, + "simulation": {"cable_object": [cable]}, + } + ) + ) + config = { + "id": "EmbodiedEnv-v1", + "environment": {"component": "environment/env.yaml"}, + "robot": {"uid": "robot"}, + } + cfg = config_to_cfg(config, source_path=tmp_path / "task.yaml") + assert cfg.cable_object[0].visual_shape.fpath == str(mesh) + assert len(resolved) == 1 + assert cable["visual_shape"]["fpath"] == reference + + +def test_cable_visual_mesh_loads_inline_geometry() -> None: + vertices = [[0.0, 0.0, 0.0], [0.2, 0.0, 0.0], [0.0, 0.01, 0.0]] + triangles = [[0, 1, 2]] + cable = { + "uid": "wire", + "points": [[0, 0, 1], [0.2, 0, 1]], + "radius": 0.01, + "visual_shape": {"vertices": vertices, "triangles": triangles}, + } + cfg = config_to_cfg( + { + "id": "EmbodiedEnv-v1", + "physics": "newton", + "env": {}, + "robot": {"uid": "robot"}, + "cable_object": [cable], + } + ) + mesh = cfg.cable_object[0].visual_shape + assert mesh.fpath is None + assert mesh.vertices == vertices + assert mesh.triangles == triangles + + +@pytest.mark.parametrize("uids", [None, ["wire"]]) +def test_cable_recording_uses_shared_observation_history_and_t_plus_one_slots( + uids, +) -> None: + from types import SimpleNamespace + + import gymnasium.spaces + import torch + from tensordict import TensorDict + + from embodichain.lab.gym.envs.episode_context import EpisodeContext + from embodichain.lab.gym.envs.managers import ( + ObservationCfg, + ObservationManager, + SceneEntityCfg, + ) + from embodichain.lab.gym.envs.managers.observations import get_entity_state + from embodichain.lab.gym.utils.gym_utils import ( + _init_raw_rollout, + build_trajectory_buffer, + ) + from embodichain.lab.gym.utils.trajectory_state import ( + export_observation_history, + trajectory_observation_cfgs, + ) + + class CableData: + def __init__(self, segments: int): + self.num_segments = segments + self.state = torch.zeros(2, segments, 13) + self.state[..., 6] = 1 + self.reads = 0 + + @property + def segment_state_w(self): + self.reads += 1 + return self.state + + wires = { + uid: SimpleNamespace(data=CableData(segments)) + for uid, segments in (("wire", 2), ("other", 4)) + } + pose = torch.zeros(2, 7) + pose[:, 6] = 1 + robot = SimpleNamespace( + uid="robot", + dof=1, + joint_names=["joint"], + get_local_pose=lambda to_matrix=False: pose, + get_qpos=lambda: torch.zeros(2, 1), + get_qvel=lambda: torch.zeros(2, 1), + ) + assets = {"robot": robot, **wires} + context = EpisodeContext(2) + context._begin_episode([0, 1]) + context._finish_initialization([0, 1]) + env = SimpleNamespace( + num_envs=2, + device=torch.device("cpu"), + robot=robot, + episode_context=context, + sim=SimpleNamespace( + _articulations={}, + _rigid_objects={}, + _cable_objects=wires, + asset_uids=list(assets), + get_asset=assets.get, + ), + ) + alias = ObservationCfg( + func=get_entity_state, + mode="add", + name="wire_alias", + params={ + "entity_cfg": SceneEntityCfg(uid="wire"), + "quantity": "segment_state_w", + }, + consumers=("policy", "recording"), + ) + manager = ObservationManager( + {"alias": alias}, env, recording=trajectory_observation_cfgs(env, uids) + ) + env.observation_manager = manager + trajectory = build_trajectory_buffer( + env, + max_steps=3, + num_envs=2, + device="cpu", + uids=uids, + action_space=gymnasium.spaces.Box(-1, 1, shape=(2, 1)), + ) + storage = TensorDict({"trajectory": trajectory}, batch_size=[2]) + env._rollout_storage = storage + fields = { + "/".join(path): value + for path, value in trajectory["states"].items( + include_nested=True, leaves_only=True + ) + } + _init_raw_rollout(storage, context, fields, trajectory["actions"][:, :2]) + manager.bind_rollout_storage(storage) + + def capture(slot): + return manager.capture( + lambda: manager.compute(TensorDict({}, batch_size=[2])), + episode_context=context, + timestamp=slot * 0.04, + ) + + for slot in range(3): + for wire in wires.values(): + wire.data.state[..., 0] = torch.tensor([slot, 100 + slot])[:, None] + wires["wire"].data.state[1, :, 6] = 0 if slot == 1 else 1 + frame = capture(slot) + # A policy consumer may mutate its view without corrupting recorded state. + frame.policy["wire_alias"].add_(1000) + if slot == 0: + EmbodiedEnv._record_initial_observation(env, frame, torch.arange(2)) + else: + env._traj_raw_action = torch.full((2, 1), float(slot)) + EmbodiedEnv._write_trajectory_step(env, frame) + + selected = set(wires) if uids is None else set(uids) + assert set(trajectory["states", "cables"].keys()) == selected + for uid, wire in wires.items(): + assert wire.data.reads == (3 if uid in selected else 0) + if uid in selected: + state = trajectory["states", "cables", uid, "segment_state_w"] + assert state.shape == (2, 3, wire.data.num_segments, 13) + torch.testing.assert_close( + state[:, :, 0, 0], + torch.tensor([[0.0, 1.0, 2.0], [100.0, 101.0, 102.0]]), + ) + field = "cables/wire/segment_state_w" + assert manager.field_ids["wire_alias"] == manager.field_ids[field] + descriptor = manager.schema[field] + assert descriptor.coordinate_frame == "world" + assert descriptor.representation == "xyz_xyzw_com_lin_vel_ang_vel" + assert descriptor.units == ("m", "unitless", "m/s", "rad/s") + window = manager.read_history_window( + [0], [2], fields=[field], episode_ids=[0], env_ids=[1] + ) + assert window["fields"][field]["valid"][0].tolist() == [True, False, True] + exported = export_observation_history(manager) + assert exported["fields"][field]["descriptor"]["shape"] == (2, 13) + assert storage["raw_steps"].tolist() == [2, 2] + + context._begin_episode([0]) + context._finish_initialization([0]) + wires["wire"].data.state[0, :, 0] = 99 + EmbodiedEnv._record_initial_observation(env, capture(3), torch.tensor([0])) + assert storage["raw_steps"].tolist() == [0, 2] + retained = manager.read_history([2], fields=[field], episode_ids=[0], env_ids=[1]) + assert retained.values[field][0, 0, 0] == 102 + with pytest.raises(ValueError, match="episode"): + manager.read_history([0], fields=[field], episode_ids=[0], env_ids=[0]) diff --git a/tests/gym/envs/test_replay_wrapper.py b/tests/gym/envs/test_replay_wrapper.py index b7fc75044f..ca9a93e927 100644 --- a/tests/gym/envs/test_replay_wrapper.py +++ b/tests/gym/envs/test_replay_wrapper.py @@ -285,6 +285,193 @@ def test_replay_rejects_unknown_mode(): ReplayWrapper(_FakeEnv(), _trajectory(), mode="unsupported") +def _cable_replay_case(): + env = _FakeEnv() + trajectory = _trajectory() + cable_state = torch.zeros(1, 3, 13) + cable_state[..., 6] = 1 + trajectory["states"]["cables"] = TensorDict( + { + "wire": { + "segment_state_w": cable_state[:, None].expand(-1, 2, -1, -1).clone() + } + }, + batch_size=[1, 2], + ) + env.sim._cable_objects = { + "wire": SimpleNamespace(data=SimpleNamespace(segment_state_w=cable_state)) + } + env.sim._articulations = {} + env.sim._rigid_objects = {} + env.robot.get_qpos = lambda: torch.tensor([[0.0, 1.0]]) + env.robot.get_local_pose = lambda: torch.zeros(1, 7) + return env, trajectory + + +def test_cable_dynamic_replay_uses_reset_without_restoring_observations(fake_restore): + env, trajectory = _cable_replay_case() + wrapper = ReplayWrapper(env, trajectory, mode="dynamic") + wrapper.reset() + assert env.last_restored is None + assert env.sim.physics_enabled == [True] + wrapper.step(None) + torch.testing.assert_close(env.received_actions[0], trajectory["actions"][:, 0]) + with pytest.raises(NotImplementedError, match="scrubbing"): + wrapper.go_to_step(1) + with pytest.raises(ValueError, match="reset_ids"): + wrapper.reset(options={"reset_ids": [0]}) + + +def test_cable_dynamic_replay_rejects_a_mid_episode_initial_observation(fake_restore): + env, trajectory = _cable_replay_case() + trajectory["states"]["cables", "wire", "segment_state_w"][0, 0, 0, 0] = 0.1 + wrapper = ReplayWrapper(env, trajectory, mode="dynamic") + with pytest.raises(ValueError, match="differs from the full reset"): + wrapper.reset() + assert env.last_restored is None and not env.received_actions + + +@pytest.mark.parametrize("mode", ["kinematic", "control"]) +def test_cable_replay_rejects_history_free_state_modes_before_mutation(mode): + env, trajectory = _cable_replay_case() + with pytest.raises(NotImplementedError, match="dynamic"): + ReplayWrapper(env, trajectory, mode=mode) + assert env.sim.physics_enabled == [] + + +def test_cable_replay_rejects_broadcast_world_states(): + env, trajectory = _cable_replay_case() + with pytest.raises(ValueError, match="environment counts"): + ReplayWrapper(_FakeEnv(num_envs=2), trajectory, mode="dynamic") + + +def test_cable_scene_replay_rejects_recording_without_cable_initial_observations(): + env, trajectory = _cable_replay_case() + del trajectory["states"]["cables"] + with pytest.raises(ValueError, match="every scene cable"): + ReplayWrapper(env, trajectory, mode="dynamic") + assert env.sim.physics_enabled == [] + + +def test_cable_replay_preserves_task_context_and_initial_observation_history( + fake_restore, +): + from unittest.mock import Mock + + env, trajectory = _cable_replay_case() + initial = {"schema_version": 1, "targets": {"goal": [0.0, 0.0, 1.0]}} + trajectory["meta"]["demo_episodes"] = [{"task_initial_state": initial}] + env.termination_manager = object() + env.restore_task_initial_state = Mock() + env.get_info = lambda: {"task_context": "restored"} + env.observation_manager = SimpleNamespace(has_history=True) + frame = SimpleNamespace(policy={"recorded_observation": torch.ones(1, 1)}) + env._capture_observation = lambda: frame + env._record_initial_observation = Mock() + env._seed_recording_state = Mock() + + wrapper = ReplayWrapper(env, trajectory, mode="dynamic") + obs, info = wrapper.reset() + + assert env.last_restored is None + env.restore_task_initial_state.assert_called_once_with(initial) + assert obs is frame.policy + assert info == {"task_context": "restored"} + assert env._record_initial_observation.call_args.args[0] is frame + assert env._record_initial_observation.call_args.args[1].tolist() == [0] + assert env._seed_recording_state.call_args.args[0] is frame.policy + + +@pytest.mark.parametrize("source_env_id", [0, 1]) +@pytest.mark.parametrize("target_origin", [(0.0, 0.0, 0.0), (10.0, -2.0, 3.0)]) +def test_auto_saved_cable_row_replays_at_target_arena_origin( + tmp_path, source_env_id, target_origin +): + from copy import deepcopy + from types import MethodType + + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + from embodichain.lab.gym.envs.episode_context import EpisodeContext + from embodichain.lab.gym.utils.gym_utils import load_trajectory + + target, _ = _cable_replay_case() + local_state = target.sim._cable_objects["wire"].data.segment_state_w.clone() + local_state[0, :, 0] = torch.tensor([0.1, 0.2, 0.3]) + local_state[..., 7:] = 0.01 + target.sim.arena_offsets = torch.tensor([target_origin]) + target_state = local_state.clone() + target_state[..., :3] += target.sim.arena_offsets[:, None, :] + target.sim._cable_objects["wire"].data.segment_state_w = target_state + + source = _FakeEnv(num_envs=2) + source.robot.uid = "robot" + source.physics_dt = 0.01 + source.control_frequency = 25 + source.sim._articulations = {} + source.sim._rigid_objects = {} + source.sim.arena_offsets = torch.tensor([[0.0, 0.0, 0.0], [5.0, 2.0, 1.0]]) + trajectory = _trajectory(num_envs=2) + trajectory["states"]["robot", "qpos"][:] = torch.tensor([0.0, 1.0]) + states = local_state[:, None].repeat(2, 2, 1, 1) + states[..., :3] += source.sim.arena_offsets[:, None, None, :] + trajectory["states"]["cables"] = TensorDict( + {"wire": {"segment_state_w": states}}, batch_size=[2, 2] + ) + source._traj_buffer = TensorDict( + {"states": trajectory["states"], "actions": trajectory["actions"]}, + batch_size=[2, 2], + ) + source._traj_steps = torch.tensor([2, 2]) + source.cfg.trajectory_auto_save = True + source.cfg.trajectory_save_dir = str(tmp_path) + source._traj_save_count = 0 + source._demo_episode_metadata = [ + {"env_id": row, "length": 2, "episode_uuid": f"episode-{row}"} + for row in range(2) + ] + source.get_demo_episode_metadata = lambda row: deepcopy( + source._demo_episode_metadata[row] + ) + source._trajectory_action_metadata = lambda: {"action_kind": "policy"} + source.episode_context = EpisodeContext(2) + source.episode_context._begin_episode([0, 1]) + source.episode_context._finish_initialization([0, 1]) + source.save_trajectory = MethodType(EmbodiedEnv.save_trajectory, source) + + path = EmbodiedEnv._save_trajectory_for_env(source, source_env_id) + saved = load_trajectory(path) + assert saved["meta"]["num_envs"] == 1 + assert saved["meta"]["env_ids"] == [source_env_id] + assert saved["meta"]["arena_origins_w"] == [ + source.sim.arena_offsets[source_env_id].tolist() + ] + recorded = saved["states"]["cables", "wire", "segment_state_w"].clone() + wrapper = ReplayWrapper(target, saved) + wrapper.reset() + wrapper.step(None) + torch.testing.assert_close( + target.received_actions[0], trajectory["actions"][[source_env_id], 0] + ) + torch.testing.assert_close( + saved["states"]["cables", "wire", "segment_state_w"], recorded + ) + torch.testing.assert_close( + target.sim._cable_objects["wire"].data.segment_state_w, target_state + ) + target_state[..., 0] += 0.1 + with pytest.raises(ValueError, match="differs from the full reset"): + wrapper.reset() + + +@pytest.mark.parametrize("origins", [[[0.0, 0.0]], [[float("nan"), 0.0, 0.0]]]) +def test_cable_replay_rejects_invalid_source_origins_before_reset(origins): + env, trajectory = _cable_replay_case() + trajectory["meta"]["arena_origins_w"] = origins + with pytest.raises(ValueError, match="arena_origins_w"): + ReplayWrapper(env, trajectory) + assert env.sim.physics_enabled == [] + + @pytest.mark.parametrize("mode", ["kinematic", "control"]) def test_fluid_kinematic_and_control_restore_recorded_frames(fake_restore, mode): env = _FakeEnv() diff --git a/tests/gym/utils/test_gym_utils.py b/tests/gym/utils/test_gym_utils.py index e5b615947c..f0e894b29a 100644 --- a/tests/gym/utils/test_gym_utils.py +++ b/tests/gym/utils/test_gym_utils.py @@ -872,6 +872,24 @@ def test_viser_launcher_flag_implies_headless_viser_commands(): assert merged_config["visualization"]["allow_commands"] is True +@pytest.mark.parametrize("allow_commands", [False, True]) +def test_viser_launcher_preserves_explicit_command_policy(allow_commands): + parser = argparse.ArgumentParser() + add_env_launcher_args_to_parser(parser) + config = { + "id": "Dummy-v0", + "physics": "default", + "visualization": {"allow_commands": allow_commands}, + } + original = deepcopy(config) + + merged = merge_args_with_gym_config(parser.parse_args(["--viser"]), config) + + assert merged["visualization"]["backend"] == "viser" + assert merged["visualization"]["allow_commands"] is allow_commands + assert config == original + + def test_sensor_and_extra_obs_together(): """Test that both sensors and extra observations work together.""" config = { @@ -2586,6 +2604,24 @@ def test_build_trajectory_buffer_uids_filter(): assert "ball" not in buf["states"]["rigid_objects"].keys() +@pytest.mark.parametrize("uids", [None, ["wire"], ["other"]]) +def test_cable_trajectory_buffer_preserves_topology_and_uid_selection(uids): + env = _stub_env() + env.sim._cable_objects = { + "wire": SimpleNamespace(data=SimpleNamespace(num_segments=3)), + "other": SimpleNamespace(data=SimpleNamespace(num_segments=5)), + } + buffer = build_trajectory_buffer( + env, max_steps=4, num_envs=2, device="cpu", uids=uids + ) + cables = buffer["states", "cables"] + assert set(cables.keys()) == set(uids or env.sim._cable_objects) + for uid in cables.keys(): + state = cables[uid, "segment_state_w"] + assert state.shape == (2, 4, env.sim._cable_objects[uid].data.num_segments, 13) + assert state.dtype == torch.float32 + + def test_load_trajectory_validates_and_returns_dict(tmp_path): data = { "states": TensorDict({"a": torch.zeros(1, 4)}, batch_size=[1, 4]), diff --git a/tests/gym/utils/test_trajectory_state.py b/tests/gym/utils/test_trajectory_state.py index 16ed488476..58b498b472 100644 --- a/tests/gym/utils/test_trajectory_state.py +++ b/tests/gym/utils/test_trajectory_state.py @@ -138,3 +138,49 @@ def test_restore_trajectory_state_writes_complete_robot_state() -> None: robot.set_qpos.assert_called_once_with(recorded_qpos, target=False) robot.set_qvel.assert_called_once_with(recorded_qvel, target=False) + + +def test_cable_capture_is_world_observation_and_selected_rows_only() -> None: + from embodichain.lab.gym.utils.gym_utils import build_trajectory_buffer + + cable_state = torch.arange(2 * 3 * 13, dtype=torch.float32).reshape(2, 3, 13) + robot = SimpleNamespace( + dof=2, + get_local_pose=lambda: torch.zeros(2, 7), + get_qpos=lambda: torch.zeros(2, 2), + get_qvel=lambda: torch.zeros(2, 2), + ) + cable = SimpleNamespace( + data=SimpleNamespace(num_segments=3, segment_state_w=cable_state) + ) + env = SimpleNamespace( + robot=robot, + sim=SimpleNamespace( + _articulations={}, _rigid_objects={}, _cable_objects={"wire": cable} + ), + ) + buffer = build_trajectory_buffer(env, max_steps=3, num_envs=2, device="cpu") + capture_trajectory_state( + env, buffer["states"], torch.tensor([1]), torch.tensor([2]) + ) + torch.testing.assert_close( + buffer["states", "cables", "wire", "segment_state_w"][1, 2], cable_state[1] + ) + assert not buffer["states", "cables", "wire", "segment_state_w"][0].any() + filtered = build_trajectory_buffer( + env, max_steps=3, num_envs=2, device="cpu", uids=["other"] + ) + assert "cables" not in filtered["states"].keys() + + +def test_cable_restore_fails_before_any_robot_or_scene_write() -> None: + import pytest + + robot = MagicMock() + states = { + "robot": {"qpos": torch.ones(2, 2), "root_pose": torch.ones(2, 7)}, + "cables": {"wire": {"segment_state_w": torch.zeros(2, 3, 13)}}, + } + with pytest.raises(NotImplementedError, match="solver-history"): + restore_trajectory_state(SimpleNamespace(robot=robot, sim=MagicMock()), states) + assert robot.mock_calls == [] diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index b7a71c8530..dcf17235e4 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -1294,10 +1294,12 @@ def test_start_visualization_rejects_open_native_window() -> None: sim.start_visualization() -def test_start_visualization_does_not_prepare_an_empty_declaration_phase( +@pytest.mark.parametrize("has_cable", [False, True], ids=["empty", "cable-only"]) +def test_start_visualization_prepares_only_when_assets_are_declared( monkeypatch: pytest.MonkeyPatch, + has_cable: bool, ) -> None: - """Starting Viser must not finalize Newton before assets are declared.""" + """Viser prepares cable-only scenes but leaves an empty builder open.""" class _Exporter: def __init__(self, sim: SimulationManager, cfg: object) -> None: @@ -1313,6 +1315,7 @@ def __init__(self, exporter: _Exporter, cfg: object) -> None: self.captures = 0 def start(self) -> None: + assert self.exporter.sim.prepare.call_count == int(has_cable) self.is_running = True def capture(self, **_kwargs: object) -> None: @@ -1336,6 +1339,7 @@ def capture(self, **_kwargs: object) -> None: sim._rigid_object_groups = {} sim._fluid_objects = {} sim._deformable_objects = {} + sim._cable_objects = {"cable": object()} if has_cable else {} sim._articulations = {} sim._robots = {} sim._sensors = {} @@ -1352,7 +1356,7 @@ def capture(self, **_kwargs: object) -> None: runtime = sim.start_visualization() - sim.prepare.assert_not_called() + assert sim.prepare.call_count == int(has_cable) sim.sync_render_state.assert_not_called() assert runtime is not None assert runtime.captures == 1 diff --git a/tests/visualization/test_runtime.py b/tests/visualization/test_runtime.py index 8d009a64d9..eacade19df 100644 --- a/tests/visualization/test_runtime.py +++ b/tests/visualization/test_runtime.py @@ -326,6 +326,81 @@ def test_runtime_keeps_backend_lifecycle_on_update_thread() -> None: assert runtime.stats.published_image_frames == 1 +@pytest.mark.parametrize("timeout", [None, 5.0], ids=["default", "explicit-timeout"]) +def test_runtime_allows_client_build_but_honors_explicit_startup_deadline( + monkeypatch: pytest.MonkeyPatch, timeout: float | None +) -> None: + """Model a 20-second frontend build without slowing down the test suite.""" + build_seconds = 20.0 + finish_build = threading.Event() + + class _BuildingBackend(_Backend): + def start(self) -> None: + super().start() + assert finish_build.wait(timeout=2.0) + + published = threading.Event() + backend = _BuildingBackend(published, threading.Event()) + runtime = VisualizationRuntime( + _Exporter(published), VisualizationCfg(backend="viser"), backend=backend + ) + real_wait = runtime._ready_event.wait + + def wait_for_build(timeout: float) -> bool: + # Model elapsed build time at the readiness boundary; keep the actual + # worker, manifest publication, and cleanup running on their real thread. + finish_build.set() + if timeout < build_seconds: + return False + return real_wait(timeout=2.0) + + monkeypatch.setattr(runtime._ready_event, "wait", wait_for_build) + try: + if timeout is None: + runtime.start() + assert runtime.health.status == "running" + assert runtime.health.published_scene_revision == 1 + assert not backend.stopped + else: + with pytest.raises( + TimeoutError, match="starting the visualization backend" + ): + runtime.start(timeout=timeout) + assert not runtime.is_running + assert backend.stopped + finally: + finish_build.set() + runtime.stop() + assert len(set(backend.thread_ids)) == 1 + + +def test_runtime_reports_backend_start_failure_without_waiting_for_timeout() -> None: + failure = ValueError("Viser client build failed") + + class _FailingBackend(_Backend): + def start(self) -> None: + super().start() + raise failure + + published = threading.Event() + backend = _FailingBackend(published, threading.Event()) + runtime = VisualizationRuntime( + _Exporter(published), VisualizationCfg(backend="viser"), backend=backend + ) + try: + with pytest.raises( + RuntimeError, match="Visualization update thread failed" + ) as error: + runtime.start() + assert error.value.__cause__ is failure + assert runtime.health.status == "failed" + finally: + with pytest.raises(RuntimeError, match="Visualization update thread failed"): + runtime.stop() + assert backend.stopped + assert not runtime.is_running + + @pytest.mark.parametrize("due", ["none", "pose", "deformable", "image", "forced"]) def test_capture_publishes_source_once_and_only_when_a_consumer_is_due(due): published = threading.Event() diff --git a/tests/visualization/test_scene_exporter.py b/tests/visualization/test_scene_exporter.py index 6aafc2c541..201d894d26 100644 --- a/tests/visualization/test_scene_exporter.py +++ b/tests/visualization/test_scene_exporter.py @@ -19,6 +19,7 @@ from types import SimpleNamespace import numpy as np +import pytest from embodichain.lab.visualization import ( FrameOverlay, @@ -226,6 +227,12 @@ def get_deformable_object_uid_list(self) -> list[str]: def get_deformable_object(self, uid: str) -> None: raise AssertionError(f"Unexpected deformable-object lookup: {uid}") + def get_cable_object_uid_list(self) -> list[str]: + return [] + + def get_cable_object(self, uid: str) -> None: + raise AssertionError(f"Unexpected cable-object lookup: {uid}") + def get_sensor_uid_list(self) -> list[str]: return [] @@ -261,6 +268,12 @@ def get_deformable_object_uid_list(self) -> list[str]: def get_deformable_object(self, uid: str) -> None: raise AssertionError(f"Unexpected deformable-object lookup: {uid}") + def get_cable_object_uid_list(self) -> list[str]: + return [] + + def get_cable_object(self, uid: str) -> None: + raise AssertionError(f"Unexpected cable-object lookup: {uid}") + def get_sensor_uid_list(self) -> list[str]: return [] @@ -268,6 +281,61 @@ def get_sensor(self, uid: str) -> None: raise AssertionError(f"Unexpected sensor lookup: {uid}") +class _CableData: + def __init__(self, arena_offsets: np.ndarray) -> None: + self.num_instances = len(arena_offsets) + self.num_segments = 3 + self.poses = np.zeros( + (self.num_instances, self.num_segments, 7), dtype=np.float32 + ) + self.poses[..., :3] = arena_offsets[:, None, :] + np.array( + [[0.1, 0.2, 0.3], [0.4, 0.5, 0.6], [0.7, 0.8, 0.9]] + ) + self.poses[..., 3:] = np.array([1.0, 2.0, 3.0, 4.0]) / np.sqrt(30.0) + self.fetch_count = 0 + self.released = False + + @property + def segment_pose_w(self) -> np.ndarray: + if self.released: + raise RuntimeError("Cable batch has been released") + self.fetch_count += 1 + return self.poses + + +class _CableObject: + def __init__(self, arena_offsets: np.ndarray) -> None: + self.cfg = SimpleNamespace( + points=np.array( + [ + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.25], + [0.25, 0.0, 0.25], + [0.25, 0.0, 0.75], + ] + ), + radius=0.02, + render_mode="tube", + ) + self.data = _CableData(arena_offsets) + + +class _CableSimulation(_EmptySimulation): + num_envs = 3 + arena_offsets = np.array( + [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [-7.0, 8.0, 9.0]], dtype=np.float32 + ) + + def __init__(self) -> None: + self.cables = {"cable/main": _CableObject(self.arena_offsets)} + + def get_cable_object_uid_list(self) -> list[str]: + return list(self.cables) + + def get_cable_object(self, uid: str) -> _CableObject: + return self.cables[uid] + + class _JointControlProvider: def __init__(self) -> None: self.specs = tuple( @@ -562,6 +630,180 @@ def test_empty_scene_can_publish_a_current_frame() -> None: assert result.frame.positions.shape == (0, 3) +@pytest.mark.parametrize("render_mode", ["tube", "capsules"]) +def test_cable_manifest_shares_closed_capsules_without_native_render_meshes( + render_mode: str, +) -> None: + sim = _CableSimulation() + sim.cables["second"] = _CableObject(sim.arena_offsets) + for cable in sim.cables.values(): + cable.cfg.render_mode = render_mode + exporter = SceneExporter(sim, VisualizationCfg(backend="viser", env_ids=[2, 0])) + + manifest = exporter.build_manifest() + + assert len(manifest.nodes) == 12 + assert len(manifest.geometries) == 2 # The two distinct rest segment lengths. + assert not exporter.has_deformables + assert all(cable.data.fetch_count == 0 for cable in sim.cables.values()) + geometries = {geometry.geometry_id: geometry for geometry in manifest.geometries} + for node in manifest.nodes: + assert node.kind == "cable_segment" + assert not node.dynamic_geometry + segment_index = int(node.node_id.rsplit(":", 1)[1]) + length = (0.25, 0.25, 0.5)[segment_index] + mesh = geometries[node.geometry_id] + np.testing.assert_allclose( + mesh.vertices.min(axis=0), [-0.02, -0.02, -length / 2 - 0.02] + ) + np.testing.assert_allclose( + mesh.vertices.max(axis=0), [0.02, 0.02, length / 2 + 0.02] + ) + triangles = mesh.vertices[mesh.faces] + normals = np.cross( + triangles[:, 1] - triangles[:, 0], triangles[:, 2] - triangles[:, 0] + ) + centroids = triangles.mean(axis=1) + axis_points = np.zeros_like(centroids) + axis_points[:, 2] = np.clip(centroids[:, 2], -length / 2, length / 2) + assert np.all(np.sum(normals * (centroids - axis_points), axis=1) > 0) + edges = np.concatenate( + [mesh.faces[:, [0, 1]], mesh.faces[:, [1, 2]], mesh.faces[:, [2, 0]]] + ) + _, counts = np.unique(np.sort(edges, axis=1), axis=0, return_counts=True) + assert np.all(counts == 2) + cable_node = next( + node + for node in manifest.nodes + if node.node_id == "env:2/cable:cable%2Fmain/segment:0" + ) + assert cable_node.path == "/envs/2/cable_objects/cable%2Fmain/segments/0" + assert exporter.resolve_node_target(cable_node.node_id) == ("cable/main", "cable") + + +@pytest.mark.parametrize( + "device", ["numpy", "cpu", pytest.param("cuda", marks=pytest.mark.gpu)] +) +def test_cable_capture_batches_world_poses_for_selected_environments( + monkeypatch: pytest.MonkeyPatch, device: str +) -> None: + import torch + from embodichain.lab.visualization import scene_exporter + + sim = _CableSimulation() + sim.cables["second"] = _CableObject(sim.arena_offsets) + sim.cables["second"].data.poses[..., 0] += 0.75 + if device != "numpy": + for cable in sim.cables.values(): + cable.data.poses = torch.as_tensor(cable.data.poses, device=device) + convert_pose = scene_exporter.pose_to_position_wxyz + + def convert_selected_pose(pose): + # Only visible rows may reach the host conversion boundary. + assert tuple(pose.shape) == (6, 7) + if isinstance(pose, torch.Tensor): + assert pose.device.type == device + return convert_pose(pose) + + monkeypatch.setattr(scene_exporter, "pose_to_position_wxyz", convert_selected_pose) + exporter = SceneExporter(sim, VisualizationCfg(backend="viser", env_ids=[2, 0])) + manifest = exporter.build_manifest() + + frame = exporter.capture( + sim_step=1, sim_time=0.01, capture_dynamic_geometry=False + ).frame + + assert {node.env_id for node in manifest.nodes} == {2, 0} + assert frame.dynamic_meshes == () + assert all(cable.data.fetch_count == 1 for cable in sim.cables.values()) + for index, node in enumerate(manifest.nodes): + uid, _ = exporter.resolve_node_target(node.node_id) + segment_index = int(node.node_id.rsplit(":", 1)[1]) + pose = sim.cables[uid].data.poses[node.env_id, segment_index] + if isinstance(pose, torch.Tensor): + pose = pose.cpu().numpy() + np.testing.assert_allclose(frame.positions[index], pose[:3]) + np.testing.assert_allclose(frame.wxyz[index], pose[[6, 3, 4, 5]], atol=1e-7) + + +def test_cable_pose_changes_keep_geometry_and_previous_frames_independent() -> None: + sim = _CableSimulation() + exporter = SceneExporter(sim, VisualizationCfg(backend="viser", env_ids=None)) + manifest = exporter.build_manifest() + first_frame = exporter.capture(sim_step=0, sim_time=0.0).frame + saved_positions = first_frame.positions.copy() + saved_wxyz = first_frame.wxyz.copy() + data = sim.cables["cable/main"].data + data.poses[..., :3] += [0.25, -0.5, 0.75] + data.poses[..., 3:] = [0.0, 0.0, 0.0, 1.0] + + next_frame = exporter.capture(sim_step=1, sim_time=0.01).frame + rebuilt = exporter.build_manifest() + + assert data.fetch_count == 2 + assert next_frame.node_ids == first_frame.node_ids + assert [node.geometry_id for node in rebuilt.nodes] == [ + node.geometry_id for node in manifest.nodes + ] + assert next_frame.scene_revision == first_frame.scene_revision + np.testing.assert_array_equal(first_frame.positions, saved_positions) + np.testing.assert_array_equal(first_frame.wxyz, saved_wxyz) + np.testing.assert_allclose( + next_frame.positions, saved_positions + [0.25, -0.5, 0.75] + ) + np.testing.assert_allclose(next_frame.wxyz, np.tile([1, 0, 0, 0], (9, 1))) + + +def test_cable_can_coexist_with_rigid_and_robot_assets() -> None: + class _MixedSimulation(_Simulation): + def __init__(self) -> None: + super().__init__() + self.cable = _CableObject(self.arena_offsets) + + def get_cable_object_uid_list(self) -> list[str]: + return ["cable"] + + def get_cable_object(self, uid: str) -> _CableObject: + return self.cable + + sim = _MixedSimulation() + exporter = SceneExporter(sim, VisualizationCfg(backend="viser", env_ids=None)) + manifest = exporter.build_manifest() + frame = exporter.capture(sim_step=0, sim_time=0).frame + + assert len(manifest.nodes) == 12 + assert {node.kind for node in manifest.nodes} == { + "rigid_object", + "robot_link", + "cable_segment", + } + np.testing.assert_allclose( + frame.positions[frame.node_ids.index("env:1/rigid:cube%2Fname")], + [2.2, 0.0, 0.3], + ) + np.testing.assert_allclose( + frame.positions[frame.node_ids.index("env:1/cable:cable/segment:0")], + [2.1, 0.2, 0.3], + ) + + +def test_cable_export_requires_prepared_data_and_propagates_released_state() -> None: + sim = _CableSimulation() + cable = sim.cables["cable/main"] + data = cable.data + cable.data = None + exporter = SceneExporter(sim, VisualizationCfg(backend="viser")) + + with pytest.raises(RuntimeError, match="prepare"): + exporter.build_manifest() + + cable.data = data + exporter.build_manifest() + data.released = True + with pytest.raises(RuntimeError, match="released"): + exporter.capture(sim_step=0, sim_time=0) + + def test_joint_control_provider_is_filtered_and_captured() -> None: exporter = SceneExporter( _EmptySimulation(),