diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 76c08e4518..274f5a96c8 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -55,7 +55,7 @@ topics: keywords: [BaseEnv, EmbodiedEnv, EpisodeContext, EnvCfg, ControlTiming, resolve_control_timing, register_env, list-task, run-task, run-example, target_control_frequency, sim_steps_per_control, step_dt, environment.component, embodiment.component, file-owned physics backend, physics_config, ControllerAction, EnvProfiler, startup_summary, dexsim_startup_info, - FluidReplayEnv, fluid Gym, 流体 Gym, ExpertTrajectoryCfg, joint_command_mode, position_velocity, qpos qvel, expert action schema, ObservationHistorySnapshot, ObservationFrame, + fluid Gym, 流体 Gym, fluid replay, 粒子回放, ExpertTrajectoryCfg, joint_command_mode, position_velocity, qpos qvel, expert action schema, ObservationHistorySnapshot, ObservationFrame, task_instruction, resolve_demo_instruction, show-task, task catalog, task gallery, 任务画廊, physical_objective, 物理目标, RLTaskCfg, rl_task] paths: [topics/env-framework/env-framework.md] @@ -79,14 +79,14 @@ topics: - embodichain/lab/gym/envs/embodied_env.py - embodichain/lab/gym/envs/episode_context.py - embodichain/lab/gym/envs/expert_trajectory.py - - embodichain/lab/gym/envs/fluid_replay.py + - embodichain/lab/gym/envs/_fluid_trajectory.py - embodichain/lab/scripts/run_fluid_task_program.py - embodichain/lab/gym/utils/trajectory_state.py - embodichain/lab/gym/envs/managers/observation_types.py - embodichain/lab/gym/envs/demo.py - embodichain/lab/gym/utils/profiler.py watch_paths: [embodichain/lab/gym/envs/managers/observation_types.py, embodichain/lab/gym/envs/episode_context.py, embodichain/lab/gym/envs/base_env.py, embodichain/lab/gym/envs/embodied_env.py, embodichain/lab/gym/envs/expert_trajectory.py, - embodichain/lab/gym/envs/demo.py, embodichain/lab/gym/envs/fluid_replay.py, + embodichain/lab/gym/envs/demo.py, embodichain/lab/gym/envs/_fluid_trajectory.py, embodichain/lab/scripts/run_fluid_task_program.py, embodichain/lab/gym/utils/, embodichain/lab/gym/envs/task_program/, embodichain/cli/, embodichain/lab/scripts/run_task.py, extensions/embodichain_tasks/, tests/gym/envs/, embodichain/lab/gym/envs/objectives/, embodichain/lab/scripts/evaluate_task_objective.py, tests/lab/scripts/test_evaluate_task_objective.py] diff --git a/agent_context/topics/env-framework/execution.md b/agent_context/topics/env-framework/execution.md index 831d980574..deb348cf27 100644 --- a/agent_context/topics/env-framework/execution.md +++ b/agent_context/topics/env-framework/execution.md @@ -304,6 +304,10 @@ not orchestrate multiple experiments or qualify kinematic playback. accepted. Objective-enabled dynamic replay requires one row and a full reset. `control` exposes `go_to_step(step)` for O(1) scrubbing. The replay env must use the same robot/objects/`actions` config as the recording env. + Qualified fluid recordings carry bounded particle-state sidecars. Kinematic + and control replay restore complete particle states at each recorded boundary; + dynamic replay restores only the initial particle state and recomputes later + fluid motion from recorded actions. - **Policy projection**: `rollout_buffer` retains actual policy observations, including sampled noise, and its existing obs/action/reward alignment for LeRobot/RL. It shares the acquisition boundary with raw history, rather than diff --git a/agent_context/topics/env-framework/fluid-replay.md b/agent_context/topics/env-framework/fluid-replay.md index c1bd29e9bf..0d7850dcb3 100644 --- a/agent_context/topics/env-framework/fluid-replay.md +++ b/agent_context/topics/env-framework/fluid-replay.md @@ -39,10 +39,14 @@ assurance alone does not prove grasp or pouring success. Optional trajectory recording stores measured online state for inspection. `--seconds` qualifies only a bounded fragment, not completion of the task. -`FluidReplayEnv` remains an explicit diagnostic utility for accepted historical -recordings; it is not the default deployment or task runner. It requires a -separate kinematic configuration, one Newton substep and fixed-root source -states, and rejects action managers and policy dataset/trajectory recording. +Accepted Task Program recordings can opt into control-aligned full particle-state +sidecars with `--record-trajectory --record-fluid-state`. Each source row has a +bounded, content-verified chunk manifest. Particle positions are arena-local in +the artifact and converted to the target arena on replay. The ordinary +`ReplayWrapper` restores particles along with rigid states in `kinematic` and +`control`; `dynamic` restores only the initial particle frame, then replays +recorded controls through DexUni. Fluid replay requires qualified sidecars and +does not infer fluid state from the scene's initial fill. ## Rendering and recording diff --git a/embodichain/lab/gym/envs/_fluid_trajectory.py b/embodichain/lab/gym/envs/_fluid_trajectory.py new file mode 100644 index 0000000000..700dd9336c --- /dev/null +++ b/embodichain/lab/gym/envs/_fluid_trajectory.py @@ -0,0 +1,280 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Bounded, control-aligned fluid states for Gym trajectory replay.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from pathlib import Path +import shutil +from uuid import uuid4 + +import numpy as np +import torch + +from embodichain.data_pipeline.recording.media import ( + MediaFrameReader, + MediaSpool, + verify_media_manifest, +) + +__all__: list[str] = [] + +_MAX_BUFFER_BYTES = 32 * 1024 * 1024 + + +class FluidTrajectoryRecorder: + """Spool one full particle-state frame per recorded control boundary.""" + + def __init__(self, directory: Path, num_envs: int, step_dt: float) -> None: + self.directory = directory / f"fluid-state-{uuid4().hex}" + self.num_envs = num_envs + self.step_dt = step_dt + self._spools: list[MediaSpool | None] = [None] * num_envs + self._parent_ids = [str(uuid4()) for _ in range(num_envs)] + self._counts = [0] * num_envs + self._published = False + self._committed = False + + def capture( + self, env: object, env_ids: Sequence[int], *, reset: bool = False + ) -> None: + """Capture selected rows, including the reset frame when requested.""" + ids = tuple(int(index) for index in env_ids) + if not ids: + return + if reset: + for index in ids: + if self._spools[index] is not None: + shutil.rmtree(self._spools[index].directory) + self._spools[index] = None + self._parent_ids[index] = str(uuid4()) + self._counts[index] = 0 + elif any(self._spools[index] is None for index in ids): + raise RuntimeError("Fluid trajectory has no initial control frame.") + offsets = env.sim.arena_offsets.detach().cpu().numpy() + frames = {} + for uid in sorted(fluid.uid for fluid in env.cfg.fluid_object): + state = env.sim.get_fluid_object(uid).data.particle_state_w + if state.dtype != torch.float32 or state.ndim != 3 or state.shape[-1] != 6: + raise ValueError("Recorded fluid state must be float32 (E, N, 6).") + values = state.detach().cpu().numpy() + if not np.isfinite(values).all(): + raise ValueError("Recorded fluid state must be finite.") + frames[uid] = values + for index in ids: + if self._spools[index] is None: + self._spools[index] = MediaSpool( + self.directory / f"env_{index:03d}", + max_buffer_bytes=_MAX_BUFFER_BYTES, + parent_episode_id=self._parent_ids[index], + calibration={"coordinate_frame": "arena_local", "channels": 6}, + ) + row = {} + for uid, values in frames.items(): + local = values[index].copy() + local[:, :3] -= offsets[index] + row[uid] = local + self._spools[index].append(self._counts[index] * self.step_dt, row) + self._counts[index] += 1 + + def finish(self, trajectory_path: Path, lengths: Sequence[int]) -> dict: + """Publish verified sidecars before the trajectory references them.""" + if self._published: + raise RuntimeError("Fluid trajectory was already published.") + if len(lengths) != self.num_envs: + raise ValueError("Fluid trajectory row count differs from source.") + manifests = [] + for index, (spool, length) in enumerate(zip(self._spools, lengths)): + if spool is None or self._counts[index] != int(length) + 1: + raise ValueError("Fluid frames do not align with trajectory lengths.") + path = spool.finish() + verify_media_manifest( + path, + expected_frames=self._counts[index], + parent_episode_id=self._parent_ids[index], + expected_timestamps=np.arange(self._counts[index]) * self.step_dt, + ) + manifests.append(str(path.relative_to(trajectory_path.parent))) + self._published = True + return { + "schema_version": 1, + "coordinate_frame": "arena_local", + "manifests": manifests, + "parent_ids": list(self._parent_ids), + } + + def commit(self) -> None: + """Retain sidecars after their referring trajectory is durable.""" + if not self._published: + raise RuntimeError("Fluid sidecars must be finalized before commit.") + self._committed = True + + def abort(self) -> None: + """Discard uncommitted particle chunks after failure or interruption.""" + if not self._committed: + shutil.rmtree(self.directory, ignore_errors=True) + + +class FluidTrajectoryReader: + """Validate and stream selected fluid frames from a qualified trajectory.""" + + def __init__(self, path: str | Path, trajectory: Mapping, cfg: object) -> None: + self.path = Path(path).resolve() + meta = trajectory["meta"] + qualification = meta.get("fluid_qualification") + if ( + meta.get("artifact_purpose") != "recording" + or not isinstance(qualification, Mapping) + or qualification.get("kind") != "task_program" + or qualification.get("accepted") is not True + ): + raise ValueError( + "Fluid replay requires an accepted Task Program recording." + ) + episodes = meta.get("demo_episodes", []) + source_envs = int(meta["num_envs"]) + if len(episodes) != source_envs or not episodes: + raise ValueError("Fluid replay source episodes do not match recorded rows.") + program_hash = qualification.get("program_hash") + if ( + not isinstance(program_hash, str) + or not program_hash + or any( + episode.get("program_hash") != program_hash + or episode.get("completed") is not True + or episode.get("success") is not True + for episode in episodes + ) + ): + raise ValueError("Fluid replay program provenance differs from the source.") + target_envs = int(cfg.num_envs) + if source_envs not in (1, target_envs): + raise ValueError( + "Fluid replay source must match env count or contain one row." + ) + sidecar = meta.get("fluid_state") + if ( + not isinstance(sidecar, Mapping) + or sidecar.get("schema_version") != 1 + or sidecar.get("coordinate_frame") != "arena_local" + ): + raise ValueError("Fluid replay requires aligned particle-state sidecars.") + paths = sidecar.get("manifests") + parent_ids = sidecar.get("parent_ids") + if not isinstance(paths, list) or not isinstance(parent_ids, list): + raise ValueError("Fluid replay sidecar manifest is malformed.") + if len(paths) != source_envs or len(parent_ids) != source_envs: + raise ValueError("Fluid replay sidecar rows do not match the trajectory.") + self._uids = tuple(sorted(fluid.uid for fluid in cfg.fluid_object)) + if not self._uids: + raise ValueError("Fluid replay requires configured fluid objects.") + expected_counts = { + fluid.uid: int(fluid.spawn.sample(fluid.particle_spacing).shape[0]) + for fluid in cfg.fluid_object + } + self._readers = [] + try: + for index, (name, parent_id) in enumerate(zip(paths, parent_ids)): + if ( + not isinstance(name, str) + or not name + or not isinstance(parent_id, str) + or not parent_id + ): + raise ValueError( + "Fluid replay manifest paths and IDs must be strings." + ) + relative = Path(name) + if relative.is_absolute() or ".." in relative.parts: + raise ValueError( + "Fluid replay manifest escapes its artifact directory." + ) + candidate = self.path.parent / relative + if any( + part.is_symlink() + for part in (candidate, *candidate.parents) + if part.is_relative_to(self.path.parent) + ): + raise ValueError("Fluid replay manifest cannot use symlinks.") + manifest_path = candidate.resolve() + if not manifest_path.is_relative_to(self.path.parent): + raise ValueError( + "Fluid replay manifest escapes its artifact directory." + ) + count = int(meta["lengths"][index]) + 1 + declared = verify_media_manifest( + manifest_path, + expected_frames=count, + parent_episode_id=parent_id, + expected_timestamps=np.arange(count) * float(meta["dt"]), + ) + if declared.get("calibration") != { + "coordinate_frame": "arena_local", + "channels": 6, + }: + raise ValueError("Fluid replay particle frame convention differs.") + if tuple(sorted(declared["features"])) != self._uids: + raise ValueError( + "Fluid replay particle UIDs differ from the scene." + ) + for uid, schema in declared["features"].items(): + if ( + len(schema["shape"]) != 2 + or schema["shape"][0] != expected_counts[uid] + or schema["shape"][1] != 6 + or np.dtype(schema["dtype"]) != np.dtype("float32") + ): + raise ValueError("Fluid replay requires float32 (N, 6) frames.") + self._readers.append(MediaFrameReader(manifest_path)) + except Exception: + self.close() + raise + self._source_envs = source_envs + self._target_envs = target_envs + + def apply(self, env: object, steps: torch.Tensor) -> None: + """Write selected control-boundary frames without advancing physics.""" + if tuple(steps.shape) != (self._target_envs,): + raise ValueError("Fluid replay step indices do not match target rows.") + indices = [int(value) for value in steps.detach().cpu().tolist()] + offsets = env.sim.arena_offsets + selected: dict[tuple[int, int], dict[str, np.ndarray]] = {} + for row, step in enumerate(indices): + source = 0 if self._source_envs == 1 else row + key = source, step + if key not in selected: + _, selected[key] = self._readers[source].read(step) + for uid in self._uids: + fluid = env.sim.get_fluid_object(uid) + values = [] + for row, step in enumerate(indices): + source = 0 if self._source_envs == 1 else row + state = selected[source, step][uid] + if ( + state.shape != (fluid.num_particles, 6) + or not np.isfinite(state).all() + ): + raise ValueError("Fluid replay particle count or state is invalid.") + values.append(state) + world = torch.as_tensor(np.stack(values), device=env.device).clone() + world[:, :, :3] += offsets[:, None, :] + fluid.write_particle_state_to_sim(world) + + def close(self) -> None: + for reader in getattr(self, "_readers", ()): + reader.close() diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 48a525ee09..7cf33b266f 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -75,6 +75,7 @@ DemoSegment, DemoSegmentResult, ) +from embodichain.lab.gym.envs._fluid_trajectory import FluidTrajectoryRecorder from embodichain.lab.gym.envs.types import ControllerAction from embodichain.lab.gym.envs.expert_trajectory import ( ExpertTrajectoryCfg, @@ -565,6 +566,7 @@ def __init__( ) self._traj_buffer: TensorDict | None = None + self._fluid_trajectory_recorder: FluidTrajectoryRecorder | None = None self._traj_steps: torch.Tensor | None = None self._traj_raw_action: EnvAction | None = None self._trajectory_action_kind: str | None = None @@ -806,6 +808,23 @@ def _record_initial_observation( policy[key] = storage[key] # Existing consumers retain their transition-length view. self.rollout_buffer = policy[:, : self._max_rollout_steps] + recorder = getattr(self, "_fluid_trajectory_recorder", None) + if recorder is not None: + recorder.capture(self, env_ids.tolist(), reset=True) + + def attach_fluid_trajectory_recorder( + self, recorder: FluidTrajectoryRecorder + ) -> None: + """Attach a bounded fluid-state recorder before the first reset.""" + if not isinstance(recorder, FluidTrajectoryRecorder): + raise TypeError("recorder must be a FluidTrajectoryRecorder.") + if not self.cfg.record_trajectory or not self.cfg.fluid_object: + raise ValueError( + "Fluid recording requires trajectory and fluid configuration." + ) + if self._fluid_trajectory_recorder is not None: + raise RuntimeError("A fluid trajectory recorder is already attached.") + self._fluid_trajectory_recorder = recorder def set_rollout_buffer(self, rollout_buffer: TensorDict) -> None: """Set the rollout buffer for episode data collection. @@ -2205,17 +2224,27 @@ def _write_trajectory_step( "Trajectory recording requires an explicit observation frame and action." ) manager._bind_schema(frame) + selected = ( + active if active is not None else getattr(self, "_demo_active_mask", None) + ) _write_raw_rollout( self._rollout_storage, self.episode_context, frame, action=self._traj_raw_action, - active=( - active - if active is not None - else getattr(self, "_demo_active_mask", None) - ), + active=selected, ) + recorder = getattr(self, "_fluid_trajectory_recorder", None) + if recorder is not None: + ids = ( + list(range(self.num_envs)) + if selected is None + else torch.as_tensor(selected, dtype=torch.bool) + .nonzero() + .flatten() + .tolist() + ) + recorder.capture(self, ids) self._traj_raw_action = None def _write_rl_rollout_step( @@ -3497,6 +3526,7 @@ def save_trajectory( env_ids: Sequence[int] | None = None, *, purpose: str = "recording", + extra_meta: Mapping[str, Any] | None = None, ) -> str: """Save a causally aligned trajectory to a ``.pt`` file. @@ -3510,6 +3540,7 @@ def save_trajectory( env_ids: Env indices to save (default: all). Each saved env's actual recorded length is stored in ``meta["lengths"]``. purpose: Normal recording or explicitly labeled diagnostic export. + extra_meta: Additional artifact metadata; standard fields cannot be replaced. Raises: RuntimeError: If trajectory recording was never enabled. @@ -3612,6 +3643,17 @@ def save_trajectory( meta["episode_uuid"] = meta["episode_uuids"][0] meta["source_episode_uuid"] = meta["source_episode_uuids"][0] meta["program_hash"] = meta["demo_episodes"][0].get("program_hash") + if extra_meta is not None: + if not isinstance(extra_meta, Mapping) or not all( + type(key) is str for key in extra_meta + ): + raise TypeError("extra_meta must be a string-keyed mapping.") + duplicate = set(extra_meta) & set(meta) + if duplicate: + raise ValueError( + f"extra_meta cannot replace trajectory fields: {sorted(duplicate)}" + ) + meta.update(copy.deepcopy(dict(extra_meta))) replay_path = str(Path(path).resolve()) for episode in meta["demo_episodes"]: episode["trajectory_path"] = replay_path diff --git a/embodichain/lab/gym/envs/fluid_replay.py b/embodichain/lab/gym/envs/fluid_replay.py deleted file mode 100644 index 06bc747571..0000000000 --- a/embodichain/lab/gym/envs/fluid_replay.py +++ /dev/null @@ -1,331 +0,0 @@ -# ---------------------------------------------------------------------------- -# 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. -# ---------------------------------------------------------------------------- - -"""One-way fluid Gym replay of an accepted Task Program recording.""" - -from __future__ import annotations - -from collections.abc import Mapping -from copy import deepcopy -from typing import Any -import math -import numpy as np -import torch -from scipy.interpolate import PchipInterpolator -from scipy.spatial.transform import Rotation, Slerp - -from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv, EmbodiedEnvCfg -from embodichain.lab.gym.utils.gym_utils import load_trajectory -from embodichain.lab.sim.cfg import NewtonPhysicsCfg - -__all__ = ["FluidReplayEnv"] - - -class FluidReplayEnv(EmbodiedEnv): - """Replay measured rigid states while continuing to solve fluid physics. - - The source must have been generated and accepted by Task Program. This is - a replay/evaluation environment: policy actions do not control the robot. - Gym reset/step, sensors and configured observation/reward managers still - execute normally. No rigid-fluid force feedback is modeled. - - Args: - cfg: Physical environment configuration, including fluid declarations. - trajectory: Accepted source recording (path or loaded trajectory dict). - speed: Positive source-time multiplier. - hold_seconds: Initial and final stationary intervals in simulation time. - kwargs: Forwarded environment constructor options. - """ - - def __init__( - self, - cfg: EmbodiedEnvCfg, - *, - trajectory: str | dict, - speed: float = 2.0, - hold_seconds: float = 1.0, - **kwargs: Any, - ) -> None: - if ( - not math.isfinite(speed) - or speed <= 0 - or not math.isfinite(hold_seconds) - or hold_seconds < 0 - ): - raise ValueError( - "Replay speed must be positive and hold_seconds nonnegative." - ) - if cfg.record_trajectory or cfg.actions is not None: - raise ValueError( - "Fluid replay does not support action managers or trajectory/action recording." - ) - if ( - not cfg.filter_dataset_saving - and cfg.dataset is not None - and any( - not key.startswith("_") - for key in ( - cfg.dataset - if isinstance(cfg.dataset, Mapping) - else vars(cfg.dataset) - ) - ) - ): - raise ValueError( - "Fluid replay does not support policy dataset recording; record measured fluid diagnostics separately." - ) - self._source = load_trajectory( - deepcopy(trajectory) if isinstance(trajectory, dict) else trajectory - ) - meta = self._source["meta"] - qualification = meta.get("fluid_source", {}) - if ( - qualification.get("kind") != "task_program" - or qualification.get("accepted") is not True - or not isinstance(qualification.get("program_hash"), str) - or not qualification["program_hash"] - ): - raise ValueError( - "Fluid replay requires a Task Program recording with accepted fluid_source qualification." - ) - if int(meta["num_envs"]) != 1: - raise ValueError( - "The initial fluid replay adapter broadcasts one qualified source row." - ) - count = int(meta["lengths"][0]) - dt = float(meta["dt"]) - if count < 2 or not math.isfinite(dt) or dt <= 0: - raise ValueError( - "Replay needs at least two source frames at a finite positive cadence." - ) - self._source_times = np.arange(count, dtype=float) * dt - self._speed = speed - self._hold_seconds = hold_seconds - self.replay_duration = 2 * hold_seconds + self._source_times[-1] / speed - cfg = deepcopy(cfg) - physics = cfg.sim_cfg.physics_cfg - solver = getattr(physics, "solver_cfg", None) - solver_type = ( - solver.get("solver_type") - if isinstance(solver, dict) - else getattr(solver, "solver_type", None) - ) - if not isinstance(physics, NewtonPhysicsCfg) or solver_type != "dexuni": - raise ValueError("Fluid replay requires Newton DexUni with DFSPH.") - if cfg.sim_cfg.physics_cfg.num_substeps != 1: - raise ValueError( - "Fluid Gym replay requires num_substeps=1; choose physics_dt for boundary cadence." - ) - states = self._source["states"] - joints = states["robot", "qpos"][0, :count].cpu().numpy() - self._joint_curve = PchipInterpolator(self._source_times, joints) - cfg.robot.init_qpos = joints[0].tolist() - roots = states["robot", "root_pose"][0, :count].cpu().numpy() - root_rotations = Rotation.from_quat(roots[:, 3:]).as_matrix() - if not np.allclose(roots[:, :3], roots[0, :3], atol=1e-5) or not np.allclose( - root_rotations, root_rotations[0], atol=1e-5 - ): - raise ValueError( - "Fluid replay currently supports fixed-root robot recordings only." - ) - root_pose = np.eye(4, dtype=np.float32) - root_pose[:3, :3] = root_rotations[0] - root_pose[:3, 3] = roots[0, :3] - cfg.robot.init_local_pose = root_pose - - self._rigid_curves = {} - objects = {item.uid: item for item in cfg.background + cfg.rigid_object} - for uid in meta.get("rigid_object_uids", []): - if uid not in objects: - continue - if objects[uid].body_type != "kinematic": - raise ValueError(f"Replayed rigid object {uid!r} must be kinematic.") - poses = states["rigid_objects", uid, "pose"][0, :count].cpu().numpy() - self._rigid_curves[uid] = ( - PchipInterpolator(self._source_times, poses[:, :3]), - Slerp(self._source_times, Rotation.from_quat(poses[:, 3:])), - ) - pose = np.eye(4, dtype=np.float32) - pose[:3, :3] = Rotation.from_quat(poses[0, 3:]).as_matrix() - pose[:3, 3] = poses[0, :3] - objects[uid].init_local_pose = pose - if not self._rigid_curves: - raise ValueError("No recorded rigid objects match the replay scene.") - self._replay_time = np.zeros(cfg.num_envs, dtype=np.float64) - self._previous_rigid = {} - super().__init__(cfg, **kwargs) - try: - if joints.shape[1] != self.robot.dof: - raise ValueError( - "Source robot DOF count does not match the replay embodiment." - ) - if "active_joint_ids" in meta and list(meta["active_joint_ids"]) != list( - self.active_joint_ids - ): - raise ValueError( - "Source active joint order does not match the replay embodiment." - ) - if "joint_names" in meta: - expected = [self.robot.joint_names[i] for i in self.active_joint_ids] - if list(meta["joint_names"]) != expected: - raise ValueError( - "Source joint names do not match the replay embodiment." - ) - except Exception: - self._cleanup_failed_initialization() - raise - self._replay_no_auto_reset = True - - def step( - self, action: Any, **kwargs: Any - ) -> tuple[Any, torch.Tensor, torch.Tensor, torch.Tensor, dict]: - """Advance replay; reset is required after every row is exhausted.""" - if np.all(self._replay_time >= self.replay_duration): - raise RuntimeError( - "Fluid replay is exhausted; reset before stepping again." - ) - return super().step(action, **kwargs) - - def check_truncated(self, obs: Any, info: dict) -> torch.Tensor: - """Signal each row's replay exhaustion independently.""" - return super().check_truncated(obs, info) | torch.as_tensor( - self._replay_time >= self.replay_duration, device=self.device - ) - - def _step_action(self, action: Any) -> Any: - """Preserve the Gym lifecycle while motion comes from recorded states.""" - return action - - def _apply_replay_state(self, ids: list[int], dt: float) -> None: - if not ids: - return - source_times = np.clip( - (self._replay_time[ids] - self._hold_seconds) * self._speed, - 0.0, - self._source_times[-1], - ) - qpos = torch.as_tensor( - self._joint_curve(source_times), dtype=torch.float32, device=self.device - ) - self.robot.set_qpos(qpos, env_ids=ids, target=False) - active = (self._replay_time[ids] >= self._hold_seconds) & ( - self._replay_time[ids] - < self._hold_seconds + self._source_times[-1] / self._speed - ) - qvel = ( - self._joint_curve.derivative()(source_times) * self._speed * active[:, None] - ) - self.robot.set_qvel( - torch.as_tensor(qvel, dtype=torch.float32, device=self.device), - env_ids=ids, - target=False, - ) - if not hasattr(self, "_replay_offsets"): - self._replay_offsets = self.sim.arena_offsets.detach().cpu().numpy().copy() - offsets = self._replay_offsets[ids] - for uid, (positions, rotations) in self._rigid_curves.items(): - poses = np.repeat(np.eye(4, dtype=np.float32)[None], len(ids), axis=0) - poses[:, :3, :3] = rotations(source_times).as_matrix() - poses[:, :3, 3] = positions(source_times) + offsets - previous = self._previous_rigid.get(uid) - if previous is None: - previous = np.repeat( - np.eye(4, dtype=np.float32)[None], self.num_envs, axis=0 - ) - self._previous_rigid[uid] = previous - previous[ids] = poses - old = previous[ids].copy() - linear = ( - (poses[:, :3, 3] - old[:, :3, 3]) / dt - if dt > 0 - else np.zeros((len(ids), 3)) - ) - angular = ( - Rotation.from_matrix( - poses[:, :3, :3] @ old[:, :3, :3].transpose(0, 2, 1) - ).as_rotvec() - / dt - if dt > 0 - else np.zeros((len(ids), 3)) - ) - self.sim.get_rigid_object(uid).set_kinematic_state( - torch.from_numpy(poses), - torch.from_numpy(linear), - torch.from_numpy(angular), - env_ids=ids, - ) - previous[ids] = poses - - def _advance_physics(self) -> None: - sensors = [ - sensor for sensor in self.sensors.values() if sensor.requires_substep_update - ] - for sensor in sensors: - sensor.begin_control_step() - ids = list(range(self.num_envs)) - for _ in range(self.cfg.sim_steps_per_control): - self._apply_replay_state(ids, self.physics_dt) - self.sim.update(step=1, render_final_step=False) - self._replay_time += self.physics_dt - for sensor in sensors: - sensor.update_physics_step(self.physics_dt) - - def _initialize_episode(self, env_ids=None, **kwargs: Any) -> None: - super()._initialize_episode(env_ids=env_ids, **kwargs) - ids = ( - list(range(self.num_envs)) - if env_ids is None - else torch.as_tensor(env_ids).cpu().tolist() - ) - self._replay_time[ids] = 0.0 - self._apply_replay_state(ids, 0.0) - # Reset-mode events may have randomized the container; refill only the - # selected rows from its measured world pose and the local fill template. - for cfg in self.cfg.fluid_object: - if cfg.spawn.reference_uid is None: - continue - from embodichain.lab.gym.envs.managers.cfg import SceneEntityCfg - from embodichain.lab.gym.envs.managers.events import ( - reset_fluid_in_container, - ) - - reset_fluid_in_container( - self, - ids, - [ - SceneEntityCfg(uid=cfg.uid), - SceneEntityCfg(uid=cfg.spawn.reference_uid), - ], - ) - - @property - def source_qualification(self) -> dict: - """Return independent Task Program provenance and source acceptance.""" - return deepcopy(self._source["meta"]["fluid_source"]) - - def get_info(self, **kwargs: Any) -> dict: - """Return normal Gym info with row-local replay time and completion.""" - info = super().get_info(**kwargs) - info["fluid_replay_time_s"] = torch.as_tensor( - self._replay_time.copy(), device=self.device - ) - info["fluid_replay_finished"] = torch.as_tensor( - self._replay_time >= self.replay_duration, device=self.device - ) - info["fluid_source_accepted"] = torch.ones( - self.num_envs, dtype=torch.bool, device=self.device - ) - return info diff --git a/embodichain/lab/gym/envs/wrapper/replay.py b/embodichain/lab/gym/envs/wrapper/replay.py index f9766e26b4..997cfe4c89 100644 --- a/embodichain/lab/gym/envs/wrapper/replay.py +++ b/embodichain/lab/gym/envs/wrapper/replay.py @@ -27,6 +27,7 @@ EXPERT_TRAJECTORY_SCHEMA_VERSION, build_expert_action_spec, ) +from embodichain.lab.gym.envs._fluid_trajectory import FluidTrajectoryReader 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 @@ -41,10 +42,12 @@ class ReplayWrapper(gym.Wrapper): """Replay a recorded environment trajectory. - In ``kinematic`` mode physics is disabled and every recorded object's - pose/qpos is written directly each step, producing observations only (no - reward / success / action). In ``dynamic`` mode the recorded robot actions - are fed back through :meth:`env.step` so physics re-simulates the scene; + In ``kinematic`` mode physics is disabled and recorded robot, rigid and + available fluid-particle states are written directly each step, producing + observations only (no reward / success / action). In ``dynamic`` mode the + recorded robot actions are fed back through :meth:`env.step` so physics + re-simulates the scene; a recorded fluid starts at its source state and + then evolves through the solver without per-frame state replacement; the full ``obs/reward/terminated/truncated/info`` tuple is returned. The ``control`` mode uses the same kinematic behavior while exposing :meth:`go_to_step` for interactive scrubbing. @@ -72,6 +75,8 @@ def __init__( env: gym.Env, trajectory: str | dict, mode: str = "dynamic", + *, + fluid_reader: FluidTrajectoryReader | None = None, ): super().__init__(env) if mode not in ("kinematic", "dynamic", "control"): @@ -82,6 +87,16 @@ def __init__( self._mode = mode self._trajectory = load_trajectory(trajectory) meta = self._trajectory["meta"] + self._fluid_reader = fluid_reader + if getattr(getattr(env, "cfg", None), "fluid_object", ()): + if self._fluid_reader is None: + if not isinstance(trajectory, str): + raise ValueError("Fluid replay requires a trajectory file path.") + self._fluid_reader = FluidTrajectoryReader( + trajectory, self._trajectory, env.cfg + ) + elif fluid_reader is not None or "fluid_state" in meta: + raise ValueError("A fluid trajectory requires configured fluid objects.") self._controller_spec = None self._task_outcome_replay = mode == "dynamic" and ( getattr(env, "termination_manager", None) is not None @@ -208,6 +223,10 @@ def reset( # 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._mode == "dynamic": self.env.sim.enable_physics(True) self.env._replay_no_auto_reset = True @@ -246,7 +265,14 @@ def step( if self._mode in ("kinematic", "control"): restore_trajectory_state(self.env, self._trajectory["states"][idx, st]) - env.sim.update(env.sim_cfg.physics_dt, env.cfg.sim_steps_per_control) + if self._fluid_reader is not None: + self._fluid_reader.apply(env, st) + # Newton's disabled-physics update can still enter the DexUni + # solver. Publish the authored state without a physical step. + with env.sim.render_frame(force_visualization=True): + pass + else: + env.sim.update(env.sim_cfg.physics_dt, env.cfg.sim_steps_per_control) obs = env.get_obs() self._replay_steps = (self._replay_steps + 1).clamp(max=self._lengths) trunc = self._replay_steps >= self._lengths @@ -286,8 +312,23 @@ def go_to_step(self, step: int) -> EnvObs: max_step = self.control_max_step step = max(0, min(int(step), max_step)) env.sim.enable_physics(False) - restore_trajectory_state(env, self._trajectory["states"][:, step]) - env.sim.update(env.sim_cfg.physics_dt, env.cfg.sim_steps_per_control) + terminal = step == int(self._lengths.min().item()) + restore_trajectory_state( + env, + ( + self._trajectory["terminal_states"] + if terminal + else self._trajectory["states"][:, step] + ), + ) + if self._fluid_reader is not None: + self._fluid_reader.apply( + env, torch.full((env.num_envs,), step, dtype=torch.long) + ) + with env.sim.render_frame(force_visualization=True): + pass + else: + env.sim.update(env.sim_cfg.physics_dt, env.cfg.sim_steps_per_control) self._replay_steps = torch.full( (env.num_envs,), step, dtype=torch.long, device=env.device ) @@ -297,10 +338,18 @@ def go_to_step(self, step: int) -> EnvObs: def control_max_step(self) -> int: """Largest state index available to interactive control replay.""" transition_count = int(self._lengths.min().item()) + if ( + self._fluid_reader is not None + and "terminal_states" in self._trajectory + and bool((self._lengths == transition_count).all()) + ): + return transition_count return transition_count - 1 def close(self) -> None: try: + if self._fluid_reader is not None: + self._fluid_reader.close() self.env.sim.enable_physics(True) self.env._replay_no_auto_reset = False finally: diff --git a/embodichain/lab/scripts/run_fluid_task_program.py b/embodichain/lab/scripts/run_fluid_task_program.py index 35d2e472c6..04f0d150be 100644 --- a/embodichain/lab/scripts/run_fluid_task_program.py +++ b/embodichain/lab/scripts/run_fluid_task_program.py @@ -30,6 +30,7 @@ from embodichain.lab.gym.envs.demo import execute_demo_episode from embodichain.lab.gym.envs.embodied_env import EmbodiedEnvCfg +from embodichain.lab.gym.envs._fluid_trajectory import FluidTrajectoryRecorder from embodichain.lab.gym.envs.managers.cfg import SceneEntityCfg from embodichain.lab.gym.envs.managers.observations import ( get_fluid_summary, @@ -146,7 +147,11 @@ def _run(args: argparse.Namespace, output: Path) -> dict[str, Any]: ) discover_task_packages() env = make(task_id, cfg=cfg) + fluid_recorder = None try: + if getattr(args, "record_fluid_state", False): + fluid_recorder = FluidTrajectoryRecorder(output, env.num_envs, env.step_dt) + env.unwrapped.attach_fluid_trajectory_recorder(fluid_recorder) env.reset(seed=0) cup = env.sim.get_rigid_object("cup") poses = [cup.get_local_pose().cpu().numpy().copy()] @@ -207,8 +212,6 @@ def stop() -> bool: matches.append(video_path) if matches: videos.append(str(sorted(matches)[-1].relative_to(output))) - if args.record_trajectory: - env.save_trajectory(str(output / "online_trajectory.pt")) report = { "task": str(path), "coupling": "online one-way rigid-to-fluid", @@ -234,6 +237,36 @@ def stop() -> bool: (output / "fluid_gym_result.json").write_text( json.dumps(report, indent=2) + "\n" ) + if args.record_trajectory: + trajectory_path = output / "online_trajectory.pt" + extra_meta = None + if fluid_recorder is not None and accepted: + episodes = [ + env.unwrapped.get_demo_episode_metadata(index) + for index in range(env.num_envs) + ] + hashes = {episode.get("program_hash") for episode in episodes} + if len(hashes) != 1 or not next(iter(hashes)): + raise RuntimeError( + "Accepted fluid source has no common program hash." + ) + extra_meta = { + "fluid_qualification": { + "kind": "task_program", + "accepted": True, + "program_hash": next(iter(hashes)), + }, + "fluid_state": fluid_recorder.finish( + trajectory_path, env.unwrapped._traj_steps.tolist() + ), + } + env.save_trajectory( + str(trajectory_path), + purpose="recording" if accepted else "diagnostic", + extra_meta=extra_meta, + ) + if fluid_recorder is not None and accepted: + fluid_recorder.commit() print( json.dumps( { @@ -249,6 +282,8 @@ def stop() -> bool: ) return report finally: + if fluid_recorder is not None: + fluid_recorder.abort() env.close(exit_process=False) del env SimulationManager.flush_cleanup_queue() @@ -273,7 +308,14 @@ def main() -> None: action="store_true", help="Optionally save measured online states for inspection.", ) + parser.add_argument( + "--record-fluid-state", + action="store_true", + help="Spool full particle states alongside an accepted trajectory.", + ) args = parser.parse_args() + if args.record_fluid_state and not args.record_trajectory: + parser.error("--record-fluid-state requires --record-trajectory") if args.seconds is not None and ( not np.isfinite(args.seconds) or args.seconds <= 0 ): diff --git a/embodichain/lab/scripts/run_task.py b/embodichain/lab/scripts/run_task.py index a939b1be56..e4abf434c6 100644 --- a/embodichain/lab/scripts/run_task.py +++ b/embodichain/lab/scripts/run_task.py @@ -39,6 +39,7 @@ execute_demo_episode, ) from embodichain.lab.gym.envs.wrapper import ReplayWrapper +from embodichain.lab.gym.envs._fluid_trajectory import FluidTrajectoryReader from embodichain.lab.gym.utils.gym_utils import ( add_env_launcher_args_to_parser, build_env_cfg_from_args, @@ -1132,7 +1133,13 @@ def _run_expansion( ) -def replay(env, trajectory_path: str, mode: str = "kinematic") -> None: +def replay( + env, + trajectory_path: str, + mode: str = "kinematic", + *, + fluid_reader: FluidTrajectoryReader | None = None, +) -> None: """Replay a recorded trajectory. The caller retains ownership of ``env``. Wrapper-specific replay state is @@ -1153,19 +1160,39 @@ def replay(env, trajectory_path: str, mode: str = "kinematic") -> None: f"num_steps={meta['num_steps']}, mode={mode}", color="green", ) - replay_env = ReplayWrapper(env.unwrapped, trajectory_path, mode=mode) + if fluid_reader is not None: + behavior = ( + "recorded particle states without physics" + if mode in {"kinematic", "control"} + else "recorded controls with newly simulated fluid" + ) + log_info( + f"Accepted Task Program fluid source; replaying {behavior}. " + "Source acceptance does not qualify this replay's physical outcome.", + color="green", + ) + replay_env = None try: + replay_env = ReplayWrapper( + env.unwrapped, trajectory_path, mode=mode, fluid_reader=fluid_reader + ) if mode == "control": replay_control(replay_env) else: replay_auto(replay_env, mode) finally: - # ReplayWrapper.close() also closes its wrapped environment. Restore - # its local state here and leave the single close() to cli(). - try: - replay_env.env.sim.enable_physics(True) - finally: - replay_env.env._replay_no_auto_reset = False + if replay_env is None: + if fluid_reader is not None: + fluid_reader.close() + else: + # ReplayWrapper.close() also closes its wrapped environment. Restore + # its local state here and leave the single close() to cli(). + try: + replay_env.env.sim.enable_physics(True) + finally: + replay_env.env._replay_no_auto_reset = False + if replay_env._fluid_reader is not None: + replay_env._fluid_reader.close() def replay_auto(replay_env: ReplayWrapper, mode: str) -> None: @@ -1445,7 +1472,13 @@ def replay_control(replay_env: ReplayWrapper) -> None: ) -def main(args: Any, env: Any, gym_config: dict[str, Any]) -> None: +def main( + args: Any, + env: Any, + gym_config: dict[str, Any], + *, + fluid_reader: FluidTrajectoryReader | None = None, +) -> None: """Run the selected workflow without taking ownership of ``env``.""" if getattr(args, "replay", False): log_info("Replay mode.", color="green") @@ -1453,6 +1486,7 @@ def main(args: Any, env: Any, gym_config: dict[str, Any]) -> None: env, args.replay_trajectory, getattr(args, "replay_mode", "kinematic"), + fluid_reader=fluid_reader, ) return @@ -1904,7 +1938,19 @@ def cli(argv: Sequence[str] | None = None) -> None: if args.replay and args.replay_mode == "control": log_info("Dataset saving disabled for control replay mode.", color="green") - env = gymnasium.make(id=gym_config["id"], cfg=env_cfg, **action_config) + fluid_reader = None + if args.replay and env_cfg.fluid_object: + fluid_reader = FluidTrajectoryReader( + args.replay_trajectory, + load_trajectory(args.replay_trajectory), + env_cfg, + ) + try: + env = gymnasium.make(id=gym_config["id"], cfg=env_cfg, **action_config) + except BaseException: + if fluid_reader is not None: + fluid_reader.close() + raise # Ensure the sim is torn down via env.close() (-> SimulationManager.destroy()) # before the interpreter shuts down. Without this, C++ resources (dexsim/warp/ @@ -1925,7 +1971,7 @@ def cli(argv: Sequence[str] | None = None) -> None: install_real2sim_tabletop_owner( env, real2sim_profile, dataset_dir=args.expansion_dataset_dir ) - main(args, env, gym_config) + main(args, env, gym_config, fluid_reader=fluid_reader) except BaseException as error: body_error = error raise diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/README.md index ef91db0fe7..c1e01ffcf3 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/README.md +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/README.md @@ -79,7 +79,18 @@ FastRT 使用蓝色液面材质,并为每个环境生成独立的 960 × 720 Gym 的 `fluid_gym_result.json` 和 Sim 的 `validation.json` 分别记录任务完成情况、 杯子抬升、最大倾角、放回误差和接水量。完整验收要求实际抬升至少 5 cm、倾角 至少 45°、放回误差不超过 3 cm,且最终接水至少 50 mL;规划成功或部分视频不能 -替代这些物理测量。`--record-trajectory` 仅保存可选的在线状态诊断。 +替代这些物理测量。使用 `--record-trajectory --record-fluid-state` 可在验收通过后 +保存完整粒子状态;每环境约需 1.28 GiB(76,607 粒、746 帧)。普通 +`ReplayWrapper` 的 `kinematic`/`control` 模式回填粒子,`dynamic` 模式只回填 +初始粒子状态,之后随录制控制动作重新求解流体。 -`FluidReplayEnv` 是独立的历史记录诊断工具,不是在线任务的默认输入。它使用 -运动学边界和固定根记录,不能替代动态抓取验证。 +```bash +python -m embodichain.lab.scripts.run_fluid_task_program \ + --num-envs 1 --record-trajectory --record-fluid-state \ + --output outputs/fluid-task-program-online +embodichain run-task \ + --gym-config extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml \ + --num-envs 1 --replay \ + --replay_trajectory outputs/fluid-task-program-online/online_trajectory.pt \ + --replay_mode kinematic # 或 dynamic +``` diff --git a/tests/gym/envs/test_fluid_integration.py b/tests/gym/envs/test_fluid_integration.py index 73ca10d550..4e64a92468 100644 --- a/tests/gym/envs/test_fluid_integration.py +++ b/tests/gym/envs/test_fluid_integration.py @@ -40,17 +40,6 @@ ) -def _replay_cfg(): - cfg = config_to_cfg(load_config(TASK), source_path=TASK) - cfg.task_program = None - cup = cfg.rigid_object.pop(0) - cup.body_type = "kinematic" - cfg.background.append(cup) - cfg.sim_cfg.physics_cfg.num_substeps = 1 - cfg.sim_cfg.physics_cfg.solver_cfg["sph_rigid_substeps"] = 1 - return cfg - - @pytest.fixture(autouse=True) def task_mesh_cache(tmp_path, monkeypatch): """Resolve task meshes locally so configuration tests never download them.""" @@ -164,108 +153,6 @@ def test_region_volume_uses_reference_pose_and_keeps_state_unchanged(): torch.testing.assert_close(state, before) -def test_fluid_replay_rejects_unqualified_recording_before_world_creation(): - from embodichain.lab.gym.envs.fluid_replay import FluidReplayEnv - from tensordict import TensorDict - - cfg = _replay_cfg() - source = { - "meta": {"num_envs": 1, "num_steps": 2, "lengths": [2]}, - "states": TensorDict({}, batch_size=[1, 2]), - "actions": torch.zeros(1, 2, 9), - } - with pytest.raises(ValueError, match="Task Program"): - FluidReplayEnv(cfg, trajectory=source) - - -def _qualified_source(root_position=(0.0, 0.0, 0.0)): - from tensordict import TensorDict - - root = torch.tensor([*root_position, 0.0, 0.0, 0.0, 1.0]).repeat(1, 2, 1) - cup = torch.tensor([-0.75, 0.0, 0.552, 0.0, 0.0, 0.0, 1.0]).repeat(1, 2, 1) - return { - "meta": { - "num_envs": 1, - "num_steps": 2, - "lengths": [2], - "dt": 0.05, - "rigid_object_uids": ["cup"], - "fluid_source": { - "kind": "task_program", - "accepted": True, - "program_hash": "test-program", - }, - }, - "states": TensorDict( - { - "robot": TensorDict( - {"qpos": torch.zeros(1, 2, 9), "root_pose": root}, batch_size=[1, 2] - ), - "rigid_objects": TensorDict( - {"cup": TensorDict({"pose": cup}, batch_size=[1, 2])}, - batch_size=[1, 2], - ), - }, - batch_size=[1, 2], - ), - "actions": torch.zeros(1, 2, 9), - } - - -def test_replay_adopts_source_root_before_scene_creation(monkeypatch): - from embodichain.lab.gym.envs.fluid_replay import FluidReplayEnv - from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv - - cfg = _replay_cfg() - cfg.dataset = {} # Empty mapping is a supported Python configuration. - captured = [] - - def initialize(self, cfg, **kwargs): - captured.append(cfg) - self.robot = SimpleNamespace(dof=9) - self.sim = SimpleNamespace(physics=SimpleNamespace(solver_type="dexuni")) - - monkeypatch.setattr(EmbodiedEnv, "__init__", initialize) - env = FluidReplayEnv(cfg, trajectory=_qualified_source((1.0, 2.0, 3.0))) - assert len(env._replay_time) == cfg.num_envs == 2 - np.testing.assert_array_equal(captured[0].robot.init_local_pose[:3, 3], [1, 2, 3]) - - -@pytest.mark.parametrize("kind", ["trajectory", "actions", "dataset"]) -def test_replay_rejects_misleading_policy_recording(kind): - from embodichain.lab.gym.envs.fluid_replay import FluidReplayEnv - - cfg = _replay_cfg() - if kind == "trajectory": - cfg.record_trajectory = True - elif kind == "actions": - cfg.actions = SimpleNamespace(control="unused") - else: - cfg.dataset = SimpleNamespace(recorder="unused") - with pytest.raises(ValueError, match="recording"): - FluidReplayEnv(cfg, trajectory=_qualified_source()) - - -def test_replay_rejects_moving_root_before_scene_creation(): - from embodichain.lab.gym.envs.fluid_replay import FluidReplayEnv - - cfg = _replay_cfg() - source = _qualified_source() - source["states"]["robot", "root_pose"][0, 1, 0] = 0.2 - with pytest.raises(ValueError, match="fixed-root"): - FluidReplayEnv(cfg, trajectory=source) - - -def test_replay_exhaustion_requires_reset(): - from embodichain.lab.gym.envs.fluid_replay import FluidReplayEnv - - env = object.__new__(FluidReplayEnv) - env._replay_time = np.array([2.0, 2.1]) - env.replay_duration = 2.0 - with pytest.raises(RuntimeError, match="reset"): - env.step(None) - - def test_refill_transforms_selected_container_pose_into_world_frame(): from embodichain.lab.gym.envs.managers.events import reset_fluid_in_container diff --git a/tests/gym/envs/test_fluid_replay_runtime.py b/tests/gym/envs/test_fluid_replay_runtime.py deleted file mode 100644 index fc74b2d295..0000000000 --- a/tests/gym/envs/test_fluid_replay_runtime.py +++ /dev/null @@ -1,178 +0,0 @@ -# ---------------------------------------------------------------------------- -# 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. -# ---------------------------------------------------------------------------- - -"""Live Gym reset and exhaustion with a small synthetic source recording.""" - -from __future__ import annotations - -from pathlib import Path -from unittest.mock import patch -import subprocess -import sys -import numpy as np -import pytest -import torch -from tensordict import TensorDict -from embodichain.lab.gym.envs.fluid_replay import FluidReplayEnv -from embodichain.lab.gym.utils.gym_utils import config_to_cfg -from embodichain.lab.sim import SimulationManager -from embodichain.lab.sim.cfg import FluidSpawnCfg, FluidRenderCfg, RenderCfg -from embodichain.utils.utility import load_config - -pytestmark = [pytest.mark.gpu, pytest.mark.subprocess_sim] - - -class _FluidReplayRuntime: - def setup_method(self): - from dexsim.engine.newton_physics.solver_runtime import ensure_solver_available - - if not getattr(ensure_solver_available("sph"), "supports_world_reset", False): - pytest.skip("requires world-isolated SPH build") - task = ( - Path(__file__).resolve().parents[3] - / "extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml" - ) - cfg = config_to_cfg(load_config(task), source_path=task) - cfg.task_program = None - cup_cfg = cfg.rigid_object.pop(0) - cup_cfg.body_type = "kinematic" - cfg.background.append(cup_cfg) - cfg.events.settle_cup = None - cfg.sim_cfg.physics_cfg.num_substeps = 1 - cfg.sim_cfg.physics_cfg.solver_cfg["sph_rigid_substeps"] = 1 - cfg.sim_cfg.render_cfg = RenderCfg(renderer="no-render") - cfg.sim_cfg.headless = True - cfg.enable_sensor = False - cfg.events.record_camera = None - cfg.sim_steps_per_control = 1 - cfg.sim_cfg.physics_dt = 0.001 - solver = cfg.sim_cfg.physics_cfg.solver_cfg - solver["sph_cfg"].update( - volume_map_boundary_enabled=False, - sdf_boundary_enabled=False, - particle_support_radius=0.0225, - ) - for obj in cfg.background: - for shape in (obj.shape, obj.particle_collision_shape): - if shape is not None and getattr(shape, "collision", None) is not None: - shape.collision.sdf_resolution = 16 - fluid = cfg.fluid_object[0] - fluid.particle_spacing = 0.01 - fluid.particle_radius = 0.003 - fluid.spawn = FluidSpawnCfg( - kind="revolved_volume", - profile=((-0.01, 0.01), (0.01, 0.01)), - fill_level=0.8, - reference_uid="cup", - ) - fluid.render = FluidRenderCfg(mode="none") - root = torch.tensor([0, 0, 0, 0, 0, 0, 1.0]).repeat(1, 2, 1) - cup = torch.tensor([-0.75, 0, 0.552, 0, 0, 0, 1.0]).repeat(1, 2, 1) - qpos = torch.tensor(cfg.robot.init_qpos).reshape(1, 1, -1).repeat(1, 2, 1) - # Synthetic provenance exercises the replay contract, not source acceptance. - source = { - "meta": { - "num_envs": 1, - "num_steps": 2, - "lengths": [2], - "dt": 0.004, - "rigid_object_uids": ["cup"], - "fluid_source": { - "kind": "task_program", - "accepted": True, - "program_hash": "synthetic-test", - }, - }, - "states": TensorDict( - { - "robot": {"qpos": qpos, "root_pose": root}, - "rigid_objects": {"cup": {"pose": cup}}, - }, - batch_size=[1, 2], - ), - "actions": torch.zeros(1, 2, qpos.shape[-1]), - } - self.env = FluidReplayEnv(cfg, trajectory=source, speed=1.0, hold_seconds=0.0) - - def teardown_method(self): - if hasattr(self, "env"): - self.env.close(exit_process=False) - del self.env - SimulationManager.flush_cleanup_queue() - - def test_partial_reset_preserves_other_world_then_finishes_independently(self): - env = self.env - with patch.object( - FluidSpawnCfg, - "sample", - side_effect=AssertionError("reset must reuse memory"), - ): - env.reset(seed=0) - action = torch.zeros( - (env.num_envs, len(env.active_joint_ids)), device=env.device - ) - env.step(action) - env.step(action) - before = env.sim.get_fluid_object("water").data.particle_state_w.clone() - with patch.object( - FluidSpawnCfg, - "sample", - side_effect=AssertionError("reset must reuse memory"), - ): - env.reset( - options={ - "reset_ids": torch.tensor([0], device=env.device), - "save_data": False, - } - ) - torch.testing.assert_close( - env.sim.get_fluid_object("water").data.particle_state_w[1], - before[1], - rtol=0, - atol=0, - ) - np.testing.assert_allclose(env._replay_time, [0, 0.002]) - env.step(action) - _, _, _, truncated, info = env.step(action) - assert info["fluid_replay_finished"].tolist() == [False, True] - assert truncated.tolist() == [False, True] - env.step(action) - env.step(action) - with pytest.raises(RuntimeError, match="exhausted"): - env.step(action) - - -def test_fluid_replay_reset_and_exhaustion_in_fresh_process(): - from dexsim.engine.newton_physics.solver_runtime import ensure_solver_available - - if not getattr(ensure_solver_available("sph"), "supports_world_reset", False): - pytest.skip("requires world-isolated SPH build") - result = subprocess.run( - [sys.executable, str(Path(__file__).resolve())], - capture_output=True, - text=True, - timeout=180, - ) - assert result.returncode == 0, result.stdout + result.stderr - - -if __name__ == "__main__": - worker = _FluidReplayRuntime() - try: - worker.setup_method() - worker.test_partial_reset_preserves_other_world_then_finishes_independently() - finally: - worker.teardown_method() diff --git a/tests/gym/envs/test_fluid_trajectory.py b/tests/gym/envs/test_fluid_trajectory.py new file mode 100644 index 0000000000..605f998fae --- /dev/null +++ b/tests/gym/envs/test_fluid_trajectory.py @@ -0,0 +1,183 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Control-aligned fluid-state artifact and replay contracts.""" + +from __future__ import annotations + +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest +import torch + +from embodichain.lab.gym.envs._fluid_trajectory import ( + FluidTrajectoryReader, + FluidTrajectoryRecorder, +) + + +class _Fluid: + def __init__(self, states: torch.Tensor) -> None: + self.data = SimpleNamespace(particle_state_w=states) + self.num_particles = states.shape[1] + self.written = None + + def write_particle_state_to_sim(self, state: torch.Tensor) -> None: + self.written = state.clone() + + +def _env(num_envs: int) -> SimpleNamespace: + states = torch.zeros(num_envs, 2, 6) + fluid = _Fluid(states) + offsets = torch.arange(num_envs, dtype=torch.float32)[:, None] * torch.tensor( + [[10.0, 0.0, 0.0]] + ) + return SimpleNamespace( + num_envs=num_envs, + device=torch.device("cpu"), + cfg=SimpleNamespace( + fluid_object=[ + SimpleNamespace( + uid="water", + particle_spacing=0.01, + spawn=SimpleNamespace(sample=lambda spacing: np.zeros((2, 3))), + ) + ] + ), + sim=SimpleNamespace( + arena_offsets=offsets, + get_fluid_object=lambda uid: fluid, + ), + fluid=fluid, + ) + + +def _source(num_envs: int, length: int) -> dict: + return { + "meta": { + "artifact_purpose": "recording", + "num_envs": num_envs, + "lengths": [length] * num_envs, + "dt": 0.025, + "demo_episodes": [ + {"program_hash": "program", "completed": True, "success": True} + for _ in range(num_envs) + ], + "fluid_qualification": { + "kind": "task_program", + "accepted": True, + "program_hash": "program", + }, + } + } + + +def _record(tmp_path: Path, env: SimpleNamespace) -> tuple[Path, dict]: + path = tmp_path / "trajectory.pt" + recorder = FluidTrajectoryRecorder(tmp_path, env.num_envs, 0.025) + env.fluid.data.particle_state_w[:, :, :3] = env.sim.arena_offsets[:, None, :] + recorder.capture(env, list(range(env.num_envs)), reset=True) + env.fluid.data.particle_state_w[:, :, 0] += 0.2 + env.fluid.data.particle_state_w[:, :, 3] = 0.5 + recorder.capture(env, list(range(env.num_envs))) + source = _source(env.num_envs, 1) + source["meta"]["fluid_state"] = recorder.finish(path, [1] * env.num_envs) + recorder.commit() + return path, source + + +def test_particle_frames_restore_position_velocity_and_arena_offsets(tmp_path: Path): + source_env = _env(1) + path, source = _record(tmp_path, source_env) + target = _env(2) + cfg = SimpleNamespace(num_envs=2, fluid_object=target.cfg.fluid_object) + reader = FluidTrajectoryReader(path, source, cfg) + try: + reader.apply(target, torch.tensor([0, 1])) + assert target.fluid.written is not None + torch.testing.assert_close(target.fluid.written[0, :, 0], torch.zeros(2)) + torch.testing.assert_close( + target.fluid.written[1, :, 0], torch.full((2,), 10.2) + ) + torch.testing.assert_close(target.fluid.written[1, :, 3], torch.full((2,), 0.5)) + finally: + reader.close() + + +def test_particle_frames_keep_multi_env_rows_independent(tmp_path: Path): + source_env = _env(2) + path, source = _record(tmp_path, source_env) + target = _env(2) + cfg = SimpleNamespace(num_envs=2, fluid_object=target.cfg.fluid_object) + reader = FluidTrajectoryReader(path, source, cfg) + try: + reader.apply(target, torch.tensor([1, 0])) + torch.testing.assert_close(target.fluid.written[0, :, 0], torch.full((2,), 0.2)) + torch.testing.assert_close( + target.fluid.written[1, :, 0], torch.full((2,), 10.0) + ) + finally: + reader.close() + + +def test_particle_source_rejects_unqualified_and_tampered_frames(tmp_path: Path): + env = _env(1) + path, source = _record(tmp_path, env) + cfg = SimpleNamespace(num_envs=1, fluid_object=env.cfg.fluid_object) + source["meta"]["fluid_qualification"]["accepted"] = False + with pytest.raises(ValueError, match="accepted"): + FluidTrajectoryReader(path, source, cfg) + source["meta"]["fluid_qualification"]["accepted"] = True + source["meta"]["fluid_qualification"]["program_hash"] = "another-program" + with pytest.raises(ValueError, match="provenance"): + FluidTrajectoryReader(path, source, cfg) + source["meta"]["fluid_qualification"]["program_hash"] = "program" + sidecar = source["meta"].pop("fluid_state") + with pytest.raises(ValueError, match="sidecars"): + FluidTrajectoryReader(path, source, cfg) + source["meta"]["fluid_state"] = sidecar + manifest = path.parent / source["meta"]["fluid_state"]["manifests"][0] + chunk = next(manifest.parent.glob("chunk-*.npz")) + with chunk.open("ab") as stream: + stream.write(b"tampered") + with pytest.raises(ValueError, match="content changed"): + FluidTrajectoryReader(path, source, cfg) + + +def test_particle_recorder_rejects_frame_length_mismatch(tmp_path: Path): + env = _env(1) + recorder = FluidTrajectoryRecorder(tmp_path, 1, 0.025) + recorder.capture(env, [0], reset=True) + with pytest.raises(ValueError, match="align"): + recorder.finish(tmp_path / "trajectory.pt", [1]) + recorder.abort() + assert not recorder.directory.exists() + + +def test_selected_row_reset_restarts_only_its_particle_stream(tmp_path: Path): + env = _env(2) + recorder = FluidTrajectoryRecorder(tmp_path, 2, 0.025) + recorder.capture(env, [0, 1], reset=True) + recorder.capture(env, [0, 1]) + recorder.capture(env, [0], reset=True) + recorder.capture(env, [0]) + + sidecar = recorder.finish(tmp_path / "trajectory.pt", [1, 1]) + + assert len(sidecar["manifests"]) == 2 + recorder.abort() diff --git a/tests/gym/envs/test_replay_wrapper.py b/tests/gym/envs/test_replay_wrapper.py index 38f3858821..b61daae861 100644 --- a/tests/gym/envs/test_replay_wrapper.py +++ b/tests/gym/envs/test_replay_wrapper.py @@ -19,6 +19,7 @@ from __future__ import annotations from types import SimpleNamespace +from contextlib import contextmanager import gymnasium as gym import pytest @@ -34,6 +35,7 @@ class _FakeSim: def __init__(self) -> None: self.physics_enabled: list[bool] = [] self.updates: list[tuple[float, int]] = [] + self.rendered = 0 def enable_physics(self, enabled: bool) -> None: self.physics_enabled.append(enabled) @@ -41,6 +43,23 @@ def enable_physics(self, enabled: bool) -> None: def update(self, physics_dt: float, substeps: int) -> None: self.updates.append((physics_dt, substeps)) + @contextmanager + def render_frame(self, *, force_visualization: bool = False): + self.rendered += 1 + yield + + +class _FakeFluidReader: + def __init__(self) -> None: + self.applied: list[list[int]] = [] + self.closed = False + + def apply(self, env, steps: torch.Tensor) -> None: + self.applied.append(steps.tolist()) + + def close(self) -> None: + self.closed = True + class _FakeEnv(gym.Env): metadata = {} @@ -204,3 +223,59 @@ def test_dynamic_replay_decodes_position_velocity_expert_action(fake_restore): def test_replay_rejects_unknown_mode(): with pytest.raises(ValueError, match="Invalid replay mode"): ReplayWrapper(_FakeEnv(), _trajectory(), mode="unsupported") + + +@pytest.mark.parametrize("mode", ["kinematic", "control"]) +def test_fluid_kinematic_and_control_restore_recorded_frames(fake_restore, mode): + env = _FakeEnv() + env.cfg.fluid_object = [object()] + reader = _FakeFluidReader() + replay = ReplayWrapper(env, _trajectory(), mode=mode, fluid_reader=reader) + + replay.reset() + replay.step(None) + if mode == "control": + replay.go_to_step(1) + + assert reader.applied == ([[0], [0], [1]] if mode == "control" else [[0], [0]]) + assert env.sim.updates == [] + assert env.sim.rendered == (2 if mode == "control" else 1) + replay.close() + assert reader.closed + + +def test_fluid_dynamic_restores_only_initial_particles(fake_restore): + env = _FakeEnv() + env.cfg.fluid_object = [object()] + reader = _FakeFluidReader() + replay = ReplayWrapper(env, _trajectory(), mode="dynamic", fluid_reader=reader) + + replay.reset() + replay.step(None) + + assert reader.applied == [[0]] + assert len(env.received_actions) == 1 + replay.close() + + +def test_fluid_control_can_seek_recorded_terminal_state(fake_restore): + env = _FakeEnv() + env.cfg.fluid_object = [object()] + reader = _FakeFluidReader() + trajectory = _trajectory() + trajectory["terminal_states"] = trajectory["states"][:, -1].clone() + replay = ReplayWrapper(env, trajectory, mode="control", fluid_reader=reader) + + replay.reset() + replay.go_to_step(2) + + assert replay.control_max_step == 2 + assert reader.applied == [[0], [2]] + replay.close() + + +def test_fluid_artifact_rejects_scene_without_fluid(): + trajectory = _trajectory() + trajectory["meta"]["fluid_state"] = {"schema_version": 1} + with pytest.raises(ValueError, match="configured fluid"): + ReplayWrapper(_FakeEnv(), trajectory, mode="kinematic") diff --git a/tests/lab/scripts/test_run_fluid_task_program.py b/tests/lab/scripts/test_run_fluid_task_program.py index 4b55d9430e..2c8e578542 100644 --- a/tests/lab/scripts/test_run_fluid_task_program.py +++ b/tests/lab/scripts/test_run_fluid_task_program.py @@ -177,3 +177,13 @@ def save_recording(*, purpose): assert order == (["save", "close"] if renderer == "fast-rt" else ["close"]) assert len(report["videos"]) == (2 if renderer == "fast-rt" and not stale else 0) assert (tmp_path / "fluid_gym_result.json").is_file() + + +def test_fluid_state_recording_requires_trajectory(monkeypatch, capsys): + import sys + from embodichain.lab.scripts import run_fluid_task_program as runner + + monkeypatch.setattr(sys, "argv", ["fluid-task", "--record-fluid-state"]) + with pytest.raises(SystemExit, match="2"): + runner.main() + assert "requires --record-trajectory" in capsys.readouterr().err