diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 36556b7217..098b1ac20f 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -15,7 +15,7 @@ topics: UsdSourceSnapshot, read_usd_source, LinkMesh, PhysicalCollisionGeometry, capture_physical_collision_geometry, ArticulationJointKinematics, get_parent_joint_chain, asset_physics_mode, joint_drive_props, enable_gravity, - fixed-cadence playback, FluidObjectCfg, FluidSpawnCfg, FluidObject, SPHSolverCfg, revolved_volume, fill_level, + fixed-cadence playback, FluidObjectCfg, FluidSpawnCfg, FluidObject, sph_cfg, DFSPH, revolved_volume, fill_level, 粒子采样, fluid, 流体, quaternion, xyzw, cable, CableObject, CableObjectCfg, CableObjectData, 线缆对象] paths: [topics/simulation-system/simulation-system.md] @@ -55,7 +55,7 @@ topics: keywords: [UnitreeG1MotionRL-v1, motion_tracking, BaseEnv, EmbodiedEnv, EpisodeContext, EnvCfg, ControlTiming, resolve_control_timing, register_env, list-task, run-task, run-example, target_control_frequency, sim_steps_per_control, step_dt, environment.component, embodiment.component, file-owned physics backend, physics_config, ControllerAction, EnvProfiler, startup_summary, dexsim_startup_info, - ExpertTrajectoryCfg, joint_command_mode, position_velocity, qpos qvel, expert action schema, ObservationHistorySnapshot, ObservationFrame, + fluid Gym, 流体 Gym, fluid replay, 粒子回放, DemoQualification, ObjectMotionMetrics, ExpertTrajectoryCfg, joint_command_mode, position_velocity, qpos qvel, expert action schema, ObservationHistorySnapshot, ObservationFrame, EvaluationSpec, RuleSession, evaluate_recorded_rules, task_instruction, resolve_demo_instruction, show-task, task catalog, task gallery, 任务画廊, physical_objective, 物理目标, RLTaskCfg, rl_task] @@ -595,7 +595,7 @@ topics: title: Data Assets aliases: [data assets, data path, asset download, 数据资产, 资产下载, 数据路径] keywords: [get_data_path, get_data_class, _resolve_data_reference, EmbodiChainDataset, TabletopFourBases, Real2SimCarton, Real2SimCartonV1, 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/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 1916327ae1..04ba222816 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -144,10 +144,13 @@ fixed-centre target. Neither option establishes path or physical feasibility. opening and source-mouth geometry. The action derives the tilt direction, source-body clearance and mouth-compensated approach/tilt/return path from the held transform. Omission retains the original local-axis rotation mode. -Geometry belongs to the goal/integration; motion timing, IK and collision -checks remain with the selected planning policy. Validate executed mouth -alignment and height throughout tilted motion with host observations, rather -than treating an accepted final pose as evidence of pouring. +Geometry belongs to the goal/integration. `PourOptions.rotation_duration` +optionally sets a minimum simulated duration for each tilt and return leg on +the control grid. The local-axis mode stretches each row's native segment timing +before control-grid resampling and batch padding; strategy, control period, +joint motion limits, IK and collision checks remain with the selected planning policy. Validate +executed mouth alignment and height throughout tilted motion with host +observations, rather than treating an accepted final pose as evidence of pouring. ExecuteDemoSkill executes task-specific demonstration assets with measured runtime contracts through the same nominal-source and runner interfaces. Its internal diff --git a/agent_context/topics/data-assets/data-assets.md b/agent_context/topics/data-assets/data-assets.md index c0acd6e28e..5badb28d36 100644 --- a/agent_context/topics/data-assets/data-assets.md +++ b/agent_context/topics/data-assets/data-assets.md @@ -51,6 +51,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 c6951bec95..4ca7b69a48 100644 --- a/agent_context/topics/env-framework/env-framework.md +++ b/agent_context/topics/env-framework/env-framework.md @@ -24,12 +24,14 @@ Lightweight RL environments have a separate owner in | Raw schema, optional history and exact historical queries | `managers/observation_manager.py`; Env dispatches capture/reset/close boundaries | | Optional measured physical objectives | `embodichain/lab/gym/envs/objectives/`; Gym owns observation/reset hooks independently of expert acceptance | | Controller-ready commands | `embodichain/lab/gym/envs/types.py`: `ControllerAction` | -| Demo segment execution / outcomes | `embodichain/lab/gym/envs/demo.py` | +| Demo segment execution / outcomes / collection qualification | `embodichain/lab/gym/envs/demo.py` | | Configured Task Program registration | `embodichain/lab/gym/envs/task_program/registration.py` | | Timing instrumentation | `embodichain/lab/gym/utils/profiler.py` | 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 DexUni rigid/DFSPH 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/execution.md b/agent_context/topics/env-framework/execution.md index 7c9ea33427..53cc7ede5e 100644 --- a/agent_context/topics/env-framework/execution.md +++ b/agent_context/topics/env-framework/execution.md @@ -331,6 +331,10 @@ not orchestrate multiple experiments or qualify kinematic playback. Use `evaluate_recorded_rules` to evaluate a compiled graph directly from a trusted trajectory in bounded chunks. It can use a recorded segment occurrence to select the matching control slots and acquisition revisions. + Qualified fluid recordings carry bounded particle-state sidecars. Kinematic + and control replay restore complete particle states at each recorded boundary; + dynamic replay restores only the initial particle state and recomputes later + fluid motion from recorded actions. - **Policy projection**: `rollout_buffer` retains actual policy observations, including sampled noise, and its existing obs/action/reward alignment for LeRobot/RL. It shares the acquisition boundary with raw history, rather than diff --git a/agent_context/topics/env-framework/fluid-replay.md b/agent_context/topics/env-framework/fluid-replay.md new file mode 100644 index 0000000000..db7adce155 --- /dev/null +++ b/agent_context/topics/env-framework/fluid-replay.md @@ -0,0 +1,80 @@ +# 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 configuration helper `gym_utils.configure_fluid_spacing` 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 + +`run-task` 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. DexUni's fluid component 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 example uses the same physical +component and relative pour target with freshly planned Atomic Action targets. + +Program completion and physical qualification are separate. The deployment's +`collection.qualification` declares generic `object_motion` bounds and named +numeric observation bounds. `demo.DemoQualification` tracks motion extrema on +the input device without storing every pose; stateless env-only raw observation functions +are sampled without policy inputs, projection or noise. `when: always` bounds apply at every +sampled boundary, while default `final` bounds apply to the last sample. The +collector combines measured truth with episode completion before commit. +Rejected attempts retain `qualification_result.json` diagnostics and video. +Projected effect assurance alone does not prove grasp or pouring success. + +Accepted Task Program recordings can opt into control-aligned full particle-state +sidecars with `run-task --record-trajectory --record-fluid-state`. Each source row has a +bounded, content-verified chunk manifest. Particle positions are arena-local in +the artifact and converted to the target arena on replay. The ordinary +`ReplayWrapper` restores particles along with rigid states in `kinematic` and +`control`; `dynamic` restores only the initial particle frame, then replays +recorded controls through DexUni. The adapters in `gym/utils/trajectory_state.py` validate particle data and frame +alignment independently of task policy. `run-task` checks accepted Task Program +provenance before creating a World. Replay does not infer fluid state from the +scene's initial fill. + +## Rendering and recording + +The pouring environment selects `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. `run-task` redirects the output directory and sets the +recording limit to the resolved environment count. Accepted episodes commit +through the ordinary reset boundary; rejected attempts use the recorder's +diagnostic purpose. Explicit NoRender selection removes the camera event before +environment construction. +Selected-row recording and camera consumption are owned by the +[recording functor](../manager-functor/manager-functor.md#camera-recording). + +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 ff298eb981..95beff4991 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` | | RL termination manager + condition functors | `managers/termination_manager.py`, `managers/conditions.py` | | Dataset manager + built-in recorders | `managers/dataset_manager.py`, `managers/datasets.py`, `managers/async_datasets.py` | @@ -177,6 +178,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 bbd5e0f53a..39a8944f77 100644 --- a/agent_context/topics/simulation-system/fluids.md +++ b/agent_context/topics/simulation-system/fluids.md @@ -2,12 +2,13 @@ `cfg/fluid.py` owns source, object and render configuration. Fluids use DexUni, which dispatches DFSPH when the scene contains fluid particles. Optional -numerical settings belong to DexSim `SPHSolverCfg`, nested under -`NewtonPhysicsCfg.solver_cfg.sph_cfg`; omitting them uses native DFSPH defaults. +numerical settings belong only under DexUni's +`NewtonPhysicsCfg.solver_cfg.sph_cfg`; DexSim materializes it inside DexUni. +There is no standalone fluid solver selection. `SimulationManager.add_fluid_object()` declares through SpawnScene and binds `objects/fluid.py` in place at `prepare()`. -The fluid declaration owns density and spacing. Default native SPH values for +The fluid declaration owns density and spacing. Default nested DFSPH values for `rest_density` and `particle_diameter` are placeholders, equally for mappings and native config objects; non-default solver values must match the fluid. DexSim derives the effective solver values from the fluid descriptors. @@ -16,9 +17,10 @@ DexSim derives the effective solver values from the fluid descriptors. Fluid simulation requires Newton CUDA, a DexSim build exposing `DexUniSolverCfg.sph_cfg`, no gradients and fixed particle topology. -Multiple environments require a DexSim SPH solver that explicitly advertises -both world isolation and selected-world reset; arena spacing alone does not -establish isolation. The repository's pinned `dexsim_engine==0.6.0rc1` build +Multiple environments require DexUni's prepared internal fluid solver to +advertise both world isolation and selected-world reset; the check runs after +scene preparation and before the first physical step. Arena spacing alone does +not establish isolation. The repository's pinned `dexsim_engine==0.6.0rc1` build provides the DexUni/DFSPH composition and both multi-world capabilities. The current native DexUni/DFSPH implementation is a prototype: DFSPH follows @@ -35,7 +37,7 @@ Writes use Scene particle batches, invalidate captured graphs and reset supported solver history. The legacy single-world solver rebaselines optional SDF boundary pose history. Reset restores the immutable initial state or transforms a saved local template without stepping the world. Explicit state -writes do not refresh density diagnostics until the next SPH step. +writes do not refresh density diagnostics until the next DexUni fluid step. A spawn reference resolves a declared rigid body's initial pose once; it is not a persistent attachment. Sources include explicit points/NPZ, primitives, @@ -70,7 +72,7 @@ follow the renderer requirements of the [material system](materials.md). `scripts/tutorials/sim/create_fluid.py` demonstrates procedural box sampling, five primitive container walls, prepare, step, state reads and reset. Pass -`--num-envs` to use multiple independent worlds on a capable SPH build. The +`--num-envs` to use multiple independent worlds on a capable DexUni build. The fixed container uses static SDF collision shapes, so DFSPH can collide with it without running a rigid-body solver. It needs no robot, task configuration or downloaded assets: @@ -84,3 +86,30 @@ 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. + +Normal Gym reset restores or refills selected fluid rows without stepping peer +worlds. Online action execution supplies physical settling. Replay uses the +ordinary object-state and fluid-state writers; its modes and artifact contracts +belong to [fluid Gym integration](../env-framework/fluid-replay.md). + +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/agent_context/topics/simulation-system/physics-configuration.md b/agent_context/topics/simulation-system/physics-configuration.md index 3b6e85d896..af30f3735b 100644 --- a/agent_context/topics/simulation-system/physics-configuration.md +++ b/agent_context/topics/simulation-system/physics-configuration.md @@ -30,8 +30,8 @@ contact-path policy. Expert settings use the native option groups (`vbd_options`, `mujoco_options`, `collision_options`); unknown solver names are rejected for mappings and native config objects, and flat DexUni options are rejected by native validation. -Standalone SPH is not a supported solver family; fluid settings belong under -[DexUni's nested SPH configuration](fluids.md). Robot presets select +Fluid settings belong under [DexUni's nested DFSPH configuration](fluids.md); +there is no separate fluid solver family. Robot presets select `newton_dexuni` before the MuJoCo aliases and portable fallbacks. Gradient mode requires an explicit `semi_implicit` solver or a direct/named provider supporting the requested gradient mode. Functional Scene calls additionally require its diff --git a/docs/architecture/curated.json b/docs/architecture/curated.json index f7fdf5e0ff..12457a0fe9 100644 --- a/docs/architecture/curated.json +++ b/docs/architecture/curated.json @@ -4289,9 +4289,9 @@ { "path": "embodichain/lab/scripts/run_task.py", "symbol": "cli", - "start_line": 1680, - "end_line": 1680, - "excerpt": " env = gymnasium.make(id=gym_config[\"id\"], cfg=env_cfg, **action_config)" + "start_line": 2180, + "end_line": 2180, + "excerpt": " env = gymnasium.make(id=gym_config[\"id\"], cfg=env_cfg, **action_config)" } ] }, diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index e902ae31af..5ae5a10a77 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -1952,11 +1952,16 @@ embodichain.lab.gym.envs.demo .. currentmodule:: embodichain.lab.gym.envs.demo +Collection qualification combines explicitly configured object-motion bounds and +raw numeric observations. The collector combines physical truth with program +completion before persistence. + .. autosummary:: DEMO_ANNOTATION_KEYS DEMO_SCHEMA_VERSION DemoExecutionCfg + DemoQualification DemoEpisodeResult DemoOutputMode DemoSegment @@ -1965,6 +1970,9 @@ embodichain.lab.gym.envs.demo execute_demo_episode resolve_demo_segments +.. autoclass:: DemoQualification + :members: + embodichain.lab.gym.envs.embodied_env ------------------------------------- @@ -2639,6 +2647,7 @@ embodichain.lab.gym.utils.gym_utils cat_tensor_with_ids clip_and_scale_action config_to_cfg + configure_fluid_spacing convert_observation_to_space dict_array_to_torch_inplace fetch_data_from_dict @@ -2708,11 +2717,15 @@ functors, preserving full robot/mimic joint order, articulation root states and rigid velocities. Declaration assembly performs no physical acquisition. The standard environment recorder uses these functors and shares pose acquisition with policy ``get_object_pose`` / ``get_rigid_object_pose`` within a boundary. +Particle adapters share the bounded media spool and preserve arena-local positions, +particle velocities and control-frame alignment. Recording admission remains host-owned. .. currentmodule:: embodichain.lab.gym.utils.trajectory_state .. autosummary:: + FluidTrajectoryRecorder + FluidTrajectoryReader capture_trajectory_state restore_trajectory_state trajectory_observation_cfgs @@ -2722,6 +2735,12 @@ with policy ``get_object_pose`` / ``get_rigid_object_pose`` within a boundary. .. autofunction:: export_observation_history +.. autoclass:: FluidTrajectoryRecorder + :members: + +.. autoclass:: FluidTrajectoryReader + :members: + embodichain.lab.scripts.analyze_workspace ----------------------------------------- @@ -4950,12 +4969,16 @@ Typed physical predicates provide shared distance and height measurements for RL .. autosummary:: ConditionResult + ObjectMotionMetrics evaluate_near_target evaluate_below_height .. autoclass:: ConditionResult :members: +.. autoclass:: ObjectMotionMetrics + :members: + .. autofunction:: evaluate_near_target .. autofunction:: evaluate_below_height @@ -5845,3 +5868,26 @@ embodichain.lab.sim.cfg parallel-jaw facade .. autoclass:: ParallelJawGripperCfg :members: model_id, catalog_id, root_link, collision_links, pad_links, root_from_contact_frame, min_opening_width, max_opening_width, grasp_generators, joint_commands, default_root_from_tcp, variant, get_gripper_defaults, from_dict :no-index: + +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 f7e467b360..df59bca0be 100644 --- a/embodichain/data/_asset_registry.py +++ b/embodichain/data/_asset_registry.py @@ -36,6 +36,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 6af401ded5..b62f1ef505 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 @@ -28,4 +28,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 0000000000..d27958c7f6 --- /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/demo.py b/embodichain/lab/gym/envs/demo.py index 798a7f9ddb..10935f7049 100644 --- a/embodichain/lab/gym/envs/demo.py +++ b/embodichain/lab/gym/envs/demo.py @@ -18,22 +18,28 @@ from __future__ import annotations -from collections.abc import Callable, Iterable, Iterator, Mapping +from collections.abc import Callable, Iterable, Iterator, Mapping, Sequence from dataclasses import dataclass, field, replace from inspect import signature from types import MappingProxyType from typing import Any, Literal +import inspect +import math +import time + import torch from embodichain.utils import configclass, logger from ._json import json_safe_copy as _json_safe_copy +from .objectives.conditions import ObjectMotionMetrics __all__ = [ "DEMO_ANNOTATION_KEYS", "DEMO_SCHEMA_VERSION", "DemoExecutionCfg", + "DemoQualification", "DemoEpisodeResult", "DemoOutputMode", "DemoSegment", @@ -77,6 +83,253 @@ """Stable, first-failure-phase outcome for one program segment row.""" +class DemoQualification: + """Measure configured object motion and numeric observation bounds. + + Args: + env: Host supplying simulation objects, observations and control timing. + declaration: Optional ``object_motion`` bounds and named ``observations`` + with elementwise ``min``/``max`` lists. + Null list entries are unbounded. An observation's ``when`` is + ``final`` (default) or ``always`` across the sampled boundaries. + + Sources must be stateless raw functions that read directly from ``env``. + They receive an empty observation input; base-policy and cross-term input + dependencies are unsupported. Acquisition does not advance history, apply + policy noise/projection or invoke a stateful functor twice. + Motion extrema stay on the host device with O(B) storage. Raw diagnostic + observations are sampled about once per second and at episode boundaries. + This monitor reports physical truth; the collector owns episode acceptance. + """ + + def __init__(self, env: Any, declaration: Mapping[str, Any]) -> None: + if not isinstance(declaration, Mapping) or set(declaration) - { + "object_motion", + "observations", + }: + raise ValueError("Unknown collection qualification fields.") + self._env = env + motion = declaration.get("object_motion") + self._object = None + self._motion_limits = {} + if motion is not None: + if not isinstance(motion, Mapping) or set(motion) != { + "uid", + "min_lift_m", + "min_tilt_deg", + "max_return_error_m", + }: + raise ValueError("object_motion requires a UID and all three bounds.") + if not isinstance(motion["uid"], str) or not motion["uid"]: + raise ValueError("object_motion.uid must be a nonempty string.") + self._object = env.sim.get_rigid_object(motion["uid"]) + if self._object is None: + raise ValueError("Unknown qualification object.") + for name in ("min_lift_m", "min_tilt_deg", "max_return_error_m"): + value = motion[name] + if ( + type(value) not in (int, float) + or not math.isfinite(value) + or value < 0 + ): + raise ValueError("Motion bounds must be finite and nonnegative.") + self._motion_limits[name] = float(value) + observations = declaration.get("observations", {}) + if not isinstance(observations, Mapping): + raise ValueError("Qualification observations must be a mapping.") + self._sources: dict[str, tuple[Any, torch.Tensor, torch.Tensor, str]] = {} + manager = getattr(env, "observation_manager", None) + for name, bounds in observations.items(): + if not isinstance(name, str) or not name: + raise ValueError("Qualification observation names must be nonempty.") + if not isinstance(bounds, Mapping) or set(bounds) - {"min", "max", "when"}: + raise ValueError("Unknown observation qualification bounds.") + lower, upper = bounds.get("min"), bounds.get("max") + supplied = [value for value in (lower, upper) if value is not None] + if not supplied or any( + not isinstance(value, (list, tuple)) or not value for value in supplied + ): + raise ValueError("Observation bounds require nonempty min/max lists.") + width = len(supplied[0]) + if any(len(value) != width for value in supplied): + raise ValueError("Observation bounds must have equal widths.") + limits = [] + for values, default in ((lower, -math.inf), (upper, math.inf)): + converted = [] + for value in ([None] * width if values is None else values): + if value is not None and ( + type(value) not in (int, float) or not math.isfinite(value) + ): + raise ValueError("Observation bounds must be finite or null.") + converted.append(default if value is None else float(value)) + limits.append( + torch.tensor(converted, dtype=torch.float64, device=env.device) + ) + if bool((limits[0] > limits[1]).any()): + raise ValueError("Observation minimum cannot exceed maximum.") + when = bounds.get("when", "final") + if when not in ("always", "final"): + raise ValueError("Observation when must be 'always' or 'final'.") + cfg = ( + manager.get_functor_cfg(name) + if manager is not None + else getattr(env.cfg.observations, name, None) + ) + if cfg is None or not inspect.isfunction(cfg.func): + raise ValueError( + "Qualification requires stateless observation functions." + ) + self._sources[name] = (cfg, limits[0], limits[1], when) + if self._object is None and not self._sources: + raise ValueError("Qualification requires motion or observation bounds.") + self._stride = max(1, round(1 / env.step_dt)) + self._motion: ObjectMotionMetrics | None = None + self._samples: list[dict[str, Any]] = [] + self._latest: dict[str, torch.Tensor] = {} + self._always_passed: dict[str, torch.Tensor] = {} + self._steps = 0 + self._started: float | None = None + + @staticmethod + def _matches_bounds( + value: torch.Tensor, lower: torch.Tensor, upper: torch.Tensor + ) -> torch.Tensor: + dtype = value.dtype if value.is_floating_point() else torch.float64 + return ( + (value >= lower.to(dtype=dtype)) & (value <= upper.to(dtype=dtype)) + ).all(-1) + + def _sample(self) -> dict[str, Any]: + from .managers.observation_types import ObservationSample + + observations = {} + for name, (cfg, lower, upper, when) in self._sources.items(): + try: + value = cfg.func(self._env, {}, **cfg.params) + except KeyError as error: + raise ValueError( + f"Cannot acquire qualification observation {name!r}; sources " + "must read raw data directly from env without obs dependencies." + ) from error + if isinstance(value, ObservationSample): + if not bool(value.valid.all()): + raise RuntimeError( + f"Qualification observation {name!r} is unavailable." + ) + value = value.values + if isinstance(value, torch.Tensor) and value.ndim == 1: + value = value[:, None] + if ( + not isinstance(value, torch.Tensor) + or value.shape != (self._env.num_envs, lower.numel()) + or value.device != torch.device(self._env.device) + ): + raise ValueError( + f"Qualification observation {name!r} has an invalid layout." + ) + if not bool(torch.isfinite(value).all()): + raise RuntimeError( + f"Qualification observation {name!r} became non-finite." + ) + self._latest[name] = value.detach().clone() + if when == "always": + self._always_passed[name] &= self._matches_bounds(value, lower, upper) + observations[name] = value.detach().cpu().tolist() + sample = { + "time_s": self._steps * self._env.step_dt, + "observations": observations, + } + if self._object is not None: + sample["object_pose"] = ( + self._object.get_local_pose().detach().cpu().tolist() + ) + return sample + + def start(self) -> None: + """Seed one attempt after its reset; no prior attempt state is retained.""" + self._motion = ( + ObjectMotionMetrics(self._object.get_local_pose()) + if self._object is not None + else None + ) + self._always_passed = { + name: torch.ones( + self._env.num_envs, dtype=torch.bool, device=self._env.device + ) + for name in self._sources + } + self._steps = 0 + self._latest.clear() + self._samples = [self._sample()] + self._started = time.perf_counter() + + def after_step(self) -> None: + """Update motion at each control boundary and sample configured observations.""" + if self._started is None: + raise RuntimeError("Qualification must start after reset.") + if self._motion is not None: + self._motion.update(self._object.get_local_pose()) + self._steps += 1 + if self._steps % self._stride == 0: + self._samples.append(self._sample()) + + def finish(self, selected_rows: Sequence[int]) -> dict[str, Any]: + """Return physical measurements for selected rows without committing data. + + Args: + selected_rows: Nonempty unique environment indices to qualify. + + Returns: + Physical acceptance, motion metrics and sampled raw observations. + """ + if self._started is None: + raise RuntimeError("Qualification must start after reset.") + selected = tuple(selected_rows) + if ( + not selected + or len(set(selected)) != len(selected) + or any( + type(row) is not int or not 0 <= row < self._env.num_envs + for row in selected + ) + ): + raise ValueError("Qualification must select unique valid rows.") + accepted = torch.ones( + self._env.num_envs, dtype=torch.bool, device=self._env.device + ) + motion_report = None + if self._motion is not None: + self._motion.update(self._object.get_local_pose()) + motion = self._motion.evaluate(**self._motion_limits) + if not bool(motion.valid.all()): + raise RuntimeError("Qualification object motion became non-finite.") + accepted &= motion.satisfied + motion_report = { + **{ + name: value.cpu().tolist() for name, value in motion.metrics.items() + }, + "accepted": motion.satisfied.cpu().tolist(), + } + self._samples.append(self._sample()) + for name, (_, lower, upper, when) in self._sources.items(): + value = self._latest[name] + accepted &= ( + self._always_passed[name] + if when == "always" + else self._matches_bounds(value, lower, upper) + ) + return { + "accepted": bool(accepted[list(selected)].all()), + "accepted_rows": accepted.cpu().tolist(), + "object_motion": motion_report, + "num_envs": self._env.num_envs, + "control_dt": self._env.step_dt, + "samples": self._samples, + "wall_seconds": time.perf_counter() - self._started, + "selected_rows": list(selected), + } + + @configclass class DemoExecutionCfg: """Collector-owned settings for demonstration persistence. diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 1eaeace1c8..094c023c5b 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -33,6 +33,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, @@ -74,6 +76,7 @@ DemoSegmentResult, _as_bool_tuple, ) +from embodichain.lab.gym.utils.trajectory_state import FluidTrajectoryRecorder from embodichain.lab.gym.envs.types import ControllerAction from embodichain.lab.gym.envs.expert_trajectory import ( ExpertTrajectoryCfg, @@ -304,6 +307,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. @@ -707,6 +713,7 @@ def __init__( ) self._traj_buffer: TensorDict | None = None + self._fluid_trajectory_recorder: FluidTrajectoryRecorder | None = None self._traj_steps: torch.Tensor | None = None self._traj_raw_action: EnvAction | None = None self._trajectory_action_kind: str | None = None @@ -962,6 +969,26 @@ def _record_initial_observation( policy[key] = storage[key] # Existing consumers retain their transition-length view. self.rollout_buffer = policy[:, : self._max_rollout_steps] + recorder = getattr(self, "_fluid_trajectory_recorder", None) + if recorder is not None: + recorder.capture(self, env_ids.tolist(), reset=True) + + def _set_fluid_trajectory_recorder( + self, recorder: FluidTrajectoryRecorder | None + ) -> None: + """Bind one episode's fluid spool, or detach it after publication.""" + if recorder is None: + self._fluid_trajectory_recorder = None + return + if not isinstance(recorder, FluidTrajectoryRecorder): + raise TypeError("recorder must be a FluidTrajectoryRecorder.") + if not self.cfg.record_trajectory or not self.cfg.fluid_object: + raise ValueError( + "Fluid recording requires trajectory and fluid configuration." + ) + if self._fluid_trajectory_recorder is not None: + raise RuntimeError("A fluid trajectory recorder is already attached.") + self._fluid_trajectory_recorder = recorder def set_rollout_buffer(self, rollout_buffer: TensorDict) -> None: """Set the rollout buffer for episode data collection. @@ -2795,17 +2822,27 @@ def _write_trajectory_step( "Trajectory recording requires an explicit observation frame and action." ) manager._bind_schema(frame) + selected = ( + active if active is not None else getattr(self, "_demo_active_mask", None) + ) _write_raw_rollout( self._rollout_storage, self.episode_context, frame, action=self._traj_raw_action, - active=( - active - if active is not None - else getattr(self, "_demo_active_mask", None) - ), + active=selected, ) + recorder = getattr(self, "_fluid_trajectory_recorder", None) + if recorder is not None: + ids = ( + list(range(self.num_envs)) + if selected is None + else torch.as_tensor(selected, dtype=torch.bool) + .nonzero() + .flatten() + .tolist() + ) + recorder.capture(self, ids) self._traj_raw_action = None def _write_rl_rollout_step( @@ -3753,6 +3790,9 @@ def validate_resolved(cfg: RigidObjectCfg) -> None: for cfg in self.cfg.rigid_object_group: self.sim.add_rigid_object_group(cfg=cfg) + for cfg in self.cfg.fluid_object: + self.sim.add_fluid_object(cfg) + def preview_sensor_data( self, name: str, @@ -4155,6 +4195,7 @@ def save_trajectory( env_ids: Sequence[int] | None = None, *, purpose: str = "recording", + extra_meta: Mapping[str, Any] | None = None, ) -> str: """Save a causally aligned trajectory to a ``.pt`` file. @@ -4170,6 +4211,7 @@ def save_trajectory( env_ids: Env indices to save (default: all). Each saved env's actual recorded length is stored in ``meta["lengths"]``. purpose: Normal recording or explicitly labeled diagnostic export. + extra_meta: Additional artifact metadata; standard fields cannot be replaced. Raises: RuntimeError: If trajectory recording was never enabled. @@ -4277,6 +4319,17 @@ def save_trajectory( meta["episode_uuid"] = meta["episode_uuids"][0] meta["source_episode_uuid"] = meta["source_episode_uuids"][0] meta["program_hash"] = meta["demo_episodes"][0].get("program_hash") + if extra_meta is not None: + if not isinstance(extra_meta, Mapping) or not all( + type(key) is str for key in extra_meta + ): + raise TypeError("extra_meta must be a string-keyed mapping.") + duplicate = set(extra_meta) & set(meta) + if duplicate: + raise ValueError( + f"extra_meta cannot replace trajectory fields: {sorted(duplicate)}" + ) + meta.update(copy.deepcopy(dict(extra_meta))) replay_path = str(Path(path).resolve()) for episode in meta["demo_episodes"]: episode["trajectory_path"] = replay_path diff --git a/embodichain/lab/gym/envs/managers/events.py b/embodichain/lab/gym/envs/managers/events.py index b8b0d05da7..437f6e861a 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", @@ -910,3 +911,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 c03edb82d3..1bbb7efaca 100644 --- a/embodichain/lab/gym/envs/managers/observations.py +++ b/embodichain/lab/gym/envs/managers/observations.py @@ -53,6 +53,9 @@ "compute_exteroception", "get_rigid_object_physics_attributes", "get_articulation_joint_drive", + "get_fluid_particle_state", + "get_fluid_summary", + "get_fluid_volume_in_region", ] if TYPE_CHECKING: @@ -1455,3 +1458,113 @@ 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 diff --git a/embodichain/lab/gym/envs/managers/record.py b/embodichain/lab/gym/envs/managers/record.py index c6c62c19e9..23e6ea0d2d 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/envs/objectives/conditions.py b/embodichain/lab/gym/envs/objectives/conditions.py index 90a929ab58..b9e526dfb3 100644 --- a/embodichain/lab/gym/envs/objectives/conditions.py +++ b/embodichain/lab/gym/envs/objectives/conditions.py @@ -19,6 +19,7 @@ from __future__ import annotations from dataclasses import dataclass, field +import math from collections.abc import Mapping, Sequence from typing import TYPE_CHECKING @@ -30,7 +31,12 @@ if TYPE_CHECKING: from embodichain.lab.evaluation import RuleResult -__all__ = ["ConditionResult", "evaluate_near_target", "evaluate_below_height"] +__all__ = [ + "ConditionResult", + "ObjectMotionMetrics", + "evaluate_near_target", + "evaluate_below_height", +] @dataclass(frozen=True) @@ -72,6 +78,81 @@ def __post_init__(self) -> None: object.__setattr__(self, "metrics", owned) +class ObjectMotionMetrics: + """Accumulate lift, world-up tilt and return distance with bounded storage. + + Args: + initial_pose: Arena-local ``(B, 7)`` xyz+xyzw poses after reset. + + Only initial/current positions and extrema are retained, on the input device. + Updating metrics does not advance simulation or establish task acceptance. + """ + + def __init__(self, initial_pose: torch.Tensor) -> None: + self._validate_pose(initial_pose) + self._initial = initial_pose[:, :3].detach().clone() + self._position = self._initial.clone() + self._max_height = initial_pose[:, 2].detach().clone() + self._max_tilt = self._tilt(initial_pose) + self._valid = torch.isfinite(initial_pose).all(dim=-1) + + @staticmethod + def _validate_pose(pose: torch.Tensor) -> None: + if pose.ndim != 2 or pose.shape[1] != 7 or not pose.is_floating_point(): + raise ValueError("Object poses must be floating (B, 7) xyz+xyzw tensors.") + + @staticmethod + def _tilt(pose: torch.Tensor) -> torch.Tensor: + vertical = 1 - 2 * (pose[:, 3].square() + pose[:, 4].square()) + return torch.rad2deg(torch.acos(vertical.clamp(-1, 1))).detach() + + def update(self, pose: torch.Tensor) -> None: + """Observe one pose batch without retaining a trajectory history. + + Args: + pose: Current poses with the initial batch, device and dtype. + """ + self._validate_pose(pose) + if ( + pose.shape[0] != self._initial.shape[0] + or pose.device != self._initial.device + or pose.dtype != self._initial.dtype + ): + raise ValueError( + "Object motion poses must retain their batch/device/dtype." + ) + self._position.copy_(pose[:, :3].detach()) + self._max_height = torch.maximum(self._max_height, pose[:, 2].detach()) + self._max_tilt = torch.maximum(self._max_tilt, self._tilt(pose)) + self._valid &= torch.isfinite(pose).all(dim=-1) + + def evaluate( + self, *, min_lift_m: float, min_tilt_deg: float, max_return_error_m: float + ) -> ConditionResult: + """Compare measured motion with explicit nonnegative acceptance bounds. + + Args: + min_lift_m: Minimum attained height above the initial position. + min_tilt_deg: Minimum attained local-Z tilt from world up, degrees. + max_return_error_m: Maximum final distance from the initial position. + + Returns: + Owned row-local truth, validity and measurements in metres/degrees. + """ + for value in (min_lift_m, min_tilt_deg, max_return_error_m): + if type(value) not in (int, float) or not math.isfinite(value) or value < 0: + raise ValueError("Motion bounds must be finite and nonnegative.") + lift = self._max_height - self._initial[:, 2] + error = torch.linalg.vector_norm(self._position - self._initial, dim=-1) + return ConditionResult( + (lift >= min_lift_m) + & (self._max_tilt >= min_tilt_deg) + & (error <= max_return_error_m), + self._valid, + {"lift_m": lift, "max_tilt_deg": self._max_tilt, "return_error_m": error}, + ) + + def evaluate_near_target( position: torch.Tensor, target: torch.Tensor, diff --git a/embodichain/lab/gym/envs/wrapper/replay.py b/embodichain/lab/gym/envs/wrapper/replay.py index a77d675f5b..01bc978f32 100644 --- a/embodichain/lab/gym/envs/wrapper/replay.py +++ b/embodichain/lab/gym/envs/wrapper/replay.py @@ -28,6 +28,7 @@ EXPERT_TRAJECTORY_SCHEMA_VERSION, build_expert_action_spec, ) +from embodichain.lab.gym.utils.trajectory_state import FluidTrajectoryReader from embodichain.lab.gym.envs.task_program._final_acceptance import ( _replay_final_acceptance, ) @@ -45,10 +46,12 @@ class ReplayWrapper(gym.Wrapper): """Replay a recorded environment trajectory. - In ``kinematic`` mode physics is disabled and every recorded object's - pose/qpos is written directly each step, producing observations only (no - reward / success / action). In ``dynamic`` mode the recorded robot actions - are fed back through :meth:`env.step` so physics re-simulates the scene; + In ``kinematic`` mode physics is disabled and recorded robot, rigid and + available fluid-particle states are written directly each step, producing + observations only (no reward / success / action). In ``dynamic`` mode the + recorded robot actions are fed back through :meth:`env.step` so physics + re-simulates the scene; a recorded fluid starts at its source state and + then evolves through the solver without per-frame state replacement; the full ``obs/reward/terminated/truncated/info`` tuple is returned. The ``control`` mode uses the same kinematic behavior while exposing :meth:`go_to_step` for interactive scrubbing. @@ -80,6 +83,8 @@ def __init__( env: gym.Env, trajectory: str | dict, mode: str = "dynamic", + *, + fluid_reader: FluidTrajectoryReader | None = None, ): super().__init__(env) if mode not in ("kinematic", "dynamic", "control"): @@ -90,6 +95,16 @@ def __init__( self._mode = mode self._trajectory = load_trajectory(trajectory) meta = self._trajectory["meta"] + self._fluid_reader = fluid_reader + if getattr(getattr(env, "cfg", None), "fluid_object", ()): + if self._fluid_reader is None: + if not isinstance(trajectory, str): + raise ValueError("Fluid replay requires a trajectory file path.") + self._fluid_reader = FluidTrajectoryReader( + trajectory, self._trajectory, env.cfg + ) + elif fluid_reader is not None or "fluid_state" in meta: + raise ValueError("A fluid trajectory requires configured fluid objects.") self._controller_spec = None self._task_outcome_replay = mode == "dynamic" and ( getattr(env, "termination_manager", None) is not None @@ -136,6 +151,8 @@ def __init__( # Per-env lengths support async vector trajectories. lengths = self._trajectory["meta"]["lengths"] self._lengths = torch.tensor(lengths, dtype=torch.long, device=self.env.device) + # CPU indices also work when stored states and the replay use different GPUs. + self._recorded_lengths = self._lengths.cpu() # Clamp replay length to the wrapped env's horizon. max_steps = int(self.env.max_episode_steps) @@ -204,7 +221,9 @@ def _expand_to_env_count(self) -> None: "only single-env trajectories can be broadcast." ) self._trajectory = dict(self._trajectory) - for key in ("states", "actions"): + for key in ("states", "actions", "terminal_states"): + if key not in self._trajectory: + continue t = self._trajectory[key] self._trajectory[key] = t.expand(env_envs, *t.shape[1:]).clone() self._trajectory["meta"] = dict( @@ -237,6 +256,10 @@ def reset( # does not integrate dynamics. self.env.sim.enable_physics(False) restore_trajectory_state(self.env, self._trajectory["states"][:, 0]) + if self._fluid_reader is not None: + self._fluid_reader.apply( + self.env, torch.zeros(self.env.num_envs, dtype=torch.long) + ) if self._mode == "dynamic": self.env.sim.enable_physics(True) self.env._replay_no_auto_reset = True @@ -275,7 +298,14 @@ def step( if self._mode in ("kinematic", "control"): restore_trajectory_state(self.env, self._trajectory["states"][idx, st]) - env.sim.update(env.sim_cfg.physics_dt, env.cfg.sim_steps_per_control) + if self._fluid_reader is not None: + self._fluid_reader.apply(env, st) + # Newton's disabled-physics update can still enter the DexUni + # solver. Publish the authored state without a physical step. + with env.sim.render_frame(force_visualization=True): + pass + else: + env.sim.update(env.sim_cfg.physics_dt, env.cfg.sim_steps_per_control) obs = env.get_obs() self._replay_steps = (self._replay_steps + 1).clamp(max=self._lengths) trunc = self._replay_steps >= self._lengths @@ -315,8 +345,23 @@ def go_to_step(self, step: int) -> EnvObs: max_step = self.control_max_step step = max(0, min(int(step), max_step)) env.sim.enable_physics(False) - restore_trajectory_state(env, self._trajectory["states"][:, step]) - env.sim.update(env.sim_cfg.physics_dt, env.cfg.sim_steps_per_control) + terminal = self._recorded_lengths == step + if bool(terminal.all()): + state = self._trajectory["terminal_states"] + else: + state = self._trajectory["states"][:, step] + if bool(terminal.any()): + state = state.clone() + state[terminal] = self._trajectory["terminal_states"][terminal] + restore_trajectory_state(env, state) + if self._fluid_reader is not None: + self._fluid_reader.apply( + env, torch.full((env.num_envs,), step, dtype=torch.long) + ) + with env.sim.render_frame(force_visualization=True): + pass + else: + env.sim.update(env.sim_cfg.physics_dt, env.cfg.sim_steps_per_control) self._replay_steps = torch.full( (env.num_envs,), step, dtype=torch.long, device=env.device ) @@ -326,10 +371,18 @@ def go_to_step(self, step: int) -> EnvObs: def control_max_step(self) -> int: """Largest state index available to interactive control replay.""" transition_count = int(self._lengths.min().item()) + if ( + self._fluid_reader is not None + and "terminal_states" in self._trajectory + and bool((self._lengths == transition_count).all()) + ): + return transition_count return transition_count - 1 def close(self) -> None: try: + if self._fluid_reader is not None: + self._fluid_reader.close() self.env.sim.enable_physics(True) self.env._replay_no_auto_reset = False finally: diff --git a/embodichain/lab/gym/utils/_component_composition.py b/embodichain/lab/gym/utils/_component_composition.py index 5af5b8f1f3..0e0f35e656 100644 --- a/embodichain/lab/gym/utils/_component_composition.py +++ b/embodichain/lab/gym/utils/_component_composition.py @@ -62,6 +62,7 @@ "rigid_object", "rigid_object_group", "articulation", + "fluid_object", } ) @@ -152,6 +153,12 @@ def _resolve_local_resource_paths(value: object, *, base_dir: Path) -> None: """ if isinstance(value, dict): for key, child in value.items(): + if key == "fluid_object" and isinstance(child, list): + for fluid in child: + 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()) if ( key in {"fpath", "urdf_path", "xml_path"} and isinstance(child, str) diff --git a/embodichain/lab/gym/utils/gym_utils.py b/embodichain/lab/gym/utils/gym_utils.py index 2cbb52ae47..528019e46c 100644 --- a/embodichain/lab/gym/utils/gym_utils.py +++ b/embodichain/lab/gym/utils/gym_utils.py @@ -17,6 +17,7 @@ from __future__ import annotations import os +import math import hashlib import json from threading import Lock @@ -41,6 +42,7 @@ from dexsim.utility import log_debug, log_error if TYPE_CHECKING: + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnvCfg from embodichain.lab.evaluation import PredicateRegistry from embodichain.lab.gym.envs.managers.cfg import RLTaskCfg from embodichain.lab.sim.cfg import RobotCfg @@ -89,6 +91,39 @@ def _declared_physics_backend(config: Mapping[str, object]) -> str: return backend +def configure_fluid_spacing(cfg: EmbodiedEnvCfg, spacing: float) -> None: + """Apply one procedural-fluid spacing override to an assembled Gym config. + + Args: + cfg: Environment configuration owning the fluid and DexUni solver. + spacing: Positive particle spacing in metres. + + Raises: + ValueError: If the source or solver cannot be resampled consistently. + """ + if not math.isfinite(spacing) or spacing <= 0: + raise ValueError("particle spacing must be finite and positive") + if len(cfg.fluid_object) != 1: + raise ValueError("particle spacing override requires one fluid object") + fluid = cfg.fluid_object[0] + if fluid.spawn.kind == "points": + raise ValueError("particle spacing requires a procedural volume source") + solver = cfg.sim_cfg.physics_cfg.solver_cfg + if solver.get("solver_type") != "dexuni" or not isinstance( + solver.get("sph_cfg"), dict + ): + raise ValueError("fluid spacing override requires DexUni with sph_cfg") + scale = 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 = spacing + fluid.particle_radius = 0.3 * spacing + solver["sph_cfg"]["particle_support_radius"] = 2.25 * spacing + + def register_manager_modules(modules: list[str]) -> None: """Register additional manager modules for functor resolution. @@ -687,6 +722,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. @@ -981,6 +1026,12 @@ 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 + + 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) @@ -989,6 +1040,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"] @@ -1000,6 +1052,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) @@ -1008,6 +1061,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) @@ -1132,6 +1187,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"], @@ -1160,7 +1221,7 @@ class ComponentCfg: name=obs_params_modified["name"], params=obs_params_modified["params"], consumers=tuple(consumers), - extra=obs_params_modified.get("extra", {}), + extra=deepcopy(obs_params_modified.get("extra", {})), ) setattr(env_cfg.observations, obs_name, observation) @@ -1499,6 +1560,7 @@ def add_env_launcher_args_to_parser( ) parser.add_argument( "--record_trajectory", + "--record-trajectory", help="Whether to record per-object kinematic trajectories (for replay). " "Episodes auto-save to --trajectory_save_dir (or " "~/.cache/embodichain_data/trajectories// by default).", @@ -1507,6 +1569,7 @@ def add_env_launcher_args_to_parser( ) parser.add_argument( "--trajectory_save_dir", + "--trajectory-save-dir", help="Directory for auto-saved trajectories (default: " "~/.cache/embodichain_data/trajectories//).", default=None, @@ -2007,6 +2070,7 @@ def _init_buffer_from_space( "cat_tensor_with_ids", "clip_and_scale_action", "config_to_cfg", + "configure_fluid_spacing", "convert_observation_to_space", "dict_array_to_torch_inplace", "fetch_data_from_dict", diff --git a/embodichain/lab/gym/utils/trajectory_state.py b/embodichain/lab/gym/utils/trajectory_state.py index cd51ccadaf..821a0896f5 100644 --- a/embodichain/lab/gym/utils/trajectory_state.py +++ b/embodichain/lab/gym/utils/trajectory_state.py @@ -22,15 +22,24 @@ from __future__ import annotations -from collections.abc import Mapping +from collections.abc import Mapping, Sequence +from pathlib import Path +import shutil +from uuid import uuid4 from typing import Any, Protocol, TypeAlias, TYPE_CHECKING from dataclasses import asdict +import numpy as np import torch from tensordict import TensorDictBase from embodichain.lab.evaluation.observations import _raw_field_kind from embodichain.utils import logger +from embodichain.data_pipeline.recording.media import ( + MediaFrameReader, + MediaSpool, + verify_media_manifest, +) if TYPE_CHECKING: from embodichain.lab.gym.envs.managers.cfg import ObservationCfg @@ -40,6 +49,8 @@ ) __all__ = [ + "FluidTrajectoryRecorder", + "FluidTrajectoryReader", "capture_trajectory_state", "restore_trajectory_state", "trajectory_observation_cfgs", @@ -371,3 +382,286 @@ def export_observation_history( name: fields[field_id] for name, field_id in observations.field_ids.items() } return snapshot + + +_MAX_BUFFER_BYTES = 32 * 1024 * 1024 + + +class FluidTrajectoryRecorder: + """Spool one particle-state frame per recorded control boundary. + + Args: + directory: Parent directory for this attempt's unique sidecars. + num_envs: Source environment row count. + step_dt: Control period in seconds. + enabled_env_ids: Optional rows eligible for recording. + + Each row uses the shared media spool with a 32 MiB buffer. Particle positions + are stored in arena-local coordinates; publication and commit are separate. + """ + + def __init__( + self, + directory: Path, + num_envs: int, + step_dt: float, + *, + enabled_env_ids: Sequence[int] | None = None, + ) -> None: + self.directory = directory / f"fluid-state-{uuid4().hex}" + self.num_envs = num_envs + self.step_dt = step_dt + enabled = ( + tuple(range(num_envs)) + if enabled_env_ids is None + else tuple(enabled_env_ids) + ) + if ( + not enabled + or any(type(index) is not int for index in enabled) + or len(set(enabled)) != len(enabled) + or any(not 0 <= index < num_envs for index in enabled) + ): + raise ValueError("Fluid recording must select unique valid rows.") + self._enabled = frozenset(enabled) + self._spools: list[MediaSpool | None] = [None] * num_envs + self._parent_ids = [str(uuid4()) for _ in range(num_envs)] + self._counts = [0] * num_envs + self._published = False + self._committed = False + + def capture(self, env: Any, env_ids: Sequence[int], *, reset: bool = False) -> None: + """Capture selected rows, including the reset frame when requested.""" + if self._published: + raise RuntimeError( + "A finalized fluid trajectory cannot capture more frames." + ) + ids = tuple(index for index in env_ids if index in self._enabled) + if not ids: + return + if reset: + for index in ids: + if self._spools[index] is not None: + shutil.rmtree(self._spools[index].directory) + self._spools[index] = None + self._parent_ids[index] = str(uuid4()) + self._counts[index] = 0 + elif any(self._spools[index] is None for index in ids): + raise RuntimeError("Fluid trajectory has no initial control frame.") + offsets = env.sim.arena_offsets[list(ids)].detach().cpu().numpy() + frames = {} + for uid in sorted(fluid.uid for fluid in env.cfg.fluid_object): + state = env.sim.get_fluid_object(uid).data.particle_state_w + if state.dtype != torch.float32 or state.ndim != 3 or state.shape[-1] != 6: + raise ValueError("Recorded fluid state must be float32 (E, N, 6).") + values = state[list(ids)].detach().cpu().numpy() + if not np.isfinite(values).all(): + raise ValueError("Recorded fluid state must be finite.") + frames[uid] = values + for row_position, index in enumerate(ids): + if self._spools[index] is None: + self._spools[index] = MediaSpool( + self.directory / f"env_{index:03d}", + max_buffer_bytes=_MAX_BUFFER_BYTES, + parent_episode_id=self._parent_ids[index], + calibration={"coordinate_frame": "arena_local", "channels": 6}, + ) + row = {} + for uid, values in frames.items(): + local = values[row_position].copy() + local[:, :3] -= offsets[row_position] + row[uid] = local + self._spools[index].append(self._counts[index] * self.step_dt, row) + self._counts[index] += 1 + + def finish( + self, + trajectory_path: Path, + lengths: Sequence[int], + *, + env_ids: Sequence[int] | None = None, + ) -> dict[str, Any]: + """Publish selected, verified sidecars before the trajectory references them.""" + if self._published: + raise RuntimeError("Fluid trajectory was already published.") + if len(lengths) != self.num_envs: + raise ValueError("Fluid trajectory row count differs from source.") + selected = tuple(range(self.num_envs)) if env_ids is None else tuple(env_ids) + if ( + not selected + or any(type(index) is not int for index in selected) + or len(set(selected)) != len(selected) + or any(not 0 <= index < self.num_envs for index in selected) + ): + raise ValueError("Fluid trajectory must select unique valid rows.") + manifests = [] + for index in selected: + spool = self._spools[index] + length = lengths[index] + if spool is None or self._counts[index] != int(length) + 1: + raise ValueError("Fluid frames do not align with trajectory lengths.") + path = spool.finish() + verify_media_manifest( + path, + expected_frames=self._counts[index], + parent_episode_id=self._parent_ids[index], + expected_timestamps=np.arange(self._counts[index]) * self.step_dt, + ) + manifests.append(str(path.relative_to(trajectory_path.parent))) + for index in set(range(self.num_envs)) - set(selected): + spool = self._spools[index] + if spool is not None: + shutil.rmtree(spool.directory) + self._spools[index] = None + self._published = True + return { + "schema_version": 1, + "coordinate_frame": "arena_local", + "manifests": manifests, + "parent_ids": [self._parent_ids[index] for index in selected], + } + + def commit(self) -> None: + """Retain sidecars after their referring trajectory is durable.""" + if not self._published: + raise RuntimeError("Fluid sidecars must be finalized before commit.") + self._committed = True + + def abort(self) -> None: + """Discard uncommitted particle chunks after failure or interruption.""" + if not self._committed: + shutil.rmtree(self.directory, ignore_errors=True) + + +class FluidTrajectoryReader: + """Validate and stream particle frames independently of collection policy. + + Args: + path: Referring trajectory file, which anchors relative sidecar paths. + trajectory: Loaded trajectory metadata with particle manifests. + cfg: Target configuration defining environment count and fluid sources. + + Content, timing, shape and frame conventions are checked before replay. + The host separately decides which recording purposes or task outcomes it accepts. + """ + + def __init__(self, path: str | Path, trajectory: Mapping, cfg: Any) -> None: + self.path = Path(path).resolve() + meta = trajectory["meta"] + source_envs = int(meta["num_envs"]) + target_envs = int(cfg.num_envs) + if source_envs not in (1, target_envs): + raise ValueError( + "Fluid replay source must match env count or contain one row." + ) + sidecar = meta.get("fluid_state") + if ( + not isinstance(sidecar, Mapping) + or sidecar.get("schema_version") != 1 + or sidecar.get("coordinate_frame") != "arena_local" + ): + raise ValueError("Fluid replay requires aligned particle-state sidecars.") + paths = sidecar.get("manifests") + parent_ids = sidecar.get("parent_ids") + if not isinstance(paths, list) or not isinstance(parent_ids, list): + raise ValueError("Fluid replay sidecar manifest is malformed.") + if len(paths) != source_envs or len(parent_ids) != source_envs: + raise ValueError("Fluid replay sidecar rows do not match the trajectory.") + self._uids = tuple(sorted(fluid.uid for fluid in cfg.fluid_object)) + if not self._uids: + raise ValueError("Fluid replay requires configured fluid objects.") + expected_counts = { + fluid.uid: int(fluid.spawn.sample(fluid.particle_spacing).shape[0]) + for fluid in cfg.fluid_object + } + self._readers = [] + try: + for index, (name, parent_id) in enumerate(zip(paths, parent_ids)): + if ( + not isinstance(name, str) + or not name + or not isinstance(parent_id, str) + or not parent_id + ): + raise ValueError( + "Fluid replay manifest paths and IDs must be strings." + ) + relative = Path(name) + if relative.is_absolute() or ".." in relative.parts: + raise ValueError( + "Fluid replay manifest escapes its artifact directory." + ) + candidate = self.path.parent / relative + if any( + part.is_symlink() + for part in (candidate, *candidate.parents) + if part.is_relative_to(self.path.parent) + ): + raise ValueError("Fluid replay manifest cannot use symlinks.") + manifest_path = candidate.resolve() + if not manifest_path.is_relative_to(self.path.parent): + raise ValueError( + "Fluid replay manifest escapes its artifact directory." + ) + count = int(meta["lengths"][index]) + 1 + declared = verify_media_manifest( + manifest_path, + expected_frames=count, + parent_episode_id=parent_id, + expected_timestamps=np.arange(count) * float(meta["dt"]), + ) + if declared.get("calibration") != { + "coordinate_frame": "arena_local", + "channels": 6, + }: + raise ValueError("Fluid replay particle frame convention differs.") + if tuple(sorted(declared["features"])) != self._uids: + raise ValueError( + "Fluid replay particle UIDs differ from the scene." + ) + for uid, schema in declared["features"].items(): + if ( + len(schema["shape"]) != 2 + or schema["shape"][0] != expected_counts[uid] + or schema["shape"][1] != 6 + or np.dtype(schema["dtype"]) != np.dtype("float32") + ): + raise ValueError("Fluid replay requires float32 (N, 6) frames.") + self._readers.append(MediaFrameReader(manifest_path)) + except Exception: + self.close() + raise + self._source_envs = source_envs + self._target_envs = target_envs + + def apply(self, env: Any, steps: torch.Tensor) -> None: + """Write selected control-boundary frames without advancing physics.""" + if tuple(steps.shape) != (self._target_envs,): + raise ValueError("Fluid replay step indices do not match target rows.") + indices = [int(value) for value in steps.detach().cpu().tolist()] + offsets = env.sim.arena_offsets + selected: dict[tuple[int, int], dict[str, np.ndarray]] = {} + for row, step in enumerate(indices): + source = 0 if self._source_envs == 1 else row + key = source, step + if key not in selected: + _, selected[key] = self._readers[source].read(step) + for uid in self._uids: + fluid = env.sim.get_fluid_object(uid) + values = [] + for row, step in enumerate(indices): + source = 0 if self._source_envs == 1 else row + state = selected[source, step][uid] + if ( + state.shape != (fluid.num_particles, 6) + or not np.isfinite(state).all() + ): + raise ValueError("Fluid replay particle count or state is invalid.") + values.append(state) + world = torch.as_tensor(np.stack(values), device=env.device).clone() + world[:, :, :3] += offsets[:, None, :] + fluid.write_particle_state_to_sim(world) + + def close(self) -> None: + for reader in getattr(self, "_readers", ()): + reader.close() diff --git a/embodichain/lab/scripts/run_task.py b/embodichain/lab/scripts/run_task.py index ef37950908..c3eec2f179 100644 --- a/embodichain/lab/scripts/run_task.py +++ b/embodichain/lab/scripts/run_task.py @@ -36,12 +36,18 @@ from embodichain.lab.gym.envs.demo import ( DemoEpisodeResult, DemoExecutionCfg, + DemoQualification, execute_demo_episode, ) from embodichain.lab.gym.envs.wrapper import ReplayWrapper +from embodichain.lab.gym.utils.trajectory_state import ( + FluidTrajectoryReader, + FluidTrajectoryRecorder, +) from embodichain.lab.gym.utils.gym_utils import ( add_env_launcher_args_to_parser, build_env_cfg_from_args, + configure_fluid_spacing, load_trajectory, _load_expansion_declaration, ) @@ -463,6 +469,61 @@ def generate_and_execute_action_list( return True +def _save_qualification_report( + directory: Path, report: Mapping[str, Any], *, batch: int, attempt: int +) -> None: + directory.mkdir(parents=True, exist_ok=True) + payload = json.dumps(dict(report), indent=2, allow_nan=False) + "\n" + (directory / "qualification_result.json").write_text(payload) + diagnostics = directory / "diagnostics" + diagnostics.mkdir(exist_ok=True) + (diagnostics / f"batch_{batch:06d}_attempt_{attempt:02d}.json").write_text(payload) + + +def _save_diagnostic_video(env: Any, selected: Sequence[int]) -> None: + event_manager = getattr(_env_target(env), "event_manager", None) + if ( + event_manager is None + or getattr(event_manager.cfg, "record_camera", None) is None + ): + return + recorder = event_manager.get_functor("record_camera") + if recorder is not None: + recorder.save_and_clear(env_ids=list(selected), purpose="diagnostic") + + +def _save_qualified_fluid_trajectory( + target: Any, + recorder: FluidTrajectoryRecorder, + directory: Path, + selected: Sequence[int], + *, + batch: int, +) -> None: + episodes = [target.get_demo_episode_metadata(row) for row in selected] + hashes = {episode.get("program_hash") for episode in episodes} + if len(hashes) != 1 or not next(iter(hashes)): + raise RuntimeError("Accepted fluid source has no common program hash") + directory.mkdir(parents=True, exist_ok=True) + path = directory / f"fluid_batch_{batch:06d}_{recorder.directory.name}.pt" + sidecar = recorder.finish(path, target._traj_steps.tolist(), env_ids=selected) + target.save_trajectory( + str(path), + env_ids=selected, + extra_meta={ + "fluid_qualification": { + "kind": "task_program", + "accepted": True, + "program_hash": next(iter(hashes)), + }, + "fluid_state": sidecar, + }, + ) + recorder.commit() + target._set_fluid_trajectory_recorder(None) + log_info(f"Qualified fluid replay source: {path}") + + def generate_function( env: Any, num_traj: int | None = None, @@ -472,6 +533,9 @@ def generate_function( debug_mode: bool = False, save_env_ids: Sequence[int] | torch.Tensor | None = None, execution_cfg: DemoExecutionCfg | None = None, + fluid_recorder: FluidTrajectoryRecorder | None = None, + qualification: Mapping[str, Any] | None = None, + qualification_output_dir: Path | None = None, **kwargs: Any, ) -> bool: """Generate, execute, and commit one demonstration collection batch. @@ -495,6 +559,9 @@ def generate_function( rows are explicitly discarded after the selected rows commit. execution_cfg: Continuous or independent segment-fragment persistence settings. Checkpoint resume is not performed in either mode. + fluid_recorder: Optional bounded particle-state spool for this batch. + qualification: Configured physical acceptance criteria. + qualification_output_dir: Destination for qualification reports and sidecars. **kwargs: Additional keyword arguments for data expansion. Returns: @@ -559,11 +626,17 @@ def generate_function( "path; ordinary run-task recording cannot yet commit this episode" ) normalized_save_env_ids = _normalize_save_env_ids(env, save_env_ids) + if fluid_recorder is not None and qualification is None: + raise ValueError("Fluid recording requires physical qualification and output") + if qualification is not None and qualification_output_dir is None: + raise ValueError("Collection qualification requires an output directory") save_failed_episodes = _save_failed_episodes_enabled(env) if execution_cfg is None: execution_cfg = DemoExecutionCfg() elif not isinstance(execution_cfg, DemoExecutionCfg): raise TypeError("execution_cfg must be a DemoExecutionCfg or None.") + if qualification is not None and execution_cfg.mode != "continuous": + raise ValueError("Collection qualification requires continuous episodes") if reset_before: _abort_pending_episode(env) @@ -574,15 +647,39 @@ def generate_function( stats["attempts"] = stats.get("attempts", 0) + 1 commit_succeeded = False try: + qualifier = ( + DemoQualification(target, qualification) + if qualification is not None + else None + ) + if qualifier is not None: + qualifier.start() + + def progress(actions: Iterable[Any], description: str) -> Iterator[Any]: + for action in _progress_wrapper(actions, description): + yield action + if qualifier is not None: + qualifier.after_step() + result: DemoEpisodeResult = execute_demo_episode( env, episode_index=time_id, execution_cfg=execution_cfg, attempt_id=attempt - 1, - progress=_progress_wrapper, + progress=progress, **kwargs, ) successful = result.completed and result.all_success + if qualifier is not None: + report = qualifier.finish(normalized_save_env_ids) + report["episode"] = result.to_metadata() + successful = successful and report["accepted"] + report["accepted"] = successful + _save_qualification_report( + qualification_output_dir, report, batch=time_id, attempt=attempt + ) + if not successful: + _save_diagnostic_video(env, normalized_save_env_ids) fragment_env_ids = _persistable_fragment_env_ids( env, result, @@ -591,6 +688,7 @@ def generate_function( ) persistable_failure = ( not successful + and qualifier is None and save_failed_episodes and _selected_rows_have_frames(result, normalized_save_env_ids) ) @@ -614,6 +712,14 @@ def generate_function( if execution_cfg.mode == "continuous" and ( successful or persistable_failure ): + if successful and fluid_recorder is not None: + _save_qualified_fluid_trajectory( + target, + fluid_recorder, + qualification_output_dir, + normalized_save_env_ids, + batch=time_id, + ) if expansion_record_sink is not None: expansion_record_sink( tuple( @@ -1132,7 +1238,42 @@ def _run_expansion( ) -def replay(env, trajectory_path: str, mode: str = "kinematic") -> None: +def _validate_fluid_replay_source(trajectory: Mapping[str, Any]) -> None: + """Enforce the collector's accepted Task Program source policy before replay.""" + meta = trajectory["meta"] + source_envs = int(meta["num_envs"]) + qualification = meta.get("fluid_qualification") + if ( + meta.get("artifact_purpose") != "recording" + or not isinstance(qualification, Mapping) + or qualification.get("kind") != "task_program" + or qualification.get("accepted") is not True + ): + raise ValueError("Fluid replay requires an accepted Task Program recording.") + episodes = meta.get("demo_episodes", []) + if len(episodes) != source_envs or not episodes: + raise ValueError("Fluid replay source episodes do not match recorded rows.") + program_hash = qualification.get("program_hash") + if ( + not isinstance(program_hash, str) + or not program_hash + or any( + episode.get("program_hash") != program_hash + or episode.get("completed") is not True + or episode.get("success") is not True + for episode in episodes + ) + ): + raise ValueError("Fluid replay program provenance differs from the source.") + + +def replay( + env, + trajectory_path: str, + mode: str = "kinematic", + *, + fluid_reader: FluidTrajectoryReader | None = None, +) -> None: """Replay a recorded trajectory. The caller retains ownership of ``env``. Wrapper-specific replay state is @@ -1153,8 +1294,25 @@ def replay(env, trajectory_path: str, mode: str = "kinematic") -> None: f"num_steps={meta['num_steps']}, mode={mode}", color="green", ) - replay_env = ReplayWrapper(env.unwrapped, trajectory_path, mode=mode) + replay_env = None try: + if fluid_reader is not None or getattr( + getattr(env.unwrapped, "cfg", None), "fluid_object", () + ): + _validate_fluid_replay_source(data) + behavior = ( + "recorded particle states without physics" + if mode in {"kinematic", "control"} + else "recorded controls with newly simulated fluid" + ) + log_info( + f"Accepted Task Program fluid source; replaying {behavior}. " + "Source acceptance does not qualify this replay's physical outcome.", + color="green", + ) + replay_env = ReplayWrapper( + env.unwrapped, trajectory_path, mode=mode, fluid_reader=fluid_reader + ) for row, report in enumerate(replay_env.final_acceptance or ()): log_info( f"Replay final acceptance row {row}: {report['status']}, " @@ -1165,12 +1323,18 @@ def replay(env, trajectory_path: str, mode: str = "kinematic") -> None: else: replay_auto(replay_env, mode) finally: - # ReplayWrapper.close() also closes its wrapped environment. Restore - # its local state here and leave the single close() to cli(). - try: - replay_env.env.sim.enable_physics(True) - finally: - replay_env.env._replay_no_auto_reset = False + if replay_env is None: + if fluid_reader is not None: + fluid_reader.close() + else: + # ReplayWrapper.close() also closes its wrapped environment. Restore + # its local state here and leave the single close() to cli(). + try: + replay_env.env.sim.enable_physics(True) + finally: + replay_env.env._replay_no_auto_reset = False + if replay_env._fluid_reader is not None: + replay_env._fluid_reader.close() def replay_auto(replay_env: ReplayWrapper, mode: str) -> None: @@ -1450,7 +1614,13 @@ def replay_control(replay_env: ReplayWrapper) -> None: ) -def main(args: Any, env: Any, gym_config: dict[str, Any]) -> None: +def main( + args: Any, + env: Any, + gym_config: dict[str, Any], + *, + fluid_reader: FluidTrajectoryReader | None = None, +) -> None: """Run the selected workflow without taking ownership of ``env``.""" if getattr(args, "replay", False): log_info("Replay mode.", color="green") @@ -1458,6 +1628,7 @@ def main(args: Any, env: Any, gym_config: dict[str, Any]) -> None: env, args.replay_trajectory, getattr(args, "replay_mode", "kinematic"), + fluid_reader=fluid_reader, ) return @@ -1484,6 +1655,22 @@ def main(args: Any, env: Any, gym_config: dict[str, Any]) -> None: return plan = resolve_collection_plan(args, gym_config) + collection = _mapping_or_empty(gym_config.get("collection"), "collection") + qualification = collection.get("qualification") + if qualification is not None and not isinstance(qualification, Mapping): + raise ValueError("collection.qualification must be a mapping") + record_fluid_state = bool(getattr(args, "record_fluid_state", False)) + if record_fluid_state and qualification is None: + raise ValueError( + "--record-fluid-state requires configured physical qualification" + ) + qualification_output_dir = ( + Path( + getattr(args, "qualification_output_dir", None) or "outputs/task-collection" + ) + if qualification is not None + else None + ) if real2sim_owner is not None and real2sim_owner.pending_episode is not None: raise RuntimeError( "unconfirmed Real2Sim submission must be repaired before reset" @@ -1537,26 +1724,46 @@ def main(args: Any, env: Any, gym_config: dict[str, Any]) -> None: while committed < plan.target_episodes: batch_size = min(num_envs, plan.target_episodes - committed) stats["prepare_reset_count"] += 1 - if real2sim_owner is None: - env.reset() - else: - env.reset(options={"save_data": False}) - collection_kwargs = ( - {"regenerate": getattr(args, "regenerate", False)} - if real2sim_owner is None - else {"_real2sim_owner": real2sim_owner} - ) - generated = generate_function( - env, - time_id=cursor, - save_path=getattr(args, "save_path", ""), - save_video=getattr(args, "save_video", False), - debug_mode=getattr(args, "debug_mode", False), - save_env_ids=tuple(range(batch_size)), - max_attempts=plan.max_attempts, - reset_before=False, - **collection_kwargs, + fluid_recorder = ( + FluidTrajectoryRecorder( + qualification_output_dir, + num_envs, + _env_target(env).step_dt, + enabled_env_ids=tuple(range(batch_size)), + ) + if record_fluid_state + else None ) + if fluid_recorder is not None: + _env_target(env)._set_fluid_trajectory_recorder(fluid_recorder) + try: + if real2sim_owner is not None or qualification is not None: + env.reset(options={"save_data": False}) + else: + env.reset() + collection_kwargs = ( + {"regenerate": getattr(args, "regenerate", False)} + if real2sim_owner is None + else {"_real2sim_owner": real2sim_owner} + ) + generated = generate_function( + env, + time_id=cursor, + save_path=getattr(args, "save_path", ""), + save_video=getattr(args, "save_video", False), + debug_mode=getattr(args, "debug_mode", False), + save_env_ids=tuple(range(batch_size)), + max_attempts=plan.max_attempts, + reset_before=False, + fluid_recorder=fluid_recorder, + qualification=qualification, + qualification_output_dir=qualification_output_dir, + **collection_kwargs, + ) + finally: + if fluid_recorder is not None: + _env_target(env)._set_fluid_trajectory_recorder(None) + fluid_recorder.abort() stats["batch_count"] += 1 if not generated: stats["rejected_episodes"] += batch_size @@ -1574,6 +1781,10 @@ def main(args: Any, env: Any, gym_config: dict[str, Any]) -> None: expected_episodes=committed ) log_info(f"Verified Real2Sim collection report: {report_path}") + if qualification_output_dir is not None: + log_info( + f"Collection qualification report: {qualification_output_dir / 'qualification_result.json'}" + ) episode_label = "episode" if committed == 1 else "episodes" batch_label = "batch" if stats["batch_count"] == 1 else "batches" @@ -1744,6 +1955,16 @@ def _create_parser() -> argparse.ArgumentParser: help="Replay mode: kinematic (exact, default), dynamic (physics), " "control (interactive scrubber).", ) + parser.add_argument( + "--record-fluid-state", + action="store_true", + help="Spool full particles beside a physically accepted Task Program trajectory.", + ) + parser.add_argument( + "--particle-spacing", + type=float, + help="Override spacing for one procedurally spawned DexUni fluid.", + ) return parser @@ -1804,7 +2025,16 @@ def cli(argv: Sequence[str] | None = None) -> None: np.set_printoptions(5, suppress=True) torch.set_printoptions(precision=5, sci_mode=False) - args = _create_parser().parse_args(argv) + parser = _create_parser() + args = parser.parse_args(argv) + if getattr(args, "record_fluid_state", False) and not getattr( + args, "record_trajectory", False + ): + parser.error("--record-fluid-state requires --record_trajectory") + if getattr(args, "record_fluid_state", False) and (args.replay or args.preview): + parser.error( + "--record-fluid-state is only available for Task Program collection" + ) if getattr(args, "replay", False): if not args.replay_trajectory: @@ -1821,11 +2051,46 @@ def cli(argv: Sequence[str] | None = None) -> None: execute_init_hooks() env_cfg, gym_config, action_config = build_env_cfg_from_args(args) + if getattr(args, "particle_spacing", None) is not None: + configure_fluid_spacing(env_cfg, args.particle_spacing) + collection = _mapping_or_empty(gym_config.get("collection"), "collection") + qualification = collection.get("qualification") + if qualification is not None: + if not isinstance(qualification, Mapping): + raise ValueError("collection.qualification must be a mapping") + output = Path(args.trajectory_save_dir or "outputs/task-collection").resolve() + args.qualification_output_dir = str(output) + camera = getattr(env_cfg.events, "record_camera", None) + if camera is not None: + camera.params["save_path"] = str(output / "videos") + camera.params["max_env_num"] = env_cfg.num_envs + if getattr(args, "record_fluid_state", False): + if not env_cfg.fluid_object or env_cfg.task_program is None: + raise ValueError("Fluid recording requires a fluid Task Program") + if qualification is None: + raise ValueError( + "--record-fluid-state requires configured physical qualification" + ) + if args.expansion_profile is not None or "expansion" in gym_config: + raise ValueError("Fluid recording cannot be combined with expansion") + env_cfg.trajectory_auto_save = False + env_cfg.trajectory_save_dir = args.qualification_output_dir + args.trajectory_save_dir = args.qualification_output_dir + if ( + getattr(env_cfg, "fluid_object", None) + and env_cfg.sim_cfg.render_cfg.renderer == "no-render" + ): + if getattr(env_cfg, "events", None) is not None: + env_cfg.events.record_camera = None + for fluid in env_cfg.fluid_object: + fluid.render.mode = "none" real2sim_profile = None real2sim_profile_path = getattr(args, "real2sim_profile", None) if real2sim_profile_path is None: real2sim_profile_path = gym_config.get("real2sim", {}).get("profile") + if getattr(args, "record_fluid_state", False) and real2sim_profile_path is not None: + raise ValueError("Fluid recording cannot use a Real2Sim collection owner") if real2sim_profile_path is not None: from embodichain.lab.task_program.integrations.simulation.real2sim_collection import ( load_real2sim_tabletop_profile, @@ -1909,7 +2174,19 @@ def cli(argv: Sequence[str] | None = None) -> None: if args.replay and args.replay_mode == "control": log_info("Dataset saving disabled for control replay mode.", color="green") - env = gymnasium.make(id=gym_config["id"], cfg=env_cfg, **action_config) + fluid_reader = None + if args.replay and env_cfg.fluid_object: + replay_source = load_trajectory(args.replay_trajectory) + _validate_fluid_replay_source(replay_source) + fluid_reader = FluidTrajectoryReader( + args.replay_trajectory, replay_source, env_cfg + ) + try: + env = gymnasium.make(id=gym_config["id"], cfg=env_cfg, **action_config) + except BaseException: + if fluid_reader is not None: + fluid_reader.close() + raise # Ensure the sim is torn down via env.close() (-> SimulationManager.destroy()) # before the interpreter shuts down. Without this, C++ resources (dexsim/warp/ @@ -1930,7 +2207,7 @@ def cli(argv: Sequence[str] | None = None) -> None: install_real2sim_tabletop_owner( env, real2sim_profile, dataset_dir=args.expansion_dataset_dir ) - main(args, env, gym_config) + main(args, env, gym_config, fluid_reader=fluid_reader) except BaseException as error: body_error = error raise diff --git a/embodichain/lab/sim/_runtime_controls.py b/embodichain/lab/sim/_runtime_controls.py index a8650e21a3..af301d7877 100644 --- a/embodichain/lab/sim/_runtime_controls.py +++ b/embodichain/lab/sim/_runtime_controls.py @@ -24,11 +24,53 @@ import numpy as np import torch -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.""" diff --git a/embodichain/lab/sim/atomic_skills/primitives/_helpers.py b/embodichain/lab/sim/atomic_skills/primitives/_helpers.py index af7c3b075e..dab1a24502 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/_helpers.py +++ b/embodichain/lab/sim/atomic_skills/primitives/_helpers.py @@ -19,6 +19,7 @@ from __future__ import annotations from collections.abc import Mapping, Sequence +import math from typing import TYPE_CHECKING import torch @@ -413,6 +414,7 @@ def split_joint_trajectory_at_pose( arrival_indices: torch.Tensor | None = None, split_target_index: int | None = None, env_ids: tuple[int, ...] | None = None, + minimum_segment_duration: float | None = None, ) -> tuple[torch.Tensor, torch.Tensor]: """Split at a declared target visit, or the closest pose when none is supplied. @@ -430,6 +432,9 @@ def split_joint_trajectory_at_pose( control_dt: Destination command period, required together with ``dt``. arrival_indices: Optional strictly ordered target visits ``(B, K)``. split_target_index: Target column identifying the intended occurrence. + minimum_segment_duration: Optional minimum duration in seconds for each + row's moving segment. Native intervals are stretched before control + resampling and batch padding, without speeding up longer segments. Returns: The resampled trajectory segments before and after the split pose. @@ -440,6 +445,13 @@ def split_joint_trajectory_at_pose( raise ValueError("split_pose must have shape (B, 4, 4).") if first_sample_count < 2 or second_sample_count < 2: raise ValueError("Both split trajectory segments require at least two samples.") + if minimum_segment_duration is not None and ( + dt is None + or control_dt is None + or not math.isfinite(minimum_segment_duration) + or minimum_segment_duration <= 0.0 + ): + raise ValueError("Minimum segment duration requires positive finite timing.") trajectory_xpos = robot.compute_batch_fk( qpos=trajectory, @@ -570,6 +582,12 @@ def split_joint_trajectory_at_pose( 0.0, ), ) + if minimum_segment_duration is not None: + durations = torch.stack((first_dt.sum(dim=1), second_dt.sum(dim=1))) + durations = torch.where(durations > 0, durations, 1.0) + scales = (minimum_segment_duration / durations).clamp_min(1.0) + first_dt = first_dt * scales[0, :, None] + second_dt = second_dt * scales[1, :, None] return ( resample_planned_trajectory( first, diff --git a/embodichain/lab/sim/atomic_skills/primitives/pour.py b/embodichain/lab/sim/atomic_skills/primitives/pour.py index a409989915..7804f6e2d8 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/pour.py +++ b/embodichain/lab/sim/atomic_skills/primitives/pour.py @@ -167,6 +167,9 @@ class PourOptions(SkillOptions): dwell_duration: float = 0.5 """Minimum time to remain at the poured orientation, in seconds.""" + rotation_duration: float | None = None + """Minimum time allocated to each outward and return rotation, in seconds.""" + mouth_height: float = 0.08 """Preferred mouth height above the receiver opening plane, in metres.""" @@ -187,6 +190,12 @@ def __post_init__(self) -> None: raise ValueError("rotate_angle must be finite.") if not math.isfinite(self.dwell_duration) or self.dwell_duration < 0.0: raise ValueError("dwell_duration must be finite and non-negative.") + if self.rotation_duration is not None and ( + isinstance(self.rotation_duration, bool) + or not math.isfinite(self.rotation_duration) + or self.rotation_duration <= 0.0 + ): + raise ValueError("rotation_duration must be finite and positive.") for name in ( "mouth_height", "max_mouth_height", @@ -334,6 +343,10 @@ def _plan( control_dt = context.require_control_dt() minimum_pour = max(2, request.motion_policy.sample_count // 2) minimum_return = max(2, request.motion_policy.sample_count - minimum_pour) + if options.rotation_duration is not None: + rotation_waypoints = math.ceil(options.rotation_duration / control_dt) + 1 + minimum_pour = max(minimum_pour, rotation_waypoints) + minimum_return = max(minimum_return, rotation_waypoints) pour_arm, return_arm = split_joint_trajectory_at_pose( result.positions, poured_eef_pose, @@ -345,6 +358,7 @@ def _plan( control_dt=control_dt, arrival_indices=result.waypoint_indices, split_target_index=0, + minimum_segment_duration=options.rotation_duration, ) dwell_steps = math.ceil(options.dwell_duration / control_dt) dwell_arm = pour_arm[:, -1:].expand(-1, dwell_steps, -1) @@ -421,6 +435,11 @@ def _target_paths( axis = torch.linalg.cross(normal, direction) if fractions is None: count = max(2, math.ceil(options.rotate_angle / options.max_rotation_step)) + if options.rotation_duration is not None: + count = max( + count, + math.ceil(options.rotation_duration / context.require_control_dt()), + ) fractions = torch.linspace(0, 1, count + 1, device=self.device) else: count = fractions.numel() - 1 diff --git a/embodichain/lab/sim/cfg/fluid.py b/embodichain/lab/sim/cfg/fluid.py index aa3a94b2fc..22d7dc0af4 100644 --- a/embodichain/lab/sim/cfg/fluid.py +++ b/embodichain/lab/sim/cfg/fluid.py @@ -149,11 +149,12 @@ class FluidObjectCfg(ObjectBaseCfg): Density and spacing determine mass and must agree across fluid objects. Solver viscosity, surface tension and iteration controls belong exclusively - to DexSim's SPHSolverCfg supplied through DexUniSolverCfg.sph_cfg. + to DexUniSolverCfg.sph_cfg. The nested DexSim fluid component is not an + independently selectable Newton solver. - This config owns density and spacing. Native SPH defaults for those fields - are placeholders in both mapping and native ``sph_cfg`` values; non-default - solver values must match the fluid declaration. + This config owns density and spacing. Native nested DFSPH defaults for + those fields are placeholders in both mapping and native ``sph_cfg`` + values; non-default solver values must match the fluid declaration. """ density: float = 1000.0 diff --git a/embodichain/lab/sim/cfg/physics.py b/embodichain/lab/sim/cfg/physics.py index 22023f80df..c04440966d 100644 --- a/embodichain/lab/sim/cfg/physics.py +++ b/embodichain/lab/sim/cfg/physics.py @@ -527,6 +527,10 @@ def to_dexsim_cfg( def _normalize_newton_solver_type(solver_type: str) -> str: """Normalize public EmbodiChain and DexSim Newton solver aliases.""" key = solver_type.replace("-", "_").lower() + if key in {"sph", "sphsolvercfg", "sph_solver_cfg"}: + logger.log_error( + "Unsupported Newton solver type 'sph'; configure DexUni with sph_cfg." + ) aliases = { "mjwarp": "mujoco_warp", "mjwarpsolver": "mujoco_warp", @@ -587,7 +591,11 @@ def _newton_solver_cfg_to_dexsim( from dexsim.engine import newton_physics provider_cfg_type = getattr(newton_physics, "ProviderNewtonSolverCfg", None) - if provider_cfg_type is not None and isinstance(solver_cfg, provider_cfg_type): + if ( + provider_cfg_type is not None + and isinstance(solver_cfg, provider_cfg_type) + and str(solver_cfg.solver_type) == "provider" + ): return solver_cfg _normalize_newton_solver_type(str(solver_cfg.solver_type)) return solver_cfg diff --git a/embodichain/lab/sim/cfg/rigid_object.py b/embodichain/lab/sim/cfg/rigid_object.py index 7101a1306f..c386881e5d 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. @@ -99,6 +112,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 3b6dcac224..5f512c57ca 100644 --- a/embodichain/lab/sim/objects/backends/newton.py +++ b/embodichain/lab/sim/objects/backends/newton.py @@ -345,7 +345,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 e6a1a1e642..476984f669 100644 --- a/embodichain/lab/sim/objects/rigid_object.py +++ b/embodichain/lab/sim/objects/rigid_object.py @@ -503,7 +503,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 diff --git a/embodichain/lab/sim/physics/newton.py b/embodichain/lab/sim/physics/newton.py index 743320f2b7..50323fbf2a 100644 --- a/embodichain/lab/sim/physics/newton.py +++ b/embodichain/lab/sim/physics/newton.py @@ -331,35 +331,40 @@ def supports_dexuni_fluids(self) -> bool: return "sph_cfg" in getattr(DexUniSolverCfg, "__dataclass_fields__", {}) + def _prepared_dexuni_fluid(self) -> tuple[object | None, object | None]: + """Resolve the fluid component owned by a prepared DexUni backend.""" + from dexsim.engine.newton_physics.backend_registry import get_newton_backend + + backend = get_newton_backend(self._manager._world) + if backend is None or backend.solver_type != "dexuni": + return None, None + # DexSim names this internal component sph_solver. It is not an + # independently selected Newton solver in EmbodiChain. + return backend, getattr(backend, "sph_solver", None) + @property def supports_fluid_worlds(self) -> bool: - """Whether the loaded SPH artifact supports isolated worlds and reset.""" + """Whether the prepared DexUni fluid supports isolated worlds and reset.""" if not self.supports_dexuni_fluids: return False - from dexsim.engine.newton_physics.solver_runtime import ensure_solver_available - - solver = ensure_solver_available("sph") + _, solver = self._prepared_dexuni_fluid() return bool( getattr(solver, "supports_world_isolation", False) and getattr(solver, "supports_world_reset", False) ) def invalidate_fluid_state(self, env_ids: Sequence[int] | None = None) -> None: - """Clear selected SPH history after an explicit state write. + """Clear selected DexUni fluid history after an explicit state write. Args: env_ids: Local worlds to clear, or all local worlds when omitted. Shared boundary history remains unchanged. Legacy single-world artifacts rebaseline their optional SDF history directly. """ - from dexsim.engine.newton_physics.backend_registry import get_newton_backend - - backend = get_newton_backend(self._manager._world) - if backend is None or backend.solver_type != "dexuni": - raise RuntimeError("Prepared DexUni fluid backend required.") - solver = getattr(backend, "sph_solver", None) + backend, solver = self._prepared_dexuni_fluid() if solver is None: - raise RuntimeError("Prepared DexUni SPH solver required.") + raise RuntimeError("Prepared DexUni fluid solver required.") + assert backend is not None backend.invalidate_cuda_graph("fluid state write") self._fluid_render_dirty = True if getattr(solver, "supports_world_reset", False): @@ -376,16 +381,17 @@ 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.""" - from dexsim.engine.newton_physics.backend_registry import get_newton_backend + """Read detached DexUni fluid diagnostics without stepping.""" import torch - backend = get_newton_backend(self._manager._world) - solver = getattr(backend, "sph_solver", None) + _, solver = self._prepared_dexuni_fluid() if solver is None: - raise RuntimeError("Prepared DexUni SPH solver required.") + raise RuntimeError("Prepared DexUni fluid solver required.") density = wp.to_torch(solver.particle_density) values = [] for entity in fluid._entities: diff --git a/embodichain/lab/sim/shapes.py b/embodichain/lab/sim/shapes.py index b4864342ec..6afd724a50 100755 --- a/embodichain/lab/sim/shapes.py +++ b/embodichain/lab/sim/shapes.py @@ -72,6 +72,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.""" @@ -127,6 +130,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, @@ -188,6 +192,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( @@ -231,6 +238,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) @@ -242,6 +251,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, @@ -284,6 +294,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 990dfb290b..3b3f77b222 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -1895,6 +1895,14 @@ def prepare(self) -> None: scene.prepare_runtime_config(result) self._prepare_spawn_runtime(result) scene.bind() + if ( + self.num_envs > 1 + and self._fluid_objects + and not self.physics.supports_fluid_worlds + ): + raise NotImplementedError( + "Batched DexUni fluids require world isolation and selected-world reset." + ) for visual in getattr(self, "_visual_objects", {}).values(): if visual.is_spawn_bound: visual._sync_spawn_state() @@ -3031,12 +3039,6 @@ def add_fluid_object(self, cfg: FluidObjectCfg) -> FluidObject: ) if getattr(self, "_deformable_objects", {}): raise NotImplementedError("DexUni fluids cannot coexist with deformables.") - if self.num_envs != 1 and not getattr( - self.physics, "supports_fluid_worlds", False - ): - raise NotImplementedError( - "Batch fluids require a DexSim SPH build with world isolation and selected-world reset." - ) if self.spawn_result is not None: raise NotImplementedError("Declare fluids before prepare().") if not isinstance(cfg, FluidObjectCfg): @@ -3075,9 +3077,11 @@ def add_fluid_object(self, cfg: FluidObjectCfg) -> FluidObject: if isinstance(configured, Mapping) else getattr(configured, "sph_cfg", None) ) - from dexsim.engine.newton_physics import SPHSolverCfg + from dexsim.engine.newton_physics import DexUniSolverCfg - defaults = SPHSolverCfg() + # Query defaults through the supported DexUni composition, not a + # standalone fluid solver configuration. + defaults = DexUniSolverCfg(sph_cfg={"sph_method": "dfsph"}).sph_cfg for key, value in ( ("rest_density", cfg.density), ("particle_diameter", cfg.particle_spacing), @@ -3131,6 +3135,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 diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index 6ea0e43228..81e3d91808 100644 --- a/embodichain/lab/sim/spawn/descriptors.py +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -375,8 +375,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, @@ -397,19 +403,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"), @@ -1944,7 +1996,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/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index 2df41efafb..b6b9131811 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -1051,6 +1051,7 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: "release_retreat_distance", "rotate_angle", "dwell_duration", + "rotation_duration", "mouth_height", "max_mouth_height", "clearance", @@ -1343,6 +1344,7 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: { "rotate_angle", "dwell_duration", + "rotation_duration", "mouth_height", "max_mouth_height", "clearance", @@ -1363,6 +1365,13 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: path=f"{path}.dwell_duration", minimum=0.0, ) + if "rotation_duration" in config: + kwargs["rotation_duration"] = _real( + config["rotation_duration"], + path=f"{path}.rotation_duration", + minimum=0.0, + strict_minimum=True, + ) for field_name in ( "mouth_height", "max_mouth_height", diff --git a/examples/README.md b/examples/README.md index 63b755cadb..29d45a38e8 100644 --- a/examples/README.md +++ b/examples/README.md @@ -13,6 +13,13 @@ embodichain run-example sim/atomic_skill --skill place --variants 8 embodichain run-example sim/atomic_skill/coordinated_pickment --help ``` +The DexUni pouring example plans a fresh Franka grasp, pour and place sequence +with live fluid physics. It requires CUDA and a DexSim build with DFSPH support: + +```bash +embodichain run-example sim/demo/franka_dexuni_pour_demo --help +``` + A directory with `__main__.py` can be selected by its directory path. All arguments after the path go to the script, including `--help`; an optional `--` separator is also accepted. `--list` discovers Python files from the directory without importing diff --git a/examples/sim/demo/franka_dexuni_pour_demo.py b/examples/sim/demo/franka_dexuni_pour_demo.py new file mode 100644 index 0000000000..3abc695471 --- /dev/null +++ b/examples/sim/demo/franka_dexuni_pour_demo.py @@ -0,0 +1,342 @@ +# ---------------------------------------------------------------------------- +# 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 +from types import SimpleNamespace + +import imageio.v2 as imageio +import numpy as np +import torch + +from embodichain.lab.gym.envs.objectives.conditions import ObjectMotionMetrics +from embodichain.lab.gym.utils.gym_utils import config_to_cfg, configure_fluid_spacing +from embodichain.lab.gym.utils.composition import resolve_gym_components +from embodichain.lab.task_program.integrations import ( + load_configured_task_program_deployment, +) +from embodichain.lab.sim import SimulationManager +from embodichain.lab.sim.atomic_skills import ( + AxisAlignAffordance, + ControlPartCommandProfile, + GraspGoal, + HeldObjectPoseGoal, + ObjectSemantics, + PlaceGoal, + PourGoal, + create_simulation_atomic_skill_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 +from embodichain.utils.config_paths import resolve_config_path +from embodichain.utils.utility import load_config + +__all__ = ["main"] + +TASK = resolve_config_path( + "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=Path("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) + declaration = load_config(str(TASK)) + cfg = config_to_cfg(declaration, source_path=TASK) + components = resolve_gym_components(declaration, base_dir=TASK.parent) + deployment = load_configured_task_program_deployment( + task_program=declaration["task_program"], + skill_profile=components.embodiment_skill_profile, + base_dir=TASK.parent, + ) + preset = next( + item + for item in deployment.integration.registration.robot_profile_binding.presets + if item.preset_id == deployment.selection.runtime_preset + ) + pick_options = preset.skill_option_template("pick") + qualification = declaration["collection"]["qualification"] + volume_observation = getattr(cfg.observations, "received_volume") + cfg.num_envs = args.num_envs + cfg.sim_cfg.headless = True + cfg.sim_cfg.render_cfg.renderer = args.renderer + cfg.enable_sensor = False + if args.renderer == "no-render": + cfg.events.record_camera = None + for fluid in cfg.fluid_object: + fluid.render.mode = "none" + configure_fluid_spacing(cfg, args.spacing) + _use_dexuni(cfg, args) + cfg.sim_cfg.num_envs = cfg.num_envs + sim = SimulationManager(cfg.sim_cfg) + writers = [] + robot = cup = camera = engine = measurement_env = 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 = pick_options.fixed_object_to_eef.to(sim.device) + measurement_env = SimpleNamespace( + sim=sim, device=sim.device, num_envs=sim.num_envs + ) + 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_skill_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 = preset.motion_policy + 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=pick_options, + ), + engine.make_invocation( + "move_held_object", + HeldObjectPoseGoal(pour_pose), + control_parts=bindings, + motion_policy=policy, + skill_options=preset.skill_option_template( + "simulation.move_held_object" + ), + ), + engine.make_invocation( + "pour", + PourGoal(), + control_parts=bindings, + motion_policy=policy, + skill_options=preset.skill_option_template("simulation.pour"), + ), + engine.make_invocation( + "place", + PlaceGoal(initial_pose @ object_to_eef), + control_parts=bindings, + motion_policy=policy, + skill_options=preset.skill_option_template("place"), + ), + ) + 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 Skill 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 + motion_metrics = ObjectMotionMetrics(cup.get_local_pose()) + 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) + motion_metrics.update(cup.get_local_pose()) + 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.") + received = volume_observation.func( + measurement_env, {}, **volume_observation.params + ) + sample = { + "time_s": (i + 1) * control_dt, + "cup_pose": cup.get_local_pose().cpu().tolist(), + "received_ml": (received[:, 0] * 1e6).cpu().tolist(), + } + samples.append(sample) + print(json.dumps(sample), flush=True) + measured_motion = motion_metrics.evaluate( + **{ + key: value + for key, value in qualification["object_motion"].items() + if key != "uid" + } + ) + report = { + "coupling": "online one-way rigid-to-fluid", + "controller": "fresh Atomic Skill joint targets", + "cup": { + **{ + key: value.cpu().tolist() + for key, value in measured_motion.metrics.items() + }, + "accepted": measured_motion.satisfied.cpu().tolist(), + }, + "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 >= qualification["observations"]["received_volume"]["min"][0] * 1e6 + 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 = measurement_env = None + sim.destroy(exit_process=False) + del sim + SimulationManager.flush_cleanup_queue() + + +if __name__ == "__main__": + main() diff --git a/extensions/embodichain_tasks/configs/components/embodiments/franka_panda_fast_fluid.yaml b/extensions/embodichain_tasks/configs/components/embodiments/franka_panda_fast_fluid.yaml new file mode 100644 index 0000000000..1e404c905f --- /dev/null +++ b/extensions/embodichain_tasks/configs/components/embodiments/franka_panda_fast_fluid.yaml @@ -0,0 +1,116 @@ +embodiment_id: franka_panda_fast_fluid +simulation: + class_type: Franka + arm_model: fr3 + uid: FrankaPanda + urdf_cfg: + fname: franka_fr3_panda_hand + init_qpos: + - 0.0 + - -0.569 + - 0.0 + - -2.81 + - 0.0 + - 3.037 + - 0.741 + - 0.04 + - 0.04 + 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]: 160.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: 3.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_fast_fluid + 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 + gripper: eef + command_presets: [] + runtime_services: + grasp_pose_generators: + eef: + kind: antipodal_parallel_jaw + sample_count: 1000 + point_spacing: 0.012 + filter_ground_collision: false +grippers: + eef: + class_type: FrankaPandaHand + mode: mounted + motion_part: arm + control_part: eef + component_type: hand + transform: + - - 0.7071067811865476 + - 0.7071067811865475 + - 0.0 + - 0.0 + - - -0.7071067811865475 + - 0.7071067811865476 + - 0.0 + - 0.0 + - - 0.0 + - 0.0 + - 1.0 + - 0.0 + - - 0.0 + - 0.0 + - 0.0 + - 1.0 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 0000000000..aa4b145462 --- /dev/null +++ b/extensions/embodichain_tasks/configs/components/execution_policies/trajectory_open_loop_fluid_pour.yaml @@ -0,0 +1,31 @@ +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: 80 + acceleration_limit: 1.5 + dynamic_collision_mode: "off" + +tracking: + kind: timed + settle_duration: 0.2 + +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 0000000000..6339b59b00 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/README.md @@ -0,0 +1,93 @@ +# Franka 在线倒水 + +倒水任务在同一个 World 中通过 DexUni 推进 Franka、动态杯子和 DFSPH 水体。机器人 +使用关节驱动和真实接触抓起杯子,将水倒入接水碗,再放回杯子。耦合方向为刚体到 +流体;水不会反作用于机器人或杯子。初始水体按杯内体积采样,不依赖预录粒子或轨迹。 + +## 依赖 + +基础流体接口来自 [PR #777](https://github.com/DexForce/EmbodiChain/pull/777)。 +仓库锁定的 `dexsim_engine==0.6.0rc1` 提供 `DexUniSolverCfg.sph_cfg`、DexUni 流体多 world +隔离和选择性 reset;在线流体运行还需要 CUDA。 +任务网格由资产缓存中的 `PourCupAssets` 提供。 + +## 运行 + +`run-task` 在线生成 `Pick → MoveHeldObject → Pour → Place` 动作,并按部署配置 +验收杯子运动与接水量: + +```bash +embodichain run-task \ + --gym-config extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml \ + --num-envs 1 --headless --renderer fast-rt +``` + +任务的执行节奏由 +[`trajectory_open_loop_fluid_pour.yaml`](../../../../components/execution_policies/trajectory_open_loop_fluid_pour.yaml) +配置。轨迹最少采样 80 点,任务级规划加速度预算为 1.5 rad/s²;规划器仍遵守 +机器人原有的关节速度上限。程序的 `wait_stable` 负责放回后的稳定检查。 + +倾倒和回正各至少运行 4 秒;`simulation.pour` 的 `rotation_duration` 在原规划轨迹 +较短时拉伸整段运动,不再用终点保持帧凑时长。它不改变抓杯与移杯的控制频率。 + +流体专用的 `franka_panda_fast_fluid` 组件将仿真夹指最大驱动力设为 160,接触 +动摩擦系数设为 3.0,以在更高加速度下保持抓取;这组任务参数不代表实机标定。 + +独立 Sim 示例从当前场景规划 Atomic Skill 目标,复用部署中的运动策略、动作选项、 +接水区域和验收阈值,通过相同的 DexUni/DFSPH 物理场景执行并录制视频: + +```bash +embodichain run-example sim/demo/franka_dexuni_pour_demo --headless --renderer fast-rt +``` + +增加 `--num-envs 4` 可在同一个 World 中并行运行四个环境,分别输出 +`fluid_sim_env0.mp4` 至 `fluid_sim_env3.mp4`。 + +默认粒子间距为 1.5 mm,初始填充 76,607 粒。`run-task` 的 +`--particle-spacing 0.003` 或 Sim 的 `--spacing 0.003` 可用于较快的物理检查, +但会改变流体分辨率。NoRender 仅用于物理检查。 + +## 求解与碰撞 + +`env.fluid.yaml` 声明 DexUni 的 `sph_cfg`、机器人和场景。外层物理步为 1/240 s, +每步 2 个 DFSPH 子步,每个 DFSPH 子步包含 15 个 MuJoCo 刚体子步;控制周期为 +0.025 s(每 6 个物理步更新一次控制)。未指定关节所有权时,流体准备阶段将完整的机器人和杯子关节拓扑分配给 +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 的 `qualification_result.json` 和 Sim 的 `validation.json` 分别记录任务完成情况、 +杯子抬升、最大倾角、放回误差和接水量。完整验收要求实际抬升至少 5 cm、倾角 +至少 45°、放回误差不超过 3 cm,且最终接水至少 50 mL;规划成功或部分视频不能 +替代这些物理测量。部署通过 `collection.qualification.object_motion` 声明运动阈值, +通过 `observations` 声明原始观测上下限;接水量按立方米配置,`0.00005` 为 50 mL。 +`water_summary` 的有效性边界在每次低频采样时检查,接水量在最终采样时检查。使用 `--record-trajectory --record-fluid-state` 可在验收通过后 +保存完整粒子状态;默认任务实测约 1.30 GiB/环境(76,607 粒、757 状态帧)。普通 +`ReplayWrapper` 的 `kinematic`/`control` 模式回填粒子,`dynamic` 模式只回填 +初始粒子状态,之后随录制控制动作重新求解流体。 + +```bash +embodichain run-task \ + --gym-config extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml \ + --num-envs 1 --headless --renderer fast-rt \ + --record-trajectory --record-fluid-state \ + --trajectory-save-dir outputs/fluid-task-program-online +embodichain run-task \ + --gym-config extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml \ + --num-envs 1 --replay \ + --replay_trajectory outputs/fluid-task-program-online/fluid_batch_000000_RUN_ID.pt \ + --replay_mode kinematic # 或 dynamic +``` + +将 `RUN_ID` 替换为录制命令输出的对应后缀。 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 0000000000..04094d0065 --- /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: 1000 +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/task.franka.fluid.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml new file mode 100644 index 0000000000..eef2267866 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml @@ -0,0 +1,39 @@ +id: PourCup-Franka-Fluid-v1 +seed: 0 +enable_sensor: false +collection: + qualification: + object_motion: + uid: cup + min_lift_m: 0.05 + min_tilt_deg: 45.0 + max_return_error_m: 0.03 + observations: + water_summary: + min: [null, null, null, null, 1.0] + when: always + received_volume: + min: [0.00005] +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_fast_fluid.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_program/integration.fluid.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/integration.fluid.yaml new file mode 100644 index 0000000000..1ae277f457 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/integration.fluid.yaml @@ -0,0 +1,98 @@ +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: 10 + 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 + rotation_duration: 4.0 + place: + kind: place + hand_interp_steps: 10 + release_settle_steps: 10 + 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/program.fluid.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task_program/program.fluid.yaml new file mode 100644 index 0000000000..2aa890ee6b --- /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/scripts/tutorials/sim/create_fluid.py b/scripts/tutorials/sim/create_fluid.py index 9e837b379e..ac9c719c55 100644 --- a/scripts/tutorials/sim/create_fluid.py +++ b/scripts/tutorials/sim/create_fluid.py @@ -98,7 +98,7 @@ def main(args: argparse.Namespace | None = None) -> None: ) fluid = None try: - # SPH queries texture SDFs; render boxes alone are not fluid boundaries. + # DexUni's DFSPH component queries texture SDFs; render boxes alone are not fluid boundaries. for uid, position, size in ( ("floor", (0, 0, 0), (0.32, 0.32, 0.02)), ("left", (-0.16, 0, 0.15), (0.02, 0.32, 0.3)), diff --git a/tests/data/assets/test_task_assets.py b/tests/data/assets/test_task_assets.py new file mode 100644 index 0000000000..65bdfce98d --- /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 f0c277ac7b..bef144d386 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 from embodichain_robots.agilex.pika.assets import PikaGripper from embodichain_robots.sharpa.wave.assets import SharpaWave @@ -53,7 +53,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, PikaGripper, SharpaWave, WujiHand, YamI2rt): +for cls in (CobotMagicArm, FlexivRizon4S, UnitreeGo2Locomotion, UniversalRobots, DexforceW1V030, PourCupAssets, PourCupDemoData, PikaGripper, SharpaWave, WujiHand, YamI2rt): try: cls() except Captured: @@ -124,6 +124,11 @@ def test_assets_download_from_the_configured_prefix(prefix: str) -> None: f"{HF_PREFIX}eef_assets/PikaGripper.zip", ] 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", @@ -154,6 +159,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_observation_functors.py b/tests/gym/envs/managers/test_observation_functors.py index 42e44f91e3..f6f71c9f57 100644 --- a/tests/gym/envs/managers/test_observation_functors.py +++ b/tests/gym/envs/managers/test_observation_functors.py @@ -26,6 +26,20 @@ from unittest.mock import MagicMock, Mock, patch +def test_public_exports_include_entity_and_fluid_queries() -> None: + from embodichain.lab.gym.envs.managers import observations + + namespace = {} + exec("from embodichain.lab.gym.envs.managers.observations import *", namespace) + for name in ( + "get_entity_state", + "get_fluid_particle_state", + "get_fluid_summary", + "get_fluid_volume_in_region", + ): + assert namespace[name] is getattr(observations, name) + + class MockRobot: """Mock robot for observation functor tests.""" diff --git a/tests/gym/envs/managers/test_record.py b/tests/gym/envs/managers/test_record.py index 08c2a22e01..c0104f97c6 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/task_program/test_configured_integration.py b/tests/gym/envs/task_program/test_configured_integration.py index 367738577d..9c7b15cf3b 100644 --- a/tests/gym/envs/task_program/test_configured_integration.py +++ b/tests/gym/envs/task_program/test_configured_integration.py @@ -2630,6 +2630,34 @@ def test_pour_dwell_duration_decodes() -> None: assert options.dwell_duration == pytest.approx(0.75) +def test_pour_rotation_duration_decodes() -> None: + from embodichain.lab.task_program.integrations.configured import ( + _decode_skill_options, + ) + + options = _decode_skill_options( + {"kind": "pour", "rotation_duration": 3.0}, + path="options", + ) + assert isinstance(options, PourOptions) + assert options.rotation_duration == pytest.approx(3.0) + + +@pytest.mark.parametrize("value", [0.0, -0.1, float("inf")]) +def test_pour_rotation_duration_rejects_nonpositive_or_nonfinite_values( + value: float, +) -> None: + from embodichain.lab.task_program.integrations.configured import ( + _decode_skill_options, + ) + + with pytest.raises(ValueError, match="rotation_duration"): + _decode_skill_options( + {"kind": "pour", "rotation_duration": value}, + path="options", + ) + + def test_configured_pour_target_lowers_live_geometry_and_is_closed() -> None: from embodichain.lab.task_program.integrations.configured import ( _decode_registered_lowerer, diff --git a/tests/gym/envs/test_demo.py b/tests/gym/envs/test_demo.py index b8f9434f7a..9e31a9e5a9 100644 --- a/tests/gym/envs/test_demo.py +++ b/tests/gym/envs/test_demo.py @@ -32,6 +32,7 @@ from embodichain.lab.gym.envs.demo import ( DemoExecutionCfg, + DemoQualification, DemoSegment, DemoSegmentResult, execute_demo_episode, @@ -2219,3 +2220,142 @@ def _append_fixture(env, frame, action): env._traj_raw_action = action EmbodiedEnv._write_trajectory_step(env, frame) + + +def test_qualification_separates_final_bounds_from_always_sampled_health() -> None: + initial = torch.tensor([[0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 1.0]]).repeat(2, 1) + env = SimpleNamespace( + num_envs=2, + device=torch.device("cpu"), + step_dt=1.0, + pose=initial.clone(), + score=torch.zeros(2, 1), + health=torch.ones(2, 1), + ) + env.sim = SimpleNamespace( + get_rigid_object=lambda uid: SimpleNamespace(get_local_pose=lambda: env.pose) + ) + env.cfg = SimpleNamespace( + observations=SimpleNamespace( + score=SimpleNamespace(func=lambda host, obs: host.score, params={}), + health=SimpleNamespace(func=lambda host, obs: host.health, params={}), + ) + ) + qualification = DemoQualification( + env, + { + "object_motion": { + "uid": "payload", + "min_lift_m": 0.05, + "min_tilt_deg": 45.0, + "max_return_error_m": 0.03, + }, + "observations": { + "score": {"min": [0.5]}, + "health": {"min": [1.0], "when": "always"}, + }, + }, + ) + tilted = initial.clone() + tilted[:, 2] += 0.12 + tilted[:, 3] = torch.sin(torch.deg2rad(torch.tensor(35.0))) + tilted[:, 6] = torch.cos(torch.deg2rad(torch.tensor(35.0))) + qualification.start() + env.pose.copy_(tilted) + env.health[1] = 0 + qualification.after_step() + env.pose.copy_(initial) + env.score.fill_(1.0) + env.health.fill_(1.0) + qualification.after_step() + report = qualification.finish([0]) + assert report["accepted"] + assert report["accepted_rows"] == [True, False] + assert report["object_motion"]["max_tilt_deg"] == pytest.approx([70.0, 70.0]) + assert report["samples"][0]["observations"]["score"] == [[0.0], [0.0]] + # A fresh attempt neither inherits the failed health sample nor motion peaks. + qualification.start() + assert not qualification.finish([0, 1])["accepted"] + env.pose.copy_(tilted) + qualification.after_step() + env.pose.copy_(initial) + qualification.after_step() + assert qualification.finish([0, 1])["accepted_rows"] == [True, True] + + +@pytest.mark.parametrize( + "bounds", + [ + {"min": [0.0], "unknown": True}, + {"min": [1.0], "max": [0.0]}, + {"min": [float("nan")]}, + ], +) +def test_qualification_rejects_invalid_observation_bounds(bounds) -> None: + env = SimpleNamespace(num_envs=1, device=torch.device("cpu"), step_dt=0.1) + with pytest.raises(ValueError): + DemoQualification(env, {"observations": {"metric": bounds}}) + + +@pytest.mark.parametrize("dtype", [torch.float32, torch.float64]) +@pytest.mark.parametrize("when", ["always", "final"]) +def test_qualification_preserves_inclusive_floating_boundaries(dtype, when) -> None: + env = SimpleNamespace(num_envs=1, device=torch.device("cpu"), step_dt=0.1) + env.cfg = SimpleNamespace( + observations=SimpleNamespace( + positive=SimpleNamespace( + func=lambda host, obs: torch.tensor([[0.1]], dtype=dtype), params={} + ), + negative=SimpleNamespace( + func=lambda host, obs: torch.tensor([[-0.1]], dtype=dtype), params={} + ), + ) + ) + qualification = DemoQualification( + env, + { + "observations": { + "positive": {"min": [0.1], "when": when}, + "negative": {"max": [-0.1], "when": when}, + } + }, + ) + qualification.start() + assert qualification.finish([0])["accepted"] + + +@pytest.mark.parametrize("dtype", [torch.int64, torch.bool]) +@pytest.mark.parametrize("minimum,accepted", [(1.0, True), (1.1, False)]) +def test_qualification_keeps_unbounded_limits_for_discrete_values( + dtype, minimum, accepted +) -> None: + env = SimpleNamespace(num_envs=1, device=torch.device("cpu"), step_dt=0.1) + env.cfg = SimpleNamespace( + observations=SimpleNamespace( + flag=SimpleNamespace( + func=lambda host, obs: torch.tensor([[1]], dtype=dtype), params={} + ), + ) + ) + qualification = DemoQualification( + env, {"observations": {"flag": {"min": [minimum]}}} + ) + qualification.start() + assert qualification.finish([0])["accepted"] is accepted + + +def test_qualification_reports_unsupported_policy_input_dependencies() -> None: + env = SimpleNamespace(num_envs=1, device=torch.device("cpu"), step_dt=0.1) + env.cfg = SimpleNamespace( + observations=SimpleNamespace( + derived=SimpleNamespace( + func=lambda host, obs: obs["robot"]["qpos"], params={} + ), + ) + ) + qualification = DemoQualification( + env, {"observations": {"derived": {"min": [0.0]}}} + ) + with pytest.raises(ValueError, match="raw data directly from env") as failure: + qualification.start() + assert isinstance(failure.value.__cause__, KeyError) diff --git a/tests/gym/envs/test_fluid_integration.py b/tests/gym/envs/test_fluid_integration.py new file mode 100644 index 0000000000..57925ea122 --- /dev/null +++ b/tests/gym/envs/test_fluid_integration.py @@ -0,0 +1,204 @@ +# ---------------------------------------------------------------------------- +# 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 pytest +import torch +from embodichain.lab.gym.utils.gym_utils import config_to_cfg +from embodichain.lab.gym.utils.gym_utils import configure_fluid_spacing +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" +) + + +@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_pour_collection_uses_measured_acceptance(): + declaration = load_config(TASK) + qualification = declaration["collection"]["qualification"] + assert qualification["object_motion"] == { + "uid": "cup", + "min_lift_m": 0.05, + "min_tilt_deg": 45.0, + "max_return_error_m": 0.03, + } + assert qualification["observations"]["received_volume"]["min"] == [0.00005] + assert qualification["observations"]["water_summary"] == { + "min": [None, None, None, None, 1.0], + "when": "always", + } + + +def test_fluid_spacing_override_keeps_solver_and_surface_scale(): + cfg = config_to_cfg(load_config(TASK), source_path=TASK) + configure_fluid_spacing(cfg, 0.003) + fluid = cfg.fluid_object[0] + assert fluid.particle_spacing == pytest.approx(0.003) + 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) + + +def test_pour_cup_embodiment_mounts_franka_hand(): + robot = config_to_cfg(load_config(TASK), source_path=TASK).robot + assert "hand" in robot.urdf_cfg.components + assert robot.control_parts["eef"] == [ + "fr3_finger_joint1", + "fr3_finger_joint2", + ] + assert len(robot.init_qpos) == 9 + + +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_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_trajectory.py b/tests/gym/envs/test_fluid_trajectory.py new file mode 100644 index 0000000000..a8867ebba6 --- /dev/null +++ b/tests/gym/envs/test_fluid_trajectory.py @@ -0,0 +1,217 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Control-aligned fluid-state artifact and replay contracts.""" + +from __future__ import annotations + +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest +import torch + +from embodichain.lab.gym.utils.trajectory_state import ( + FluidTrajectoryReader, + FluidTrajectoryRecorder, +) + + +class _Fluid: + def __init__(self, states: torch.Tensor) -> None: + self.data = SimpleNamespace(particle_state_w=states) + self.num_particles = states.shape[1] + self.written = None + + def write_particle_state_to_sim(self, state: torch.Tensor) -> None: + self.written = state.clone() + + +def _env(num_envs: int) -> SimpleNamespace: + states = torch.zeros(num_envs, 2, 6) + fluid = _Fluid(states) + offsets = torch.arange(num_envs, dtype=torch.float32)[:, None] * torch.tensor( + [[10.0, 0.0, 0.0]] + ) + return SimpleNamespace( + num_envs=num_envs, + device=torch.device("cpu"), + cfg=SimpleNamespace( + fluid_object=[ + SimpleNamespace( + uid="water", + particle_spacing=0.01, + spawn=SimpleNamespace(sample=lambda spacing: np.zeros((2, 3))), + ) + ] + ), + sim=SimpleNamespace( + arena_offsets=offsets, + get_fluid_object=lambda uid: fluid, + ), + fluid=fluid, + ) + + +def _source(num_envs: int, length: int) -> dict: + return { + "meta": { + "artifact_purpose": "recording", + "num_envs": num_envs, + "lengths": [length] * num_envs, + "dt": 0.025, + "demo_episodes": [ + {"program_hash": "program", "completed": True, "success": True} + for _ in range(num_envs) + ], + "fluid_qualification": { + "kind": "task_program", + "accepted": True, + "program_hash": "program", + }, + } + } + + +def _record(tmp_path: Path, env: SimpleNamespace) -> tuple[Path, dict]: + path = tmp_path / "trajectory.pt" + recorder = FluidTrajectoryRecorder(tmp_path, env.num_envs, 0.025) + env.fluid.data.particle_state_w[:, :, :3] = env.sim.arena_offsets[:, None, :] + recorder.capture(env, list(range(env.num_envs)), reset=True) + env.fluid.data.particle_state_w[:, :, 0] += 0.2 + env.fluid.data.particle_state_w[:, :, 3] = 0.5 + recorder.capture(env, list(range(env.num_envs))) + source = _source(env.num_envs, 1) + source["meta"]["fluid_state"] = recorder.finish(path, [1] * env.num_envs) + recorder.commit() + return path, source + + +def test_particle_frames_restore_position_velocity_and_arena_offsets(tmp_path: Path): + source_env = _env(1) + path, source = _record(tmp_path, source_env) + target = _env(2) + cfg = SimpleNamespace(num_envs=2, fluid_object=target.cfg.fluid_object) + reader = FluidTrajectoryReader(path, source, cfg) + try: + reader.apply(target, torch.tensor([0, 1])) + assert target.fluid.written is not None + torch.testing.assert_close(target.fluid.written[0, :, 0], torch.zeros(2)) + torch.testing.assert_close( + target.fluid.written[1, :, 0], torch.full((2,), 10.2) + ) + torch.testing.assert_close(target.fluid.written[1, :, 3], torch.full((2,), 0.5)) + finally: + reader.close() + + +def test_particle_frames_keep_multi_env_rows_independent(tmp_path: Path): + source_env = _env(2) + path, source = _record(tmp_path, source_env) + target = _env(2) + cfg = SimpleNamespace(num_envs=2, fluid_object=target.cfg.fluid_object) + reader = FluidTrajectoryReader(path, source, cfg) + try: + reader.apply(target, torch.tensor([1, 0])) + torch.testing.assert_close(target.fluid.written[0, :, 0], torch.full((2,), 0.2)) + torch.testing.assert_close( + target.fluid.written[1, :, 0], torch.full((2,), 10.0) + ) + finally: + reader.close() + + +def test_particle_reader_does_not_require_task_program_metadata(tmp_path: Path): + env = _env(1) + path, source = _record(tmp_path, env) + for key in ("fluid_qualification", "demo_episodes", "artifact_purpose"): + source["meta"].pop(key) + cfg = SimpleNamespace(num_envs=env.num_envs, fluid_object=env.cfg.fluid_object) + reader = FluidTrajectoryReader(path, source, cfg) + try: + reader.apply(env, torch.tensor([1])) + torch.testing.assert_close(env.fluid.written[0, :, 0], torch.full((2,), 0.2)) + finally: + reader.close() + + +def test_particle_source_rejects_malformed_and_tampered_sidecars(tmp_path: Path): + env = _env(1) + path, source = _record(tmp_path, env) + cfg = SimpleNamespace(num_envs=1, fluid_object=env.cfg.fluid_object) + sidecar = source["meta"].pop("fluid_state") + with pytest.raises(ValueError, match="sidecars"): + FluidTrajectoryReader(path, source, cfg) + source["meta"]["fluid_state"] = sidecar + manifest = path.parent / source["meta"]["fluid_state"]["manifests"][0] + chunk = next(manifest.parent.glob("chunk-*.npz")) + with chunk.open("ab") as stream: + stream.write(b"tampered") + with pytest.raises(ValueError, match="content changed"): + FluidTrajectoryReader(path, source, cfg) + + +def test_particle_recorder_rejects_frame_length_mismatch(tmp_path: Path): + env = _env(1) + recorder = FluidTrajectoryRecorder(tmp_path, 1, 0.025) + recorder.capture(env, [0], reset=True) + with pytest.raises(ValueError, match="align"): + recorder.finish(tmp_path / "trajectory.pt", [1]) + recorder.abort() + assert not recorder.directory.exists() + + +def test_selected_row_reset_restarts_only_its_particle_stream(tmp_path: Path): + env = _env(2) + recorder = FluidTrajectoryRecorder(tmp_path, 2, 0.025) + recorder.capture(env, [0, 1], reset=True) + recorder.capture(env, [0, 1]) + recorder.capture(env, [0], reset=True) + recorder.capture(env, [0]) + + sidecar = recorder.finish(tmp_path / "trajectory.pt", [1, 1]) + + assert len(sidecar["manifests"]) == 2 + recorder.commit() + manifest = tmp_path / sidecar["manifests"][0] + with pytest.raises(RuntimeError, match="finalized"): + recorder.capture(env, [0], reset=True) + assert manifest.is_file() + recorder.abort() + + +def test_partial_batch_publishes_only_selected_particle_rows(tmp_path: Path): + env = _env(2) + recorder = FluidTrajectoryRecorder(tmp_path, 2, 0.025, enabled_env_ids=[0]) + recorder.capture(env, [0, 1], reset=True) + recorder.capture(env, [0, 1]) + path = tmp_path / "trajectory.pt" + sidecar = recorder.finish(path, [1, 1], env_ids=[0]) + recorder.commit() + assert len(sidecar["manifests"]) == len(sidecar["parent_ids"]) == 1 + assert not (recorder.directory / "env_001").exists() + source = _source(1, 1) + source["meta"]["fluid_state"] = sidecar + target = _env(2) + reader = FluidTrajectoryReader( + path, source, SimpleNamespace(num_envs=2, fluid_object=target.cfg.fluid_object) + ) + try: + reader.apply(target, torch.tensor([0, 1])) + assert target.fluid.written is not None + finally: + reader.close() diff --git a/tests/gym/envs/test_objectives.py b/tests/gym/envs/test_objectives.py index db4d3b722a..c4eb81178f 100644 --- a/tests/gym/envs/test_objectives.py +++ b/tests/gym/envs/test_objectives.py @@ -469,3 +469,37 @@ def test_inclusive_bounds_and_speed_limits_use_measurement_precision(): objective = _runtime() objective.update(_state((0.1, -0.1), linear=0.05, angular=0.1), 0.2) assert objective.snapshot()["progress"].tolist() == [1, 1] + + +def test_object_motion_retains_invalid_evidence_and_owns_snapshots() -> None: + from embodichain.lab.gym.envs.objectives.conditions import ObjectMotionMetrics + + initial = torch.tensor([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]).repeat(2, 1) + metrics = ObjectMotionMetrics(initial) + invalid = initial.clone() + invalid[0, 2] = float("nan") + metrics.update(invalid) + metrics.update(initial) + result = metrics.evaluate(min_lift_m=0.0, min_tilt_deg=0.0, max_return_error_m=0.0) + assert result.valid.tolist() == [False, True] + assert result.satisfied.tolist() == [False, True] + result.metrics["return_error_m"].fill_(9.0) + fresh = metrics.evaluate(min_lift_m=0.0, min_tilt_deg=0.0, max_return_error_m=0.0) + torch.testing.assert_close(fresh.metrics["return_error_m"], torch.zeros(2)) + + +def test_object_motion_requires_measured_lift_tilt_and_return(): + import numpy as np + from embodichain.lab.gym.envs.objectives.conditions import ObjectMotionMetrics + + 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)) + metrics = ObjectMotionMetrics(torch.from_numpy(initial)) + metrics.update(torch.from_numpy(moving)) + metrics.update(torch.from_numpy(initial)) + result = metrics.evaluate(min_lift_m=0.05, min_tilt_deg=45, max_return_error_m=0.03) + assert result.satisfied.tolist() == [True, False] + assert result.metrics["max_tilt_deg"].tolist() == pytest.approx([70, 0]) diff --git a/tests/gym/envs/test_official_task_layout.py b/tests/gym/envs/test_official_task_layout.py index 72027af4a0..af6cbc55ed 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/envs/test_replay_wrapper.py b/tests/gym/envs/test_replay_wrapper.py index fbf28d2a74..b7fc75044f 100644 --- a/tests/gym/envs/test_replay_wrapper.py +++ b/tests/gym/envs/test_replay_wrapper.py @@ -19,6 +19,7 @@ from __future__ import annotations from types import SimpleNamespace +from contextlib import contextmanager import gymnasium as gym import pytest @@ -34,6 +35,7 @@ class _FakeSim: def __init__(self) -> None: self.physics_enabled: list[bool] = [] self.updates: list[tuple[float, int]] = [] + self.rendered = 0 def enable_physics(self, enabled: bool) -> None: self.physics_enabled.append(enabled) @@ -41,6 +43,23 @@ def enable_physics(self, enabled: bool) -> None: def update(self, physics_dt: float, substeps: int) -> None: self.updates.append((physics_dt, substeps)) + @contextmanager + def render_frame(self, *, force_visualization: bool = False): + self.rendered += 1 + yield + + +class _FakeFluidReader: + def __init__(self) -> None: + self.applied: list[list[int]] = [] + self.closed = False + + def apply(self, env, steps: torch.Tensor) -> None: + self.applied.append(steps.tolist()) + + def close(self) -> None: + self.closed = True + class _FakeEnv(gym.Env): metadata = {} @@ -264,3 +283,95 @@ def test_dynamic_replay_decodes_position_velocity_expert_action(fake_restore): def test_replay_rejects_unknown_mode(): with pytest.raises(ValueError, match="Invalid replay mode"): ReplayWrapper(_FakeEnv(), _trajectory(), mode="unsupported") + + +@pytest.mark.parametrize("mode", ["kinematic", "control"]) +def test_fluid_kinematic_and_control_restore_recorded_frames(fake_restore, mode): + env = _FakeEnv() + env.cfg.fluid_object = [object()] + reader = _FakeFluidReader() + replay = ReplayWrapper(env, _trajectory(), mode=mode, fluid_reader=reader) + + replay.reset() + replay.step(None) + if mode == "control": + replay.go_to_step(1) + + assert reader.applied == ([[0], [0], [1]] if mode == "control" else [[0], [0]]) + assert env.sim.updates == [] + assert env.sim.rendered == (2 if mode == "control" else 1) + replay.close() + assert reader.closed + + +def test_fluid_dynamic_restores_only_initial_particles(fake_restore): + env = _FakeEnv() + env.cfg.fluid_object = [object()] + reader = _FakeFluidReader() + replay = ReplayWrapper(env, _trajectory(), mode="dynamic", fluid_reader=reader) + + replay.reset() + replay.step(None) + + assert reader.applied == [[0]] + assert len(env.received_actions) == 1 + replay.close() + + +@pytest.mark.parametrize("num_envs", [1, 2]) +def test_fluid_control_can_seek_recorded_terminal_state(fake_restore, num_envs): + env = _FakeEnv(num_envs=num_envs) + env.cfg.fluid_object = [object()] + reader = _FakeFluidReader() + trajectory = _trajectory() + trajectory["terminal_states"] = trajectory["states"][:, -1].clone() + trajectory["terminal_states"]["robot"]["qpos"].fill_(10.0) + replay = ReplayWrapper(env, trajectory, mode="control", fluid_reader=reader) + + replay.reset() + replay.go_to_step(2) + + assert replay.control_max_step == 2 + assert reader.applied == [[0] * num_envs, [2] * num_envs] + torch.testing.assert_close( + env.last_restored["robot"]["qpos"], torch.full((num_envs, 2), 10.0) + ) + replay.close() + + +@pytest.mark.parametrize( + "lengths,expected_qpos", + [([2], [[2.0, 3.0]]), ([1, 2], [[10.0, 10.0], [6.0, 7.0]])], +) +@pytest.mark.parametrize( + "target_device", ["cpu", pytest.param("cuda", marks=pytest.mark.gpu)] +) +def test_fluid_control_horizon_keeps_rigid_and_particle_frames_aligned( + fake_restore, lengths, expected_qpos, target_device +): + env = _FakeEnv(num_envs=len(lengths)) + env.device = torch.device(target_device) + env.max_episode_steps = 1 + env.cfg.fluid_object = [object()] + reader = _FakeFluidReader() + trajectory = _trajectory(num_envs=len(lengths), meta_updates={"lengths": lengths}) + trajectory["terminal_states"] = trajectory["states"][:, -1].clone() + trajectory["terminal_states"]["robot"]["qpos"].fill_(10.0) + replay = ReplayWrapper(env, trajectory, mode="control", fluid_reader=reader) + + replay.reset() + replay.go_to_step(99) + + assert replay.control_max_step == 1 + assert reader.applied[-1] == [1] * len(lengths) + torch.testing.assert_close( + env.last_restored["robot"]["qpos"], torch.tensor(expected_qpos) + ) + replay.close() + + +def test_fluid_artifact_rejects_scene_without_fluid(): + trajectory = _trajectory() + trajectory["meta"]["fluid_state"] = {"schema_version": 1} + with pytest.raises(ValueError, match="configured fluid"): + ReplayWrapper(_FakeEnv(), trajectory, mode="kinematic") diff --git a/tests/gym/envs/test_usd_actor_roles.py b/tests/gym/envs/test_usd_actor_roles.py index b1e718497d..6d9f1a5bd4 100644 --- a/tests/gym/envs/test_usd_actor_roles.py +++ b/tests/gym/envs/test_usd_actor_roles.py @@ -55,6 +55,7 @@ def declare(*, cfg, validate_resolved): articulation=[], visual_object=[], rigid_object_group=[], + fluid_object=[], ), sim=SimpleNamespace(add_rigid_object=declare), ) diff --git a/tests/gym/utils/test_gym_utils.py b/tests/gym/utils/test_gym_utils.py index 1dc9bcb274..0d9f205753 100644 --- a/tests/gym/utils/test_gym_utils.py +++ b/tests/gym/utils/test_gym_utils.py @@ -2729,6 +2729,82 @@ 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"] + + +@pytest.mark.parametrize("owner", ["inline", "environment", "scene"]) +def test_fluid_point_files_resolve_from_declaration_owner( + owner: str, tmp_path: Path +) -> None: + physical_dir = tmp_path / "physical" + physical_dir.mkdir() + points = np.array([[0.0, 0.0, 0.0], [0.02, 0.0, 0.0]], dtype=np.float32) + np.savez(physical_dir / "water.npz", positions=points) + fluid = {"uid": "water", "spawn": {"kind": "points", "file_path": "water.npz"}} + config = { + "id": "EmbodiedEnv-v1", + "physics": "newton", + "env": {}, + "robot": {"uid": "robot"}, + } + source_path = tmp_path / "task.yaml" + if owner == "inline": + config["fluid_object"] = [fluid] + source_path = physical_dir / "task.yaml" + else: + component = {"simulation": {"fluid_object": [fluid]}} + if owner == "environment": + component.update(environment_id="fluid_test", physics="newton", env={}) + del config["physics"], config["env"] + else: + component["scene_id"] = "fluid_test" + save_config(physical_dir / "component.yaml", component) + config[owner] = {"component": "physical/component.yaml"} + + cfg = config_to_cfg( + config, manager_modules=DEFAULT_MANAGER_MODULES, source_path=source_path + ) + + spawn = cfg.fluid_object[0].spawn + assert spawn.file_path == str(physical_dir / "water.npz") + np.testing.assert_array_equal(spawn.sample(0.01), points) + assert fluid["spawn"]["file_path"] == "water.npz" + + 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_task.py b/tests/lab/scripts/test_run_task.py index f9a192e4f8..c9bdb1ec19 100644 --- a/tests/lab/scripts/test_run_task.py +++ b/tests/lab/scripts/test_run_task.py @@ -315,6 +315,7 @@ def test_replay_restores_wrapper_state_without_closing_caller_env(monkeypatch) - """Replay leaves the environment close to its CLI owner.""" env = MagicMock() env.unwrapped = env + env.cfg = SimpleNamespace(fluid_object=[]) replay_env = MagicMock() monkeypatch.setattr( run_task, @@ -418,6 +419,115 @@ def test_generate_function_discards_retry_then_commits_once(monkeypatch) -> None assert env.reset_options == [{"save_data": False}, None] +@pytest.mark.parametrize( + "physical,program_success", [(True, True), (False, True), (True, False)] +) +def test_fluid_qualification_gates_sidecar_commit( + monkeypatch, tmp_path, physical: bool, program_success: bool +) -> None: + accepted = physical and program_success + env = _ResetTrackingEnv() + diagnostic_videos = [] + env.event_manager = SimpleNamespace( + cfg=SimpleNamespace(record_camera=object()), + get_functor=lambda _: SimpleNamespace( + save_and_clear=lambda **kwargs: diagnostic_videos.append(kwargs) + ), + ) + monkeypatch.setattr( + run_task, + "execute_demo_episode", + lambda *args, **kwargs: _episode_result( + success=program_success, reason="success" + ), + ) + monkeypatch.setattr( + run_task, + "DemoQualification", + lambda *_: SimpleNamespace( + start=lambda: None, + after_step=lambda: None, + finish=lambda *_: {"accepted": physical}, + ), + ) + saved = [] + monkeypatch.setattr( + run_task, + "_save_qualified_fluid_trajectory", + lambda *args, **kwargs: saved.append((args, kwargs)), + ) + generated = generate_function( + env, + max_attempts=1, + reset_before=False, + fluid_recorder=object(), + qualification={"observations": {"metric": {"min": [1.0]}}}, + qualification_output_dir=tmp_path, + ) + assert generated is accepted + assert len(saved) == int(accepted) + assert diagnostic_videos == ( + [] if accepted else [{"env_ids": [0], "purpose": "diagnostic"}] + ) + assert env.reset_options == ([None] if accepted else [{"save_data": False}]) + assert (tmp_path / "qualification_result.json").is_file() + assert ( + json.loads((tmp_path / "qualification_result.json").read_text())["accepted"] + is accepted + ) + + +def test_fluid_recording_requires_ordinary_trajectory(capsys) -> None: + with pytest.raises(SystemExit, match="2"): + run_task.cli(["--gym-config", "task.yaml", "--record-fluid-state"]) + assert "requires --record_trajectory" in capsys.readouterr().err + + +def test_fluid_batch_prepare_does_not_publish_initial_camera_frames( + monkeypatch, tmp_path +) -> None: + env = _ResetTrackingEnv() + monkeypatch.setattr(run_task, "generate_function", lambda *args, **kwargs: True) + run_task.main( + SimpleNamespace( + qualification_output_dir=str(tmp_path), record_trajectory=False + ), + env, + { + "id": GYM_ID, + "collection": { + "qualification": {"observations": {"metric": {"min": [1.0]}}} + }, + }, + ) + assert env.reset_options == [{"save_data": False}, {"save_data": False}] + + +def test_qualified_fluid_sources_keep_distinct_run_artifacts(tmp_path) -> None: + paths = [] + target = SimpleNamespace( + _traj_steps=torch.tensor([1]), + get_demo_episode_metadata=lambda row: {"program_hash": "program"}, + save_trajectory=lambda path, **kwargs: paths.append((path, kwargs)), + _set_fluid_trajectory_recorder=lambda recorder: None, + ) + for run in ("first", "second"): + recorder = SimpleNamespace( + directory=tmp_path / f"fluid-state-{run}", + finish=lambda path, lengths, env_ids: {"manifests": [run]}, + commit=lambda: None, + ) + run_task._save_qualified_fluid_trajectory( + target, recorder, tmp_path, [0], batch=0 + ) + assert paths[0][0] != paths[1][0] + assert all( + item[1]["extra_meta"]["fluid_qualification"] + == {"kind": "task_program", "accepted": True, "program_hash": "program"} + for item in paths + ) + + def test_source_bound_recording_cannot_bypass_real2sim_receipt() -> None: env = _ResetTrackingEnv() env._task_program_adapter = SimpleNamespace(_configured_nominal_sources=(object(),)) @@ -484,7 +594,7 @@ def test_cli_selects_configured_real2sim_owner_and_explicit_profile_override( monkeypatch.setattr(real2sim_collection, "load_real2sim_tabletop_profile", load) monkeypatch.setattr(real2sim_collection, "install_real2sim_tabletop_owner", install) monkeypatch.setattr(run_task.gymnasium, "make", lambda **_: env) - monkeypatch.setattr(run_task, "main", lambda *args: None) + monkeypatch.setattr(run_task, "main", lambda *args, **kwargs: None) monkeypatch.setattr(run_task, "_abort_and_close_env", lambda *args, **kwargs: None) monkeypatch.setattr( "embodichain.lab.sim.sim_manager.SimulationManager.flush_cleanup_queue", @@ -1548,3 +1658,51 @@ def publish_replay_control( REPLAY_NUM_STEPS - 1, False, ) + + +@pytest.mark.parametrize( + "problem", ["rejected", "diagnostic", "program_hash", "incomplete"] +) +def test_launcher_requires_a_qualified_program_before_fluid_replay(problem) -> None: + source = { + "meta": { + "artifact_purpose": "recording", + "num_envs": 1, + "fluid_qualification": { + "kind": "task_program", + "accepted": True, + "program_hash": "p", + }, + "demo_episodes": [ + {"completed": True, "success": True, "program_hash": "p"} + ], + } + } + run_task._validate_fluid_replay_source(source) + if problem == "rejected": + source["meta"]["fluid_qualification"]["accepted"] = False + elif problem == "diagnostic": + source["meta"]["artifact_purpose"] = "diagnostic" + elif problem == "program_hash": + source["meta"]["demo_episodes"][0]["program_hash"] = "other" + else: + source["meta"]["demo_episodes"][0]["completed"] = False + with pytest.raises(ValueError): + run_task._validate_fluid_replay_source(source) + + +def test_fluid_replay_policy_runs_without_a_precreated_reader(monkeypatch) -> None: + env = SimpleNamespace(cfg=SimpleNamespace(fluid_object=[object()])) + env.unwrapped = env + monkeypatch.setattr( + run_task, + "load_trajectory", + lambda path: { + "meta": {"num_envs": 1, "num_steps": 1, "lengths": [1]}, + }, + ) + wrapper = MagicMock() + monkeypatch.setattr(run_task, "ReplayWrapper", wrapper) + with pytest.raises(ValueError, match="accepted Task Program"): + run_task.replay(env, "source.pt") + wrapper.assert_not_called() diff --git a/tests/sim/atomic_skills/test_actions.py b/tests/sim/atomic_skills/test_actions.py index b3129fbc27..6a4b0d8424 100644 --- a/tests/sim/atomic_skills/test_actions.py +++ b/tests/sim/atomic_skills/test_actions.py @@ -2756,6 +2756,95 @@ def compute_ik( assert plan.expected_effects.is_empty assert context.task is task + slower = _plan_action( + action, + SkillInvocation( + skill_id="pour", + goal=PourGoal(), + binding=_binding(action), + motion_policy=MotionPolicy( + sample_count=10, + velocity_limit=100.0, + acceleration_limit=1000.0, + ), + skill_options=PourOptions(rotation_duration=8.0 / 60.0), + ), + context, + ) + segments = {segment.name: segment.waypoint_count for segment in slower.segments} + assert segments["pour"] >= 9 + assert segments["return"] >= 8 + slower_arm = _joint_trajectory(slower).positions[:, :, :ARM_DOF] + pour_end = segments["pour"] - 1 + return_start = segments["pour"] + segments["dwell"] + return_end = return_start + segments["return"] - 1 + assert not torch.allclose(slower_arm[:, pour_end // 2], slower_arm[:, pour_end]) + assert not torch.allclose( + slower_arm[:, (return_start + return_end) // 2], + slower_arm[:, return_end], + ) + + +def test_pour_rotation_duration_stretches_each_native_batch_row() -> None: + generator = _motion_generator() + positions = torch.zeros(NUM_ENVS, 9, ARM_DOF) + positions[:, :, 0] = torch.tensor( + [ + [0.0, 0.5, 1.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.25, 0.5, 0.75, 1.0, 0.75, 0.5, 0.25, 0.0], + ] + ) + # Native tilt/return each take 1 s in row 0 and 3 s in row 1. + intervals = torch.tensor( + [ + [0.0, 0.5, 0.5, 0.5, 0.5, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.75, 0.75, 0.75, 0.75, 0.75, 0.75, 0.75, 0.75], + ] + ) + generator.generate = Mock( + return_value=PlanResult( + success=torch.ones(NUM_ENVS, dtype=torch.bool), + positions=positions, + dt=intervals, + waypoint_indices=torch.tensor([[2, 4], [4, 8]]), + ) + ) + action = _bind_action(generator, Pour()) + semantics = ObjectSemantics( + affordance=AxisAlignAffordance(internal_axis=torch.tensor([1.0, 0.0, 0.0])), + geometry={}, + entity_id="cup", + label="cup", + ) + task = TaskState( + batch_size=NUM_ENVS, device="cpu", held_objects={"arm": _held(semantics)} + ) + + plan = _plan_action( + action, + SkillInvocation( + skill_id="pour", + goal=PourGoal(), + binding=_binding(action), + motion_policy=MotionPolicy(sample_count=4), + skill_options=PourOptions(rotation_duration=2.0, dwell_duration=0.0), + ), + _context(task), + ) + + assert plan.plan_success.all() + trajectory = _joint_trajectory(plan).positions[:, :, 0] + pour_count = plan.segments[0].waypoint_count + for row, expected_duration in enumerate((2.0, 3.0)): + first_arrival = torch.isclose(trajectory[row, :pour_count], torch.tensor(1.0)) + arrival_time = first_arrival.nonzero()[0].item() * CONTROL_DT + assert arrival_time == pytest.approx(expected_duration, abs=CONTROL_DT) + return_path = trajectory[row, pour_count:] + return_arrival = torch.isclose(return_path, torch.tensor(0.0)).nonzero()[0] + assert (return_arrival.item() + 1) * CONTROL_DT == pytest.approx( + expected_duration, abs=CONTROL_DT + ) + def test_pour_reads_held_state_from_the_bound_logical_resource() -> None: """Profile resource IDs need not match native motion control-part names.""" @@ -2906,6 +2995,8 @@ def test_pour_options_validate_rotation_and_dwell_duration() -> None: PourOptions(rotate_angle=float("nan")) with pytest.raises(ValueError, match="dwell_duration"): PourOptions(dwell_duration=-0.1) + with pytest.raises(ValueError, match="rotation_duration"): + PourOptions(rotation_duration=0.0) with pytest.raises(ValueError, match="max_mouth_height"): PourOptions(mouth_height=0.2, max_mouth_height=0.1) with pytest.raises(ValueError, match="max_rotation_step"): @@ -2913,8 +3004,10 @@ def test_pour_options_validate_rotation_and_dwell_duration() -> None: @pytest.mark.parametrize("inverted_receiver", [False, True]) +@pytest.mark.parametrize("rotation_duration", [None, 2.0]) def test_targeted_pour_keeps_mouth_over_receiver_and_body_above_rim( inverted_receiver: bool, + rotation_duration: float | None, ) -> None: """Two opposite approach directions share one compensated mouth path.""" generator = _motion_generator() @@ -2983,12 +3076,24 @@ def plan_geometry(states, options): goal=PourGoal(PourTarget(SceneEntityPose("target"), mouth, 0.04, 0.01)), binding=_binding(action), motion_policy=MotionPolicy(sample_count=80), - skill_options=PourOptions(rotate_angle=math.pi / 3), + skill_options=PourOptions( + rotate_angle=math.pi / 3, + rotation_duration=rotation_duration, + ), ), context, ) assert plan.plan_success.tolist() == [True, not inverted_receiver] assert plan.scene_dependencies == ("target",) + if rotation_duration is not None: + requested = math.ceil(rotation_duration / CONTROL_DT) + 1 + assert ( + sum( + call.kwargs["options"].sample_count >= requested + for call in generator.generate.call_args_list + ) + >= 2 + ) poses = torch.stack(sampled_poses, dim=1) @ torch.linalg.inv(attachment[:, None]) mouths = (poses @ mouth)[:, :, :3, 3] pouring = poses[:, :, 2, 2] < math.cos(math.radians(50)) 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 0000000000..c8dd977865 --- /dev/null +++ b/tests/sim/objects/test_fluid_batch_runtime.py @@ -0,0 +1,125 @@ +# ---------------------------------------------------------------------------- +# 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 + + ensure_solver_available("dexuni") + 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 + + ensure_solver_available("dexuni") + 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_online_runtime.py b/tests/sim/objects/test_fluid_online_runtime.py new file mode 100644 index 0000000000..09a30728e4 --- /dev/null +++ b/tests/sim/objects/test_fluid_online_runtime.py @@ -0,0 +1,309 @@ +# ---------------------------------------------------------------------------- +# 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, gripper state 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_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_franka_gripper_stays_stable_with_inner_rigid_solver(self): + from embodichain.lab.gym.utils.gym_utils import config_to_cfg + from embodichain.utils.utility import load_config + + 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: + task = ( + Path(__file__).resolve().parents[3] + / "extensions/embodichain_tasks/configs/tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml" + ) + robot_cfg = config_to_cfg(load_config(task), source_path=task).robot + 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() + joint_ids = robot.get_joint_ids("eef") + assert len(joint_ids) == 2 + 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_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_live_dynamic_boundary_and_selected_reset() + worker.test_franka_gripper_stays_stable_with_inner_rigid_solver() + worker.test_normal_gym_partial_reset_does_not_advance_peer() diff --git a/tests/sim/objects/test_fluid_runtime.py b/tests/sim/objects/test_fluid_runtime.py index 7247216fdc..9b1c37c630 100644 --- a/tests/sim/objects/test_fluid_runtime.py +++ b/tests/sim/objects/test_fluid_runtime.py @@ -50,18 +50,13 @@ def test_prepare_write_reset_and_repeat( from dexsim.engine.newton_physics import DexUniSolverCfg if "sph_cfg" not in getattr(DexUniSolverCfg, "__dataclass_fields__", {}): - pytest.skip("This DexSim build does not compose SPH through DexUni.") + pytest.skip("This DexSim build lacks DexUni fluid composition.") if num_envs > 1: from dexsim.engine.newton_physics.solver_runtime import ( ensure_solver_available, ) - solver = ensure_solver_available("sph") - if not ( - getattr(solver, "supports_world_isolation", False) - and getattr(solver, "supports_world_reset", False) - ): - pytest.skip("This DexSim SPH build supports only a single world.") + ensure_solver_available("dexuni") def run_worlds(count: int, use_default: bool) -> dict: result_path = tmp_path / f"lifecycle-{count}-{use_default}.json" @@ -97,7 +92,7 @@ def run_worlds(count: int, use_default: bool) -> dict: def _run_fluid_lifecycle(num_envs: int, automatic: bool) -> dict[str, object]: """Run the complete NoRender lifecycle in a fresh native process.""" - from dexsim.engine.newton_physics import DexUniSolverCfg, SPHSolverCfg + from dexsim.engine.newton_physics import DexUniSolverCfg from dexsim.engine.newton_physics.backend_registry import get_newton_backend from embodichain.lab.sim.cfg import RigidObjectCfg from embodichain.lab.sim.shapes import CubeCfg @@ -107,7 +102,7 @@ def _run_fluid_lifecycle(num_envs: int, automatic: bool) -> dict[str, object]: headless=True, device="cuda", num_envs=num_envs, - # The two fluid volumes overlap in world space. SPH worlds must + # The two fluid volumes overlap in world space. DexUni fluid worlds must # remain isolated even when their particles are nearby. arena_space=0.02, physics_dt=0.001, @@ -119,11 +114,11 @@ def _run_fluid_lifecycle(num_envs: int, automatic: bool) -> dict[str, object]: None if automatic else DexUniSolverCfg( - sph_cfg=SPHSolverCfg( - sph_method="dfsph", - particle_support_radius=0.0225, - volume_map_boundary_enabled=False, - ) + sph_cfg={ + "sph_method": "dfsph", + "particle_support_radius": 0.0225, + "volume_map_boundary_enabled": False, + } ) ), ), diff --git a/tests/sim/objects/test_fluid_visibility.py b/tests/sim/objects/test_fluid_visibility.py index acfc10ab94..0c059754a7 100644 --- a/tests/sim/objects/test_fluid_visibility.py +++ b/tests/sim/objects/test_fluid_visibility.py @@ -35,7 +35,7 @@ def test_fluid_surface_changes_record_camera_pixels_in_every_environment( from dexsim.engine.newton_physics import DexUniSolverCfg if "sph_cfg" not in getattr(DexUniSolverCfg, "__dataclass_fields__", {}): - pytest.skip("This DexSim build does not compose SPH through DexUni.") + pytest.skip("This DexSim build lacks DexUni fluid composition.") result = subprocess.run( [ sys.executable, @@ -75,13 +75,8 @@ def _run_visibility_worker(output: Path, clipping: bool) -> None: from embodichain.lab.sim.material import VisualMaterialCfg from embodichain.lab.sim.sensors import CameraCfg - solver = ensure_solver_available("sph") - num_envs = ( - 2 - if getattr(solver, "supports_world_isolation", False) - and getattr(solver, "supports_world_reset", False) - else 1 - ) + ensure_solver_available("dexuni") + num_envs = 2 sim = SimulationManager( SimulationManagerCfg( headless=True, diff --git a/tests/sim/spawn/test_descriptors.py b/tests/sim/spawn/test_descriptors.py index 601e058f54..3cf6a0d8b5 100644 --- a/tests/sim/spawn/test_descriptors.py +++ b/tests/sim/spawn/test_descriptors.py @@ -2741,6 +2741,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 b2684cd1a9..4cf7c4da71 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 @@ -45,16 +47,14 @@ def test_fluid_round_trip_keeps_native_render_options(): assert cfg.render.mesh_surface["iso_density"] == 0.57 -def test_native_sph_and_yaml_mapping_use_same_solver(): - from dexsim.engine.newton_physics import DexUniSolverCfg, SPHSolverCfg +def test_native_and_yaml_dexuni_fluid_options_match(): + from dexsim.engine.newton_physics import DexUniSolverCfg if "sph_cfg" not in getattr(DexUniSolverCfg, "__dataclass_fields__", {}): - pytest.skip("This DexSim build does not compose SPH through DexUni.") + pytest.skip("This DexSim build lacks DexUni fluid composition.") - source = SPHSolverCfg(sph_method="dfsph", viscosity=0.0006) - native = NewtonPhysicsCfg(solver_cfg=DexUniSolverCfg(sph_cfg=source)).to_dexsim_cfg( - 0 - ) + source = DexUniSolverCfg(sph_cfg={"sph_method": "dfsph", "viscosity": 0.0006}) + native = NewtonPhysicsCfg(solver_cfg=source).to_dexsim_cfg(0) mapping = NewtonPhysicsCfg( solver_cfg={ "solver_type": "dexuni", @@ -63,22 +63,26 @@ def test_native_sph_and_yaml_mapping_use_same_solver(): ).to_dexsim_cfg(0) assert type(native.solver_cfg) is DexUniSolverCfg assert type(mapping.solver_cfg) is DexUniSolverCfg - assert native.solver_cfg.sph_cfg == source - assert mapping.solver_cfg.sph_cfg == source + assert native.solver_cfg.sph_cfg == source.sph_cfg + assert mapping.solver_cfg.sph_cfg == source.sph_cfg @pytest.mark.parametrize("as_mapping", [False, True]) -def test_direct_sph_solver_config_uses_registered_sdk_contract(as_mapping): - from dexsim.engine.newton_physics import RegisteredNewtonSolverCfg, SPHSolverCfg +def test_direct_sph_solver_config_is_rejected(as_mapping): + from dexsim.engine.newton_physics import SPHSolverCfg solver_cfg = {"solver_type": "sph"} if as_mapping else SPHSolverCfg() - resolved = NewtonPhysicsCfg(solver_cfg=solver_cfg).to_dexsim_cfg(0).solver_cfg - assert resolved.solver_type == "sph" - if as_mapping: - assert isinstance(resolved, RegisteredNewtonSolverCfg) - assert resolved.options == {} - else: - assert resolved == solver_cfg + with pytest.raises(RuntimeError, match="Unsupported Newton solver type 'sph'"): + NewtonPhysicsCfg(solver_cfg=solver_cfg).to_dexsim_cfg(0) + + +def test_direct_provider_cannot_advertise_standalone_sph(): + from dexsim.engine.newton_physics import ProviderNewtonSolverCfg + + provider = ProviderNewtonSolverCfg(provider_factory=lambda: object()) + provider.solver_type = "sph" + with pytest.raises(RuntimeError, match="Unsupported Newton solver type 'sph'"): + NewtonPhysicsCfg(solver_cfg=provider).to_dexsim_cfg(0) @pytest.mark.parametrize("kind", ["box", "sphere", "cylinder"]) @@ -131,6 +135,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 @@ -167,6 +213,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): @@ -186,8 +252,10 @@ def test_dexuni_nested_sph_density_conflict_is_rejected(monkeypatch): manager._spawn_scene.declare.assert_not_called() -@pytest.mark.parametrize("solver,capability", [("sph", True), ("dexuni", False)]) -def test_fluid_requires_a_dexuni_build_with_sph_support(solver, capability): +@pytest.mark.parametrize( + "solver,capability", [("mujoco_warp", True), ("dexuni", False)] +) +def test_fluid_requires_dexuni_with_fluid_support(solver, capability): from embodichain.lab.sim import SimulationManager manager = _fluid_manager_for_renderer("no-render") @@ -239,7 +307,31 @@ def test_dexuni_fluid_capability_tracks_native_config(monkeypatch, has_sph_cfg): assert backend.supports_dexuni_fluids is has_sph_cfg -def test_fluid_reset_invalidates_sph_without_resetting_dexuni_bodies(monkeypatch): +def test_dexuni_fluid_world_capability_uses_prepared_backend(monkeypatch): + from types import SimpleNamespace + import dexsim.engine.newton_physics as native + from dexsim.engine.newton_physics import backend_registry + from embodichain.lab.sim.physics.newton import NewtonPhysicsBackend + + fake_cfg = type("FakeDexUniCfg", (), {"__dataclass_fields__": {"sph_cfg": None}}) + monkeypatch.setattr(native, "DexUniSolverCfg", fake_cfg) + fluid = SimpleNamespace(supports_world_isolation=True, supports_world_reset=True) + monkeypatch.setattr( + backend_registry, + "get_newton_backend", + lambda _: SimpleNamespace(solver_type="dexuni", sph_solver=fluid), + ) + backend = NewtonPhysicsBackend(SimpleNamespace(_world=object())) + backend._configured_solver_type = "dexuni" + + assert backend.supports_fluid_worlds + fluid.supports_world_reset = False + assert not backend.supports_fluid_worlds + + +def test_fluid_reset_invalidates_internal_solver_without_resetting_dexuni_bodies( + monkeypatch, +): from types import SimpleNamespace from unittest.mock import Mock @@ -343,12 +435,37 @@ def _fluid_manager_for_renderer(renderer): _fluid_objects={}, _rigid_objects={}, _spawn_scene=Mock(spec=SpawnScene), + 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", @@ -444,20 +561,20 @@ def test_fluid_renderer_gate_allows_supported_backends( def test_solver_defaults_allow_fluid_owned_density_and_spacing( representation, density, spacing, monkeypatch ): - from dataclasses import asdict from unittest.mock import Mock - from dexsim.engine.newton_physics import DexUniSolverCfg, SPHSolverCfg + from dataclasses import asdict + from dexsim.engine.newton_physics import DexUniSolverCfg from embodichain.lab.sim import SimulationManager - sph = SPHSolverCfg(sph_method="dfsph") + if "sph_cfg" not in getattr(DexUniSolverCfg, "__dataclass_fields__", {}): + pytest.skip("This DexSim build lacks DexUni fluid composition.") + fluid_options = DexUniSolverCfg(sph_cfg={"sph_method": "dfsph"}).sph_cfg if representation == "native": - if "sph_cfg" not in getattr(DexUniSolverCfg, "__dataclass_fields__", {}): - pytest.skip("This DexSim build does not compose SPH through DexUni.") - solver = DexUniSolverCfg(sph_cfg=sph) + solver = DexUniSolverCfg(sph_cfg={"sph_method": "dfsph"}) elif representation == "mapping": solver = {"solver_type": "dexuni", "sph_cfg": {"sph_method": "dfsph"}} else: - solver = {"solver_type": "dexuni", "sph_cfg": asdict(sph)} + solver = {"solver_type": "dexuni", "sph_cfg": asdict(fluid_options)} manager = _fluid_manager_for_renderer("no-render") manager.sim_config.physics_cfg.solver_cfg = solver monkeypatch.setattr(SimulationManager, "_get_process_renderer", lambda: None) @@ -476,9 +593,11 @@ def test_solver_defaults_allow_fluid_owned_density_and_spacing( assert descriptor.density == density assert descriptor.particle_spacing == spacing if representation != "mapping": - values = asdict(sph) if representation == "native" else solver["sph_cfg"] - assert values["rest_density"] == SPHSolverCfg().rest_density - assert values["particle_diameter"] == SPHSolverCfg().particle_diameter + values = ( + asdict(solver.sph_cfg) if representation == "native" else solver["sph_cfg"] + ) + assert values["rest_density"] == fluid_options.rest_density + assert values["particle_diameter"] == fluid_options.particle_diameter @pytest.mark.parametrize("representation", ["native", "mapping"]) @@ -486,23 +605,24 @@ def test_solver_defaults_allow_fluid_owned_density_and_spacing( def test_solver_nondefault_physical_values_must_match_fluid( representation, conflict, monkeypatch ): - from dataclasses import asdict from unittest.mock import Mock - from dexsim.engine.newton_physics import DexUniSolverCfg, SPHSolverCfg + from dexsim.engine.newton_physics import DexUniSolverCfg from embodichain.lab.sim import SimulationManager density, spacing = 800.0, 0.003 - sph = SPHSolverCfg( - sph_method="dfsph", rest_density=density, particle_diameter=spacing - ) + fluid_options = { + "sph_method": "dfsph", + "rest_density": density, + "particle_diameter": spacing, + } if conflict is not None: - setattr(sph, conflict, 1200.0 if conflict == "rest_density" else 0.02) + fluid_options[conflict] = 1200.0 if conflict == "rest_density" else 0.02 if representation == "mapping": - solver = {"solver_type": "dexuni", "sph_cfg": asdict(sph)} + solver = {"solver_type": "dexuni", "sph_cfg": fluid_options} else: if "sph_cfg" not in getattr(DexUniSolverCfg, "__dataclass_fields__", {}): - pytest.skip("This DexSim build does not compose SPH through DexUni.") - solver = DexUniSolverCfg(sph_cfg=sph) + pytest.skip("This DexSim build lacks DexUni fluid composition.") + solver = DexUniSolverCfg(sph_cfg=fluid_options) manager = _fluid_manager_for_renderer("no-render") manager.sim_config.physics_cfg.solver_cfg = solver monkeypatch.setattr(SimulationManager, "_get_process_renderer", lambda: None) diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index 1a723ae7d7..b7a71c8530 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -2346,6 +2346,8 @@ def test_prepare_initializes_runtime_for_backend_device_matrix( spawn_scene.bind.side_effect = lambda: events.append("bind") sim = object.__new__(SimulationManager) + sim._fluid_objects = {} + sim.num_envs = 1 sync_render_state = MagicMock() sim.device = device sim._world = MagicMock() @@ -2398,6 +2400,8 @@ def test_prepare_retries_runtime_and_binding_without_recommit() -> None: spawn_scene.builder.has_pending_changes = False sim = object.__new__(SimulationManager) + sim._fluid_objects = {} + sim.num_envs = 1 sync_render_state = MagicMock() prepare_spawn_runtime = MagicMock(side_effect=[RuntimeError("first attempt"), None]) sim.physics = SimpleNamespace( @@ -2436,6 +2440,8 @@ def test_prepare_retries_camera_attachment_without_recommit() -> None: spawn_scene.builder.has_pending_changes = False sim = object.__new__(SimulationManager) + sim._fluid_objects = {} + sim.num_envs = 1 sync_render_state = MagicMock() sim.physics = SimpleNamespace( name="default", @@ -2475,6 +2481,8 @@ def test_prepare_syncs_render_state_once_per_topology_revision() -> None: sync_render_state = MagicMock(side_effect=lambda _result: events.append("sync")) sim = object.__new__(SimulationManager) + sim._fluid_objects = {} + sim.num_envs = 1 sim.physics = SimpleNamespace( name="newton", prepare_spawn_runtime=MagicMock(), @@ -2518,6 +2526,8 @@ def test_prepare_retries_render_state_sync_without_recommit() -> None: ) sim = object.__new__(SimulationManager) + sim._fluid_objects = {} + sim.num_envs = 1 sim.physics = SimpleNamespace( name="newton", prepare_spawn_runtime=MagicMock(), @@ -2824,6 +2834,7 @@ def make_view() -> MagicMock: def _make_camera_attachment_manager(num_envs: int = 2) -> SimulationManager: sim = object.__new__(SimulationManager) + sim._fluid_objects = {} sim.num_envs = num_envs sim.device = torch.device("cpu") sim._robots = {} @@ -3329,6 +3340,8 @@ def commit(): scene.commit.side_effect = commit sim = object.__new__(SimulationManager) + sim._fluid_objects = {} + sim.num_envs = 1 sim._spawn_scene = scene sim._sensors = {"camera": camera} sim._default_plane = object() @@ -3997,7 +4010,7 @@ def after_substep(_dt: float) -> None: assert runtime.capture_calls[0]["sim_step"] == 1 -def test_reset_fluid_selects_rows_and_honors_exclusions(): +def test_reset_fluids_selects_rows_and_honors_exclusions(): sim = object.__new__(SimulationManager) sim.physics = NewtonPhysicsBackend(sim) sim.num_envs = 2 @@ -4011,10 +4024,10 @@ 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} - sim.reset_objects_state(env_ids=[1], excluded_uids=["other"]) - excluded.reset.assert_not_called() + fluid, excluded_fluid = MagicMock(), MagicMock() + sim._fluid_objects = {"water": fluid, "other_fluid": excluded_fluid} + sim.reset_objects_state(env_ids=[1], excluded_uids=["other_fluid"]) + excluded_fluid.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 6add4c6c8d..87b6ca1405 100644 --- a/tests/test_task_program_package_data.py +++ b/tests/test_task_program_package_data.py @@ -121,6 +121,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_fast_fluid", + ), } _DEPLOYMENTS[ Path("tasks/manipulation/hand_over/task.dual_franka_robotiq_arg2f_140.yaml") @@ -230,6 +235,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_fast_fluid.yaml"), + Path("components/execution_policies/trajectory_open_loop_fluid_pour.yaml"), Path("components/embodiments/franka_panda_robotiq85_rgbd.yaml"), Path( "tasks/manipulation/carton_stocking/task_program/integration.ordinary.yaml" @@ -435,6 +448,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(