diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index efddeb2e5..3cf5e563f 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -49,7 +49,7 @@ topics: keywords: [BaseEnv, EmbodiedEnv, EpisodeContext, EnvCfg, 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, - ExpertTrajectoryCfg, joint_command_mode, position_velocity, qpos qvel, expert action schema, + FluidReplayEnv, fluid Gym, 流体 Gym, ExpertTrajectoryCfg, joint_command_mode, position_velocity, qpos qvel, expert action schema, task_instruction, resolve_demo_instruction, show-task, task catalog, task gallery, 任务画廊, physical_objective, 物理目标] paths: [topics/env-framework/env-framework.md] @@ -72,11 +72,13 @@ 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/scripts/run_fluid_task_program.py - embodichain/lab/gym/envs/demo.py - embodichain/lab/gym/utils/profiler.py watch_paths: [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/utils/, embodichain/lab/gym/envs/task_program/, embodichain/cli/, embodichain/lab/scripts/run_task.py, + embodichain/lab/gym/envs/demo.py, embodichain/lab/gym/envs/fluid_replay.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] related_topics: [simulation-system, manager-functor, sim-visualization, task-programs, data-pipeline] @@ -445,7 +447,7 @@ topics: title: Data Assets aliases: [data assets, data path, asset download, 数据资产, 资产下载, 数据路径] keywords: [get_data_path, get_data_class, EmbodiChainDataset, TabletopFourBases, demo assets, EMBODICHAIN_DATA_ROOT, EMBODICHAIN_DATASET_ROOT, - CATEGORY_MODULES, embodichain.assets, ROBOT_ASSETS, 资产注册, download lock, get_registry, DefaultMaterials, list_default_materials, get_default_material_info] + CATEGORY_MODULES, embodichain.assets, ROBOT_ASSETS, 资产注册, download lock, get_registry, DefaultMaterials, list_default_materials, get_default_material_info, task_assets, PourCupAssets, PourCupDemoData] paths: [topics/data-assets/data-assets.md] source_of_truth: - embodichain/data/dataset.py diff --git a/agent_context/topics/data-assets/data-assets.md b/agent_context/topics/data-assets/data-assets.md index ce312e0cf..99d9c17b2 100644 --- a/agent_context/topics/data-assets/data-assets.md +++ b/agent_context/topics/data-assets/data-assets.md @@ -50,6 +50,10 @@ Keep the lock outside the directory repair may delete. POSIX uses `flock`; Windows uses a polled byte lock. Direct Open3D subclasses do not automatically receive the project's locking/repair behavior. +The `task` category registers `PourCupAssets` runtime meshes and +`PourCupDemoData` historical recordings through `assets/task_assets.py`. +Both use the shared download cache and class-prefixed paths. + The CLI honors an asset class's optional `cache_prefix` when checking readiness and ensuring extraction; otherwise the class name is the cache entry. The shared registry merges core category modules and `embodichain.assets` diff --git a/agent_context/topics/env-framework/env-framework.md b/agent_context/topics/env-framework/env-framework.md index a546fab9d..c28fa08d3 100644 --- a/agent_context/topics/env-framework/env-framework.md +++ b/agent_context/topics/env-framework/env-framework.md @@ -28,6 +28,8 @@ Lightweight RL environments have a separate owner in Read [configuration and registration](configuration.md) for task-first layout, component ownership, path resolution, config-owned IDs, and task listing. +Read [fluid Gym integration](fluid-replay.md) for online Task Program execution +with rigid/SPH dynamics, fluid configuration and coordinate conventions. Read [execution](execution.md) for hooks, bridge acceptance, reset ordering, wrappers, and replay. Read [profiling](profiling.md) only for instrumentation. diff --git a/agent_context/topics/env-framework/fluid-replay.md b/agent_context/topics/env-framework/fluid-replay.md new file mode 100644 index 000000000..c1bd29e9b --- /dev/null +++ b/agent_context/topics/env-framework/fluid-replay.md @@ -0,0 +1,70 @@ +# Fluid Gym integration + +`EmbodiedEnvCfg.fluid_object` owns fixed-count fluid declarations. Physical +components own these alongside rigid entities; `config_to_cfg()` resolves +fluid point files and explicit relative rigid/visual mesh paths against the +owning component. Spawn rigid references are resolved before fluid binding. +The default pour deployment generates its fill from a local revolved profile; +no particle asset file is required. Pouring meshes use `PourCupAssets/...` +references resolved through the [data asset cache](../data-assets/data-assets.md), +so installed task packages carry configuration without mesh payloads. The runner's spacing override resamples the +same volume recipe and rejects fixed point imports. It scales explicit render +radius, mesh cell size and reconstruction kernel radius by the spacing ratio, +keeping the surface reconstruction consistent with the new particle density. Fill definitions and +in-memory template ownership are documented under +[simulation fluids](../simulation-system/fluids.md). + +`managers/observations.py` provides particle state, summary and volume-in-region +observations. Particle arrays are world-frame; rigid `get_local_pose()` is +arena-local. Region measurement and `events.reset_fluid_in_container()` add +arena offsets before comparing or placing particles. Region volume is particle +count times nominal spacing cubed, not a reconstructed-surface measurement. + +## Online Task Program workflow + +`lab/scripts/run_fluid_task_program.py` runs the configured pour-cup deployment +through ordinary `EmbodiedEnv` and `execute_demo_episode()`. The environment +component selects DexUni with DFSPH settings nested under `sph_cfg`. Robot +drive targets, dynamic grasp contacts and fluid state +advance in one World, with no recorded trajectory input. SPH reads live rigid +boundaries; fluid reaction forces are not fed back into the rigid solver. +The fluid program measures cup lift and alignment, transports the held cup +to a receiver-relative pour pose through the existing MoveHeldObject lowerer, +then pours and returns it. The standalone Sim tutorial uses the same physical +component and relative pour target with freshly planned Atomic Action targets. + +Program completion and physical qualification are separate: the runner measures +cup lift, maximum tilt, final return error and received volume. Projected effect +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. + +## Rendering and recording + +The pouring environment and runner select `fast-rt`. Hybrid is rejected for +native fluid rendering: OptiX-only liquid geometry is absent from its Vulkan +primary surface. Particle-state and video-decode checks alone do not qualify +liquid visibility; use the image-based fluid regression linked below. The +default record view focuses on the cup and receiver; the mesh kernel radius +is 1.5 times the particle spacing to retain a connected surface after settling. +Its interval `record_camera` event uses the existing `record_camera_data` +functor with `fps=None`, recording independent environment views at the +control frequency. The runner only redirects the output directory and sets +the recording limit to the resolved environment count. It explicitly commits +before close, which otherwise discards pending frames. Incomplete or rejected +runs use the recorder's diagnostic purpose rather than qualifying the output. +Selected-row recording and camera consumption are owned by the +[recording functor](../manager-functor/manager-functor.md#camera-recording). +NoRender qualification removes the recorder before constructing the scene. + +Production configuration/measurement checks are in +`tests/gym/envs/test_fluid_integration.py`; live isolation checks are in +`tests/sim/objects/test_fluid_batch_runtime.py` and +`tests/sim/objects/test_fluid_online_runtime.py`; actual primary-surface visibility +is checked by `tests/sim/objects/test_fluid_visibility.py`. Generated trajectories/videos +are local qualification evidence, not packaged task assets. diff --git a/agent_context/topics/manager-functor/manager-functor.md b/agent_context/topics/manager-functor/manager-functor.md index 2e83242ac..0f19cdbbe 100644 --- a/agent_context/topics/manager-functor/manager-functor.md +++ b/agent_context/topics/manager-functor/manager-functor.md @@ -9,6 +9,7 @@ | Observation manager + built-in functors | `managers/observation_manager.py`, `managers/observations.py` | | Reward manager + built-in functors | `managers/reward_manager.py`, `managers/rewards.py` | | Event manager + built-in functors | `managers/event_manager.py`, `managers/events.py` | +| Episode camera recording events | `managers/record.py` | | Action manager + built-in terms | `managers/action_manager.py`, `managers/actions.py` | | Dataset manager + built-in recorders | `managers/dataset_manager.py`, `managers/datasets.py`, `managers/async_datasets.py` | | Randomization functors (event sub-type) | `managers/randomization/` (spatial, visual, physics, geometry) | @@ -112,6 +113,22 @@ Event and observation calls have per-functor profiling through `ManagerBase._call_functor()`; reward/action/dataset calls do not yet use that helper. See [profiling](../env-framework/profiling.md). +### Camera recording + +`record_camera_data` is an interval event, independent of policy dataset +recording. It renders its camera group before reading the buffers because +interval events precede observation rendering. Views use arena-relative camera +extrinsics. It retains separate owned CPU frames and episode counters per row. +It captures only the event's selected `env_ids` within `max_env_num`, preserving +their original arena IDs; `None` captures all views within that limit. Selected +save/discard only touches those rows. `fps=None` follows control frequency for +an every-step recording event; an explicit fps fixes the output cadence. + +Reset commits or discards frames via `save_and_clear` / `discard_and_clear`. +Close discards uncommitted frames, so a host ending without reset must explicitly +commit its last episode. Source and regression tests live in `record.py` and +`tests/gym/envs/managers/test_record.py`. + ## Change sites and validation Change the selected manager's dispatch for scheduling changes; change its diff --git a/agent_context/topics/simulation-system/fluids.md b/agent_context/topics/simulation-system/fluids.md index 51016ff70..4249ab375 100644 --- a/agent_context/topics/simulation-system/fluids.md +++ b/agent_context/topics/simulation-system/fluids.md @@ -86,3 +86,31 @@ Focused checks are `tests/compute/test_particles.py`, `tests/sim/objects/test_fluid_runtime.py`; `test_fluid_visibility.py` checks camera pixels after stepping and direct state writes. Tutorial checks remain syntax/CLI smoke checks rather than tutorial import tests. + +## Online task and replay layer + +The pouring task uses DexUni with nested DFSPH, a shared simulation clock and +one-way rigid-to-fluid coupling. When no joint ownership was explicitly authored, +the fluid setup assigns every finalized joint to MuJoCo before solver construction, +which covers a robot and a free cup across replicated worlds. Explicit ownership +selectors remain caller-owned. The current native prototype requires the pure +MuJoCo rigid path when bodies exist and does not support fluid reaction forces. + +`RigidObjectCfg.particle_collision_shape` puts a particle-only SDF boundary on +the same body as its rigid contact hulls. Where DexSim exposes +`NewtonCollisionDesc.contributes_to_mass`, the boundary explicitly excludes +itself from mass. Older builds need explicit mass and inertia for dynamic bodies; +the pouring cup authors its COM and body-frame inertia from its contact hulls. +Static/kinematic boundaries retain their respective motion semantics. Independent +`visual_shape` settings preserve the render mesh. + +Replay may write `RigidObject.set_kinematic_state()` or register a manager-owned +kinematic rigid trajectory before preparation. These host controls disable graph +replay and rewind selected clocks on object reset; they do not qualify online +grasping. Normal Gym reset restores or refills selected fluid rows without +stepping peer worlds. Online action execution supplies physical settling. + +The robot demonstrations, task assets, Gym declarations, observations, recording, +Task Program runner and replay belong to the pouring task layer. See +[fluid Gym integration](../env-framework/fluid-replay.md) and the task's +`extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/README.md`. diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 63b2e6fd5..6b254c94b 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -2875,3 +2875,26 @@ Asset generalization contracts :maxdepth: 2 asset_generalization + +Fluid task interfaces +===================== + +The pouring task supplies downloadable scene assets and a diagnostic fluid +replay environment. Its runner executes the configured Task Program against +live one-way rigid-to-fluid simulation. + +.. autosummary:: + + embodichain.data.assets.task_assets.PourCupAssets + embodichain.data.assets.task_assets.PourCupDemoData + embodichain.lab.gym.envs.fluid_replay.FluidReplayEnv + embodichain.lab.scripts.run_fluid_task_program.main + +.. autoclass:: embodichain.data.assets.task_assets.PourCupAssets + +.. autoclass:: embodichain.data.assets.task_assets.PourCupDemoData + +.. autoclass:: embodichain.lab.gym.envs.fluid_replay.FluidReplayEnv + :members: + +.. autofunction:: embodichain.lab.scripts.run_fluid_task_program.main diff --git a/embodichain/data/_asset_registry.py b/embodichain/data/_asset_registry.py index f7b2eef1c..571b10d3e 100644 --- a/embodichain/data/_asset_registry.py +++ b/embodichain/data/_asset_registry.py @@ -37,6 +37,7 @@ "embodichain.data.assets.so101_assets", ), "scene": ("embodichain.data.assets.scene_assets",), + "task": ("embodichain.data.assets.task_assets",), "w1": ("embodichain.data.assets.w1_assets",), } diff --git a/embodichain/data/assets/__init__.py b/embodichain/data/assets/__init__.py index c82b89ddf..6e403d3aa 100644 --- a/embodichain/data/assets/__init__.py +++ b/embodichain/data/assets/__init__.py @@ -14,7 +14,7 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Preset configuration objects for scene assets (robots, end-effectors, objects, materials, sensors, planners, solvers, demo scenes) ready to reference from task configs.""" +"""Preset configuration objects for scene assets (robots, end-effectors, objects, materials, sensors, planners, solvers, demo scenes and task bundles) ready to reference from task configs.""" from __future__ import annotations @@ -29,4 +29,5 @@ from .planner_assets import * from .locomotion_assets import * +from .task_assets import * from .so101_assets import * diff --git a/embodichain/data/assets/task_assets.py b/embodichain/data/assets/task_assets.py new file mode 100644 index 000000000..d27958c7f --- /dev/null +++ b/embodichain/data/assets/task_assets.py @@ -0,0 +1,78 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Downloadable task meshes and reference demonstration data.""" + +from __future__ import annotations + +import open3d as o3d + +from embodichain.data.constants import ( + EMBODICHAIN_DEFAULT_DATA_ROOT, + EMBODICHAIN_DOWNLOAD_PREFIX, +) +from embodichain.data.dataset import EmbodiChainDataset + +__all__ = ["PourCupAssets", "PourCupDemoData"] + +_HUB_PREFIX = ( + "https://huggingface.co/datasets/DexForceAI/embodichain_data/resolve/main/" +) + + +class _TaskAsset(EmbodiChainDataset): + """Download one task archive through the shared locked dataset cache.""" + + archive_md5: str + + def __init__(self, data_root: str | None = None) -> None: + """Initialize and extract the task bundle. + + Args: + data_root: Optional cache root overriding the EmbodiChain default. + """ + prefix = type(self).__name__ + filename = f"task_assets/{prefix}.zip" + urls = [f"{EMBODICHAIN_DOWNLOAD_PREFIX}{filename}"] + if EMBODICHAIN_DOWNLOAD_PREFIX != _HUB_PREFIX: + urls.append(f"{_HUB_PREFIX}{filename}") + descriptor = o3d.data.DataDescriptor(urls, self.archive_md5) + super().__init__( + prefix, + descriptor, + EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root, + ) + + +class PourCupAssets(_TaskAsset): + """Cup, fluid boundary, receiver, table and floor meshes for cup pouring. + + Resolve individual meshes with ``get_data_path("PourCupAssets/cup.glb")``. + The bundle contains no initial-fluid particle file. + """ + + archive_md5 = "f97857b639189be4daf717a873ab84cf" + + +class PourCupDemoData(_TaskAsset): + """Recorded pouring trajectory, videos and original validation evidence. + + ``trajectory.pt`` is a historical MuJoCo recording for diagnostic replay. + Online pouring tutorials and tasks do not load it. ``particles_30.npz`` is a final reference snapshot and is not used + to initialize fluids. Resolve files through ``get_data_path()``. + """ + + archive_md5 = "8eb96cb756ac08efe403417f50ad2bf2" diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 94333bc6d..4cd42b050 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -31,6 +31,8 @@ import numpy as np import gymnasium as gym +from embodichain.lab.sim.cfg.fluid import FluidObjectCfg + from dataclasses import MISSING, fields from typing import ( TYPE_CHECKING, @@ -214,6 +216,9 @@ class EnvLightCfg: articulation: List[ArticulationCfg] = [] + fluid_object: list[FluidObjectCfg] = [] + """Fixed-count fluids owned by the physical environment component.""" + events: Union[object, None] = None """Event settings. Defaults to None, in which case no events are applied through the event manager. @@ -2789,6 +2794,9 @@ def _setup_interactive_objects(self) -> None: for cfg in self.cfg.rigid_object_group: self.sim.add_rigid_object_group(cfg=cfg) + for cfg in self.cfg.fluid_object: + self.sim.add_fluid_object(cfg) + def preview_sensor_data( self, name: str, diff --git a/embodichain/lab/gym/envs/fluid_replay.py b/embodichain/lab/gym/envs/fluid_replay.py new file mode 100644 index 000000000..06bc74757 --- /dev/null +++ b/embodichain/lab/gym/envs/fluid_replay.py @@ -0,0 +1,331 @@ +# ---------------------------------------------------------------------------- +# 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/managers/events.py b/embodichain/lab/gym/envs/managers/events.py index a808482e2..a72bff9ea 100644 --- a/embodichain/lab/gym/envs/managers/events.py +++ b/embodichain/lab/gym/envs/managers/events.py @@ -59,6 +59,7 @@ __all__ = [ + "reset_fluid_in_container", "apply_expansion_profile_reset", "replace_assets_from_group", "prepare_extra_attr", @@ -866,3 +867,34 @@ def remove_rigid_constraint( name: Base constraint name to remove. """ env.sim.remove_rigid_constraint(name, env_ids=env_ids) + + +def reset_fluid_in_container( + env: EmbodiedEnv, + env_ids: Sequence[int] | torch.Tensor, + entity_cfgs: list[SceneEntityCfg], +) -> None: + """Refill selected rows from the declared local template after container reset. + + Args: + env: Owning environment. + env_ids: Selected environment indices. + entity_cfgs: Fluid and rigid container selections, in that order. + """ + from embodichain.lab.sim.spawn.descriptors import _pose_from_cfg + from embodichain.utils.math import quat_from_matrix + + if len(entity_cfgs) != 2: + raise ValueError("Expected fluid and container selections.") + fluid = env.sim.get_fluid_object(entity_cfgs[0].uid) + container = env.sim.get_rigid_object(entity_cfgs[1].uid) + if fluid is None or container is None: + raise ValueError("Unknown fluid or container.") + ids = torch.as_tensor(env_ids, device=env.device, dtype=torch.long) + local = torch.as_tensor( + _pose_from_cfg(fluid.cfg), dtype=torch.float32, device=env.device + ) + world = container.get_local_pose(to_matrix=True)[ids] @ local + world[:, :3, 3] += env.sim.arena_offsets[ids] + pose = torch.cat((world[:, :3, 3], quat_from_matrix(world[:, :3, :3])), dim=-1) + fluid.reset_to_pose(pose, ids) diff --git a/embodichain/lab/gym/envs/managers/observations.py b/embodichain/lab/gym/envs/managers/observations.py index cdbc75f9b..13543e73a 100644 --- a/embodichain/lab/gym/envs/managers/observations.py +++ b/embodichain/lab/gym/envs/managers/observations.py @@ -1207,3 +1207,134 @@ def __call__( self._cache[uid] = result.clone() return result + + +def get_fluid_particle_state( + env: EmbodiedEnv, obs: EnvObs, entity_cfg: SceneEntityCfg +) -> torch.Tensor: + """Return opt-in world-frame particle state ``(E, N, 6)``. + + Args: + env: Owning environment. + obs: Current observations, unused. + entity_cfg: Fluid selection. + + Returns: + Independent positions/velocities; no full-particle recording is enabled implicitly. + """ + del obs + fluid = env.sim.get_fluid_object(entity_cfg.uid) + if fluid is None: + raise ValueError(f"Unknown fluid {entity_cfg.uid!r}.") + return fluid.data.particle_state_w + + +def get_fluid_summary( + env: EmbodiedEnv, obs: EnvObs, entity_cfg: SceneEntityCfg +) -> torch.Tensor: + """Return mass, nominal volume, mean speed, RMS speed and finite fraction. + + Args: + env: Owning environment. + obs: Current observations, unused. + entity_cfg: Fluid selection. + + Returns: + ``(E, 5)`` GPU tensor in kg, cubic metres and metres/second. + """ + state = get_fluid_particle_state(env, obs, entity_cfg) + fluid = env.sim.get_fluid_object(entity_cfg.uid) + speeds = state[..., 3:].norm(dim=-1) + return torch.stack( + ( + speeds.new_full((env.num_envs,), fluid.particle_mass * fluid.num_particles), + speeds.new_full((env.num_envs,), fluid.nominal_volume), + speeds.mean(-1), + speeds.square().mean(-1).sqrt(), + torch.isfinite(state).all(-1).float().mean(-1), + ), + dim=-1, + ) + + +def get_fluid_volume_in_region( + env: EmbodiedEnv, + obs: EnvObs, + entity_cfgs: list[SceneEntityCfg], + lower: list[float], + upper: list[float], + sphere_center: list[float] | None = None, + sphere_radius: float | None = None, +) -> torch.Tensor: + """Measure nominal fluid volume inside a rigid-local region. + + Args: + env: Owning environment. + obs: Current observations, unused. + entity_cfgs: Fluid followed by the rigid reference object. + lower: Inclusive local AABB lower corner, metres. + upper: Inclusive local AABB upper corner, metres. + sphere_center: Optional local sphere centre intersected with the AABB. + sphere_radius: Positive sphere radius, metres, paired with sphere_center. + + Returns: + ``(E, 1)`` contained rest volume in cubic metres. + """ + from embodichain.utils.math import matrix_from_quat + import math + + if len(entity_cfgs) != 2: + raise ValueError( + "Region measurement requires fluid and rigid-reference selections." + ) + fluid = env.sim.get_fluid_object(entity_cfgs[0].uid) + rigid = env.sim.get_rigid_object(entity_cfgs[1].uid) + if fluid is None or rigid is None: + raise ValueError("Unknown fluid or region reference.") + bounds = torch.as_tensor([lower, upper], dtype=torch.float32, device=env.device) + if ( + bounds.shape != (2, 3) + or not torch.isfinite(bounds).all() + or not (bounds[1] > bounds[0]).all() + ): + raise ValueError("Region bounds must be finite increasing 3D corners.") + pose = rigid.get_local_pose() + origin_w = pose[:, :3] + env.sim.arena_offsets + local = torch.bmm( + fluid.data.particle_pos_w - origin_w[:, None], matrix_from_quat(pose[:, 3:]) + ) + inside = ((local >= bounds[0]) & (local <= bounds[1])).all(-1) + if (sphere_center is None) != (sphere_radius is None): + raise ValueError("sphere_center and sphere_radius must be supplied together.") + if sphere_center is not None: + center = torch.as_tensor(sphere_center, dtype=torch.float32, device=env.device) + if ( + center.shape != (3,) + or not torch.isfinite(center).all() + or not math.isfinite(sphere_radius) + or sphere_radius <= 0 + ): + raise ValueError("Sphere needs a finite centre and positive radius.") + inside &= (local - center).square().sum(-1) <= sphere_radius**2 + return inside.sum(-1, keepdim=True).float() * fluid.cfg.particle_spacing**3 + + +__all__ = [ + "get_object_pose", + "get_rigid_object_pose", + "get_object_body_scale", + "get_object_uid", + "get_rigid_object_velocity", + "normalize_robot_joint_data", + "get_sensor_pose_in_robot_frame", + "get_sensor_intrinsics", + "compute_semantic_mask", + "get_robot_eef_pose", + "target_position", + "compute_exteroception", + "get_rigid_object_physics_attributes", + "get_articulation_joint_drive", + "get_fluid_particle_state", + "get_fluid_summary", + "get_fluid_volume_in_region", +] diff --git a/embodichain/lab/gym/envs/managers/record.py b/embodichain/lab/gym/envs/managers/record.py index c6c62c19e..23e6ea0d2 100644 --- a/embodichain/lab/gym/envs/managers/record.py +++ b/embodichain/lab/gym/envs/managers/record.py @@ -17,6 +17,7 @@ from __future__ import annotations import os +import math import random import threading from concurrent.futures import Future, ThreadPoolExecutor @@ -55,7 +56,7 @@ def __init__(self, cfg: FunctorCfg, env: EmbodiedEnv) -> None: env: The environment instance. Raises: - ValueError: If the asset is not a RigidObject or an Articulation. + ValueError: If the video frame rate is not finite and positive. """ super().__init__(cfg, env) @@ -68,6 +69,10 @@ def __init__(self, cfg: FunctorCfg, env: EmbodiedEnv) -> None: intrinsics = cfg.params.get( "intrinsics", (600, 600, int(resolution[0] / 2), int(resolution[1] / 2)) ) + fps = cfg.params.get("fps", 20.0) + self._fps = float(env.control_frequency if fps is None else fps) + if not math.isfinite(self._fps) or self._fps <= 0: + raise ValueError("Camera recording fps must be finite and positive.") self.camera: Camera = env.sim.add_sensor( sensor_cfg=CameraCfg( @@ -161,7 +166,7 @@ def _write_job(self, key: tuple[int, int, str], job: tuple) -> None: name = f"episode_{episode:06d}_{self._name}" # Publish complete files only. Failed jobs retain their frames and name. with tempfile.TemporaryDirectory(prefix=".camera-", dir=directory) as temporary: - images_to_video(frames, temporary, name, fps=20) + images_to_video(frames, temporary, name, fps=self._fps) staged = Path(temporary) (staged / (name + ".json")).write_text( json.dumps(metadata, indent=2) + "\n" @@ -213,6 +218,7 @@ def save_and_clear( "env_id": row, "episode_context": deepcopy(snapshot), "frames": len(frames), + "fps": self._fps, } self._jobs[key] = (tuple(frame.copy() for frame in frames), metadata) self._frames_list[row] = [] @@ -262,6 +268,7 @@ def __call__( intrinsics: tuple[float, float, float, float] = (600, 600, 320, 240), max_env_num: int = 16, save_path: str = "./outputs/videos", + fps: float | None = 20.0, ) -> None: """Copy selected camera rows without consuming any other row's frames. @@ -276,8 +283,10 @@ def __call__( intrinsics: Construction-only camera calibration. max_env_num: Upper bound on captured row indices. save_path: Construction-only output directory. + fps: Video frame rate, or None for the environment control rate. """ self._ensure_open() + env.sim.render_camera_group([self.camera.group_id]) self.camera.update(fetch_only=True) rgb = self.camera.get_data()["color"] for row in self._normalize_env_ids(env_ids): diff --git a/embodichain/lab/gym/utils/_component_composition.py b/embodichain/lab/gym/utils/_component_composition.py index ca1baf229..f138e1584 100644 --- a/embodichain/lab/gym/utils/_component_composition.py +++ b/embodichain/lab/gym/utils/_component_composition.py @@ -60,10 +60,29 @@ "rigid_object", "rigid_object_group", "articulation", + "fluid_object", } ) +def _resolve_physical_source_paths(simulation: dict, base_dir: Path) -> dict: + """Anchor file-backed fluid sources and explicitly relative visual meshes.""" + result = deepcopy(dict(simulation)) + for field in ("background", "rigid_object"): + for item in result.get(field, []): + for key in ("shape", "visual_shape", "particle_collision_shape"): + shape = item.get(key) or {} + path = shape.get("fpath") + if isinstance(path, str) and path.startswith(("./", "../")): + shape["fpath"] = str((base_dir / path).resolve()) + for fluid in result.get("fluid_object", []): + spawn = fluid.get("spawn", {}) + path = spawn.get("file_path") + if isinstance(path, str) and not Path(path).is_absolute(): + spawn["file_path"] = str((base_dir / path).resolve()) + return result + + @dataclass(frozen=True, slots=True) class _ResolvedGymComponents: """One Gym config after its reusable components are expanded.""" @@ -246,7 +265,9 @@ def _resolve_environment_component( for key, value in component.items() if key not in {"environment_id", "simulation"} } - environment_values.update(deepcopy(dict(simulation))) + environment_values.update( + _resolve_physical_source_paths(dict(simulation), component_path.parent) + ) _resolve_local_resource_paths(environment_values, base_dir=component_path.parent) deployment_physics_fields = sorted( @@ -356,7 +377,7 @@ def _resolve_scene_component( required=frozenset(), optional=_SCENE_SIMULATION_FIELDS, ) - resolved = deepcopy(dict(simulation)) + resolved = _resolve_physical_source_paths(dict(simulation), component_path.parent) _resolve_local_resource_paths(resolved, base_dir=component_path.parent) return resolved diff --git a/embodichain/lab/gym/utils/gym_utils.py b/embodichain/lab/gym/utils/gym_utils.py index 9f885ccc3..1bdd7d9e3 100644 --- a/embodichain/lab/gym/utils/gym_utils.py +++ b/embodichain/lab/gym/utils/gym_utils.py @@ -536,6 +536,16 @@ def resolve_asset_paths( elif isinstance(asset, ArticulationCfg) and path is not None: asset.fpath = get_data_path(path) + def _resolve_mesh_asset_paths(obj_dict: dict[str, Any]) -> None: + for key in ("visual_shape", "particle_collision_shape"): + shape = obj_dict.get(key) + if ( + shape is not None + and shape.get("shape_type") == "Mesh" + and shape.get("fpath") is not None + ): + shape["fpath"] = get_data_path(shape["fpath"]) + @configclass class ComponentCfg: """Configuration for env events. @@ -773,6 +783,16 @@ class ComponentCfg: ] env_cfg.light.indirect = config["light"].get("indirect", None) + # Fluids use one typed declaration path for inline and component scenes. + from embodichain.lab.sim.cfg import FluidObjectCfg + from embodichain.lab.gym.utils._component_composition import ( + _resolve_physical_source_paths, + ) + + config = _resolve_physical_source_paths(config, base_dir) + env_cfg.fluid_object = [ + FluidObjectCfg.from_dict(item) for item in config.get("fluid_object", []) + ] for visual_dict in config.get("visual_object", []): visual = VisualObjectCfg.from_dict(visual_dict) resolve_asset_paths(visual) @@ -781,6 +801,7 @@ class ComponentCfg: # parser background objects config if "background" in config: for obj_dict in config["background"]: + _resolve_mesh_asset_paths(obj_dict) # Set to static object if not specified. obj_dict["body_type"] = ( "static" if "body_type" not in obj_dict else obj_dict["body_type"] @@ -792,6 +813,7 @@ class ComponentCfg: # parser scene objects config if "rigid_object" in config: for obj_dict in config["rigid_object"]: + _resolve_mesh_asset_paths(obj_dict) cfg = RigidObjectCfg.from_dict(obj_dict) resolve_asset_paths(cfg) env_cfg.rigid_object.append(cfg) @@ -800,6 +822,8 @@ class ComponentCfg: for obj_dict in config["rigid_object_group"]: if "folder_path" in obj_dict: obj_dict["folder_path"] = get_data_path(obj_dict["folder_path"]) + for rigid_obj in obj_dict["rigid_objects"].values(): + _resolve_mesh_asset_paths(rigid_obj) cfg = RigidObjectGroupCfg.from_dict(obj_dict) for member in cfg.rigid_objects.values(): resolve_asset_paths(member) @@ -918,6 +942,12 @@ class ComponentCfg: ) obs_params_modified["params"]["entity_cfg"] = entity_cfg + if "entity_cfgs" in obs_params_modified["params"]: + obs_params_modified["params"]["entity_cfgs"] = [ + SceneEntityCfg(**item) + for item in obs_params_modified["params"]["entity_cfgs"] + ] + # Find the function from multiple modules using the utility function obs_func = find_function_from_modules( obs_params["func"], @@ -930,6 +960,7 @@ class ComponentCfg: mode=obs_params_modified["mode"], name=obs_params_modified["name"], params=obs_params_modified["params"], + extra=deepcopy(obs_params_modified.get("extra", {})), ) setattr(env_cfg.observations, obs_name, observation) diff --git a/embodichain/lab/scripts/run_fluid_task_program.py b/embodichain/lab/scripts/run_fluid_task_program.py new file mode 100644 index 000000000..76975f14a --- /dev/null +++ b/embodichain/lab/scripts/run_fluid_task_program.py @@ -0,0 +1,287 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Run a Task Program with live robot/cup contact and one-way SPH fluids.""" + +from __future__ import annotations + +import argparse +from collections.abc import Iterable, Iterator +import json +from pathlib import Path +import time +from typing import Any + +import numpy as np +import torch + +from embodichain.lab.gym.envs.demo import execute_demo_episode +from embodichain.lab.gym.envs.embodied_env import EmbodiedEnvCfg +from embodichain.lab.gym.envs.managers.cfg import SceneEntityCfg +from embodichain.lab.gym.envs.managers.observations import ( + get_fluid_summary, + get_fluid_volume_in_region, +) +from embodichain.lab.gym.utils.gym_utils import config_to_cfg +from embodichain.lab.gym.utils.registration import discover_task_packages, make +from embodichain.lab.sim import SimulationManager +from embodichain.utils.config_paths import resolve_config_path +from embodichain.utils.utility import load_config + +__all__ = ["main"] + +_TASK = ( + "extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/" + "task.franka.fluid.yaml" +) + + +def _configure( + task: Path, + *, + num_envs: int, + renderer: str, + output: Path, + particle_spacing: float | None = None, + record_trajectory: bool = False, +) -> tuple[str, EmbodiedEnvCfg]: + declaration = load_config(str(task)) + cfg = config_to_cfg(declaration, source_path=task) + if cfg.task_program is None: + raise ValueError("The online fluid deployment must select a Task Program.") + solver = cfg.sim_cfg.physics_cfg.solver_cfg + if solver.get("solver_type") != "dexuni" or not isinstance( + solver.get("sph_cfg"), dict + ): + raise ValueError("Online fluid execution requires DexUni with sph_cfg.") + if num_envs < 1: + raise ValueError("num_envs must be positive.") + cfg.num_envs = num_envs + cfg.sim_cfg.headless = True + cfg.sim_cfg.render_cfg.renderer = renderer + cfg.enable_sensor = False + cfg.record_trajectory = record_trajectory + if renderer == "no-render": + cfg.events.record_camera = None + for fluid in cfg.fluid_object: + fluid.render.mode = "none" + else: + cfg.events.record_camera.params["save_path"] = str(output / "videos") + cfg.events.record_camera.params["max_env_num"] = num_envs + if particle_spacing is not None: + if not np.isfinite(particle_spacing) or particle_spacing <= 0: + raise ValueError("particle_spacing must be finite and positive.") + fluid = cfg.fluid_object[0] + if fluid.spawn.kind == "points": + raise ValueError("particle-spacing requires a procedural volume source.") + scale = particle_spacing / fluid.particle_spacing + if fluid.render.render_radius is not None: + fluid.render.render_radius *= scale + for key in ("cell_size", "kernel_radius"): + if key in fluid.render.mesh_surface: + fluid.render.mesh_surface[key] *= scale + fluid.particle_spacing = particle_spacing + fluid.particle_radius = 0.3 * particle_spacing + solver["sph_cfg"]["particle_support_radius"] = 2.25 * particle_spacing + return declaration["id"], cfg + + +def _sample(env: Any) -> dict[str, Any]: + summary = get_fluid_summary(env, {}, SceneEntityCfg(uid="water")) + if not torch.isfinite(summary).all() or not torch.all(summary[:, -1] == 1): + raise RuntimeError("Fluid state became non-finite.") + received = get_fluid_volume_in_region( + env, + {}, + [SceneEntityCfg(uid="water"), SceneEntityCfg(uid="receiver")], + lower=[-0.1, -0.1, 0.0045], + upper=[0.1, 0.1, 0.0675], + sphere_center=[0, 0, 0.088], + sphere_radius=0.093, + ) + return { + "time_s": (env.elapsed_steps * env.step_dt).cpu().tolist(), + "received_ml": (received[:, 0] * 1e6).cpu().tolist(), + "summary": summary.cpu().tolist(), + "cup_pose": env.sim.get_rigid_object("cup").get_local_pose().cpu().tolist(), + } + + +def _cup_qualification(poses: list[np.ndarray]) -> dict[str, Any]: + measured = np.stack(poses, axis=1) + lift = measured[..., 2].max(axis=1) - measured[:, 0, 2] + vertical = 1 - 2 * (measured[..., 3] ** 2 + measured[..., 4] ** 2) + tilt = np.rad2deg(np.arccos(np.clip(vertical, -1, 1))).max(axis=1) + return_error = np.linalg.norm(measured[:, -1, :3] - measured[:, 0, :3], axis=1) + return { + "lift_m": lift.tolist(), + "max_tilt_deg": tilt.tolist(), + "return_error_m": return_error.tolist(), + "accepted": ((lift >= 0.05) & (tilt >= 45) & (return_error <= 0.03)).tolist(), + } + + +def _run(args: argparse.Namespace, output: Path) -> dict[str, Any]: + path = resolve_config_path(args.task).resolve() + task_id, cfg = _configure( + path, + num_envs=args.num_envs, + renderer=args.renderer, + output=output, + particle_spacing=args.particle_spacing, + record_trajectory=args.record_trajectory, + ) + discover_task_packages() + env = make(task_id, cfg=cfg) + try: + env.reset(seed=0) + cup = env.sim.get_rigid_object("cup") + poses = [cup.get_local_pose().cpu().numpy().copy()] + samples = [_sample(env)] + started = time.perf_counter() + sample_stride = max(1, round(1 / env.step_dt)) + + def observe(actions: Iterable[Any], description: str) -> Iterator[Any]: + print(description, flush=True) + for action in actions: + yield action + poses.append(cup.get_local_pose().cpu().numpy().copy()) + if int(env.elapsed_steps.max()) % sample_stride == 0: + sample = _sample(env) + samples.append(sample) + print(json.dumps(sample), flush=True) + + def stop() -> bool: + return ( + args.seconds is not None + and float(env.elapsed_steps.max()) * env.step_dt >= args.seconds + ) + + result = execute_demo_episode(env, progress=observe, should_stop=stop) + if result.completed and args.settle_seconds: + target = env.robot.get_qpos()[:, env.active_joint_ids].clone() + for _ in range(round(args.settle_seconds / env.step_dt)): + env.step(target) + poses.append(cup.get_local_pose().cpu().numpy().copy()) + samples.append(_sample(env)) + qualification = _cup_qualification(poses) + accepted = bool( + result.completed + and result.all_success + and all(qualification["accepted"]) + and all(v >= 50 for v in samples[-1]["received_ml"]) + ) + videos = [] + if args.renderer != "no-render": + recorder = env.event_manager.get_functor("record_camera") + purpose = "recording" if accepted else "diagnostic" + name = cfg.events.record_camera.params["name"] + video_root = output / "videos" + if purpose == "diagnostic": + video_root /= "diagnostics" + existing = { + path: (path.stat().st_ino, path.stat().st_mtime_ns) + for path in video_root.glob(f"env_*/episode_*_{name}.mp4") + } + recorder.save_and_clear(purpose=purpose) + for i in range(env.num_envs): + matches = [] + for path in (video_root / f"env_{i:03d}").glob(f"episode_*_{name}.mp4"): + stat = path.stat() + if existing.get(path) != (stat.st_ino, stat.st_mtime_ns): + matches.append(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", + "num_envs": env.num_envs, + "renderer": args.renderer, + "videos": videos, + "video_fps": ( + cfg.events.record_camera.params.get("fps") or env.control_frequency + if videos + else None + ), + "particle_count_per_env": env.sim.get_fluid_object("water").num_particles, + "fluid_source": cfg.fluid_object[0].spawn.to_dict(), + "particle_spacing_m": cfg.fluid_object[0].particle_spacing, + "physics_dt": cfg.sim_cfg.physics_dt, + "control_dt": env.step_dt, + "task_program": result.to_metadata(), + "cup": qualification, + "wall_seconds": time.perf_counter() - started, + "samples": samples, + } + report["accepted"] = accepted + (output / "fluid_gym_result.json").write_text( + json.dumps(report, indent=2) + "\n" + ) + print( + json.dumps( + { + key: report[key] + for key in ("accepted", "coupling", "cup", "wall_seconds") + } + ), + flush=True, + ) + if args.seconds is None and not report["accepted"]: + raise RuntimeError( + "Online pouring failed physical qualification; inspect fluid_gym_result.json." + ) + return report + finally: + env.close(exit_process=False) + del env + SimulationManager.flush_cleanup_queue() + + +def main() -> None: + """Execute the configured Task Program and fluid simulation in one World.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--task", default=_TASK) + parser.add_argument( + "--output", type=Path, default=Path("outputs/fluid-task-program-online") + ) + parser.add_argument("--num-envs", type=int, default=2) + parser.add_argument( + "--renderer", choices=["fast-rt", "no-render"], default="fast-rt" + ) + parser.add_argument("--seconds", type=float) + parser.add_argument("--particle-spacing", type=float) + parser.add_argument("--settle-seconds", type=float, default=2.0) + parser.add_argument( + "--record-trajectory", + action="store_true", + help="Optionally save measured online states for inspection.", + ) + args = parser.parse_args() + if args.seconds is not None and ( + not np.isfinite(args.seconds) or args.seconds <= 0 + ): + parser.error("seconds must be finite and positive") + if not np.isfinite(args.settle_seconds) or args.settle_seconds < 0: + parser.error("settle-seconds must be finite and non-negative") + output = args.output.resolve() + output.mkdir(parents=True, exist_ok=True) + _run(args, output) + + +if __name__ == "__main__": + main() diff --git a/embodichain/lab/sim/_runtime_controls.py b/embodichain/lab/sim/_runtime_controls.py index 6c0b23954..f17c3b0c4 100644 --- a/embodichain/lab/sim/_runtime_controls.py +++ b/embodichain/lab/sim/_runtime_controls.py @@ -18,15 +18,58 @@ from __future__ import annotations +from collections.abc import Callable from typing import Any import numpy as np -from dexsim.engine.newton_physics.runtime_control import RuntimeControl +from dexsim.engine.newton_physics.runtime_control import ( + GraphRuntimeControl, + RuntimeControl, +) __all__: list[str] = [] +class _DexUniFluidJointOwnership(GraphRuntimeControl): + """Assign the complete finalized joint topology to the fluid rigid solver.""" + + def __init__(self) -> None: + self._assigned: tuple[int, ...] | None = None + + def initialize_model(self, model: Any, result: Any) -> None: + """Select all joints before DexUni constructs its coupled solver.""" + from dexsim.engine.newton_physics.backend_registry import get_newton_backend + + backend = get_newton_backend(result.world) + if backend is None or backend.solver_type != "dexuni": + raise RuntimeError("Fluid joint ownership requires a DexUni backend.") + cfg = backend.cfg.solver_cfg + if ( + cfg.mujoco_articulations is not None + or cfg.mujoco_articulation_paths is not None + or (cfg.mujoco_joints is not None and cfg.mujoco_joints != self._assigned) + ): + raise RuntimeError( + "Fluid joint ownership conflicts with an explicit selector." + ) + self._assigned = tuple(range(model.joint_count)) + cfg.mujoco_joints = self._assigned + + def __call__( + self, context: Any, substep_index: int, substep_count: int, substep_dt: float + ) -> None: + """Keep the solver-owned topology unchanged while stepping.""" + del context, substep_index, substep_count, substep_dt + + def prepare_episode_reset( + self, arena_indices: tuple[int, ...] + ) -> Callable[[], None]: + """Keep the fixed topology unchanged during selective episode resets.""" + del arena_indices + return lambda: None + + class _KinematicNodalTrajectoryControl(RuntimeControl): """Drive selected particles along offsets from their initialized positions.""" @@ -122,3 +165,67 @@ def close(self) -> None: """Release runtime particle references retained after initialization.""" self._particle_set = None self._initial_positions = None + + +class _KinematicRigidTrajectoryControl(RuntimeControl): + """Sample world pose tracks on the native solver substep clock.""" + + def __init__(self, target: str, poses: np.ndarray, fps: float) -> None: + from scipy.spatial.transform import Rotation, Slerp + + self.target = target + self.poses = poses.copy() + self.fps = fps + self._times = np.arange(len(poses)) / fps + self._rotations = Rotation.from_quat(poses[:, 3:]) + self._slerp = Slerp(self._times, self._rotations) if len(poses) > 1 else None + self._actor = None + self.reset() + + def initialize(self, context: Any) -> None: + self._actor = context.result.get_actor(self.target) + self.reset() + + def exclusive_resource_claims(self) -> tuple[object, ...]: + return (("kinematic_rigid_trajectory", self.target),) + + def reset(self) -> None: + self._elapsed = 0.0 + self._previous = None + + def prepare_episode_reset( + self, arena_indices: tuple[int, ...] + ) -> Callable[[], None]: + """Rewind only when this trajectory's arena is selected.""" + if self._actor is None: + raise RuntimeError("Kinematic rigid trajectory is not initialized.") + return self.reset if self._actor._arena_index in arena_indices else lambda: None + + def __call__( + self, context: Any, substep_index: int, substep_count: int, substep_dt: float + ) -> None: + from scipy.spatial.transform import Rotation + + del context, substep_index, substep_count + if self._actor is None: + raise RuntimeError("Kinematic rigid trajectory is not initialized.") + t = min(self._elapsed, self._times[-1]) + position = np.array( + [np.interp(t, self._times, self.poses[:, axis]) for axis in range(3)] + ) + rotation = self._rotations[0] if self._slerp is None else self._slerp(t) + pose = np.eye(4, dtype=np.float32) + pose[:3, :3] = rotation.as_matrix() + pose[:3, 3] = position + previous = pose if self._previous is None else self._previous + self._actor.set_world_pose(pose) + self._actor.set_linear_velocity((pose[:3, 3] - previous[:3, 3]) / substep_dt) + self._actor.set_angular_velocity( + Rotation.from_matrix(pose[:3, :3] @ previous[:3, :3].T).as_rotvec() + / substep_dt + ) + self._previous = pose + self._elapsed += substep_dt + + def close(self) -> None: + self._actor = None diff --git a/embodichain/lab/sim/cfg/rigid_object.py b/embodichain/lab/sim/cfg/rigid_object.py index 4ea774d10..e73854a78 100644 --- a/embodichain/lab/sim/cfg/rigid_object.py +++ b/embodichain/lab/sim/cfg/rigid_object.py @@ -50,6 +50,19 @@ class RigidObjectCfg(ObjectBaseCfg): # TODO: supoort basic primitive shapes, such as box, sphere, etc cfg and spawn method. + visual_shape: ShapeCfg | None = None + """Optional independent render geometry; shape remains collision geometry.""" + + particle_collision_shape: ShapeCfg | None = None + """Optional Newton SDF mesh for particles on this same rigid body. + + Rigid contacts and mass use :attr:`shape`. This boundary follows the body's + live pose and twist and contributes no mass or rigid contact geometry. + """ + + render_enabled: bool = True + """Whether to create native render geometry for this rigid object.""" + attrs: RigidBodyPhysicsCfg = RigidBodyPhysicsCfg() """Rigid-body physics. @@ -77,6 +90,9 @@ def resolve_asset_physics_mode(self) -> AssetPhysicsMode: def from_dict(cls, init_dict: Dict[str, Any]) -> RigidObjectCfg: """Parse a rigid object and normalize legacy mesh collision ownership.""" data = dict(init_dict) + for key in ("visual_shape", "particle_collision_shape"): + if isinstance(data.get(key), Mapping): + data[key] = ShapeCfg.from_dict(data[key]) attrs_value = data.get("attrs") if isinstance(attrs_value, Mapping) and "mesh_collision_props" in attrs_value: shape_value = data.get("shape") diff --git a/embodichain/lab/sim/objects/backends/newton.py b/embodichain/lab/sim/objects/backends/newton.py index 1763ea528..3b921a8d7 100644 --- a/embodichain/lab/sim/objects/backends/newton.py +++ b/embodichain/lab/sim/objects/backends/newton.py @@ -310,7 +310,10 @@ def _configure_newton_mimic_compliance( eq_solref = np.asarray(mjw_model.eq_solref.numpy()).copy() mimic_solref = _default_mujoco_mimic_solref( float(backend.cfg.dt), - int(backend.cfg.num_substeps), + int(backend.cfg.num_substeps) + * int( + getattr(getattr(backend.cfg, "solver_cfg", None), "sph_rigid_substeps", 1) + ), ) eq_solref[selected] = mimic_solref mjw_model.eq_solref.assign(eq_solref) diff --git a/embodichain/lab/sim/objects/rigid_object.py b/embodichain/lab/sim/objects/rigid_object.py index e56ca6abc..6e3bdc091 100644 --- a/embodichain/lab/sim/objects/rigid_object.py +++ b/embodichain/lab/sim/objects/rigid_object.py @@ -497,7 +497,9 @@ def _initialize_spawn_bound(self, result: Scene) -> None: def _apply_spawn_visual_uv(self) -> None: """Apply configured projective UV mapping to spawned render bodies.""" - shape = self.cfg.shape + shape = getattr(self.cfg, "visual_shape", None) or getattr( + self.cfg, "shape", None + ) if not isinstance(shape, MeshCfg) or not shape.compute_uv: return @@ -936,6 +938,78 @@ def add_force_torque( else: logger.log_error("Cannot apply force or torque before body view is ready.") + def set_kinematic_state( + self, + pose_w: torch.Tensor, + lin_vel_w: torch.Tensor, + ang_vel_w: torch.Tensor, + env_ids: Sequence[int] | None = None, + ) -> None: + """Set a kinematic boundary's world pose and twist without stepping. + + Args: + pose_w: World poses, ``(E, 7)`` xyz+xyzw or ``(E, 4, 4)``. + lin_vel_w: World linear velocity ``(E, 3)``, m/s. + ang_vel_w: World angular velocity ``(E, 3)``, rad/s. + env_ids: Selected replicas, or None for all. + """ + if self.body_type != "kinematic": + raise ValueError("set_kinematic_state requires a kinematic rigid body.") + selection = torch.as_tensor(self._all_indices if env_ids is None else env_ids) + if selection.ndim != 1 or ( + selection.numel() and selection.dtype not in (torch.int32, torch.int64) + ): + raise ValueError("env_ids must be a one-dimensional integer selection.") + ids = selection.tolist() + if len(set(ids)) != len(ids) or any( + i < 0 or i >= self.num_instances for i in ids + ): + raise ValueError("env_ids must be unique valid indices.") + if tuple(pose_w.shape) not in {(len(ids), 7), (len(ids), 4, 4)}: + raise ValueError("pose_w has an invalid shape.") + if any(tuple(v.shape) != (len(ids), 3) for v in (lin_vel_w, ang_vel_w)): + raise ValueError("Boundary velocities must have shape (E, 3).") + if not all(torch.isfinite(v).all() for v in (pose_w, lin_vel_w, ang_vel_w)): + raise ValueError("Kinematic state must be finite.") + if pose_w.ndim == 2: + if not torch.allclose( + pose_w[:, 3:].norm(dim=-1), + torch.ones(len(ids), device=pose_w.device, dtype=pose_w.dtype), + atol=1e-5, + ): + raise ValueError("pose_w requires unit xyzw quaternions.") + else: + rotations = pose_w[:, :3, :3] + identity = torch.eye(3, device=pose_w.device, dtype=pose_w.dtype).expand( + len(ids), -1, -1 + ) + if ( + not torch.allclose( + rotations.transpose(-1, -2) @ rotations, identity, atol=1e-5 + ) + or not torch.allclose( + torch.linalg.det(rotations), + torch.ones(len(ids), device=pose_w.device, dtype=pose_w.dtype), + atol=1e-5, + ) + or not torch.allclose( + pose_w[:, 3], pose_w.new_tensor([0, 0, 0, 1]).expand(len(ids), -1) + ) + ): + raise ValueError("pose_w requires rigid homogeneous transforms.") + if self._data is None or not self._data.body_view.is_ready: + raise RuntimeError("Kinematic state requires prepare().") + if pose_w.ndim == 2: + pose = torch.eye(4).repeat(len(ids), 1, 1) + pose[:, :3, :3] = matrix_from_quat(pose_w[:, 3:].cpu()) + pose[:, :3, 3] = pose_w[:, :3].cpu() + else: + pose = pose_w.cpu() + for row, index in enumerate(ids): + self._entities[index].set_world_pose(pose[row].numpy()) + self._entities[index].set_linear_velocity(lin_vel_w[row].cpu().numpy()) + self._entities[index].set_angular_velocity(ang_vel_w[row].cpu().numpy()) + def set_velocity( self, lin_vel: torch.Tensor | None = None, diff --git a/embodichain/lab/sim/physics/newton.py b/embodichain/lab/sim/physics/newton.py index 20f4b933f..7e8aa4cfe 100644 --- a/embodichain/lab/sim/physics/newton.py +++ b/embodichain/lab/sim/physics/newton.py @@ -276,6 +276,9 @@ def invalidate_fluid_state(self, env_ids: Sequence[int] | None = None) -> None: boundary = solver.sdf_boundary if boundary is not None and boundary._body_q_prev is not None: boundary._body_q_prev.assign(backend.state_0.body_q) + reset_warm_start = getattr(solver, "reset_dfsph_warm_start", None) + if reset_warm_start is not None: + reset_warm_start() def fluid_diagnostics(self, fluid: FluidObject) -> dict[str, object]: """Read detached native SPH diagnostics without publishing or stepping.""" diff --git a/embodichain/lab/sim/shapes.py b/embodichain/lab/sim/shapes.py index 2c2e4612b..dba47693e 100755 --- a/embodichain/lab/sim/shapes.py +++ b/embodichain/lab/sim/shapes.py @@ -71,6 +71,9 @@ class MeshCollisionCfg: VisACD requires a DexSim build with CUDA/OptiX support. """ + force_sdf: bool | None = None + """Optional Newton SDF strategy override; False uses an eagerly baked SDF.""" + sdf_resolution: int | None = None """Maximum SDF grid resolution; valid only for the ``sdf`` strategy.""" @@ -126,6 +129,7 @@ def __post_init__(self) -> None: ) sdf_values = { + "force_sdf": self.force_sdf, "sdf_resolution": self.sdf_resolution, "is_hydroelastic": self.is_hydroelastic, "sdf_narrow_band_range": self.sdf_narrow_band_range, @@ -187,6 +191,9 @@ def from_dict(cls, init_dict: Dict[str, Any]) -> MeshCollisionCfg: "force_sdf", "sdf_max_resolution", } + explicit_force_sdf = data.get("force_sdf") if "approximation" in data else None + if "approximation" in data: + legacy_fields.discard("force_sdf") has_legacy_fields = bool(legacy_fields.intersection(data)) if has_legacy_fields: warnings.warn( @@ -230,6 +237,8 @@ def from_dict(cls, init_dict: Dict[str, Any]) -> MeshCollisionCfg: if data.get("sdf_resolution") == 0: data.pop("sdf_resolution") + if explicit_force_sdf is not None: + data["force_sdf"] = explicit_force_sdf return cls(**data) @@ -241,6 +250,7 @@ def _mesh_collision_cfg_init_with_legacy_max_hulls( approximation: MeshCollisionApproximation | None = None, max_hulls: int | None = None, acd_method: Literal["visacd", "coacd", "vhacd"] | None = None, + force_sdf: bool | None = None, sdf_resolution: int | None = None, is_hydroelastic: bool | None = None, sdf_narrow_band_range: tuple[float, float] | None = None, @@ -283,6 +293,7 @@ def _mesh_collision_cfg_init_with_legacy_max_hulls( approximation="convex_hull" if approximation is None else approximation, max_hulls=max_hulls, acd_method=acd_method, + force_sdf=force_sdf, sdf_resolution=sdf_resolution, is_hydroelastic=is_hydroelastic, sdf_narrow_band_range=sdf_narrow_band_range, diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index fc6d5e30a..90db2766c 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -16,7 +16,6 @@ from __future__ import annotations -from collections.abc import Mapping from embodichain.lab.sim.cfg.fluid import FluidObjectCfg from embodichain.lab.sim.objects.fluid import FluidObject @@ -618,6 +617,7 @@ def _initialize_simulation( self._markers: dict[str, _AxisMarkerGroup] = {} self._constraints: dict[str, RigidConstraint] = {} self._fluid_objects: dict[str, FluidObject] = {} + self._rigid_trajectory_controls: dict[str, tuple] = {} self._visual_materials: dict[str, VisualMaterial] = {} self._texture_cache: dict[str, torch.Tensor | list[torch.Tensor]] = {} @@ -1340,6 +1340,61 @@ def _init_sim_resources(self) -> None: self._default_resources = SimResources() + def register_kinematic_rigid_trajectory( + self, + uid: str, + world_poses: torch.Tensor | np.ndarray, + *, + fps: float, + ) -> None: + """Drive declared kinematic rigid bodies at each Newton solver substep. + + Args: + uid: Previously declared kinematic rigid-object UID. + world_poses: Finite unit-quaternion xyz+xyzw tracks, ``(E, frames, 7)``. + fps: Positive pose sample frequency in Hz. Positions interpolate + linearly, orientations use SLERP, and the last pose is held. + + The host-side control disables CUDA graph replay. Register before + prepare; reset_objects_state rewinds only selected instance clocks. + """ + from embodichain.lab.sim._runtime_controls import ( + _KinematicRigidTrajectoryControl, + ) + + if not self.is_newton_backend or self._spawn_scene.builder.is_finalized: + raise RuntimeError("Register Newton rigid trajectories before prepare().") + rigid = self._rigid_objects.get(uid) + if rigid is None or rigid.body_type != "kinematic": + raise ValueError("uid must name a declared kinematic rigid object.") + if uid in self._rigid_trajectory_controls: + raise ValueError("A rigid trajectory already controls this object.") + poses = ( + world_poses.detach().cpu().numpy() + if isinstance(world_poses, torch.Tensor) + else np.asarray(world_poses) + ) + if ( + poses.ndim != 3 + or poses.shape[0] != self.num_envs + or poses.shape[1] == 0 + or poses.shape[2] != 7 + ): + raise ValueError("world_poses must have shape (E, frames, 7).") + if not np.isfinite(poses).all() or not np.allclose( + np.linalg.norm(poses[..., 3:], axis=-1), 1.0, atol=1e-5 + ): + raise ValueError("world_poses must be finite with unit xyzw quaternions.") + if not np.isfinite(fps) or fps <= 0: + raise ValueError("fps must be finite and positive.") + controls = tuple( + _KinematicRigidTrajectoryControl(f"{arena}/{uid}", poses[i], fps) + for i, arena in enumerate(self._spawn_scene.arena_names) + ) + for control in controls: + self._spawn_scene.builder.add_runtime_control(control) + self._rigid_trajectory_controls[uid] = controls + def register_kinematic_joint_trajectory( self, uid: str, @@ -3049,6 +3104,25 @@ def add_fluid_object(self, cfg: FluidObjectCfg) -> FluidObject: points, pose[:3, :3], on_write=self._invalidate_fluid_state ) self._spawn_scene.declare("fluid_object", cfg.uid, descriptor, facade=fluid) + selectors = ( + "mujoco_articulations", + "mujoco_articulation_paths", + "mujoco_joints", + ) + solver_cfg = self.sim_config.physics_cfg.solver_cfg + if not getattr(self, "_fluid_joint_ownership_control", None) and all( + ( + solver_cfg.get(name) + if isinstance(solver_cfg, Mapping) + else getattr(solver_cfg, name, None) + ) + is None + for name in selectors + ): + from embodichain.lab.sim._runtime_controls import _DexUniFluidJointOwnership + + self._fluid_joint_ownership_control = _DexUniFluidJointOwnership() + self.add_runtime_control(self._fluid_joint_ownership_control) self._fluid_objects[cfg.uid] = fluid self.notify_visualization_topology_changed() return fluid @@ -5192,6 +5266,10 @@ def reset_objects_state( for uid, rigid_obj_group in self._rigid_object_groups.items(): if uid not in excluded_uids: rigid_obj_group.reset(env_ids) + for uid, controls in self._rigid_trajectory_controls.items(): + if uid not in excluded_uids: + for index in (range(self.num_envs) if env_ids is None else env_ids): + controls[int(index)].reset() for uid, fluid_obj in self._fluid_objects.items(): if uid not in excluded_uids: fluid_obj.reset(env_ids) @@ -5406,6 +5484,7 @@ def _deferred_destroy(self) -> None: finally: self._spawn_scene = None + self._rigid_trajectory_controls.clear() self.clean_materials() if self._env: diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index fd688144c..86d8560ec 100644 --- a/embodichain/lab/sim/spawn/descriptors.py +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -358,8 +358,14 @@ def rigid_desc_from_cfg( ) ) geometry, approximation, max_hulls, acd_method = _compile_geometry(cfg) + render_shape = cfg.visual_shape or cfg.shape + render_geometry = geometry + if cfg.visual_shape is not None: + render_cfg = cfg.copy() + render_cfg.shape = cfg.visual_shape + render_geometry = _compile_geometry(render_cfg)[0] material_ref, material_entry = _compile_visual_material( - uid, cfg.shape.visual_material + uid, render_shape.visual_material ) collision = CollisionDesc.from_geometry( geometry, @@ -380,19 +386,65 @@ def rigid_desc_from_cfg( cfg.shape.collision if isinstance(cfg.shape, MeshCfg) else None ), ) - collision.render_source_index = 0 + collision.render_source_index = ( + 0 if cfg.visual_shape is None and cfg.render_enabled else None + ) + + collisions = [collision] + if cfg.particle_collision_shape is not None: + boundary_shape = cfg.particle_collision_shape + if newton_solver_type is None: + raise ValueError("particle_collision_shape requires Newton physics.") + if ( + not isinstance(boundary_shape, MeshCfg) + or boundary_shape.collision is None + or boundary_shape.collision.approximation != "sdf" + ): + raise ValueError( + "particle_collision_shape requires a MeshCfg with sdf collision." + ) + boundary_cfg = cfg.copy() + boundary_cfg.shape = boundary_shape + boundary_geometry = _compile_geometry(boundary_cfg)[0] + boundary = CollisionDesc.from_geometry( + boundary_geometry, approximation=CollisionApproximation.SDF + ) + boundary.enable_collision = False + boundary_newton = _compile_newton_collision( + physics, + newton_solver_type=newton_solver_type, + author_shape_defaults=True, + mesh_collision=boundary_shape.collision, + ) + boundary_options = {"has_particle_collision": True} + if "contributes_to_mass" in getattr( + type(boundary_newton), "__dataclass_fields__", {} + ): + boundary_options["contributes_to_mass"] = False + elif cfg.body_type == "dynamic" and physics.mass_props.get("inertia") is None: + raise NotImplementedError( + "A dynamic particle-only collision shape requires either a DexSim " + "build supporting contributes_to_mass or explicit mass and inertia." + ) + boundary.newton = replace(boundary_newton, **boundary_options) + collision.newton = replace(collision.newton, has_particle_collision=False) + collisions.append(boundary) descriptor = ObjectDesc( name=uid, pose=_pose_from_cfg(cfg), - renders=[ - RenderDesc.from_geometry( - geometry, - load_option=_compile_load_option(cfg.shape), - material_ref=material_ref, - ) - ], - collisions=[collision], + renders=( + [ + RenderDesc.from_geometry( + render_geometry, + load_option=_compile_load_option(render_shape), + material_ref=material_ref, + ) + ] + if cfg.render_enabled + else [] + ), + collisions=collisions, physics=_compile_rigid_physics(physics, cfg.body_type), per_env=per_env, body_scale=_vector3(cfg.body_scale, field_name="body_scale"), @@ -1875,7 +1927,9 @@ def _compile_newton_collision( values["gap"] = gap values.update(physics.newton_collision_props) if mesh_collision is not None and mesh_collision.approximation == "sdf": - values["force_sdf"] = True + values["force_sdf"] = ( + True if mesh_collision.force_sdf is None else mesh_collision.force_sdf + ) for field_name in ( "is_hydroelastic", "sdf_narrow_band_range", diff --git a/extensions/embodichain_tasks/configs/components/embodiments/franka_panda_firm_grip.yaml b/extensions/embodichain_tasks/configs/components/embodiments/franka_panda_firm_grip.yaml new file mode 100644 index 000000000..90bacd060 --- /dev/null +++ b/extensions/embodichain_tasks/configs/components/embodiments/franka_panda_firm_grip.yaml @@ -0,0 +1,88 @@ +embodiment_id: franka_panda_firm_grip +simulation: + class_type: FrankaPanda + robot_type: panda + uid: FrankaPanda + joint_drive_props: + drive_type: force + stiffness: + fr3_joint[1-7]: 10000.0 + fr3_finger_joint[1-2]: 10000.0 + damping: + fr3_joint[1-7]: 1000.0 + fr3_finger_joint[1-2]: 100.0 + max_effort: + fr3_joint[1-7]: 100000.0 + fr3_finger_joint[1-2]: 80.0 + link_attrs: + # Override the existing finger selector with Default-backend contact properties. + newton_gripper_contacts: + link_names_expr: [fr3_leftfinger, fr3_rightfinger] + attrs: + collision_props: + backend: default + torsional_patch_radius: 0.009 + min_torsional_patch_radius: 0.008 + material_props: + static_friction: 1.0 + dynamic_friction: 1.0 +sensor: +- sensor_type: Camera + uid: cam_high + width: 640 + height: 480 + intrinsics: + - 488.1665 + - 488.1665 + - 320.0 + - 240.0 + extrinsics: + eye: + - -0.5 + - -0.9 + - 0.88 + target: + - -0.5 + - 0.1 + - 0.2 + up: + - 0.0 + - 0.0 + - 1.0 +skill_profile: + contract_id: single_arm_parallel_gripper + profile_id: franka_panda_firm_grip + resources: + - resource_id: primary_manipulator + endpoints: + - endpoint_id: motion + control_part: arm + capabilities: + - kinematics.batch_inverse + - motion.cartesian_pose + - kinematics.forward + - endpoint_id: grasp + control_part: eef + capabilities: + - interaction.grasp + command_preset: parallel_jaw_commands + command_presets: + - preset_id: parallel_jaw_commands + control_part: eef + commands: + open: + - 0.04 + - 0.04 + grasp: + - 0.0 + - 0.0 + runtime_services: + grasp_pose_generators: + eef: + kind: antipodal_parallel_jaw + sample_count: 1000 + opening_margin: 0.002 + point_sample_density: 0.012 + filter_ground_collision: false + model: + model_id: franka_panda_hand diff --git a/extensions/embodichain_tasks/configs/components/embodiments/franka_panda_firm_grip_newton.yaml b/extensions/embodichain_tasks/configs/components/embodiments/franka_panda_firm_grip_newton.yaml new file mode 100644 index 000000000..216e95d37 --- /dev/null +++ b/extensions/embodichain_tasks/configs/components/embodiments/franka_panda_firm_grip_newton.yaml @@ -0,0 +1,91 @@ +embodiment_id: franka_panda_firm_grip_newton +simulation: + class_type: FrankaPanda + robot_type: panda + uid: FrankaPanda + joint_drive_props: + drive_type: force + stiffness: + fr3_joint[1-7]: 10000.0 + fr3_finger_joint[1-2]: 8000.0 + damping: + fr3_joint[1-7]: 1000.0 + fr3_finger_joint[1-2]: 100.0 + max_effort: + fr3_joint[1-7]: 100000.0 + fr3_finger_joint[1-2]: 80.0 + link_attrs: + # Configure the existing finger selector for MuJoCo torsional and rolling contacts. + newton_gripper_contacts: + link_names_expr: [fr3_leftfinger, fr3_rightfinger] + attrs: + collision_props: + backend: newton + condim: 6 + material_props: + backend: newton + dynamic_friction: 1.0 + ke: 400000.0 + kd: 1265.0 + torsional_friction: 0.1 + rolling_friction: 0.03 +sensor: +- sensor_type: Camera + uid: cam_high + width: 640 + height: 480 + intrinsics: + - 488.1665 + - 488.1665 + - 320.0 + - 240.0 + extrinsics: + eye: + - -0.5 + - -0.9 + - 0.88 + target: + - -0.5 + - 0.1 + - 0.2 + up: + - 0.0 + - 0.0 + - 1.0 +skill_profile: + contract_id: single_arm_parallel_gripper + profile_id: franka_panda_firm_grip_newton + resources: + - resource_id: primary_manipulator + endpoints: + - endpoint_id: motion + control_part: arm + capabilities: + - kinematics.batch_inverse + - motion.cartesian_pose + - kinematics.forward + - endpoint_id: grasp + control_part: eef + capabilities: + - interaction.grasp + command_preset: parallel_jaw_commands + command_presets: + - preset_id: parallel_jaw_commands + control_part: eef + commands: + open: + - 0.04 + - 0.04 + grasp: + - 0.0 + - 0.0 + runtime_services: + grasp_pose_generators: + eef: + kind: antipodal_parallel_jaw + sample_count: 1000 + opening_margin: 0.002 + point_sample_density: 0.012 + filter_ground_collision: false + model: + model_id: franka_panda_hand diff --git a/extensions/embodichain_tasks/configs/components/execution_policies/trajectory_open_loop_fluid_pour.yaml b/extensions/embodichain_tasks/configs/components/execution_policies/trajectory_open_loop_fluid_pour.yaml new file mode 100644 index 000000000..f3b54e5ea --- /dev/null +++ b/extensions/embodichain_tasks/configs/components/execution_policies/trajectory_open_loop_fluid_pour.yaml @@ -0,0 +1,30 @@ +policy_id: trajectory_open_loop_fluid_pour_v1 +preset_id: trajectory + +requires: + embodiment_contract: single_arm_parallel_gripper + +motion: + strategy: ik_interp + # This is a minimum; interpolation adds samples to respect robot limits. + sample_count: 160 + dynamic_collision_mode: "off" + +tracking: + kind: timed + settle_duration: 0.4 + +recovery: + max_replans: 0 + max_action_retries: 0 + goal_rotation_threshold: 3.141592653589793 + +workflow_recovery: + max_recovery_attempts: 0 + +runner: + minimum_cycle_time: 0.0 + hold_on_completion: true + hold_during_effect_verification: false + +effect_assurance: projected diff --git a/extensions/embodichain_tasks/configs/components/execution_policies/trajectory_open_loop_gentle.yaml b/extensions/embodichain_tasks/configs/components/execution_policies/trajectory_open_loop_gentle.yaml new file mode 100644 index 000000000..56835899e --- /dev/null +++ b/extensions/embodichain_tasks/configs/components/execution_policies/trajectory_open_loop_gentle.yaml @@ -0,0 +1,29 @@ +policy_id: trajectory_open_loop_gentle_v1 +preset_id: trajectory + +requires: + embodiment_contract: single_arm_parallel_gripper + +motion: + strategy: ik_interp + sample_count: 360 + dynamic_collision_mode: "off" + +tracking: + kind: timed + settle_duration: 0.4 + +recovery: + max_replans: 0 + max_action_retries: 0 + goal_rotation_threshold: 3.141592653589793 + +workflow_recovery: + max_recovery_attempts: 0 + +runner: + minimum_cycle_time: 0.0 + hold_on_completion: true + hold_during_effect_verification: false + +effect_assurance: projected diff --git a/extensions/embodichain_tasks/configs/components/execution_policies/trajectory_open_loop_pour_cup.yaml b/extensions/embodichain_tasks/configs/components/execution_policies/trajectory_open_loop_pour_cup.yaml new file mode 100644 index 000000000..2ee9b3f6f --- /dev/null +++ b/extensions/embodichain_tasks/configs/components/execution_policies/trajectory_open_loop_pour_cup.yaml @@ -0,0 +1,29 @@ +policy_id: trajectory_open_loop_pour_cup_v1 +preset_id: trajectory + +requires: + embodiment_contract: single_arm_parallel_gripper + +motion: + strategy: ik_interp + sample_count: 240 + dynamic_collision_mode: "off" + +tracking: + kind: timed + settle_duration: 0.4 + +recovery: + max_replans: 0 + max_action_retries: 0 + goal_rotation_threshold: 3.141592653589793 + +workflow_recovery: + max_recovery_attempts: 0 + +runner: + minimum_cycle_time: 0.0 + hold_on_completion: true + hold_during_effect_verification: false + +effect_assurance: projected 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 new file mode 100644 index 000000000..9a0d9150a --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/README.md @@ -0,0 +1,78 @@ +# Franka 在线倒水 + +倒水任务在同一个 World 中通过 DexUni 推进 Franka、动态杯子和 DFSPH 水体。机器人 +使用关节驱动和真实接触抓起杯子，将水倒入接水碗，再放回杯子。耦合方向为刚体到 +流体；水不会反作用于机器人或杯子。初始水体按杯内体积采样，不依赖预录粒子或轨迹。 + +## 依赖 + +基础流体接口来自 [PR #777](https://github.com/DexForce/EmbodiChain/pull/777)。 +运行此任务需要提供 `DexUniSolverCfg.sph_cfg`、SPH world 隔离和选择性 reset 的 +DexSim 开发构建；`dexsim_engine==0.5.1rc1` 不支持该组合。当前验证使用 +`0.5.1+0b8422cc6f6717058bd15371e61bd35cdbbe3c0c`、Newton 1.7 和 CUDA。 +任务网格由资产缓存中的 `PourCupAssets` 提供。 + +## 运行 + +Gym Task Program 在线生成 `Pick → MoveHeldObject → Pour → Place` 动作， +并记录实测流体与杯子状态： + +```bash +python -m embodichain.lab.scripts.run_fluid_task_program \ + --num-envs 1 --renderer fast-rt --output outputs/fluid-task-program-online +``` + +任务的执行节奏由 +[`trajectory_open_loop_fluid_pour.yaml`](../../../../components/execution_policies/trajectory_open_loop_fluid_pour.yaml) +配置。轨迹采样数是下限；规划器会根据关节速度和加速度约束增加采样点，并保留 +抓取及放回时的稳定等待。 + +也可使用普通任务入口： + +```bash +embodichain run-task \ + --gym-config extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml \ + --headless --renderer fast-rt +``` + +独立 Sim 示例从当前场景规划 Atomic Action 目标，通过相同的 DexUni/DFSPH 物理场景 +执行并录制视频： + +```bash +python scripts/tutorials/sim/franka_dexuni_pour_demo.py --headless --renderer fast-rt +``` + +增加 `--num-envs 4` 可在同一个 World 中并行运行四个环境，分别输出 +`fluid_sim_env0.mp4` 至 `fluid_sim_env3.mp4`。 + +默认粒子间距为 1.5 mm，初始填充 76,607 粒。Gym 的 `--particle-spacing 0.003` +或 Sim 的 `--spacing 0.003` 可用于较快的物理检查，但会改变流体分辨率。 +`--seconds 2` 只运行起始片段，不代表完整任务验收。NoRender 仅用于物理检查。 + +## 求解与碰撞 + +`env.fluid.yaml` 声明 DexUni 的 `sph_cfg`、机器人和场景。外层物理步为 1/240 s， +每步 2 个 DFSPH 子步，每个 DFSPH 子步包含 15 个 MuJoCo 刚体子步；控制周期为 +0.025 s。未指定关节所有权时，流体准备阶段将完整的机器人和杯子关节拓扑分配给 +MuJoCo，以满足当前 DexUni/DFSPH 原型的纯 MuJoCo 刚体要求。任务使用完整机器人 +关节目标，使主从手指都收到夹持目标。 + +杯子质量为 0.30 kg，凸分解形状提供机器人抓取接触，独立的 +`particle_collision_shape` SDF 提供流体边界。当前 DexSim 的碰撞描述符没有单独 +排除边界质量的字段，因此杯子的质心和惯量显式使用仅由刚体接触形状计算的值， +避免额外 SDF 改变质量属性。使用支持 `contributes_to_mass` 的新构建时，边界也会 +显式标记为不参与质量计算。 + +## 录制与验收 + +FastRT 使用蓝色液面材质，并为每个环境生成独立的 960 × 720、40 fps 视频。Gym +正常录制保存到 `videos/env_000/episode_000000_fluid_gym.mp4`；未完成或不合格的 +片段保存到 `videos/diagnostics/`。Sim 示例输出 `fluid_sim_env0.mp4`。 + +Gym 的 `fluid_gym_result.json` 和 Sim 的 `validation.json` 分别记录任务完成情况、 +杯子抬升、最大倾角、放回误差和接水量。完整验收要求实际抬升至少 5 cm、倾角 +至少 45°、放回误差不超过 3 cm，且最终接水至少 50 mL；规划成功或部分视频不能 +替代这些物理测量。`--record-trajectory` 仅保存可选的在线状态诊断。 + +`FluidReplayEnv` 是独立的历史记录诊断工具，不是在线任务的默认输入。它使用 +运动学边界和固定根记录，不能替代动态抓取验证。 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/env.fluid.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/env.fluid.yaml new file mode 100644 index 000000000..6fd0a6d13 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/env.fluid.yaml @@ -0,0 +1,356 @@ +environment_id: pour_cup_fluid +physics: newton +render_cfg: + renderer: fast-rt +num_envs: 2 +arena_space: 2.5 +max_episode_steps: 2000 +max_episodes: 1 +physics_config: + physics_dt: 0.004166666666666667 + num_substeps: 2 + collision_cfg: null + solver_cfg: + solver_type: dexuni + joint_mode: dynamic + contact_mode: auto + mujoco_options: + solver: newton + integrator: implicitfast + iterations: 50 + ls_iterations: 100 + cone: elliptic + impratio: 1000.0 + use_mujoco_contacts: true + enable_multiccd: false + ccd_iterations: 10 + sph_rigid_substeps: 15 + sph_rne_mode: disabled + sph_cfg: + sph_method: dfsph + particle_support_radius: 0.003375 + max_neighbors: 192 + viscosity: 0.0006 + surface_tension_coefficient: 0.002 + surface_normal_threshold: 0.08 + pressure_relaxation: 0.2 + divergence_max_iterations: 10 + density_max_iterations: 20 + max_velocity: 2.0 + volume_map_boundary_enabled: true + volume_map_resolution: 192 + volume_map_quadrature_samples: 6 + volume_map_volume_scale: 0.8 + sdf_boundary_enabled: false +simulation: + light: + direct: + - uid: main_light + light_type: sun + color: + - 1.0 + - 1.0 + - 1.0 + intensity: 4.0 + direction: + - -0.3 + - 0.2 + - -1.0 + - uid: fill_light + light_type: rect + color: + - 1.0 + - 1.0 + - 1.0 + intensity: 6.0 + init_pos: + - -0.5 + - -0.4 + - 1.2 + direction: + - 0.0 + - 0.0 + - -1.0 + rect_width: 0.7 + rect_height: 0.7 + background: + - uid: cup_platform + body_type: static + shape: + shape_type: Cube + size: + - 0.4 + - 0.5 + - 0.5 + init_pos: + - -0.9 + - 0.0 + - 0.25 + attrs: + collision_props: + backend: newton + condim: 6 + material_props: + backend: newton + dynamic_friction: 1.0 + ke: 400000.0 + kd: 1265.0 + torsional_friction: 0.1 + rolling_friction: 0.03 + particle_collision_shape: + shape_type: Mesh + fpath: PourCupAssets/fluid_table.obj + collision: + approximation: sdf + force_sdf: true + sdf_resolution: 192 + sdf_narrow_band_range: + - -0.025 + - 0.025 + sdf_padding: 0.012 + sdf_texture_format: uint16 + visual_material: &id001 + uid: fluid_table + base_color: + - 0.23 + - 0.29 + - 0.34 + - 1.0 + roughness: 0.6 + metallic: 0.5 + ior: 1.0 + visual_shape: + shape_type: Mesh + fpath: PourCupAssets/fluid_table.obj + visual_material: *id001 + - uid: fluid_floor + body_type: static + shape: + shape_type: Mesh + fpath: PourCupAssets/fluid_floor.obj + collision: + approximation: sdf + force_sdf: false + sdf_resolution: 192 + sdf_narrow_band_range: + - -0.025 + - 0.025 + sdf_padding: 0.012 + sdf_texture_format: uint16 + init_pos: + - -0.5 + - 0 + - -0.055 + attrs: + collision_props: + backend: newton + has_particle_collision: true + material_props: + backend: newton + dynamic_friction: 0.2 + render_enabled: false + - uid: receiver + body_type: static + shape: + shape_type: Mesh + fpath: PourCupAssets/receiver_wall.obj + collision: + approximation: sdf + force_sdf: false + sdf_resolution: 192 + sdf_narrow_band_range: + - -0.025 + - 0.025 + sdf_padding: 0.012 + sdf_texture_format: uint16 + visual_material: + uid: fluid_receiver + base_color: + - 0.82 + - 0.85 + - 0.88 + - 1.0 + roughness: 0.22 + metallic: 0.5 + ior: 1.0 + init_pos: + - -0.755 + - -0.15 + - 0.5 + attrs: + collision_props: + backend: newton + has_particle_collision: true + material_props: + backend: newton + dynamic_friction: 0.2 + fluid_object: + - uid: water + density: 1000.0 + particle_spacing: 0.0015 + particle_radius: 0.00045 + spawn: + kind: revolved_volume + profile: + - - -0.044 + - 0.031 + - - 0.05 + - 0.0385 + fill_level: 0.91 + clearance: 0.003 + reference_uid: cup + render: + mode: mesh_surface + render_radius: 0.00045 + clip_below_z: 0.49 + material: + base_color: + - 0.03 + - 0.4 + - 0.88 + - 1.0 + roughness: 0.12 + metallic: 0.0 + ior: 1.333 + mesh_surface: + gpu_enabled: true + cell_size: 0.0008100000000000001 + max_res: 512 + kernel_radius: 0.00225 + iso_density: 0.57 + density_smooth_iters: 2 + aabb_trim_fraction: 0.0 + min_component_tris: 32 + laplacian_iters: 4 + laplacian_alpha: 0.11 + update_period: 1 + rigid_object: + - uid: cup + shape: + shape_type: Mesh + fpath: PourCupAssets/cup.glb + compute_uv: true + collision: + approximation: convex_decomposition + max_hulls: 32 + body_type: dynamic + init_pos: + - -0.75 + - 0.0 + - 0.552 + body_scale: + - 1.0 + - 1.0 + - 1.0 + attrs: + mass_props: + mass: 0.3 + # Mass properties of the cup's rigid contact hulls, excluding its + # separate particle-only SDF boundary. + com_position: [0.0044176364, -0.0040134112, 0.00003322245] + inertia: + - [0.00045226974, -0.000005947599, -0.0000012150645] + - [-0.000005947599, 0.0004061172, -0.00000016583306] + - [-0.0000012150645, -0.00000016583306, 0.00052080594] + collision_props: + backend: newton + contact_offset: 0.0005 + rest_offset: 0.0001 + condim: 6 + has_particle_collision: true + material_props: + backend: newton + dynamic_friction: 1.2 + ke: 1600000.0 + kd: 2530.0 + torsional_friction: 0.1 + rolling_friction: 0.03 + particle_collision_shape: + shape_type: Mesh + fpath: PourCupAssets/mug_wall.obj + collision: + approximation: sdf + force_sdf: true + sdf_resolution: 192 + sdf_narrow_band_range: + - -0.025 + - 0.025 + sdf_padding: 0.012 + sdf_texture_format: uint16 +env: + sim_steps_per_control: 6 + # Keep the default full joint action so both gripper fingers receive targets. + events: + record_camera: + func: record_camera_data + mode: interval + interval_step: 1 + is_global: true + params: + name: fluid_gym + resolution: + - 960 + - 720 + intrinsics: + - 1000.0 + - 1000.0 + - 480.0 + - 360.0 + eye: + - -1.12 + - -0.73 + - 0.96 + target: + - -0.76 + - -0.07 + - 0.61 + up: + - 0.0 + - 0.0 + - 1.0 + max_env_num: 16 + fps: null + save_path: outputs/fluid-task-program/videos + refill_water: + func: reset_fluid_in_container + mode: reset + params: + entity_cfgs: + - uid: water + - uid: cup + observations: + water_summary: + func: get_fluid_summary + mode: add + name: fluid/summary + params: + entity_cfg: + uid: water + extra: + shape: + - 5 + received_volume: + func: get_fluid_volume_in_region + mode: add + name: fluid/received_volume + params: + entity_cfgs: + - uid: water + - uid: receiver + lower: + - -0.1 + - -0.1 + - 0.0045000000000000005 + upper: + - 0.1 + - 0.1 + - 0.0675 + sphere_center: + - 0 + - 0 + - 0.088 + sphere_radius: 0.093 + extra: + shape: + - 1 + dataset: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/env.newton.yaml new file mode 100644 index 000000000..2a1e9fa22 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/env.newton.yaml @@ -0,0 +1,116 @@ +environment_id: pour_cup +physics: newton +max_episodes: 1 +max_episode_steps: 1400 +num_envs: 1 +arena_space: 2.5 + +physics_config: + physics_dt: 0.005 + gravity: [0.0, 0.0, -9.81] + # Each 5 ms physics step contains fifty 0.1 ms MuJoCo solver steps. + num_substeps: 50 + collision_cfg: null + solver_cfg: + solver_type: mujoco_warp + solver: newton + integrator: implicitfast + iterations: 50 + ls_iterations: 100 + cone: elliptic + impratio: 1000.0 + use_mujoco_contacts: true + enable_multiccd: true + +simulation: + light: + direct: + - uid: main_light + light_type: sun + color: [1.0, 1.0, 1.0] + intensity: 4.0 + direction: [-0.3, 0.2, -1.0] + - uid: fill_light + light_type: rect + color: [1.0, 1.0, 1.0] + intensity: 6.0 + init_pos: [-0.5, -0.4, 1.2] + direction: [0.0, 0.0, -1.0] + rect_width: 0.7 + rect_height: 0.7 + background: + - uid: cup_platform + shape: + shape_type: Cube + size: [0.4, 0.5, 0.5] + body_type: static + init_pos: [-0.9, 0.0, 0.25] + attrs: + collision_props: + backend: newton + condim: 6 + material_props: + backend: newton + dynamic_friction: 1.0 + ke: 400000.0 + kd: 1265.0 + torsional_friction: 0.1 + rolling_friction: 0.03 + rigid_object: + - uid: cup + shape: + shape_type: Mesh + fpath: PourCupAssets/cup.glb + compute_uv: true + collision: + approximation: convex_decomposition + max_hulls: 32 + body_type: dynamic + init_pos: [-0.75, 0.0, 0.552] + body_scale: [1.0, 1.0, 1.0] + attrs: + mass_props: + mass: 0.30 + collision_props: + backend: newton + contact_offset: 0.0005 + rest_offset: 0.0001 + condim: 6 + material_props: + backend: newton + dynamic_friction: 1.2 + ke: 400000.0 + kd: 1265.0 + torsional_friction: 0.1 + rolling_friction: 0.03 + rigid_object_group: [] + articulation: [] + +env: + sim_steps_per_control: 10 + control_parts: [arm, eef] + events: + settle_cup: + func: wait_for_dynamic_objects_to_settle + mode: reset + params: + entity_cfgs: + - uid: cup + min_steps: 10 + max_steps: 120 + check_interval_steps: 2 + required_stable_checks: 3 + timeout_behavior: raise + record_demo: + func: record_camera_data + mode: interval + interval_step: 1 + params: + name: franka_newton_smooth_pour_mug + resolution: [1280, 960] + intrinsics: [1000.0, 1000.0, 640.0, 480.0] + eye: [-1.4, -1.5, 1.15] + target: [-0.4, 0.0, 0.45] + up: [0.0, 0.0, 1.0] + save_path: outputs/franka_pour_cup/videos + dataset: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/env.yaml new file mode 100644 index 000000000..001b92002 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/env.yaml @@ -0,0 +1,90 @@ +environment_id: pour_cup +physics: default +max_episodes: 1 +max_episode_steps: 1200 +num_envs: 1 +arena_space: 2.5 + +physics_config: + physics_dt: 0.005 + gravity: [0.0, 0.0, -9.81] + enable_ccd: true + +simulation: + light: + direct: + - uid: main_light + light_type: sun + color: [1.0, 1.0, 1.0] + intensity: 4.0 + direction: [-0.3, 0.2, -1.0] + - uid: fill_light + light_type: rect + color: [1.0, 1.0, 1.0] + intensity: 6.0 + init_pos: [-0.5, -0.4, 1.2] + direction: [0.0, 0.0, -1.0] + rect_width: 0.7 + rect_height: 0.7 + background: [] + rigid_object: + - uid: cup + shape: + shape_type: Mesh + fpath: PourCupAssets/cup.glb + compute_uv: true + collision: + approximation: convex_decomposition + max_hulls: 32 + body_type: dynamic + init_pos: [-0.485, -0.08, 0.052] + body_scale: [1.0, 1.0, 1.0] + attrs: + mass_props: + mass: 0.30 + rigid_props: + linear_damping: 0.2 + angular_damping: 0.2 + min_position_iters: 64 + min_velocity_iters: 16 + max_depenetration_velocity: 1.0 + collision_props: + contact_offset: 0.0005 + rest_offset: 0.0001 + torsional_patch_radius: 0.009 + min_torsional_patch_radius: 0.008 + material_props: + dynamic_friction: 1.2 + static_friction: 1.2 + restitution: 0.0 + rigid_object_group: [] + articulation: [] + +env: + sim_steps_per_control: 10 + control_parts: [arm, eef] + events: + settle_cup: + func: wait_for_dynamic_objects_to_settle + mode: reset + params: + entity_cfgs: + - uid: cup + min_steps: 10 + max_steps: 120 + check_interval_steps: 2 + required_stable_checks: 3 + timeout_behavior: raise + record_demo: + func: record_camera_data + mode: interval + interval_step: 1 + params: + name: franka_side_pour_mug + resolution: [1280, 960] + intrinsics: [1000.0, 1000.0, 640.0, 480.0] + eye: [-1.1, -1.25, 0.85] + target: [-0.22, -0.02, 0.32] + up: [0.0, 0.0, 1.0] + save_path: outputs/franka_pour_cup/videos + dataset: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml new file mode 100644 index 000000000..83b655949 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml @@ -0,0 +1,26 @@ +id: PourCup-Franka-Fluid-v1 +seed: 0 +enable_sensor: false +environment: + component: env.fluid.yaml +task_program: + program: task_program/program.fluid.yaml + integration: task_program/integration.fluid.yaml + execution_policy: ../../../../components/execution_policies/trajectory_open_loop_fluid_pour.yaml +embodiment: + component: ../../../../components/embodiments/franka_panda_firm_grip_newton.yaml + overrides: + init_rot: + - 0.0 + - 0.0 + - 180.0 + init_qpos: + - 0.0023885604459792376 + - -0.09648692607879639 + - -0.0022385704796761274 + - -2.388152599334717 + - 0.00024301925441250205 + - 3.862462282180786 + - 0.7850000262260437 + - 0.04 + - 0.04 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.newton.yaml new file mode 100644 index 000000000..675b375c2 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.newton.yaml @@ -0,0 +1,26 @@ +id: PourCup-Franka-Newton-v1 +seed: 0 +enable_sensor: false +environment: + component: env.newton.yaml +task_program: + program: task_program/program.newton.yaml + integration: task_program/integration.newton.yaml + execution_policy: ../../../../components/execution_policies/trajectory_open_loop_gentle.yaml +embodiment: + component: ../../../../components/embodiments/franka_panda_firm_grip_newton.yaml + overrides: + init_rot: + - 0.0 + - 0.0 + - 180.0 + init_qpos: + - 0.0023885604459792376 + - -0.09648692607879639 + - -0.0022385704796761274 + - -2.388152599334717 + - 0.00024301925441250205 + - 3.862462282180786 + - 0.7850000262260437 + - 0.04 + - 0.04 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.yaml new file mode 100644 index 000000000..c4d180c8f --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.yaml @@ -0,0 +1,16 @@ +id: PourCup-Franka-v1 +seed: 0 +enable_sensor: false +environment: + component: env.yaml +task_program: + program: task_program/program.yaml + integration: task_program/integration.yaml + execution_policy: ../../../../components/execution_policies/trajectory_open_loop_pour_cup.yaml +embodiment: + component: ../../../../components/embodiments/franka_panda_firm_grip.yaml + overrides: + init_rot: + - 0.0 + - 0.0 + - 180.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/integration.fluid.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/integration.fluid.yaml new file mode 100644 index 000000000..9b247f142 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/integration.fluid.yaml @@ -0,0 +1,97 @@ +integration_id: pour_cup_fluid_franka_v1 +program_id: pick_aim_pour_return_cup +requires: + scene_contract: pour_cup_fluid_scene_v1 + embodiment_contract: single_arm_parallel_gripper +scene_binding: + contract_id: pour_cup_fluid_scene_v1 + registry_id: task_program_pour_cup_fluid + rigid_objects: + - entity_id: cup + simulation_uid: cup + dynamics: dynamic + semantic_type: cup + affordances: + - entity_id: cup_grasp + kind: antipodal_grasp + internal_axis: + - 1.0 + - 0.0 + - 0.0 + - entity_id: receiver + simulation_uid: receiver + dynamics: static + semantic_type: bowl +profile: + defaults: + pick_up: + primary: primary_manipulator + pour: + primary: primary_manipulator + place: + primary: primary_manipulator + move_held_object: + primary: primary_manipulator + action_options: + pick: + kind: pick_up + hand_interp_steps: 20 + grasp_settle_steps: 20 + lift_height: 0.12 + pre_grasp_distance: 0.08 + approach_direction: + - -1.0 + - 0.0 + - 0.0 + fixed_object_to_eef: + - 0.0 + - 0.0 + - -1.0 + - 0.065 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + simulation.pour: + kind: pour + rotate_angle: 1.3962634015954636 + place: + kind: place + hand_interp_steps: 20 + release_settle_steps: 20 + preserve_current_object_orientation: false + simulation.move_held_object: + kind: move_held_object + effect_monitors: {} +runtime_services: + registered_semantic_lowerers: + - kind: move_held_object + target_id: receiver_pour_pose + reference_entity_id: receiver + relative_pose: + - 1.0 + - 0.0 + - 0.0 + - 0.005 + - 0.0 + - 1.0 + - 0.0 + - 0.09 + - 0.0 + - 0.0 + - 1.0 + - 0.17 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - kind: pour + object_id: cup diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/integration.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/integration.newton.yaml new file mode 100644 index 000000000..43db3b170 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/integration.newton.yaml @@ -0,0 +1,67 @@ +integration_id: pour_cup_franka_newton_v1 +program_id: pick_pour_return_cup + +requires: + scene_contract: pour_cup_scene_v1 + embodiment_contract: single_arm_parallel_gripper + +scene_binding: + contract_id: pour_cup_scene_v1 + registry_id: task_program_pour_cup + rigid_objects: + - entity_id: cup + simulation_uid: cup + dynamics: dynamic + semantic_type: cup + affordances: + - entity_id: cup_grasp + kind: antipodal_grasp + internal_axis: [1.0, 0.0, 0.0] + +profile: + defaults: + pick_up: + primary: primary_manipulator + pour: + primary: primary_manipulator + place: + primary: primary_manipulator + action_options: + pick: + kind: pick_up + hand_interp_steps: 20 + grasp_settle_steps: 20 + lift_height: 0.12 + pre_grasp_distance: 0.08 + approach_direction: [-1.0, 0.0, 0.0] + fixed_object_to_eef: + - 0.0 + - 0.0 + - -1.0 + - 0.065 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + simulation.pour: + kind: pour + rotate_angle: 1.3962634015954636 + place: + kind: place + hand_interp_steps: 20 + release_settle_steps: 20 + preserve_current_object_orientation: false + effect_monitors: {} + +runtime_services: + registered_semantic_lowerers: + - kind: pour + object_id: cup diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/integration.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/integration.yaml new file mode 100644 index 000000000..50308ae4f --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/integration.yaml @@ -0,0 +1,67 @@ +integration_id: pour_cup_franka_v1 +program_id: pick_pour_return_cup + +requires: + scene_contract: pour_cup_scene_v1 + embodiment_contract: single_arm_parallel_gripper + +scene_binding: + contract_id: pour_cup_scene_v1 + registry_id: task_program_pour_cup + rigid_objects: + - entity_id: cup + simulation_uid: cup + dynamics: dynamic + semantic_type: cup + affordances: + - entity_id: cup_grasp + kind: antipodal_grasp + internal_axis: [1.0, 0.0, 0.0] + +profile: + defaults: + pick_up: + primary: primary_manipulator + pour: + primary: primary_manipulator + place: + primary: primary_manipulator + action_options: + pick: + kind: pick_up + hand_interp_steps: 20 + grasp_settle_steps: 20 + lift_height: 0.20 + pre_grasp_distance: 0.12 + approach_direction: [-1.0, 0.0, 0.0] + fixed_object_to_eef: + - 0.0 + - 0.0 + - -1.0 + - 0.065 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + simulation.pour: + kind: pour + rotate_angle: 1.3962634015954636 + place: + kind: place + hand_interp_steps: 20 + release_settle_steps: 20 + preserve_current_object_orientation: false + effect_monitors: {} + +runtime_services: + registered_semantic_lowerers: + - kind: pour + object_id: cup diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/program.fluid.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/program.fluid.yaml new file mode 100644 index 000000000..2aa890ee6 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/program.fluid.yaml @@ -0,0 +1,96 @@ +program_id: pick_aim_pour_return_cup +targets: + cup_return_pose: + kind: cyclic_pose + values: + - position: + - -0.75 + - 0.0 + - 0.552 + quaternion_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + cup_lift_pose: + kind: cyclic_pose + values: + - position: + - -0.75 + - 0.0 + - 0.672 + quaternion_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 + cup_pour_pose: + kind: cyclic_pose + values: + - position: + - -0.75 + - -0.06 + - 0.67 + quaternion_xyzw: + - 0.0 + - 0.0 + - 0.0 + - 1.0 +program: + kind: sequence + items: + - kind: segment + name: pick_cup + steps: + kind: invoke + call: + kind: pick + object: cup + grasp: cup_grasp + validators: + - kind: object_near_target + object: cup + target: cup_lift_pose + position_tolerance: 0.03 + - kind: segment + name: aim_cup_above_bowl + steps: + kind: invoke + call: + kind: registered + call_id: simulation.move_held_object + arguments: + target: receiver_pour_pose + validators: + - kind: object_near_target + object: cup + target: cup_pour_pose + position_tolerance: 0.03 + - kind: segment + name: pour_water + steps: + kind: invoke + call: + kind: registered + call_id: simulation.pour + arguments: + object: cup + - kind: segment + name: return_cup + steps: + kind: invoke + call: + kind: place + object: cup + at: + kind: target_ref + target: cup_return_pose + post: + - kind: wait_stable + entity: cup + validators: + - kind: object_near_target + object: cup + target: cup_return_pose + position_tolerance: 0.03 +instruction: Pour water from the cup into the bowl and return the cup to the table. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/program.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/program.newton.yaml new file mode 100644 index 000000000..1f77d32e4 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/program.newton.yaml @@ -0,0 +1,40 @@ +program_id: pick_pour_return_cup +targets: + cup_return_pose: + kind: cyclic_pose + values: + - position: [-0.75, 0.0, 0.552] + quaternion_xyzw: [0.0, 0.0, 0.0, 1.0] + +program: + kind: segment + name: pick_pour_return_cup + steps: + kind: sequence + items: + - kind: invoke + call: + kind: pick + object: cup + grasp: cup_grasp + - kind: invoke + call: + kind: registered + call_id: simulation.pour + arguments: + object: cup + - kind: invoke + call: + kind: place + object: cup + at: + kind: target_ref + target: cup_return_pose + post: + - kind: wait_stable + entity: cup + validators: + - kind: object_near_target + object: cup + target: cup_return_pose + position_tolerance: 0.03 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/program.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/program.yaml new file mode 100644 index 000000000..18bf4cb97 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/program.yaml @@ -0,0 +1,40 @@ +program_id: pick_pour_return_cup +targets: + cup_return_pose: + kind: cyclic_pose + values: + - position: [-0.485, -0.08, 0.052] + quaternion_xyzw: [0.0, 0.0, 0.0, 1.0] + +program: + kind: segment + name: pick_pour_return_cup + steps: + kind: sequence + items: + - kind: invoke + call: + kind: pick + object: cup + grasp: cup_grasp + - kind: invoke + call: + kind: registered + call_id: simulation.pour + arguments: + object: cup + - kind: invoke + call: + kind: place + object: cup + at: + kind: target_ref + target: cup_return_pose + post: + - kind: wait_stable + entity: cup + validators: + - kind: object_near_target + object: cup + target: cup_return_pose + position_tolerance: 0.03 diff --git a/scripts/tutorials/sim/franka_dexuni_pour_demo.py b/scripts/tutorials/sim/franka_dexuni_pour_demo.py new file mode 100644 index 000000000..f3665eb35 --- /dev/null +++ b/scripts/tutorials/sim/franka_dexuni_pour_demo.py @@ -0,0 +1,337 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Drive a live Franka grasp/pour/place with DexUni and one-way DFSPH. + +DexUni advances the MuJoCo-owned robot and cup and then solves the water +against their live boundaries. Coupling is one-way. + +Defaults: DFSPH at 480 Hz (two steps per 1/240 s physics step) with 15 MuJoCo +substeps each (7,200 Hz). The task config supplies the cup contact settings. +Requires a DexSim build with ``DexUniSolverCfg.sph_cfg``. +""" + +from __future__ import annotations + +import argparse +import json +from pathlib import Path +import time + +import imageio.v2 as imageio +import numpy as np +import torch + +from embodichain.lab.scripts.run_fluid_task_program import ( + _configure, + _cup_qualification, +) +from embodichain.lab.sim import SimulationManager +from embodichain.lab.sim.atomic_actions import ( + AxisAlignAffordance, + ControlPartCommandProfile, + GraspGoal, + HeldObjectPoseGoal, + MoveHeldObjectOptions, + MotionPolicy, + ObjectSemantics, + PickUpOptions, + PlaceGoal, + PlaceOptions, + PourGoal, + PourOptions, + create_simulation_atomic_action_engine, +) +from embodichain.lab.sim.motion.motion_generator import MotionGenerator, MotionGenCfg +from embodichain.lab.sim.motion.planners import ToppraPlannerCfg +from embodichain.lab.sim.sensors import CameraCfg + +__all__ = ["main"] + +ROOT = Path(__file__).resolve().parents[3] +TASK = ( + ROOT + / "extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml" +) + + +def _use_dexuni(cfg, args) -> None: + """Apply the requested DexUni and DFSPH substep counts.""" + physics = cfg.sim_cfg.physics_cfg + solver = dict(physics.solver_cfg) + physics.num_substeps = args.fluid_substeps + solver["sph_rigid_substeps"] = args.rigid_substeps + physics.solver_cfg = solver + + +def main() -> None: + """Plan fresh joint commands and solve the robot, cup and fluid online.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--output", type=Path, default=ROOT / "outputs/fluid-dexuni-online" + ) + parser.add_argument("--spacing", type=float, default=0.0015) + parser.add_argument("--seconds", type=float) + parser.add_argument("--num-envs", type=int, default=1) + parser.add_argument( + "--renderer", choices=["fast-rt", "no-render"], default="fast-rt" + ) + parser.add_argument("--headless", action="store_true") + parser.add_argument( + "--fluid-substeps", + type=int, + default=2, + help="DFSPH steps per 1/240 s physics step", + ) + parser.add_argument( + "--rigid-substeps", + type=int, + default=15, + help="MuJoCo substeps per DFSPH step", + ) + args = parser.parse_args() + if args.fluid_substeps < 1 or args.rigid_substeps < 1: + parser.error("substep counts must be positive") + if args.seconds is not None and ( + not np.isfinite(args.seconds) or args.seconds <= 0 + ): + parser.error("seconds must be finite and positive") + output = args.output.resolve() + output.mkdir(parents=True, exist_ok=True) + _, cfg = _configure( + TASK, + num_envs=args.num_envs, + renderer=args.renderer, + output=output, + particle_spacing=args.spacing, + ) + _use_dexuni(cfg, args) + cfg.sim_cfg.num_envs = cfg.num_envs + sim = SimulationManager(cfg.sim_cfg) + writers = [] + robot = cup = camera = engine = None + try: + robot = sim.add_robot(cfg.robot) + for object_cfg in [*cfg.background, *cfg.rigid_object]: + sim.add_rigid_object(object_cfg) + for fluid_cfg in cfg.fluid_object: + sim.add_fluid_object(fluid_cfg) + if sim.has_native_renderer: + for light_cfg in cfg.light.direct: + sim.add_light(light_cfg) + sim.prepare() + robot.build_pk_serial_chain() + sim.reset_objects_state() + cup = sim.get_rigid_object("cup") + # Live gravity/contact settling before observing the planning scene. + sim.update(step=24, render_final_step=False) + initial_pose = cup.get_local_pose(to_matrix=True).clone() + fill_pose = cup.get_local_pose().clone() + fill_pose[:, :3] += sim.arena_offsets + sim.get_fluid_object("water").reset_to_pose(fill_pose) + object_to_eef = torch.tensor( + [[0, 0, -1, 0.065], [0, 1, 0, 0], [1, 0, 0, 0], [0, 0, 0, 1]], + dtype=torch.float32, + device=sim.device, + ) + semantics = ObjectSemantics( + entity_id="cup", + label="cup", + geometry={}, + affordance=AxisAlignAffordance( + mesh_vertices=cup.get_vertices(env_ids=[0], scale=True)[0], + mesh_triangles=cup.get_triangles(env_ids=[0])[0], + internal_axis=torch.tensor([1.0, 0, 0], device=sim.device), + ), + ) + motion = MotionGenerator( + MotionGenCfg( + planner_cfg=ToppraPlannerCfg( + robot_uid=robot.uid, sim_instance_id=sim.instance_id + ) + ) + ) + engine = create_simulation_atomic_action_engine( + motion, + (cup,), + control_profiles={ + "eef": ControlPartCommandProfile.joint_positions( + open=torch.tensor([0.04, 0.04], device=sim.device), + grasp=torch.zeros(2, device=sim.device), + ) + }, + ) + bindings = {"primary": {"motion": "arm", "grasp": "eef"}} + policy = MotionPolicy( + strategy="ik_interp", sample_count=160, dynamic_collision_mode="off" + ) + receiver_pose = sim.get_rigid_object("receiver").get_local_pose(to_matrix=True) + receiver_to_cup = torch.eye(4, device=sim.device) + receiver_to_cup[:3, 3] = torch.tensor([0.005, 0.09, 0.17], device=sim.device) + pour_pose = receiver_pose @ receiver_to_cup + invocations = ( + engine.make_invocation( + "pick_up", + GraspGoal(semantics), + control_parts=bindings, + motion_policy=policy, + skill_options=PickUpOptions( + fixed_object_to_eef=object_to_eef, + approach_direction=torch.tensor([-1.0, 0, 0], device=sim.device), + pre_grasp_distance=0.08, + lift_height=0.12, + hand_interp_steps=20, + grasp_settle_steps=20, + ), + ), + engine.make_invocation( + "move_held_object", + HeldObjectPoseGoal(pour_pose), + control_parts=bindings, + motion_policy=policy, + skill_options=MoveHeldObjectOptions(), + ), + engine.make_invocation( + "pour", + PourGoal(), + control_parts=bindings, + motion_policy=policy, + skill_options=PourOptions(rotate_angle=np.deg2rad(80)), + ), + engine.make_invocation( + "place", + PlaceGoal(initial_pose @ object_to_eef), + control_parts=bindings, + motion_policy=policy, + skill_options=PlaceOptions( + hand_interp_steps=20, release_settle_steps=20 + ), + ), + ) + control_dt = cfg.sim_steps_per_control * sim.sim_config.physics_dt + plan = engine.compile( + invocations, engine.initial_context(control_dt=control_dt) + ) + if not bool(plan.plan_success.all()): + raise RuntimeError("Atomic Action planning failed.") + if args.renderer != "no-render": + params = cfg.events.record_camera.params + camera = sim.add_sensor( + CameraCfg( + uid="fluid_sim_record", + width=params["resolution"][0], + height=params["resolution"][1], + intrinsics=params["intrinsics"], + visualization_role="record", + extrinsics=CameraCfg.ExtrinsicsCfg( + eye=params["eye"], target=params["target"], up=params["up"] + ), + ) + ) + writers = [ + imageio.get_writer( + output / f"fluid_sim_env{i}.mp4", + fps=1 / control_dt, + codec="libx264", + quality=8, + macro_block_size=1, + ) + for i in range(sim.num_envs) + ] + if not args.headless: + sim.open_window() + positions = plan.trajectory.positions + poses = [cup.get_local_pose().cpu().numpy().copy()] + samples = [] + started = time.perf_counter() + count = positions.shape[1] + round(2 / control_dt) + if args.seconds is not None: + count = min(count, int(np.ceil(args.seconds / control_dt))) + for i in range(count): + # Set targets only. Every measured joint/cup pose comes from physics. + robot.set_qpos(positions[:, min(i, positions.shape[1] - 1)], target=True) + sim.update(step=cfg.sim_steps_per_control, render_final_step=False) + poses.append(cup.get_local_pose().cpu().numpy().copy()) + if camera is not None: + with sim.render_frame(): + sim.sync_render_state() + camera.update() + rgb = camera.get_data()["color"][..., :3].cpu().numpy() + for env_id, writer in enumerate(writers): + writer.append_data(rgb[env_id]) + if i % round(1 / control_dt) == 0 or i == count - 1: + fluid = sim.get_fluid_object("water") + state = fluid.data.particle_state_w + if not bool(torch.isfinite(state).all()): + raise RuntimeError("Non-finite online fluid state.") + local = state[..., :3] - sim.arena_offsets[:, None, :] + receiver_pose = sim.get_rigid_object("receiver").get_local_pose( + to_matrix=True + ) + receiver = torch.bmm( + local - receiver_pose[:, None, :3, 3], receiver_pose[:, :3, :3] + ) + received = ( + (receiver[..., 2] > 0.0045) + & (receiver[..., 2] < 0.0675) + & ( + torch.linalg.vector_norm( + receiver - torch.tensor([0, 0, 0.088], device=sim.device), + dim=-1, + ) + < 0.093 + ) + ) + sample = { + "time_s": (i + 1) * control_dt, + "cup_pose": poses[-1].tolist(), + "received_ml": (received.sum(dim=1) * args.spacing**3 * 1e6) + .cpu() + .tolist(), + } + samples.append(sample) + print(json.dumps(sample), flush=True) + report = { + "coupling": "online one-way rigid-to-fluid", + "controller": "fresh Atomic Action joint targets", + "cup": _cup_qualification(poses), + "num_envs": sim.num_envs, + "particle_count_per_env": sim.get_fluid_object("water").num_particles, + "control_dt": control_dt, + "simulated_seconds": count * control_dt, + "wall_seconds": time.perf_counter() - started, + "samples": samples, + } + report["accepted"] = bool( + all(report["cup"]["accepted"]) + and all(v >= 50 for v in samples[-1]["received_ml"]) + ) + (output / "validation.json").write_text(json.dumps(report, indent=2) + "\n") + print(json.dumps(report["cup"]), flush=True) + if args.seconds is None and not report["accepted"]: + raise RuntimeError( + "Online Sim pouring failed physical qualification; inspect validation.json." + ) + finally: + for writer in writers: + writer.close() + robot = cup = camera = engine = None + sim.destroy(exit_process=False) + del sim + SimulationManager.flush_cleanup_queue() + + +if __name__ == "__main__": + main() diff --git a/tests/data/assets/test_task_assets.py b/tests/data/assets/test_task_assets.py new file mode 100644 index 000000000..65bdfce98 --- /dev/null +++ b/tests/data/assets/test_task_assets.py @@ -0,0 +1,102 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Task-asset descriptors, cache selection and public registry contracts.""" + +from __future__ import annotations + +from argparse import Namespace +from pathlib import Path + +import open3d as o3d +import pytest + +from embodichain.data import download +from embodichain.data.assets import task_assets +from embodichain.data.dataset import EmbodiChainDataset, get_data_class + +pytestmark = pytest.mark.no_sim + +_HUB = "https://huggingface.co/datasets/DexForceAI/embodichain_data/resolve/main/" +_ASSETS = ( + ("PourCupAssets", "f97857b639189be4daf717a873ab84cf"), + ("PourCupDemoData", "8eb96cb756ac08efe403417f50ad2bf2"), +) + + +@pytest.mark.parametrize("name,md5", _ASSETS) +@pytest.mark.parametrize( + "prefix", [_HUB, "https://hf-mirror.com/assets/", "https://example.test/"] +) +@pytest.mark.parametrize("custom_root", [False, True]) +def test_task_archive_descriptor_uses_configured_source_and_locked_cache( + name: str, + md5: str, + prefix: str, + custom_root: bool, + tmp_path: Path, + monkeypatch: pytest.MonkeyPatch, +) -> None: + calls = [] + + class DownloadIntercepted(Exception): + """Stop before the Open3D base performs network I/O.""" + + def initialize( + self: EmbodiChainDataset, + asset: str, + descriptor: o3d.data.DataDescriptor, + root: str, + ) -> None: + calls.append((asset, descriptor, root)) + raise DownloadIntercepted + + default = str(tmp_path / "default") + override = str(tmp_path / "custom") if custom_root else None + monkeypatch.setattr(task_assets, "EMBODICHAIN_DEFAULT_DATA_ROOT", default) + monkeypatch.setattr(task_assets, "EMBODICHAIN_DOWNLOAD_PREFIX", prefix) + monkeypatch.setattr(EmbodiChainDataset, "__init__", initialize) + with pytest.raises(DownloadIntercepted): + getattr(task_assets, name)(data_root=override) + assert len(calls) == 1 + asset, descriptor, root = calls[0] + assert asset == name + assert root == (override or default) + relative = f"task_assets/{name}.zip" + assert descriptor.urls == [f"{prefix}{relative}"] + ( + [] if prefix == _HUB else [f"{_HUB}{relative}"] + ) + assert descriptor.md5 == md5 + + +@pytest.mark.parametrize("name,_", _ASSETS) +def test_task_assets_are_shared_by_config_and_cli_registry( + name: str, _: str, monkeypatch: pytest.MonkeyPatch, capsys +) -> None: + registry = download.get_registry() + cls = get_data_class(name) + assert dict(registry["task"])[name] is cls + assert all(not key.startswith("_") for key, _ in registry["task"]) + download.cmd_list(Namespace(category="task")) + assert name in capsys.readouterr().out + calls = [] + monkeypatch.setattr( + download, + "download_asset", + lambda key, value: calls.append((key, value)) or True, + ) + download.cmd_download(Namespace(all=False, category=None, name=name.lower())) + assert calls == [(name.lower(), cls)] diff --git a/tests/data/test_download_prefix.py b/tests/data/test_download_prefix.py index e487fb52e..f83b02916 100644 --- a/tests/data/test_download_prefix.py +++ b/tests/data/test_download_prefix.py @@ -36,7 +36,7 @@ _COLLECT_URLS = """ import json from embodichain.data import dataset -from embodichain.data.assets import CobotMagicArm, FlexivRizon4S, UnitreeGo2Locomotion, UniversalRobots +from embodichain.data.assets import CobotMagicArm, FlexivRizon4S, UnitreeGo2Locomotion, UniversalRobots, PourCupAssets, PourCupDemoData from embodichain.data.assets.w1_assets import DexforceW1V030 urls = {} @@ -49,7 +49,7 @@ def capture(self, prefix, descriptor, path): raise Captured # stop before anything is downloaded dataset.EmbodiChainDataset.__init__ = capture -for cls in (CobotMagicArm, FlexivRizon4S, UnitreeGo2Locomotion, UniversalRobots, DexforceW1V030): +for cls in (CobotMagicArm, FlexivRizon4S, UnitreeGo2Locomotion, UniversalRobots, DexforceW1V030, PourCupAssets, PourCupDemoData): try: cls() except Captured: @@ -108,6 +108,11 @@ def test_download_prefix_gets_a_trailing_slash(monkeypatch, reload_constants) -> def test_assets_download_from_the_configured_prefix(prefix: str) -> None: urls = _consumer_urls(prefix) assert urls["CobotMagicArm"] == [f"{MIRROR}robot_assets/CobotMagicArmV4.zip"] + for name in ("PourCupAssets", "PourCupDemoData"): + assert urls[name] == [ + f"{MIRROR}task_assets/{name}.zip", + f"{HF_PREFIX}task_assets/{name}.zip", + ] # Locomotion tries the configured source first and keeps the Hub as fallback. assert urls["UnitreeGo2Locomotion"] == [ f"{MIRROR}robot_assets/UnitreeGo2Locomotion.zip", @@ -132,6 +137,8 @@ def test_assets_do_not_repeat_the_hub_url() -> None: assert urls["UnitreeGo2Locomotion"] == [ f"{HF_PREFIX}robot_assets/UnitreeGo2Locomotion.zip" ] + for name in ("PourCupAssets", "PourCupDemoData"): + assert urls[name] == [f"{HF_PREFIX}task_assets/{name}.zip"] assert urls["UniversalRobotsV4"] == [ f"{HF_PREFIX}robot_assets/UniversalRobots_v4.zip" ] diff --git a/tests/gym/envs/managers/test_record.py b/tests/gym/envs/managers/test_record.py index 08c2a22e0..c0104f97c 100644 --- a/tests/gym/envs/managers/test_record.py +++ b/tests/gym/envs/managers/test_record.py @@ -66,6 +66,15 @@ def test_record_camera_is_marked_for_visualization() -> None: assert env.sim.sensor_cfg.visualization_role == "record" +@pytest.mark.parametrize("fps", [0, -1, float("nan"), float("inf")]) +def test_invalid_recording_rate_does_not_create_camera(fps): + env = _Environment() + cfg = FunctorCfg(func=record_camera_data, params={"name": "camera", "fps": fps}) + with pytest.raises(ValueError, match="fps"): + record_camera_data(cfg, env) + assert env.sim.sensor_cfg is None + + @pytest.fixture def camera_env(): context = EpisodeContext(4) @@ -79,12 +88,43 @@ def camera_env(): num_envs=4, device=torch.device("cpu"), episode_context=context, - sim=SimpleNamespace(add_sensor=lambda **kwargs: camera), + sim=SimpleNamespace( + add_sensor=lambda **kwargs: camera, render_camera_group=lambda groups: None + ), add_camera_group_id=lambda group_id: None, ) return env, rgb +def test_camera_recording_uses_control_rate_and_renders_current_state( + camera_env, tmp_path, monkeypatch +): + from embodichain.lab.gym.envs.managers import record + + env, _ = camera_env + env.control_frequency = 40.0 + rendered = [] + env.sim.render_camera_group = rendered.append + rates = [] + + def save(frames, directory, name, fps): + rates.append(fps) + (Path(directory) / (name + ".mp4")).write_bytes(b"video") + + monkeypatch.setattr(record, "images_to_video", save) + recorder = record_camera_data( + FunctorCfg( + func=record_camera_data, + params={"name": "camera", "save_path": str(tmp_path), "fps": None}, + ), + env, + ) + recorder(env, [0], name="camera", fps=None) + recorder.save_and_clear([0]) + assert rendered == [[1]] + assert rates == [40.0] + + @pytest.mark.parametrize("kind", [record_camera_data, record_camera_data_async]) def test_videos_commit_valid_rows_and_keep_other_live_buffers( camera_env, kind, tmp_path, monkeypatch diff --git a/tests/gym/envs/test_fluid_integration.py b/tests/gym/envs/test_fluid_integration.py new file mode 100644 index 000000000..c74fe9367 --- /dev/null +++ b/tests/gym/envs/test_fluid_integration.py @@ -0,0 +1,279 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Fluid Gym configuration, measurement and replay acceptance 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.utils.gym_utils import config_to_cfg +from embodichain.utils.utility import load_config +from embodichain.lab.gym.envs.managers.cfg import SceneEntityCfg +from embodichain.lab.gym.envs.managers.observations import ( + get_fluid_summary, + get_fluid_volume_in_region, +) +from embodichain.lab.sim.cfg import FluidObjectCfg + +pytestmark = pytest.mark.no_sim +ROOT = Path(__file__).resolve().parents[3] +TASK = ( + ROOT + / "extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml" +) + + +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.""" + from types import SimpleNamespace + from embodichain.data import dataset + + directory = tmp_path / "PourCupAssets" + directory.mkdir() + for name in ( + "cup.glb", + "mug_wall.obj", + "receiver_wall.obj", + "fluid_table.obj", + "fluid_floor.obj", + ): + (directory / name).touch() + original = dataset.get_data_class + + def resolve(name, extra_modules=None): + if name == "PourCupAssets": + return lambda: SimpleNamespace(extract_dir=str(directory)) + return original(name, extra_modules) + + monkeypatch.setattr(dataset, "get_data_class", resolve) + return directory + + +def test_fluid_deployment_resolves_sources_and_observation_entities(): + cfg = config_to_cfg(load_config(TASK), source_path=TASK) + assert cfg.sim_cfg.render_cfg.renderer == "fast-rt" + assert cfg.events.record_camera.params["fps"] is None + assert cfg.events.record_camera.interval_step == 1 + assert len(cfg.fluid_object) == 1 + fluid = cfg.fluid_object[0] + assert isinstance(fluid, FluidObjectCfg) + assert fluid.spawn.kind == "revolved_volume" + assert fluid.spawn.file_path is None + assert fluid.spawn.sample(fluid.particle_spacing).shape == (76607, 3) + assert cfg.observations.received_volume.extra["shape"] == [1] + assert all( + isinstance(item, SceneEntityCfg) + for item in cfg.observations.received_volume.params["entity_cfgs"] + ) + cup = next(item for item in cfg.rigid_object if item.uid == "cup") + assert Path(cup.particle_collision_shape.fpath).is_absolute() + assert cup.body_type == "dynamic" + assert cfg.task_program is not None + + +def test_fluid_component_rejects_duplicate_inline_ownership(): + config = load_config(TASK) + config["fluid_object"] = [] + with pytest.raises(ValueError, match="owns|duplicate|must not"): + config_to_cfg(config, source_path=TASK) + + +def test_region_volume_uses_reference_pose_and_keeps_state_unchanged(): + state = torch.tensor( + [ + [[1.0, 2.0, 3.0, 0.0, 0.0, 0.0], [1.5, 2.0, 3.0, 0.0, 0.0, 0.0]], + [[10.0, 2.0, 3.0, 0.0, 0.0, 0.0], [10.5, 2.0, 3.0, 0.0, 0.0, 0.0]], + ] + ) + poses = torch.tensor( + [[1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0], [1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]] + ) + fluid = SimpleNamespace( + data=SimpleNamespace(particle_state_w=state, particle_pos_w=state[..., :3]), + cfg=SimpleNamespace(particle_spacing=0.1), + particle_mass=0.001, + num_particles=2, + nominal_volume=0.002, + ) + rigid = SimpleNamespace(get_local_pose=lambda: poses) + env = SimpleNamespace( + num_envs=2, + device=torch.device("cpu"), + sim=SimpleNamespace( + get_fluid_object=lambda _: fluid, + get_rigid_object=lambda _: rigid, + arena_offsets=torch.tensor([[0.0, 0, 0], [9.0, 0, 0]]), + ), + ) + before = state.clone() + volume = get_fluid_volume_in_region( + env, + {}, + [SceneEntityCfg(uid="water"), SceneEntityCfg(uid="receiver")], + [-0.1] * 3, + [0.1] * 3, + ) + torch.testing.assert_close(volume, torch.full((2, 1), 0.001)) + torch.testing.assert_close( + get_fluid_summary(env, {}, SceneEntityCfg(uid="water"))[:, 4], torch.ones(2) + ) + 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 + + recorded = [] + fluid = SimpleNamespace( + cfg=FluidObjectCfg(uid="water", init_pos=(0, 0, 0.1)), + reset_to_pose=lambda pose, ids: recorded.append((pose, ids)), + ) + poses = torch.eye(4).repeat(2, 1, 1) + poses[:, :3, 3] = torch.tensor([1.0, 2.0, 3.0]) + env = SimpleNamespace( + device=torch.device("cpu"), + sim=SimpleNamespace( + get_fluid_object=lambda _: fluid, + get_rigid_object=lambda _: SimpleNamespace( + get_local_pose=lambda **_: poses + ), + arena_offsets=torch.tensor([[0.0, 0, 0], [9.0, 0, 0]]), + ), + ) + reset_fluid_in_container( + env, [1], [SceneEntityCfg(uid="water"), SceneEntityCfg(uid="cup")] + ) + pose, ids = recorded[0] + torch.testing.assert_close(pose[:, :3], torch.tensor([[10.0, 2.0, 3.1]])) + assert ids.tolist() == [1] diff --git a/tests/gym/envs/test_fluid_replay_runtime.py b/tests/gym/envs/test_fluid_replay_runtime.py new file mode 100644 index 000000000..fc74b2d29 --- /dev/null +++ b/tests/gym/envs/test_fluid_replay_runtime.py @@ -0,0 +1,178 @@ +# ---------------------------------------------------------------------------- +# 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_official_task_layout.py b/tests/gym/envs/test_official_task_layout.py index 72027af4a..af6cbc55e 100644 --- a/tests/gym/envs/test_official_task_layout.py +++ b/tests/gym/envs/test_official_task_layout.py @@ -57,6 +57,7 @@ "blocks_ranking_size", "match_object_container", "place_object_drawer", + "pour_cup", "pour_water", "stack_blocks_two", "stack_cups", diff --git a/tests/gym/utils/test_gym_utils.py b/tests/gym/utils/test_gym_utils.py index d71b3cce7..aefbba5f7 100644 --- a/tests/gym/utils/test_gym_utils.py +++ b/tests/gym/utils/test_gym_utils.py @@ -2643,6 +2643,43 @@ def test_launcher_can_explicitly_disable_configured_headless() -> None: assert merged["headless"] is False +@pytest.mark.parametrize( + "collection", ["background", "rigid_object", "rigid_object_group"] +) +def test_mesh_asset_paths_resolve_collision_and_visual_geometry( + collection: str, tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + """Separate render meshes use the same asset resolver as collision meshes.""" + paths = { + "TaskMeshes/collision.obj": str(tmp_path / "collision.obj"), + "TaskMeshes/visual.glb": str(tmp_path / "visual.glb"), + } + monkeypatch.setattr("embodichain.data.get_data_path", paths.__getitem__) + obj = { + "uid": "cup", + "shape": {"shape_type": "Mesh", "fpath": "TaskMeshes/collision.obj"}, + "visual_shape": {"shape_type": "Mesh", "fpath": "TaskMeshes/visual.glb"}, + } + entries = [obj] + if collection == "rigid_object_group": + entries = [{"uid": "cups", "rigid_objects": {"cup": obj}}] + cfg = config_to_cfg( + { + "id": "EmbodiedEnv-v1", + "physics": "default", + "env": {}, + "robot": {"uid": "robot"}, + collection: entries, + }, + manager_modules=DEFAULT_MANAGER_MODULES, + ) + parsed = getattr(cfg, collection)[0] + if collection == "rigid_object_group": + parsed = parsed.rigid_objects["cup"] + assert parsed.shape.fpath == paths["TaskMeshes/collision.obj"] + assert parsed.visual_shape.fpath == paths["TaskMeshes/visual.glb"] + + def test_config_to_cfg_loads_render_only_objects(): """The production Gym loader keeps visuals outside the physical registries.""" from embodichain.lab.sim.cfg import VisualObjectCfg diff --git a/tests/lab/scripts/test_run_fluid_task_program.py b/tests/lab/scripts/test_run_fluid_task_program.py new file mode 100644 index 000000000..ad58987eb --- /dev/null +++ b/tests/lab/scripts/test_run_fluid_task_program.py @@ -0,0 +1,178 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Online fluid deployment and measured physical qualification.""" + +from __future__ import annotations + +from pathlib import Path +import numpy as np +import pytest + +from embodichain.lab.scripts.run_fluid_task_program import ( + _configure, + _cup_qualification, +) + +pytestmark = pytest.mark.no_sim +ROOT = Path(__file__).resolve().parents[3] +TASK = ( + ROOT + / "extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml" +) + + +@pytest.fixture(autouse=True) +def task_mesh_cache(tmp_path, monkeypatch): + """Resolve task meshes locally so configuration tests never download them.""" + from types import SimpleNamespace + from embodichain.data import dataset + + directory = tmp_path / "PourCupAssets" + directory.mkdir() + for name in ( + "cup.glb", + "mug_wall.obj", + "receiver_wall.obj", + "fluid_table.obj", + "fluid_floor.obj", + ): + (directory / name).touch() + original = dataset.get_data_class + + def resolve(name, extra_modules=None): + if name == "PourCupAssets": + return lambda: SimpleNamespace(extract_dir=str(directory)) + return original(name, extra_modules) + + monkeypatch.setattr(dataset, "get_data_class", resolve) + return directory + + +def test_online_task_uses_live_grasp_and_same_body_particle_boundary(tmp_path): + task_id, cfg = _configure(TASK, num_envs=2, renderer="fast-rt", output=tmp_path) + assert task_id == "PourCup-Franka-Fluid-v1" + assert cfg.task_program is not None + cup = next(c for c in cfg.rigid_object if c.uid == "cup") + assert cup.body_type == "dynamic" + assert Path(cup.particle_collision_shape.fpath).is_absolute() + assert cup.particle_collision_shape.collision.approximation == "sdf" + assert cup.shape.collision.approximation == "convex_decomposition" + solver = cfg.sim_cfg.physics_cfg.solver_cfg + assert solver["solver_type"] == "dexuni" + assert solver["sph_cfg"]["sph_method"] == "dfsph" + assert solver["sph_rigid_substeps"] == 15 + assert cfg.events.record_camera.params["max_env_num"] == 2 + assert cfg.record_trajectory is False + assert cfg.control_parts is None and cfg.active_joint_ids == [] + assert cfg.sim_steps_per_control * cfg.sim_cfg.physics_dt == pytest.approx(0.025) + assert not hasattr(cfg.events, "settle_cup") + + +def test_no_render_coarse_override_resamples_and_keeps_physical_scale(tmp_path): + _, cfg = _configure( + TASK, num_envs=1, renderer="no-render", output=tmp_path, particle_spacing=0.003 + ) + fluid = cfg.fluid_object[0] + assert fluid.render.mode == "none" + assert fluid.particle_radius == pytest.approx(0.0009) + assert fluid.render.mesh_surface["kernel_radius"] == pytest.approx(0.0045) + assert cfg.sim_cfg.physics_cfg.solver_cfg["sph_cfg"][ + "particle_support_radius" + ] == pytest.approx(0.00675) + assert cfg.events.record_camera is None + + +def test_physical_qualification_does_not_accept_motion_commands_alone(): + initial = np.array([[0, 0, 0.5, 0, 0, 0, 1], [1, 0, 0.5, 0, 0, 0, 1]], dtype=float) + moving = initial.copy() + moving[0, 2] += 0.12 + moving[0, 3] = np.sin(np.deg2rad(35)) + moving[0, 6] = np.cos(np.deg2rad(35)) + result = _cup_qualification([initial, moving, initial]) + assert result["accepted"] == [True, False] + assert result["max_tilt_deg"] == pytest.approx([70, 0]) + assert result["lift_m"] == pytest.approx([0.12, 0]) + + +@pytest.mark.parametrize( + "renderer,stale", [("fast-rt", False), ("fast-rt", True), ("no-render", False)] +) +def test_online_runner_commits_last_recording_before_close( + renderer, stale, tmp_path, monkeypatch +): + import argparse + from types import SimpleNamespace + from unittest.mock import Mock + import torch + from embodichain.lab.scripts import run_fluid_task_program as runner + + order = [] + + def save_recording(*, purpose): + order.append("save") + assert purpose == "diagnostic" + if stale: + return + for row in range(2): + directory = tmp_path / "videos" / "diagnostics" / f"env_{row:03d}" + directory.mkdir(parents=True) + (directory / "episode_000000_fluid_gym.mp4").touch() + + if stale: + directory = tmp_path / "videos" / "diagnostics" / "env_000" + directory.mkdir(parents=True) + (directory / "episode_000000_fluid_gym.mp4").touch() + recorder = SimpleNamespace(save_and_clear=Mock(side_effect=save_recording)) + env = SimpleNamespace( + num_envs=2, + step_dt=0.05, + control_frequency=20, + elapsed_steps=torch.zeros(2), + reset=Mock(), + event_manager=SimpleNamespace(get_functor=lambda name: recorder), + sim=SimpleNamespace( + get_rigid_object=lambda uid: SimpleNamespace( + get_local_pose=lambda: torch.tensor([[0.0, 0, 0, 0, 0, 0, 1]]).repeat( + 2, 1 + ) + ), + get_fluid_object=lambda uid: SimpleNamespace(num_particles=1), + ), + close=Mock(side_effect=lambda **kwargs: order.append("close")), + ) + result = SimpleNamespace( + completed=False, all_success=False, to_metadata=lambda: {"completed": False} + ) + monkeypatch.setattr(runner, "make", lambda *args, **kwargs: env) + monkeypatch.setattr(runner, "discover_task_packages", lambda: None) + monkeypatch.setattr(runner, "execute_demo_episode", lambda *args, **kwargs: result) + monkeypatch.setattr(runner, "_sample", lambda env: {"received_ml": [0.0, 0.0]}) + monkeypatch.setattr(runner.SimulationManager, "flush_cleanup_queue", lambda: None) + args = argparse.Namespace( + task=str(TASK), + num_envs=2, + renderer=renderer, + particle_spacing=None, + record_trajectory=False, + seconds=0.05, + settle_seconds=0, + ) + report = runner._run(args, tmp_path) + assert report["accepted"] is False + 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() diff --git a/tests/sim/objects/test_fluid_batch_runtime.py b/tests/sim/objects/test_fluid_batch_runtime.py new file mode 100644 index 000000000..9410edc9a --- /dev/null +++ b/tests/sim/objects/test_fluid_batch_runtime.py @@ -0,0 +1,127 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Batch fluid declarations and row-local reset through the public manager.""" + +from __future__ import annotations + +from pathlib import Path +import subprocess +import sys +import pytest +import torch +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import ( + FluidObjectCfg, + FluidSpawnCfg, + FluidRenderCfg, + NewtonPhysicsCfg, + RenderCfg, +) + +pytestmark = [pytest.mark.gpu, pytest.mark.subprocess_sim] + + +def _run_fluid_partial_reset(): + 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") + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + device="cuda", + num_envs=2, + arena_space=1, + physics_dt=0.001, + render_cfg=RenderCfg(renderer="no-render"), + physics_cfg=NewtonPhysicsCfg( + num_substeps=1, + collision_cfg=None, + solver_cfg={ + "solver_type": "dexuni", + "sph_cfg": { + "sph_method": "dfsph", + "volume_map_boundary_enabled": False, + }, + }, + ), + ) + ) + fluid = None + try: + fluid = sim.add_fluid_object( + FluidObjectCfg( + uid="water", + particle_spacing=0.01, + init_pos=(0.0, 0.0, 1.0), + spawn=FluidSpawnCfg(kind="box", size=(0.04,) * 3), + render=FluidRenderCfg(mode="none"), + ) + ) + sim.prepare() + initial = fluid.data.default_particle_state_w + offsets = torch.as_tensor( + sim.arena_offsets, device=sim.device, dtype=torch.float32 + ) + torch.testing.assert_close( + initial[:, :, :3].mean(1), + offsets + torch.tensor([0.0, 0.0, 1.0], device=sim.device), + ) + changed = initial[[1]].clone() + changed[..., 3] = 1 + fluid.write_particle_state_to_sim(changed, [1]) + sim.update(step=3, render_final_step=False) + before = fluid.data.particle_state_w + density = sim.get_fluid_diagnostics("water")["particle_density"] + fluid.reset([1]) + torch.testing.assert_close( + fluid.data.particle_state_w[0], before[0], rtol=0, atol=0 + ) + torch.testing.assert_close( + fluid.data.particle_state_w[1], initial[1], rtol=0, atol=0 + ) + torch.testing.assert_close( + sim.get_fluid_diagnostics("water")["particle_density"][0], + density[0], + rtol=0, + atol=0, + ) + assert not sim.get_fluid_diagnostics("water")["particle_density"][1].any() + sim.update(step=1, render_final_step=False) + assert torch.isfinite(fluid.data.particle_state_w).all() + finally: + fluid = None + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +def test_fluid_partial_reset_preserves_other_world(): + 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=120, + ) + assert result.returncode == 0, result.stdout + result.stderr + + +if __name__ == "__main__": + _run_fluid_partial_reset() diff --git a/tests/sim/objects/test_fluid_object.py b/tests/sim/objects/test_fluid_object.py index b02033f7f..1f1a78d1f 100644 --- a/tests/sim/objects/test_fluid_object.py +++ b/tests/sim/objects/test_fluid_object.py @@ -151,6 +151,63 @@ def test_fluid_write_respects_render_frame_cache_scope(inside_frame): assert sim._render_state_published is (True if inside_frame else None) +def test_rigid_trajectory_samples_solver_substeps_and_rewinds(): + from embodichain.lab.sim._runtime_controls import _KinematicRigidTrajectoryControl + + poses = np.array([[0, 0, 0, 0, 0, 0, 1], [1, 0, 0, 0, 0, 0, 1]], dtype=np.float32) + samples, velocities = [], [] + actor = SimpleNamespace( + _arena_index=1, + set_world_pose=lambda value: samples.append(value.copy()), + set_linear_velocity=lambda value: velocities.append(value.copy()), + set_angular_velocity=lambda value: None, + ) + control = _KinematicRigidTrajectoryControl("arena_0/cup", poses, fps=1.0) + control.initialize( + SimpleNamespace(result=SimpleNamespace(get_actor=lambda _: actor)) + ) + for index in range(3): + control(None, index, 3, 0.1) + np.testing.assert_allclose([p[0, 3] for p in samples], [0, 0.1, 0.2]) + np.testing.assert_allclose(velocities[1], [1, 0, 0]) + control.prepare_episode_reset((2,))() + control(None, 0, 1, 0.1) + assert samples[-1][0, 3] == pytest.approx(0.3) + control.prepare_episode_reset((1,))() + control(None, 0, 1, 0.1) + np.testing.assert_array_equal(samples[-1], samples[0]) + control.close() + assert control._actor is None + + +def test_rigid_trajectory_rotation_hold_across_outer_steps(): + from scipy.spatial.transform import Rotation + from embodichain.lab.sim._runtime_controls import _KinematicRigidTrajectoryControl + + axis = np.array([1.0, 2.0, 3.0]) + axis /= np.linalg.norm(axis) + end = Rotation.from_rotvec(0.6 * axis).as_quat() + poses = np.array([[0, 0, 0, 0, 0, 0, 1], [1, 2, 3, *end]]) + captured = [] + actor = SimpleNamespace( + set_world_pose=lambda value: captured.append({"pose": value.copy()}), + set_linear_velocity=lambda value: captured[-1].update(linear=value.copy()), + set_angular_velocity=lambda value: captured[-1].update(angular=value.copy()), + ) + control = _KinematicRigidTrajectoryControl("arena_0/cup", poses, fps=1) + control.initialize( + SimpleNamespace(result=SimpleNamespace(get_actor=lambda _: actor)) + ) + for outer in range(3): + for substep in range(2): + control(None, substep, 2, 0.25) + np.testing.assert_allclose(captured[3]["pose"][:3, 3], [0.75, 1.5, 2.25]) + np.testing.assert_allclose(captured[3]["angular"], axis * 0.6, atol=1e-6) + np.testing.assert_allclose(captured[4]["pose"], captured[5]["pose"]) + np.testing.assert_allclose(captured[5]["linear"], 0, atol=1e-6) + np.testing.assert_allclose(captured[5]["angular"], 0, atol=1e-6) + + def test_constructor_only_copies_config_without_sampling(monkeypatch): from unittest.mock import Mock diff --git a/tests/sim/objects/test_fluid_online_runtime.py b/tests/sim/objects/test_fluid_online_runtime.py new file mode 100644 index 000000000..77d8fd0a1 --- /dev/null +++ b/tests/sim/objects/test_fluid_online_runtime.py @@ -0,0 +1,363 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Online rigid boundaries, mass ownership and selected fluid resets.""" + +from __future__ import annotations + +import pytest +from pathlib import Path +import subprocess +import sys +import torch +import trimesh + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import ( + FluidObjectCfg, + FluidSpawnCfg, + FluidRenderCfg, + NewtonPhysicsCfg, + RenderCfg, + RigidObjectCfg, +) +from embodichain.lab.sim.shapes import CubeCfg, MeshCfg, MeshCollisionCfg + +pytestmark = [pytest.mark.gpu, pytest.mark.subprocess_sim] + + +class _FluidOnlineRuntime: + def test_kinematic_trajectory_reset_keeps_other_world_clock(self): + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + device="cuda", + num_envs=2, + arena_space=0, + physics_dt=0.001, + render_cfg=RenderCfg(renderer="no-render"), + physics_cfg=NewtonPhysicsCfg( + num_substeps=1, + collision_cfg=None, + solver_cfg={ + "solver_type": "dexuni", + "sph_cfg": { + "sph_method": "dfsph", + "volume_map_boundary_enabled": False, + }, + }, + ), + ) + ) + boundary = fluid = None + try: + boundary = sim.add_rigid_object( + RigidObjectCfg( + uid="boundary", body_type="kinematic", shape=CubeCfg(size=[0.1] * 3) + ) + ) + poses = torch.tensor( + [[[0.0, 0, 1, 0, 0, 0, 1], [1.0, 0, 1, 0, 0, 0, 1]]] * 2 + ) + sim.register_kinematic_rigid_trajectory("boundary", poses, fps=10) + fluid = sim.add_fluid_object( + FluidObjectCfg( + uid="water", + particle_spacing=0.01, + init_pos=(0, 0, 2), + spawn=FluidSpawnCfg(kind="box", size=(0.02,) * 3), + render=FluidRenderCfg(mode="none"), + ) + ) + sim.prepare() + sim.update(step=2, render_final_step=False) + sim.reset_objects_state(env_ids=[0]) + sim.update(step=1, render_final_step=False) + torch.testing.assert_close( + boundary.get_local_pose()[:, 0], + torch.tensor([0.0, 0.02], device=sim.device), + rtol=0, + atol=1e-5, + ) + finally: + boundary = fluid = None + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + def test_live_dynamic_boundary_and_selected_reset(self): + from dexsim.engine.newton_physics.backend_registry import get_newton_backend + + mesh = trimesh.creation.box(extents=[0.1] * 3) + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + device="cuda", + num_envs=2, + arena_space=0, + physics_dt=0.001, + render_cfg=RenderCfg(renderer="no-render"), + physics_cfg=NewtonPhysicsCfg( + num_substeps=1, + collision_cfg=None, + solver_cfg={ + "solver_type": "dexuni", + "sph_rne_mode": "disabled", + "sph_rigid_substeps": 2, + "sph_cfg": { + "sph_method": "dfsph", + "particle_support_radius": 0.0225, + "volume_map_resolution": 16, + "volume_map_quadrature_samples": 2, + }, + }, + ), + ) + ) + body = fluid = None + try: + body = sim.add_rigid_object( + RigidObjectCfg.from_dict( + { + "uid": "boundary", + "shape": CubeCfg(size=[0.1] * 3).to_dict(), + "particle_collision_shape": MeshCfg( + vertices=mesh.vertices.tolist(), + triangles=mesh.faces.tolist(), + collision=MeshCollisionCfg( + approximation="sdf", + sdf_resolution=32, + sdf_narrow_band_range=(-0.05, 0.05), + sdf_padding=0.02, + ), + ).to_dict(), + "body_type": "dynamic", + "init_pos": [0, 0, 1], + "attrs": { + "mass_props": { + "mass": 0.3, + "inertia": [0.0005, 0.0005, 0.0005], + } + }, + } + ) + ) + fluid = sim.add_fluid_object( + FluidObjectCfg( + uid="water", + particle_spacing=0.01, + particle_radius=0.003, + spawn=FluidSpawnCfg( + kind="points", + positions=[ + [0.055, -0.005, 0.995], + [0.055, 0.005, 0.995], + [0.055, -0.005, 1.005], + [0.055, 0.005, 1.005], + ], + ), + render=FluidRenderCfg(mode="none"), + ) + ) + sim.prepare() + backend = get_newton_backend(sim._world) + assert backend.solver.sph_solver.sdf_shape_inventory.shape_count == 2 + torch.testing.assert_close( + torch.as_tensor(backend.model.body_mass.numpy()), + torch.tensor([0.3, 0.3]), + ) + torch.testing.assert_close( + torch.as_tensor(backend.model.body_inertia.numpy()), + torch.eye(3).expand(2, 3, 3) * 0.0005, + rtol=1e-5, + atol=1e-8, + ) + torch.testing.assert_close( + torch.as_tensor(backend.model.body_inertia.numpy())[ + :, [0, 1, 2], [0, 1, 2] + ], + torch.full((2, 3), 0.0005), + rtol=0.001, + atol=1e-7, + ) + body.set_velocity( + lin_vel=torch.tensor([[0.5, 0, 0], [0.5, 0, 0]], device=sim.device) + ) + sim.update(step=200, render_final_step=False) + assert backend.sim_time == pytest.approx(0.2) + assert sim.physics.cuda_graph_status == "captured" + pose = body.get_local_pose() + assert bool((pose[:, 0] > 0.095).all()) + assert bool((fluid.data.particle_pos_w[..., 0] > 0.12).all()) + # Water has no reaction force on the prescribed initial rigid momentum. + torch.testing.assert_close( + pose[:, 0], + torch.full((2,), 0.1, device=sim.device), + rtol=0.01, + atol=0.001, + ) + particles = fluid.data.particle_state_w.clone() + fluid.reset([0]) + torch.testing.assert_close( + fluid.data.particle_state_w[1], particles[1], rtol=0, atol=0 + ) + torch.testing.assert_close(body.get_local_pose(), pose, rtol=0, atol=0) + sim.reset_objects_state(env_ids=[0]) + torch.testing.assert_close( + body.get_local_pose()[1], pose[1], rtol=0, atol=0 + ) + sim.update(step=1, render_final_step=False) + assert bool(torch.isfinite(fluid.data.particle_state_w).all()) + finally: + body = fluid = None + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + def test_mimic_follower_uses_inner_rigid_solver(self): + from embodichain_robots import FrankaPandaCfg + + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + device="cuda", + num_envs=1, + physics_dt=0.001, + render_cfg=RenderCfg(renderer="no-render"), + physics_cfg=NewtonPhysicsCfg( + num_substeps=1, + collision_cfg=None, + solver_cfg={ + "solver_type": "dexuni", + "sph_rne_mode": "disabled", + "sph_rigid_substeps": 2, + "sph_cfg": { + "sph_method": "dfsph", + "volume_map_boundary_enabled": False, + "sdf_boundary_enabled": False, + }, + }, + ), + ) + ) + robot = fluid = None + try: + robot_cfg = FrankaPandaCfg.from_dict({}) + robot_cfg.root_props.newton_mimic_stabilizer = True + robot = sim.add_robot(robot_cfg) + fluid = sim.add_fluid_object( + FluidObjectCfg( + uid="water", + particle_spacing=0.01, + init_pos=(2, 0, 2), + spawn=FluidSpawnCfg(kind="box", size=(0.02,) * 3), + render=FluidRenderCfg(mode="none"), + ) + ) + sim.prepare() + assert robot._newton_mimic_compliance_configured + joint_ids = robot.get_joint_ids("eef", remove_mimic=True) + robot.set_qpos( + torch.zeros((1, len(joint_ids)), device=sim.device), joint_ids=joint_ids + ) + sim.update(step=500, render_final_step=False) + fingers = robot.get_qpos(name="eef") + assert bool((fingers.abs() < 0.004).all()), fingers + assert float((fingers[:, 0] - fingers[:, 1]).abs().max()) < 0.002 + finally: + robot = fluid = None + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + def test_normal_gym_partial_reset_does_not_advance_peer(self): + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + from embodichain.lab.gym.utils.registration import discover_task_packages, make + from embodichain.utils.utility import load_config + + task = ( + Path(__file__).resolve().parents[3] + / "extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml" + ) + discover_task_packages() + declaration = load_config(task) + cfg = config_to_cfg(declaration, source_path=task) + cfg.num_envs = 2 + cfg.enable_sensor = False + cfg.sim_cfg.render_cfg = RenderCfg(renderer="no-render") + cfg.sim_cfg.headless = True + cfg.sim_steps_per_control = 1 + cfg.events.record_camera = None + cfg.sim_cfg.physics_cfg.solver_cfg["sph_cfg"].update( + volume_map_boundary_enabled=False, + sdf_boundary_enabled=False, + particle_support_radius=0.0225, + ) + for obj in cfg.background + cfg.rigid_object: + for shape in (obj.shape, obj.particle_collision_shape): + collision = getattr(shape, "collision", None) + if collision is not None and collision.approximation == "sdf": + collision.sdf_resolution = 16 + fluid_cfg = cfg.fluid_object[0] + fluid_cfg.particle_spacing = 0.01 + fluid_cfg.particle_radius = 0.003 + fluid_cfg.spawn = FluidSpawnCfg( + kind="box", size=(0.02,) * 3, reference_uid="cup" + ) + fluid_cfg.render = FluidRenderCfg(mode="none") + env = make(declaration["id"], cfg=cfg) + try: + env.reset(seed=0) + action = env.robot.get_qpos()[:, env.active_joint_ids].clone() + for _ in range(3): + env.step(action) + fluid = env.sim.get_fluid_object("water") + before_water = fluid.data.particle_state_w[1].clone() + before_robot = env.robot.get_qpos()[1].clone() + before_cup = env.sim.get_rigid_object("cup").get_local_pose()[1].clone() + env.reset(options={"reset_ids": [0], "save_data": False}) + torch.testing.assert_close( + fluid.data.particle_state_w[1], before_water, rtol=0, atol=0 + ) + torch.testing.assert_close( + env.robot.get_qpos()[1], before_robot, rtol=0, atol=0 + ) + torch.testing.assert_close( + env.sim.get_rigid_object("cup").get_local_pose()[1], + before_cup, + rtol=0, + atol=0, + ) + assert env.elapsed_steps.tolist() == [0, 3] + finally: + env.close(exit_process=False) + del env + SimulationManager.flush_cleanup_queue() + + +def test_online_fluid_boundaries_and_mimic_reset_in_fresh_process(): + 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 = _FluidOnlineRuntime() + worker.test_kinematic_trajectory_reset_keeps_other_world_clock() + worker.test_live_dynamic_boundary_and_selected_reset() + worker.test_mimic_follower_uses_inner_rigid_solver() + worker.test_normal_gym_partial_reset_does_not_advance_peer() diff --git a/tests/sim/spawn/test_descriptors.py b/tests/sim/spawn/test_descriptors.py index 16a5570ec..99c8b5a44 100644 --- a/tests/sim/spawn/test_descriptors.py +++ b/tests/sim/spawn/test_descriptors.py @@ -2676,6 +2676,76 @@ def test_rigid_descriptor_rejects_orientation_without_principal_moments() -> Non rigid_desc_from_cfg(cfg) +@pytest.mark.parametrize("solver", ["dexuni", "mujoco_warp"]) +def test_particle_boundary_shares_body_without_replacing_rigid_contact(solver): + from embodichain.lab.sim.cfg import RigidObjectCfg + from embodichain.lab.sim.shapes import MeshCollisionCfg + from embodichain.lab.sim.spawn.descriptors import rigid_desc_from_cfg + + cfg = RigidObjectCfg( + uid="cup", + shape=MeshCfg( + fpath="/assets/cup.glb", + collision=MeshCollisionCfg( + approximation="convex_decomposition", max_hulls=32 + ), + ), + particle_collision_shape=MeshCfg( + fpath="/assets/cavity.obj", + collision=MeshCollisionCfg(approximation="sdf", sdf_resolution=192), + ), + attrs=RigidBodyPhysicsCfg( + mass_props=MassPropertiesCfg(mass=0.3, inertia=[0.001, 0.001, 0.001]) + ), + ) + desc, _ = rigid_desc_from_cfg(cfg, newton_solver_type=solver) + assert len(desc.collisions) == 2 + contact, boundary = desc.collisions + assert contact.approximation == CollisionApproximation.CONVEX_DECOMPOSITION + assert contact.newton.has_particle_collision is False + assert boundary.approximation == CollisionApproximation.SDF + assert boundary.enable_collision is False + assert boundary.newton.has_particle_collision is True + if hasattr(boundary.newton, "contributes_to_mass"): + assert boundary.newton.contributes_to_mass is False + assert boundary.newton.sdf_max_resolution == 192 + assert desc.renders[0].file_path == "/assets/cup.glb" + assert cfg.shape.collision.approximation == "convex_decomposition" + + +def test_particle_boundary_requires_newton_and_sdf(): + from embodichain.lab.sim.cfg import RigidObjectCfg + from embodichain.lab.sim.spawn.descriptors import rigid_desc_from_cfg + + cfg = RigidObjectCfg( + uid="cup", + shape=MeshCfg(fpath="/assets/cup.glb"), + particle_collision_shape=MeshCfg(fpath="/assets/cavity.obj"), + ) + with pytest.raises(ValueError, match="Newton"): + rigid_desc_from_cfg(cfg) + with pytest.raises(ValueError, match="sdf"): + rigid_desc_from_cfg(cfg, newton_solver_type="dexuni") + + +def test_dynamic_particle_boundary_requires_safe_mass_properties(): + from dexsim.spawn import NewtonCollisionDesc + + if "contributes_to_mass" in NewtonCollisionDesc.__dataclass_fields__: + pytest.skip("This DexSim build can exclude the boundary from mass directly.") + cfg = RigidObjectCfg( + uid="cup", + shape=CubeCfg(size=[0.1, 0.1, 0.1]), + particle_collision_shape=MeshCfg( + fpath="/assets/cavity.obj", + collision=MeshCollisionCfg(approximation="sdf"), + ), + attrs=RigidBodyPhysicsCfg(mass_props=MassPropertiesCfg(mass=0.3)), + ) + with pytest.raises(NotImplementedError, match="explicit mass and inertia"): + rigid_desc_from_cfg(cfg, newton_solver_type="dexuni") + + @pytest.mark.parametrize( "kind", ["rigid", "visual", "volume", "surface", "surface_visual"] ) diff --git a/tests/sim/test_fluid_cfg.py b/tests/sim/test_fluid_cfg.py index b1ab4c12f..dc0ca4429 100644 --- a/tests/sim/test_fluid_cfg.py +++ b/tests/sim/test_fluid_cfg.py @@ -24,7 +24,9 @@ FluidObjectCfg, FluidSpawnCfg, NewtonPhysicsCfg, + RigidObjectCfg, ) +from embodichain.lab.sim.spawn.descriptors import rigid_desc_from_cfg from embodichain.compute.geometry.particles import sample_volume_particles pytestmark = pytest.mark.no_sim @@ -122,6 +124,48 @@ def test_fill_mesh_must_be_closed(tmp_path): sample_volume_particles("mesh_volume", 0.01, mesh_path=str(path)) +def test_rigid_visual_mesh_does_not_replace_sdf_collision(): + cfg = RigidObjectCfg.from_dict( + { + "uid": "cup", + "body_type": "kinematic", + "shape": { + "shape_type": "Mesh", + "vertices": [[0, 0, 0], [1, 0, 0], [0, 1, 0]], + "triangles": [[0, 1, 2]], + "collision": { + "approximation": "sdf", + "force_sdf": False, + "sdf_resolution": 192, + }, + }, + "visual_shape": {"shape_type": "Mesh", "fpath": "cup.glb"}, + } + ) + desc, _ = rigid_desc_from_cfg(cfg, newton_solver_type="dexuni") + assert desc.renders[0].file_path == "cup.glb" + assert desc.collisions[0].newton.force_sdf is False + assert desc.collisions[0].render_source_index is None + + +def test_rigid_material_preserves_transmission(): + import json + from embodichain.lab.sim.shapes import CubeCfg + from embodichain.lab.sim.material import VisualMaterialCfg + + cfg = RigidObjectCfg( + uid="glass", + shape=CubeCfg( + visual_material=VisualMaterialCfg(parameters={"transmission": 0.8}) + ), + ) + _, materials = rigid_desc_from_cfg(cfg, newton_solver_type="dexuni") + assert ( + json.loads(next(iter(materials.values())).parameters_json)["transmission"] + == 0.8 + ) + + @pytest.mark.parametrize("backend,num_envs", [("default", 1), ("newton", 2)]) def test_manager_rejects_unsupported_fluid_worlds_before_declaration(backend, num_envs): from types import SimpleNamespace @@ -158,6 +202,26 @@ def test_fluid_declaration_accepts_dexuni_dispatch(solver_cfg, monkeypatch): SimulationManager.add_fluid_object(manager, cfg) manager._spawn_scene.declare.assert_called_once() + manager.add_runtime_control.assert_called_once() + + +def test_fluid_declaration_preserves_explicit_joint_ownership(monkeypatch): + from unittest.mock import Mock + from embodichain.lab.sim import SimulationManager + + manager = _fluid_manager_for_renderer("no-render") + manager.sim_config.physics_cfg.solver_cfg = { + "solver_type": "dexuni", + "mujoco_joints": [0], + "sph_cfg": {}, + } + monkeypatch.setattr(SimulationManager, "_get_process_renderer", lambda: None) + monkeypatch.setattr("embodichain.lab.sim.sim_manager.FluidObject", Mock()) + cfg = FluidObjectCfg( + uid="water", spawn=FluidSpawnCfg(positions=[[0, 0, 0]]), render={"mode": "none"} + ) + SimulationManager.add_fluid_object(manager, cfg) + manager.add_runtime_control.assert_not_called() def test_dexuni_nested_sph_density_conflict_is_rejected(monkeypatch): @@ -333,12 +397,37 @@ def _fluid_manager_for_renderer(renderer): _fluid_objects={}, _rigid_objects={}, _spawn_scene=SimpleNamespace(declare=Mock()), + add_runtime_control=Mock(), create_visual_material=Mock(), _invalidate_fluid_state=Mock(), notify_visualization_topology_changed=Mock(), ) +def test_fluid_joint_ownership_selects_finalized_topology_and_rebuilds(monkeypatch): + from types import SimpleNamespace + from embodichain.lab.sim._runtime_controls import _DexUniFluidJointOwnership + + cfg = SimpleNamespace( + mujoco_articulations=None, mujoco_articulation_paths=None, mujoco_joints=None + ) + backend = SimpleNamespace(solver_type="dexuni", cfg=SimpleNamespace(solver_cfg=cfg)) + monkeypatch.setattr( + "dexsim.engine.newton_physics.backend_registry.get_newton_backend", + lambda world: backend, + ) + control = _DexUniFluidJointOwnership() + result = SimpleNamespace(world=object()) + control.initialize_model(SimpleNamespace(joint_count=3), result) + assert cfg.mujoco_joints == (0, 1, 2) + control.initialize_model(SimpleNamespace(joint_count=5), result) + assert cfg.mujoco_joints == (0, 1, 2, 3, 4) + control.prepare_episode_reset((0,))() + cfg.mujoco_joints = (1,) + with pytest.raises(RuntimeError, match="explicit selector"): + control.initialize_model(SimpleNamespace(joint_count=5), result) + + @pytest.mark.parametrize("mode", ["particles", "mesh_surface", "none"]) @pytest.mark.parametrize( "requested,process_renderer", diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index ce6496b2b..0cade8fc8 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -487,6 +487,7 @@ def _make_runtime_control_sim_manager( sim._articulations = {} sim._rigid_objects = {"table": object()} sim._fluid_objects = {} + sim._rigid_trajectory_controls = {} sim._deformable_objects = { "cloth": SimpleNamespace( cfg=SurfaceDeformableObjectCfg(uid="cloth", particle_flags=[0, 1, 0]) @@ -569,6 +570,7 @@ def test_deferred_destroy_prepares_backend_before_releasing_world( sim._constraints = {} sim._rigid_object_groups = {} sim._fluid_objects = {} + sim._rigid_trajectory_controls = {} sim._deformable_objects = {} sim._articulations = {} sim._robots = {} @@ -1312,6 +1314,7 @@ def capture(self, **_kwargs: object) -> None: sim._rigid_objects = {} sim._rigid_object_groups = {} sim._fluid_objects = {} + sim._rigid_trajectory_controls = {} sim._deformable_objects = {} sim._articulations = {} sim._robots = {} @@ -2548,6 +2551,7 @@ def test_remove_asset_marks_visualization_topology_dirty() -> None: sim._rigid_objects = {"cube": rigid_object} sim._rigid_object_groups = {} sim._fluid_objects = {} + sim._rigid_trajectory_controls = {} sim._deformable_objects = {} sim._articulations = {} sim._robots = {} @@ -2813,6 +2817,7 @@ def test_reset_objects_state_includes_deformable_assets() -> None: sim._lights = {} sim._sensors = {} sim._fluid_objects = {} + sim._rigid_trajectory_controls = {} sim._deformable_objects = { "soft": MagicMock(), "cloth": MagicMock(), @@ -2849,6 +2854,7 @@ def test_newton_reset_clears_all_articulations_in_selected_worlds_together( sim._rigid_objects = {} sim._rigid_object_groups = {} sim._fluid_objects = {} + sim._rigid_trajectory_controls = {} sim._deformable_objects = {} sim._lights = {} sim._sensors = {} @@ -2874,6 +2880,7 @@ def test_newton_reset_rejects_excluding_one_articulation_from_selected_worlds( sim._rigid_objects = {} sim._rigid_object_groups = {} sim._fluid_objects = {} + sim._rigid_trajectory_controls = {} sim._deformable_objects = {} sim._lights = {} sim._sensors = {} @@ -2903,6 +2910,7 @@ def test_newton_non_mujoco_reset_allows_excluding_an_articulation() -> None: sim._rigid_objects = {} sim._rigid_object_groups = {} sim._fluid_objects = {} + sim._rigid_trajectory_controls = {} sim._deformable_objects = {} sim._lights = {} sim._sensors = {} @@ -3135,7 +3143,7 @@ def test_process_no_render_is_detected_after_world_cleanup(monkeypatch) -> None: assert SimulationManager._get_process_renderer() == "no-render" -def test_reset_fluid_selects_rows_and_honors_exclusions(): +def test_reset_fluid_and_rigid_controls_selects_rows_and_honors_exclusions(): sim = object.__new__(SimulationManager) sim.physics = SimpleNamespace(name="default") sim.num_envs = 2 @@ -3149,9 +3157,16 @@ def test_reset_fluid_selects_rows_and_honors_exclusions(): "_deformable_objects", ): setattr(sim, name, {}) - fluid, excluded = MagicMock(), MagicMock() - sim._fluid_objects = {"water": fluid, "other": excluded} + first, second, excluded = MagicMock(), MagicMock(), MagicMock() + fluid = MagicMock() + sim._rigid_trajectory_controls = { + "cup": (first, second), + "other": (excluded, excluded), + } + sim._fluid_objects = {"water": fluid} sim.reset_objects_state(env_ids=[1], excluded_uids=["other"]) + first.reset.assert_not_called() + second.reset.assert_called_once_with() excluded.reset.assert_not_called() fluid.reset.assert_called_once_with([1]) diff --git a/tests/test_task_program_package_data.py b/tests/test_task_program_package_data.py index eee40afe7..78b24cd9f 100644 --- a/tests/test_task_program_package_data.py +++ b/tests/test_task_program_package_data.py @@ -117,6 +117,11 @@ "task_program_pour_water", "cobotmagic", ), + Path("tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml"): ( + "pick_aim_pour_return_cup", + "task_program_pour_cup_fluid", + "franka_panda_firm_grip_newton", + ), } _DUAL_ARM_VARIANTS = ( "dual_ur5_dh_pgi_140_80", @@ -183,6 +188,14 @@ _RESOURCE_PATHS = frozenset( { + Path("tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml"), + Path("tasks/manipulation/tableware/pour_cup/env.fluid.yaml"), + Path("tasks/manipulation/tableware/pour_cup/task_program/program.fluid.yaml"), + Path( + "tasks/manipulation/tableware/pour_cup/task_program/integration.fluid.yaml" + ), + Path("components/embodiments/franka_panda_firm_grip_newton.yaml"), + Path("components/execution_policies/trajectory_open_loop_fluid_pour.yaml"), *_DUAL_ARM_RESOURCES, *_PROGRAMS, *_DEPLOYMENTS, @@ -350,6 +363,10 @@ def test_setup_stages_all_official_task_programs( for relative_path in _RESOURCE_PATHS } assert expected_outputs <= staged_config_package.relative_outputs + assert not ( + staged_config_package.build_lib + / "extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/assets" + ).exists() def test_staged_task_program_resources_decode_through_installed_paths(