diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index b4405c2904..ac41dd1d75 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -15,7 +15,9 @@ 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, 粒子采样, fluid, 流体, quaternion, xyzw] + fixed-cadence playback, FluidObjectCfg, FluidSpawnCfg, FluidObject, SPHSolverCfg, revolved_volume, fill_level, + 粒子采样, fluid, 流体, quaternion, xyzw, cable, CableObject, CableObjectCfg, + CableObjectData, 线缆对象] paths: [topics/simulation-system/simulation-system.md] source_of_truth: - embodichain/lab/sim/motion/__init__.py @@ -38,6 +40,7 @@ topics: - embodichain/lab/sim/objects/visual_object.py - embodichain/lab/sim/objects/backends/ - embodichain/lab/sim/objects/deformable/ + - embodichain/lab/sim/objects/cable/ - embodichain/lab/sim/objects/gizmo.py - embodichain/lab/gym/envs/base_env.py watch_paths: [embodichain/lab/sim/sim_manager.py, embodichain/lab/sim/cfg/, embodichain/lab/sim/physics/, diff --git a/agent_context/topics/simulation-system/deformables.md b/agent_context/topics/simulation-system/deformables.md index e7d35e0c1f..165ad49dfe 100644 --- a/agent_context/topics/simulation-system/deformables.md +++ b/agent_context/topics/simulation-system/deformables.md @@ -102,3 +102,67 @@ Focused coverage: `tests/sim/test_deformable_cfg.py`, `tests/sim/objects/test_deformable_object.py`, `test_soft_object.py`, `test_cloth_object.py`, `tests/sim/spawn/test_descriptors.py`, and `tests/sim/test_runtime_controls.py`. + +## Cable objects + +`CableObjectCfg` and `CableObject(BatchEntity)` are independent of particle-set +deformables. `SimulationManager.add_cable_object()` lowers through +`spawn/descriptors.py` into `CableDesc`, declares in the existing `SpawnScene`, +and binds the same facade in place during `prepare()`. Backend capabilities +reject Default, CPU, other Newton solvers and gradient mode; use DexUni on CUDA. +With this backend, an arena containing a cable cannot also contain fluid, +cloth or soft-body particles: cable episode reset does not support those +particles. Spawn checks the native cable and particle descriptors for direct +declarations and USD imports, including mixtures within a single USD source. +It checks topology and UIDs before publishing declaration materials. + +`CableObjectData` uses `Scene.create_cable_batch()` and exposes independent +world snapshots for equal-topology replicated segments. Public poses are +`xyz + xyzw`; the adapter permutes DexSim's quaternion-first batch output +without adding Arena offsets again. Authoring material quaternions are likewise +xyzw and convert once to CableDesc's wxyz. Defaults come from the engine's +immutable creation state, not a live observation at binding. + +External force/torque and pending-load clearing use public `SpawnedCable` +methods. `write_segment_state_to_sim()` accepts world states in the same public +layout and uses `CableBatch.apply_state()` for both engine buffers. This teleport +clears history for each owning world, including other cables, while preserving +controls and loads. Object reset restores that cable's defaults and clears its +loads. It is not a complete episode reset or a solver-history checkpoint. + +`EmbodiedEnvCfg.cable_object` declares Gym cables before preparation and belongs +to the physical environment component. Cable recordings and replay restrictions +are owned by [Gym execution](../env-framework/execution.md). + +Newton's pre-build backend hook preflights cable attachment compatibility and +registers nonempty `init_qpos` through public model-initialization and batch-target +hooks before engine creation defaults are frozen. Input vectors are copied to +finite host float32 values, including CUDA tensors. The initializer does not +overwrite runtime commands; episode reset restores nonzero poses and targets. +When preparation is retried with a changed declaration, the initializer uses +the current articulation targets and joint values. +Mimic followers use the same state-order mapping and projection as articulation +reset. The opt-in MuJoCo mimic stabilizer retains authored positions; unsupported +solver paths project them before creation defaults are captured. +Permanent attachment to an articulation with nonzero `init_qpos` is rejected +before commit: the current engine resolves attachments before model-initialization +hooks. Supporting that combination requires an engine authoring API that applies +initial joint poses before attachment construction. +Zero authoring vectors whose mimic offsets produce a nonzero pose are rejected +when the initialization hook resolves source mimic metadata, before writing that pose. + +Newton's scene-reset backend hook uses one public `Scene.reset_arenas()` call for all physical assets, +targets, loads and supported controls in selected environments. The manager then +resets selected visual objects, lights and sensors once, honoring their exclusions. Resolve Arena +indices by name: the Scene's default Arena means environment IDs are not Arena +indices. Physical exclusions are rejected before mutation. Runtime property edits +are outside this reset contract. Engine preflight rejects unsupported controls, +selected particles and unsupported solver modes. Global time remains monotonic; +contact queries need another update. For seeded initialization, reset the manager +then write caller-owned perturbed default segment states, preserving attachment +anchors. Finalized physical topology and individual removal remain unsupported. +Do not access backend arrays to implement missing engine operations. + +Focused coverage: `tests/sim/test_cable_cfg.py`, +`tests/sim/objects/test_cable_object.py`, and +`tests/sim/objects/test_cable_object_cuda.py` (explicit serial CUDA run). diff --git a/agent_context/topics/simulation-system/lifecycle.md b/agent_context/topics/simulation-system/lifecycle.md index 021c2c36c9..f0565459ac 100644 --- a/agent_context/topics/simulation-system/lifecycle.md +++ b/agent_context/topics/simulation-system/lifecycle.md @@ -9,7 +9,8 @@ Return to the [simulation overview](simulation-system.md) for ownership. readiness; `spawn/scene.py` owns declaration coordination. The ordering is: 1. If unfinalized or dirty, detach parented cameras before rebuilding their - native parents. Resolve/configure sources and commit Spawn topology. + native parents. Run the backend pre-build validation/initialization hook, + then resolve/configure sources and commit Spawn topology. 2. Apply Default articulation root properties before Direct GPU buffers capture them. Delegate runtime setup to `PhysicsBackend.prepare_spawn_runtime()`. 3. Replay the current per-segment [material assignments](materials.md) captured @@ -42,6 +43,11 @@ baselines while other assets retain runtime state. Native preparation failure propagates without rollback. Randomization requiring the replacements runs after this boundary. +Spawn-owned asset removal checks pending-removal and topology restrictions +before detaching mounted cameras or marker groups. A rejected removal leaves +the asset, sensors and marker attachments usable. NoRender declaration-only +visual objects remain removable without rebuilding physical topology. + Pending Newton topology/descriptor changes requiring a new immutable model become usable only after preparation. Keep model rebuilding in Spawn; object facades must not create a second runtime ownership path. @@ -62,6 +68,12 @@ solver state with only some of its articulations; excluding a subset is rejected before any facade state changes. Other resource exclusions and environment selections remain honored. +The backend may handle physical reset atomically before the per-object path. +Newton scenes containing cables use a complete `Scene.reset_arenas()` boundary; +the manager retains visual-object, light and sensor reset ownership, including +their environment selections and exclusions. Physical exclusions and runtime property restoration are +unsupported in this path; see [cable state and reset](deformables.md#cable-objects). + `destroy(exit_process=False)` queues native cleanup. Flush with `SimulationManager.flush_cleanup_queue()` after caller scene/object locals unwind. Constructor failure before World allocation removes only the failed @@ -100,10 +112,10 @@ indexing rules are in [deformables](deformables.md). Use `sim_manager.py` for lifecycle sequencing, `spawn/scene.py` for commit/bind coordination, `physics/base.py` and backend implementations for runtime hooks, -and `_runtime_controls.py` for nodal controls. `BaseEnv` owns Gym ordering. +and `_runtime_controls.py` for nodal controls and creation joint-state seeding. `BaseEnv` owns Gym ordering. `tests/sim/test_sim_manager.py` covers retry without recommit, render sync, camera attachment, replacement, control registration, reset and teardown. `tests/sim/spawn/test_scene.py` covers declaration/source/binding state; -`tests/sim/test_runtime_controls.py` covers nodal runtime behavior. Physical +`tests/sim/test_runtime_controls.py` covers nodal behavior and one-shot joint seeding. Physical baseline tests live with the corresponding objects under `tests/sim/objects/`. diff --git a/agent_context/topics/simulation-system/simulation-system.md b/agent_context/topics/simulation-system/simulation-system.md index 488f0b68bc..bf6d39a88f 100644 --- a/agent_context/topics/simulation-system/simulation-system.md +++ b/agent_context/topics/simulation-system/simulation-system.md @@ -169,6 +169,9 @@ tests need non-symmetric rotations to expose component-order mistakes. reset only selected environments and honor supported exclusions. - Declare deformables and Newton controls before finalization. Deformables require Newton, CUDA and a supported particle solver without gradient mode. +- Cables require DexUni on CUDA without gradient mode. Declare them + before preparation. Cable episodes reset through the owning Scene; finalized + physical topology remains fixed. See [cable objects](deformables.md#cable-objects). - Flush deferred cleanup after caller-owned scene locals unwind when using `destroy(exit_process=False)`; release child views before native parents. @@ -181,6 +184,7 @@ tests need non-symmetric rotations to expose component-order mistakes. | Joint state order, drive lowering, mimic coupling, native adaptation | [Articulation and batch adapters](articulation-adapters.md); `objects/backends/`, `objects/articulation.py` | | Initial-fill fluid configuration, state and rendering | [Fluids](fluids.md); `cfg/fluid.py`, `objects/fluid.py` | | Volume/surface state, topology and kinematic nodes | [Deformables](deformables.md); `cfg/deformable.py`, `objects/deformable/` | +| Cable declaration, segment state and external loads | [Cable objects](deformables.md#cable-objects); `cfg/cable.py`, `objects/cable/` | | Camera synchronization, native window/offscreen denoising, startup readiness | [Rendering and diagnostics](rendering.md); manager, `cfg/simulation.py`, `_startup_summary.py` | | Robot definition, control parts, robot presets | [Robot system](../robot-system/robot-system.md) | | Camera attachment resolution or contact sensors | [Sensor system](../sensor-system/sensor-system.md) | diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst index 440f315ece..6f3758a17a 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst @@ -74,6 +74,9 @@ UV seams and render detail. LightCfg VisualObjectCfg RigidObjectCfg + CableObjectCfg + CablePhysicsCfg + CableAttachmentCfg RigidObjectGroupCfg RigidConstraintCfg URDFCfg @@ -175,6 +178,34 @@ properties are grouped under ``attrs.surface_props``. Volume elasticity uses ``youngs`` and ``poissons``. Configuration dictionaries serialize this same schema; no legacy aliases or field migration are provided. +Cable authoring +--------------- + +``CableObjectCfg`` authors an ordered centreline, radius and optional material +frames in EmbodiChain's ``xyzw`` convention. ``CablePhysicsCfg`` preserves +unspecified four-mode coefficients as ``None`` and reuses existing collision +and contact-material schemas. ``CableAttachmentCfg`` describes permanent +creation-time endpoint welds. Declare cables before ``prepare()`` with explicit +DexUni on CUDA. Segment writes and complete environment reset require a DexSim +build exposing ``CableBatch.apply_state`` and ``Scene.reset_arenas``. + +.. currentmodule:: embodichain.lab.sim.cfg.cable + +.. autosummary:: + + CablePhysicsCfg + CableAttachmentCfg + CableObjectCfg + +.. autoclass:: CablePhysicsCfg + :members: + +.. autoclass:: CableAttachmentCfg + :members: + +.. autoclass:: CableObjectCfg + :members: + Rendering configuration module ------------------------------ diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst index 0e131c33e4..85af104aab 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst @@ -575,3 +575,61 @@ Deformable implementation import paths .. autosummary:: VolumeDeformableObject + +Cable objects +------------- + +``CableObject`` derives from ``BatchEntity`` and retains one public +``SpawnedCable`` handle per environment. Declare it through +``SimulationManager.add_cable_object()`` and call ``prepare()`` before accessing +``data`` or external loads. ``CableObjectData.segment_state_w`` returns one +coherent ``(environments, segments, 13)`` snapshot in +``[xyz, xyzw, world COM linear velocity, world angular velocity]`` order. +Positions already include environment origins. Returned tensors are independent +snapshots; ``default_segment_state_w`` is immutable engine creation state, not a +solver-history checkpoint. + +Current operations include ``set_force()``, ``set_torque()`` and +``clear_wrenches()`` for selected environments. Loads follow DexSim's one-outer-step +lifetime. ``write_segment_state_to_sim()`` accepts selected environment rows in +the same state layout. This teleport writes both engine buffers and clears +history for each owning world, including other cables, while preserving targets +and pending loads. ``CableObject.reset()`` restores its creation state and clears +its loads; it preserves other bodies and targets. + +Use ``SimulationManager.reset_objects_state()`` for complete cable episodes. +It restores all physical assets, targets, loads and supported runtime controls in +selected Arenas through one ``Scene.reset_arenas()`` call. Physical asset +exclusions are rejected before mutation. Global time is preserved. Reset covers +state, not runtime property or topology edits. Existing contact queries require +another update after reset or teleport. A cable has no single rigid root pose; +individual removal and finalized-scene physical topology changes remain unsupported. + +.. currentmodule:: embodichain.lab.sim.objects.cable + +.. autosummary:: + + CableObject + CableObjectData + +.. autoclass:: CableObject + :members: + :show-inheritance: + +.. autoclass:: CableObjectData + :members: + +Cable implementation import paths +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +.. currentmodule:: embodichain.lab.sim.objects.cable.cable + +.. autosummary:: + + CableObject + +.. currentmodule:: embodichain.lab.sim.objects.cable.data + +.. autosummary:: + + CableObjectData diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index e09b87b2b0..5c7a1cc2af 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -3722,6 +3722,7 @@ embodichain.lab.sim.spawn.descriptors configure_articulation_desc rigid_desc_from_cfg visual_desc_from_cfg + cable_desc_from_cfg surface_deformable_desc_from_cfg volume_deformable_desc_from_cfg diff --git a/docs/source/quick_start/install.md b/docs/source/quick_start/install.md index 78a7369e7c..9749789bd4 100644 --- a/docs/source/quick_start/install.md +++ b/docs/source/quick_start/install.md @@ -67,8 +67,10 @@ python -m pip install torch==2.11.0 torchvision==0.26.0 torchcodec==0.11.1 \ The SDK matrix CI checks a clean CPU installation, recording/loading, video decoding, and an official ACT update/checkpoint/reset. GPU simulation and full policy learning acceptance -are separate checks. Python versions above 3.12 select the newer SDK but are not -part of this validation matrix. +are separate checks. This release requires Python >=3.11 and pins DexSim +0.6.0rc1. The DexForce index provides Linux x86_64 wheels for CPython 3.11 and +3.12; use one of those versions for this release. Python 3.10 is unsupported, +and versions above 3.12 are outside the current wheel/validation matrix. Recordings retain the existing task/subtask table and depth-sidecar schema on both routes. LeRobot 0.6.1 exposes `subtask_index` but does not automatically @@ -114,6 +116,11 @@ select `py311` or `py312` with `EMBODICHAIN_ENV`. See the for internal registry access, environment selection, source development, and dependency locks. +The published image digests in the container guide's validation record belong +to the earlier DexSim 0.5.1rc1 runtime. For this checkout's DexSim 0.6.0rc1 +requirements, build a candidate from the current source or follow the guide's +source-development instructions to update the selected environment. + ### Prerequisites - [Docker](https://docs.docker.com/engine/install/) with [NVIDIA Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html) @@ -342,6 +349,8 @@ for the current compatibility requirements, and see [cuRobo V2 Planner](../overview/sim/motion/planners/curobo_planner.md) for EmbodiChain configuration and usage. cuRobo planning always runs on CUDA, but the SimulationManager physics device may be either CPU or CUDA. +cuRobo's upstream Python range does not expand this release's EmbodiChain +runtime matrix; use Python 3.11 or 3.12 when installing them together. ## Optional: generative simulation (`gensim`) diff --git a/embodichain/lab/sim/_runtime_controls.py b/embodichain/lab/sim/_runtime_controls.py index 6c0b23954b..a8650e21a3 100644 --- a/embodichain/lab/sim/_runtime_controls.py +++ b/embodichain/lab/sim/_runtime_controls.py @@ -18,9 +18,11 @@ from __future__ import annotations +from collections.abc import Callable from typing import Any import numpy as np +import torch from dexsim.engine.newton_physics.runtime_control import RuntimeControl @@ -122,3 +124,157 @@ def close(self) -> None: """Release runtime particle references retained after initialization.""" self._particle_set = None self._initial_positions = None + + +class _InitialJointState: + """Seed creation defaults through engine public initialization hooks only. + + Unlike a realtime controller this never writes targets during stepping. + Scene.reset_arenas restores the engine-owned immutable defaults thereafter. + Omitted positions resolve to zeros from finalized joint metadata, matching + articulation binding before applying the same mimic projection. + """ + + @staticmethod + def normalize_positions(positions: Any) -> np.ndarray: + """Copy a finite source-order vector at the one-time host boundary.""" + if isinstance(positions, torch.Tensor): + if positions.is_complex() or positions.dtype == torch.bool: + raise ValueError("init_qpos must be a finite real vector.") + positions = positions.detach().to(device="cpu", dtype=torch.float32).numpy() + values = np.asarray(positions) + if values.ndim != 1 or values.dtype.kind not in "iuf": + raise ValueError("init_qpos must be a finite real one-dimensional vector.") + with np.errstate(over="ignore", invalid="ignore"): + values = values.astype(np.float32, copy=True) + if not np.isfinite(values).all(): + raise ValueError("init_qpos must contain finite float32 values.") + return values + + def __init__( + self, + target: str, + positions: Any, + *, + mimic_stabilizer: bool = False, + has_permanent_attachment: bool = False, + ) -> None: + self.target = target + self.positions = ( + None if positions is None else self.normalize_positions(positions)[None, :] + ) + self._mimic_stabilizer = mimic_stabilizer + self._has_permanent_attachment = has_permanent_attachment + self._initializer: Any | None = None + self._public_positions: np.ndarray | None = None + self._mimic_info: tuple[np.ndarray, ...] | None = None + + def initialize_model(self, model: Any, result: Any) -> None: + from dexsim.engine.newton_physics import KinematicJointTrajectoryControl + from embodichain.lab.sim.objects.backends.articulation_state import ( + _project_mimic_positions, + map_source_qpos_to_state_order, + read_state_mimic_info, + ) + + handle = result.get_articulation(self.target) + source_positions = ( + np.zeros((1, handle.get_dof()), dtype=np.float32) + if self.positions is None + else self.positions + ) + if not source_positions.size: + self._public_positions = source_positions + return + positions = map_source_qpos_to_state_order( + handle, torch.as_tensor(source_positions), is_spawn_bound=True + ) + self._mimic_info = read_state_mimic_info(handle) + if not self._mimic_stabilizer: + positions = _project_mimic_positions(positions, *self._mimic_info) + self._public_positions = positions.numpy() + self._validate_attachment_pose() + self._initializer = KinematicJointTrajectoryControl( + self.target, self._public_positions, initialize_model_pose=True + ) + self._initializer.initialize_model(model, result) + + def initialize(self, context: Any) -> None: + # The backend creates both states and runs FK from the seeded model + # before this hook. Seed drive targets separately through the public + # batch API; no MuJoCo-specific realtime controller is required. + if self._public_positions is None: + raise RuntimeError("Initial joint model state has not been seeded.") + if not self._public_positions.size: + return + articulation = context.result.get_articulation(self.target) + batch = context.result.create_articulation_batch([articulation]) + if self._mimic_stabilizer: + from embodichain.lab.sim.objects.backends.articulation_state import ( + _project_mimic_positions, + ) + from embodichain.lab.sim.objects.backends.newton import ( + _supports_newton_mimic_compliance, + ) + + if not _supports_newton_mimic_compliance(context.result): + # The opt-in may resolve to a solver without MuJoCo. Match the + # ordinary articulation reset fallback before episode defaults + # are captured, using the Scene writer for state and kinematics. + self._public_positions = _project_mimic_positions( + torch.as_tensor(self._public_positions), *self._mimic_info + ).numpy() + self._validate_attachment_pose() + batch.apply_state(joint_position=self._public_positions) + batch.apply_state(joint_target_position=self._public_positions) + + def _validate_attachment_pose(self) -> None: + if self._has_permanent_attachment and np.any(self._public_positions != 0): + raise NotImplementedError( + f"Cable attachment to {self.target!r} requires a nonzero joint " + "pose after mimic projection. DexSim must author that pose " + "before attachment construction; initialization was rejected." + ) + + def close(self) -> None: + if self._initializer is not None: + self._initializer.close() + self._initializer = None + self._public_positions = None + self._mimic_info = None + + +class _SceneInitialJointStates(RuntimeControl): + """Seed all articulation defaults of a cable scene during its build.""" + + graph_safe = True + + def __init__(self, seeds: tuple[_InitialJointState, ...]) -> None: + self.seeds = seeds + + def replace(self, seeds: tuple[_InitialJointState, ...]) -> None: + """Discard resources from an unsuccessful build before retrying.""" + self.close() + self.seeds = seeds + + def initialize_model(self, model: Any, result: Any) -> None: + for seed in self.seeds: + seed.initialize_model(model, result) + + def initialize(self, context: Any) -> None: + for seed in self.seeds: + seed.initialize(context) + + def __call__( + self, context: Any, substep_index: int, substep_count: int, substep_dt: float + ) -> None: + pass + + def prepare_episode_reset( + self, arena_indices: tuple[int, ...] + ) -> Callable[[], None]: + return lambda: None + + def close(self) -> None: + for seed in self.seeds: + seed.close() diff --git a/embodichain/lab/sim/cfg/__init__.py b/embodichain/lab/sim/cfg/__init__.py index c790a9b80f..f3e20f4f81 100644 --- a/embodichain/lab/sim/cfg/__init__.py +++ b/embodichain/lab/sim/cfg/__init__.py @@ -41,6 +41,7 @@ from .asset import AssetPhysicsMode, ObjectBaseCfg, _resolve_asset_physics_mode from .fluid import FluidObjectCfg, FluidSpawnCfg, FluidRenderCfg from .source import UsdSourceCfg +from .cable import CableAttachmentCfg, CableObjectCfg, CablePhysicsCfg from .usd import UsdImportCfg from .deformable import ( SurfaceElementPropertiesCfg, @@ -130,6 +131,9 @@ "LightCfg", "RigidObjectCfg", "VisualObjectCfg", + "CableObjectCfg", + "CablePhysicsCfg", + "CableAttachmentCfg", "DeformableObjectCfg", "VolumeDeformableObjectCfg", "SurfaceDeformableObjectCfg", diff --git a/embodichain/lab/sim/cfg/cable.py b/embodichain/lab/sim/cfg/cable.py new file mode 100644 index 0000000000..19532eb3af --- /dev/null +++ b/embodichain/lab/sim/cfg/cable.py @@ -0,0 +1,161 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Cable authoring configuration for the Newton DexUni backend.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +from dataclasses import MISSING +from typing import Any, Literal + +import numpy as np + +from embodichain.utils import configclass + +from ..material import VisualMaterialCfg +from ..shapes import MeshCfg +from .asset import ObjectBaseCfg +from .rigid import CollisionPropertiesCfg, RigidBodyMaterialCfg, RigidBodyPhysicsCfg + +__all__ = ["CablePhysicsCfg", "CableAttachmentCfg", "CableObjectCfg"] + + +@configclass +class CablePhysicsCfg: + """Uniform per-connection stretch, shear, bend and twist coefficients. + + Linear stiffness/damping use N/m and N s/m; angular coefficients use + N m/rad and N m s/rad. ``None`` preserves DexSim's solver defaults and + is distinct from an explicit zero. Dahl options remain scene-owned in + ``NewtonPhysicsCfg.solver_cfg``. + """ + + density: float = 1000.0 + """Volume density [kg/m³].""" + stretch_stiffness: float | None = None + stretch_damping: float | None = None + shear_stiffness: float | None = None + shear_damping: float | None = None + bend_stiffness: float | None = None + bend_damping: float | None = None + twist_stiffness: float | None = None + twist_damping: float | None = None + wall_thickness: float | None = None + """Material wall thickness [m], affecting mass and inertia only. + + Zero selects solid material; positive values up to the cable radius select + a hollow cylinder. None preserves DexSim's collision-shape mass defaults. + """ + collision_props: CollisionPropertiesCfg | None = None + """Existing portable or Newton contact-envelope configuration.""" + material_props: RigidBodyMaterialCfg | None = None + """Existing contact-material configuration for segment capsules.""" + + def __post_init__(self) -> None: + contact = RigidBodyPhysicsCfg.from_dict( + { + "collision_props": self.collision_props, + "material_props": self.material_props, + } + ) + self.collision_props = contact.collision_props + self.material_props = contact.material_props + + +@configclass +class CableAttachmentCfg: + """Permanent endpoint weld to a sibling rigid object or articulation link. + + The endpoint must coincide with ``position`` in the target body frame at + preparation. This creation-time configuration does not provide runtime + grasping or detachment. + """ + + target: str = MISSING + """Target simulation UID in the same environment.""" + position: Sequence[float] | np.ndarray = (0.0, 0.0, 0.0) + """Anchor position in the target body frame [m].""" + link_name: str | None = None + """Articulation link name; omit for a rigid object.""" + + +@configclass +class CableObjectCfg(ObjectBaseCfg): + """One cable replicated through the existing Spawn scene lifecycle. + + Points are ordered endpoints in object-local metres. Physical topology + and material changes require creating a new simulation. Runtime segment + teleports and whole-environment episode reset use the public Scene API. + """ + + points: Sequence[Sequence[float]] | np.ndarray = MISSING + """Finite ``(N, 3)`` centreline endpoints, with ``N >= 2``.""" + radius: float = MISSING + """Positive capsule/tube radius [m].""" + segment_quaternions: Sequence[Sequence[float]] | np.ndarray | None = None + """Optional local material frames ``(N - 1, 4)`` in **xyzw** order. + + Local +Z follows each segment. Rotation about +Z specifies material twist. + ``None`` uses DexSim's parallel-transport frames. + """ + closed: bool = False + fixed_segments: Sequence[int] = () + attrs: CablePhysicsCfg = CablePhysicsCfg() + collision_filter_span: int = 1 + start_attachment: CableAttachmentCfg | None = None + end_attachment: CableAttachmentCfg | None = None + render_mode: Literal["tube", "capsules"] = "tube" + visual_material: VisualMaterialCfg | None = None + """Material of generated tube/capsule visuals.""" + visual_shape: MeshCfg | None = None + """Optional custom visual mesh; its own ``visual_material`` owns styling.""" + + def __post_init__(self) -> None: + if isinstance(self.attrs, Mapping): + self.attrs = CablePhysicsCfg(**self.attrs) + for name in ("start_attachment", "end_attachment"): + value = getattr(self, name) + if isinstance(value, Mapping): + setattr(self, name, CableAttachmentCfg(**value)) + if isinstance(self.visual_material, Mapping): + self.visual_material = VisualMaterialCfg(**self.visual_material) + if isinstance(self.visual_shape, Mapping): + self.visual_shape = MeshCfg.from_dict( + {"shape_type": "Mesh", **self.visual_shape} + ) + + @classmethod + def from_dict(cls, init_dict: Mapping[str, Any]) -> CableObjectCfg: + """Parse typed nested configuration and the common asset pose. + + Args: + init_dict: Cable configuration from YAML/JSON or ``to_dict()``. + + Returns: + A cable config with typed physics, attachments and visuals. + """ + cfg = cls(**dict(init_dict)) + pose = ObjectBaseCfg.from_dict( + { + "init_pos": cfg.init_pos, + "init_rot": cfg.init_rot, + "init_local_pose": cfg.init_local_pose, + } + ) + cfg.init_pos, cfg.init_rot = pose.init_pos, pose.init_rot + cfg.init_local_pose = pose.init_local_pose + return cfg diff --git a/embodichain/lab/sim/objects/__init__.py b/embodichain/lab/sim/objects/__init__.py index 81f166ed66..7def08319f 100644 --- a/embodichain/lab/sim/objects/__init__.py +++ b/embodichain/lab/sim/objects/__init__.py @@ -22,6 +22,7 @@ from __future__ import annotations from ..common import BatchEntity +from .cable import CableObject, CableObjectData from .rigid_object import CollisionShapeDesc, RigidObject, RigidBodyData, RigidObjectCfg from .visual_object import VisualObject, VisualObjectCfg from .rigid_object_group import ( diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index 3b7b856ffa..21da357033 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -79,6 +79,7 @@ ) from embodichain.lab.sim.objects.backends._kinematics import _build_pk_chain from embodichain.lab.sim.objects.backends.articulation_state import ( + _project_mimic_positions, get_state_joint_names, map_source_qpos_to_state_order, read_state_mimic_info, @@ -1103,22 +1104,13 @@ def _project_mimic_qpos(self, qpos: torch.Tensor) -> torch.Tensor: """Return qpos with every mimic child projected from its parent.""" if not self.mimic_ids: return qpos - - projected = qpos.clone() - mimic_ids = torch.as_tensor( - self.mimic_ids, dtype=torch.long, device=qpos.device - ) - parent_ids = torch.as_tensor( - self.mimic_parents, dtype=torch.long, device=qpos.device - ) - multipliers = torch.as_tensor( - self.mimic_multipliers, dtype=qpos.dtype, device=qpos.device - ) - offsets = torch.as_tensor( - self.mimic_offsets, dtype=qpos.dtype, device=qpos.device + return _project_mimic_positions( + qpos, + self.mimic_ids, + self.mimic_parents, + self.mimic_multipliers, + self.mimic_offsets, ) - projected[..., mimic_ids] = projected[..., parent_ids] * multipliers + offsets - return projected def _stabilize_newton_mimic_target_write( self, diff --git a/embodichain/lab/sim/objects/backends/articulation_state.py b/embodichain/lab/sim/objects/backends/articulation_state.py index 64dfe2aaed..9cac130df3 100644 --- a/embodichain/lab/sim/objects/backends/articulation_state.py +++ b/embodichain/lab/sim/objects/backends/articulation_state.py @@ -17,6 +17,8 @@ from __future__ import annotations +from collections.abc import Sequence + import numpy as np import torch @@ -27,6 +29,25 @@ ] +def _project_mimic_positions( + qpos: torch.Tensor, + mimic_ids: Sequence[int] | np.ndarray, + parent_ids: Sequence[int] | np.ndarray, + multipliers: Sequence[float] | np.ndarray, + offsets: Sequence[float] | np.ndarray, +) -> torch.Tensor: + """Project configured positions using state-ordered mimic relations.""" + if len(mimic_ids) == 0: + return qpos + projected = qpos.clone() + children = torch.as_tensor(mimic_ids, dtype=torch.long, device=qpos.device) + parents = torch.as_tensor(parent_ids, dtype=torch.long, device=qpos.device) + scales = torch.as_tensor(multipliers, dtype=qpos.dtype, device=qpos.device) + shifts = torch.as_tensor(offsets, dtype=qpos.dtype, device=qpos.device) + projected[..., children] = projected[..., parents] * scales + shifts + return projected + + def get_state_joint_names(entity: object) -> list[str]: """Return active joint names in the backing state-buffer order.""" try: diff --git a/embodichain/lab/sim/objects/backends/newton.py b/embodichain/lab/sim/objects/backends/newton.py index 1763ea5287..16219b98f8 100644 --- a/embodichain/lab/sim/objects/backends/newton.py +++ b/embodichain/lab/sim/objects/backends/newton.py @@ -178,6 +178,22 @@ def _default_mujoco_mimic_solref(physics_dt: float, num_substeps: int) -> np.nda ) +def _supports_newton_mimic_compliance(result: Scene | None) -> bool: + """Whether the finalized solver can configure the opt-in mimic stabilizer.""" + if result is None or result.backend != "newton": + return False + from dexsim.engine.newton_physics.backend_registry import get_newton_backend + + backend = get_newton_backend(result.world) + return bool( + backend is not None + and backend.solver_type in {"mujoco_warp", "dexuni"} + and not backend.cfg.requires_grad + and not (backend.model is not None and backend.model.requires_grad) + and backend.mujoco_solver is not None + ) + + def _configure_newton_mimic_compliance( *, result: Scene | None, @@ -196,7 +212,7 @@ def _configure_newton_mimic_compliance( native equality rows enabled preserves mechanical force coupling; the drive is only a stabilizer and never mirrors measured follower state. """ - if result is None or result.backend != "newton" or not mimic_ids: + if not mimic_ids or not _supports_newton_mimic_compliance(result): return False from dexsim.engine.newton_physics.backend_registry import get_newton_backend diff --git a/embodichain/lab/sim/objects/cable/__init__.py b/embodichain/lab/sim/objects/cable/__init__.py new file mode 100644 index 0000000000..33ec9b988c --- /dev/null +++ b/embodichain/lab/sim/objects/cable/__init__.py @@ -0,0 +1,24 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Cable objects backed by the finalized DexSim Scene.""" + +from __future__ import annotations + +from .cable import CableObject +from .data import CableObjectData + +__all__ = ["CableObject", "CableObjectData"] diff --git a/embodichain/lab/sim/objects/cable/cable.py b/embodichain/lab/sim/objects/cable/cable.py new file mode 100644 index 0000000000..b25e414906 --- /dev/null +++ b/embodichain/lab/sim/objects/cable/cable.py @@ -0,0 +1,320 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Replicated cable facade using only DexSim's public Scene interfaces.""" + +from __future__ import annotations + +from collections.abc import Sequence +from copy import deepcopy +from numbers import Integral +from typing import TYPE_CHECKING, Any + +import numpy as np +import torch +from dexsim.scene import Scene, SpawnedCable + +from embodichain.lab.sim.common import BatchEntity + +from .data import CableObjectData + +if TYPE_CHECKING: + from embodichain.lab.sim.cfg import CableObjectCfg + +__all__ = ["CableObject"] + + +class CableObject(BatchEntity): + """Declared cable, bound in place by ``SimulationManager.prepare()``. + + Cable state and resources remain owned by DexSim. There is no single + deformable root pose. Segment writes are teleports; complete episode reset + belongs to ``SimulationManager.reset_objects_state()`` and its owning Scene. + + Args: + cfg: Typed cable authoring configuration. + device: Newton compute device selected by the manager. + """ + + def __init__( + self, cfg: CableObjectCfg, device: torch.device = torch.device("cpu") + ) -> None: + self.cfg = deepcopy(cfg) + self.uid = self.cfg.uid + self.device = torch.device(device) + self._entities: list[SpawnedCable] = [] + self._declared_num_instances: int | None = None + self._spawn_result: Scene | None = None + self._world = None + self._data: CableObjectData | None = None + + def _initialize_spawn_declaration(self, num_instances: int) -> None: + if num_instances <= 0: + raise ValueError("A declared CableObject requires num_instances > 0.") + if self._declared_num_instances not in (None, num_instances): + raise RuntimeError("CableObject was already declared for another count.") + self._declared_num_instances = num_instances + + @property + def is_declared(self) -> bool: + """Whether this facade is waiting for its finalized Scene binding.""" + return self._spawn_result is None + + @property + def is_spawn_bound(self) -> bool: + """Whether this facade has completed Spawn binding.""" + return self._spawn_result is not None + + @property + def num_instances(self) -> int: + """Materialized or declared environment count.""" + if self._declared_num_instances is None: + raise RuntimeError("Register CableObject through SimulationManager first.") + return self._declared_num_instances + + @property + def data(self) -> CableObjectData | None: + """Read-only segment data after successful ``prepare()``.""" + return self._data + + def attach_spawn_handles(self, entities: Sequence[SpawnedCable]) -> None: + """Attach replicated cable handles before final data binding. + + Args: + entities: One public cable handle per declared environment. + """ + handles = list(entities) + if len(handles) != self.num_instances: + raise ValueError( + f"CableObject expected {self.num_instances} handles, got {len(handles)}." + ) + if not all(isinstance(handle, SpawnedCable) for handle in handles): + raise TypeError("CableObject requires SpawnedCable handles.") + self._entities = handles + + def bind_spawn(self, result: Scene) -> None: + """Create the public cable batch after finalization, without stepping. + + Args: + result: Finalized Newton Scene owning the attached handles. + """ + if self.is_spawn_bound: + raise RuntimeError(f"CableObject {self.uid!r} is already Spawn-bound.") + if not isinstance(result, Scene): + raise TypeError("CableObject requires a finalized DexSim Scene.") + if result.backend != "newton": + raise NotImplementedError("CableObject requires the Newton backend.") + if len(self._entities) != self.num_instances: + raise ValueError("Attach all cable handles before binding.") + # Construct first: a failed batch/default-state read remains retryable. + world = result.world + data = CableObjectData(self._entities, result, self.device) + self._data = data + self._spawn_result = result + self._world = world + + def __str__(self) -> str: + return ( + f"CableObject | uid: {self.uid} | instances: " + f"{self._declared_num_instances} | bound: {self.is_spawn_bound}" + ) + + def _resolve_env_ids(self, env_ids: Sequence[int] | None) -> list[int]: + if env_ids is None: + return list(range(self.num_instances)) + values = ( + env_ids.detach().cpu().reshape(-1).tolist() + if isinstance(env_ids, torch.Tensor) + else list(env_ids) + ) + if any( + isinstance(value, (bool, np.bool_)) or not isinstance(value, Integral) + for value in values + ): + raise TypeError("env_ids must contain integer environment indices.") + ids = [int(value) for value in values] + if len(set(ids)) != len(ids): + raise ValueError("env_ids must not contain duplicates.") + if any(value < 0 or value >= self.num_instances for value in ids): + raise IndexError("env_ids are outside this CableObject batch.") + return ids + + def _require_bound(self) -> None: + if self._data is None or self._spawn_result is None: + raise RuntimeError( + "Call SimulationManager.prepare() before cable state use." + ) + if not self._data.batch.is_valid: + raise RuntimeError("CableObject Scene was cleared or released.") + + def _apply_load( + self, + operation: str, + value: Any, + segment_index: int, + env_ids: Sequence[int] | None, + ) -> None: + self._require_bound() + ids = self._resolve_env_ids(env_ids) + assert self._data is not None + count = self._data.num_segments + if ( + isinstance(segment_index, (bool, np.bool_)) + or not isinstance(segment_index, Integral) + or not -count <= segment_index < count + ): + raise IndexError("segment_index is outside this cable.") + if isinstance(value, torch.Tensor): + value = value.detach().cpu().numpy() + values = np.asarray(value) + if values.dtype.kind not in "iuf": + raise ValueError("Cable loads must be finite real XYZ vectors.") + if values.shape == (3,): + values = np.broadcast_to(values, (len(ids), 3)) + if values.shape != (len(ids), 3) or not np.isfinite(values).all(): + raise ValueError(f"Cable loads require shape (3,) or ({len(ids)}, 3).") + values = values.astype(np.float32) + if not np.isfinite(values).all(): + raise ValueError("Cable loads must be representable as finite float32.") + handles = [self._entities[index] for index in ids] + if any(not handle.is_valid for handle in handles): + raise RuntimeError("CableObject contains an invalid cable handle.") + for handle, vector in zip(handles, values, strict=True): + getattr(handle, operation)(vector, segment_index=int(segment_index)) + + def set_force( + self, + force: torch.Tensor | np.ndarray | Sequence[float], + *, + segment_index: int = -1, + env_ids: Sequence[int] | None = None, + ) -> None: + """Set world COM force for the next outer update, then auto-clear. + + Args: + force: XYZ newtons broadcast to selected environments, or ``(E,3)``. + segment_index: Segment to load; negative indices count from the tip. + env_ids: Selected environments in force-row order; None selects all. + """ + self._apply_load("set_force", force, segment_index, env_ids) + + def set_torque( + self, + torque: torch.Tensor | np.ndarray | Sequence[float], + *, + segment_index: int = -1, + env_ids: Sequence[int] | None = None, + ) -> None: + """Set world torque for the next outer update, then auto-clear. + + Args: + torque: XYZ N m broadcast to selected environments, or ``(E,3)``. + segment_index: Segment to load; negative indices count from the tip. + env_ids: Selected environments in torque-row order; None selects all. + """ + self._apply_load("set_torque", torque, segment_index, env_ids) + + def clear_wrenches(self, env_ids: Sequence[int] | None = None) -> None: + """Clear pending external loads in selected environments. + + Args: + env_ids: Selected environments; None selects all. + """ + self._require_bound() + ids = self._resolve_env_ids(env_ids) + handles = [self._entities[index] for index in ids] + if any(not handle.is_valid for handle in handles): + raise RuntimeError("CableObject contains an invalid cable handle.") + for handle in handles: + handle.clear_wrenches() + + def set_local_pose( + self, pose: torch.Tensor, env_ids: Sequence[int] | None = None + ) -> None: + """Reject a single rigid root pose; use segment state writes instead. + + Args: + pose: Requested root pose. + env_ids: Requested environment selection. + """ + raise NotImplementedError("A cable has no single rigid root pose.") + + def write_segment_state_to_sim( + self, state: torch.Tensor, env_ids: Sequence[int] | None = None + ) -> None: + """Teleport selected cables using world segment states. + + Writes both engine states through the public CableBatch. Solver history + for each owning world (including other cables) is cleared. Controls and + pending loads are preserved. This is suitable for initialization after + manager reset, not for resuming a solver-history checkpoint. Call between + updates, outside caller CUDA Graph capture and gradient mode. Existing + contact queries require another physics update before fetching. + + Args: + state: Contiguous float32 ``(E,S,13)`` on this object's device, in + ``[xyz, xyzw, world COM linear velocity, angular velocity]`` + order. Quaternions must be unit length; values must be finite. + env_ids: Selected environments in state-row order; None selects all. + """ + self._require_bound() + ids = self._resolve_env_ids(env_ids) + assert self._data is not None + if not isinstance(state, torch.Tensor): + raise TypeError("Cable state must be a torch.Tensor.") + expected = (len(ids), self._data.num_segments, 13) + if tuple(state.shape) != expected: + raise ValueError(f"Cable state requires shape {expected}.") + if state.dtype != torch.float32 or state.device != torch.device( + self._data.batch.device + ): + raise ValueError("Cable state must be float32 on the cable device.") + if state.requires_grad or not state.is_contiguous(): + raise ValueError("Cable state must be contiguous without gradients.") + if not ids: + return + pose = torch.cat((state[..., 3:7], state[..., :3]), -1) + velocity = state[..., 7:].contiguous() + self._data.batch.select(ids).apply_state(pose=pose, velocity=velocity) + + def get_local_pose(self, to_matrix: bool = False) -> torch.Tensor: + """Reject a single root pose; use ``data.segment_pose_w``. + + Args: + to_matrix: Requested representation. + """ + raise NotImplementedError("A cable has no single rigid root pose.") + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Restore selected cable creation states and clear their pending loads. + + This clears solver history in their owning worlds while preserving other + bodies and drive targets. Use manager reset for a whole episode, including + attached robots, other cables and runtime controls. + + Args: + env_ids: Selected environments; None selects all. + """ + self._require_bound() + ids = self._resolve_env_ids(env_ids) + assert self._data is not None + state = self._data.default_segment_state_w[ids].contiguous() + self.write_segment_state_to_sim(state, ids) + self.clear_wrenches(ids) + + def destroy(self) -> None: + """Reject individual removal; the manager releases the owning Scene.""" + raise NotImplementedError("DexSim does not expose individual cable removal.") diff --git a/embodichain/lab/sim/objects/cable/data.py b/embodichain/lab/sim/objects/cable/data.py new file mode 100644 index 0000000000..6283c5947f --- /dev/null +++ b/embodichain/lab/sim/objects/cable/data.py @@ -0,0 +1,138 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Read-only segment state from DexSim's public cable batch.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING +from collections.abc import Sequence + +import torch + +if TYPE_CHECKING: + from dexsim.scene import Scene, SpawnedCable + +__all__ = ["CableObjectData"] + + +class CableObjectData: + """Independent world-state snapshots for one replicated cable asset. + + Segment poses use EmbodiChain order ``xyz + xyzw``; velocities are world + COM linear then angular. World positions already include Arena origins. + All instances of one asset must share the same segment count. Reading + state never advances physics and never exposes writable simulation views. + + Args: + entities: Replicated cable handles in environment order. + scene: Finalized Scene owning those handles. + device: Device of the Newton cable state. + """ + + def __init__( + self, + entities: Sequence[SpawnedCable], + scene: Scene, + device: torch.device, + ) -> None: + self.entities = list(entities) + if not self.entities: + raise ValueError("A cable batch cannot be empty.") + counts = tuple(entity.segment_count for entity in self.entities) + if min(counts) <= 0 or len(set(counts)) != 1: + raise ValueError( + "Instances of one cable asset require equal positive segment " + f"counts, got {counts}." + ) + self.scene = scene + self.device = torch.device(device) + self.num_instances = len(self.entities) + self.num_segments = counts[0] + self.batch = scene.create_cable_batch(self.entities) + self._pose_buffer = torch.empty( + (self.num_instances, self.num_segments, 7), + dtype=torch.float32, + device=self.device, + ) + self._velocity_buffer = torch.empty( + (self.num_instances, self.num_segments, 6), + dtype=torch.float32, + device=self.device, + ) + self._state_step = 0 + self.batch.fetch_default_state(self._pose_buffer, self._velocity_buffer) + self._default_segment_state_w = torch.cat( + ( + self._pose_buffer[..., 4:], + self._pose_buffer[..., :4], + self._velocity_buffer, + ), + -1, + ) + + def _fetch_state(self) -> tuple[torch.Tensor, torch.Tensor]: + self._state_step = self.batch.fetch_state( + self._pose_buffer, self._velocity_buffer + ) + pose = torch.cat((self._pose_buffer[..., 4:], self._pose_buffer[..., :4]), -1) + return pose, self._velocity_buffer.clone() + + @property + def state_step(self) -> int: + """Outer simulation step of the most recently fetched snapshot.""" + return self._state_step + + @property + def segment_pose_w(self) -> torch.Tensor: + """World segment poses ``(B, S, 7)`` in ``xyz + xyzw`` order.""" + return self._fetch_state()[0] + + @property + def segment_pos_w(self) -> torch.Tensor: + """World segment-centre positions ``(B, S, 3)`` [m].""" + return self.segment_pose_w[..., :3] + + @property + def segment_quat_w(self) -> torch.Tensor: + """World material-frame quaternions ``(B, S, 4)`` in xyzw order.""" + return self.segment_pose_w[..., 3:] + + @property + def segment_vel_w(self) -> torch.Tensor: + """World COM linear/angular velocities ``(B, S, 6)``.""" + return self._fetch_state()[1] + + @property + def segment_lin_vel_w(self) -> torch.Tensor: + """World COM linear velocities ``(B, S, 3)`` [m/s].""" + return self.segment_vel_w[..., :3] + + @property + def segment_ang_vel_w(self) -> torch.Tensor: + """World angular velocities ``(B, S, 3)`` [rad/s].""" + return self.segment_vel_w[..., 3:] + + @property + def segment_state_w(self) -> torch.Tensor: + """One coherent ``[xyz, xyzw, linear, angular]`` snapshot ``(B,S,13)``.""" + pose, velocity = self._fetch_state() + return torch.cat((pose, velocity), -1) + + @property + def default_segment_state_w(self) -> torch.Tensor: + """Immutable creation state; this is not a solver-history checkpoint.""" + return self._default_segment_state_w.clone() diff --git a/embodichain/lab/sim/physics/base.py b/embodichain/lab/sim/physics/base.py index 50606d8ce2..c53d320582 100644 --- a/embodichain/lab/sim/physics/base.py +++ b/embodichain/lab/sim/physics/base.py @@ -39,6 +39,7 @@ from __future__ import annotations from abc import ABC, abstractmethod +from collections.abc import Sequence from typing import TYPE_CHECKING if TYPE_CHECKING: @@ -108,6 +109,24 @@ def prepare_spawn_runtime(self, result: "dexsim.scene.Scene") -> None: """ del result + def _prepare_scene_build(self) -> None: + """Validate and register backend initialization before Spawn commits. + + Implementations must preflight unsupported combinations before mutating + the builder, and leave successful registrations idempotent for retries. + """ + + def _reset_scene_state( + self, env_ids: Sequence[int] | None, excluded_uids: set[str] + ) -> list[int] | None: + """Optionally reset physical state atomically through the owning Scene. + + Return selected environment IDs when the backend handled every physical + asset, or None to use ordinary per-object reset. Lights and sensors + remain manager-owned. Reject unsupported exclusions before any mutation. + """ + return None + def sync_render_state(self, result: "dexsim.scene.Scene") -> None: """Publish the current physics state to render resources without stepping. @@ -169,6 +188,11 @@ def solver_type(self) -> str | None: # ------------------------------------------------------------------ # # Capabilities (override in subclasses; defaults are conservative) # ------------------------------------------------------------------ # + @property + def supports_cables(self) -> bool: + """Whether this backend can declare cables (solver/device gates apply).""" + return False + @property def supports_volume_deformables(self) -> bool: """Whether this backend has a volume-deformable object adapter.""" diff --git a/embodichain/lab/sim/physics/newton.py b/embodichain/lab/sim/physics/newton.py index 20f4b933f9..743320f2b7 100644 --- a/embodichain/lab/sim/physics/newton.py +++ b/embodichain/lab/sim/physics/newton.py @@ -18,11 +18,12 @@ from __future__ import annotations import importlib -from typing import TYPE_CHECKING from collections.abc import Sequence +from typing import TYPE_CHECKING import weakref import warp as wp +import numpy as np from .base import PhysicsBackend @@ -32,6 +33,7 @@ from embodichain.lab.sim.sim_manager import SimulationManagerCfg from embodichain.lab.sim.objects.fluid import FluidObject + from embodichain.lab.sim._runtime_controls import _SceneInitialJointStates __all__ = ["NewtonPhysicsBackend"] @@ -64,6 +66,7 @@ def __init__(self, manager) -> None: self._fluid_render_clip_key: tuple[object, ...] | None = None self._fluid_render_clip_z: torch.Tensor | None = None self._fluid_render_dirty = False + self._joint_defaults: _SceneInitialJointStates | None = None @property def solver_type(self) -> str | None: @@ -106,6 +109,103 @@ def activate(self, sim_config: "SimulationManagerCfg") -> None: # SceneBuilder.finalize() completes its model; no second manager-level # activation or rebuild domain participates. + def _prepare_scene_build(self) -> None: + """Preflight cable anchors and prepare defaults for the current declarations.""" + manager = self._manager + cables = getattr(manager, "_cable_objects", {}) + if not cables: + return + from embodichain.lab.sim._runtime_controls import ( + _InitialJointState, + _SceneInitialJointStates, + ) + + articulations = {**manager._robots, **manager._articulations} + # Normalize the entire input before registration so an invalid config + # cannot leave an incomplete set of initialization controls behind. + positions = { + uid: ( + None + if obj.cfg.init_qpos is None + else _InitialJointState.normalize_positions(obj.cfg.init_qpos) + ) + for uid, obj in articulations.items() + } + attached_articulations: set[str] = set() + for cable in cables.values(): + for anchor in (cable.cfg.start_attachment, cable.cfg.end_attachment): + if anchor is not None and anchor.target in positions: + attached_articulations.add(anchor.target) + if positions[anchor.target] is not None and np.any( + positions[anchor.target] != 0 + ): + raise NotImplementedError( + f"Cable {cable.uid!r} attachment to {anchor.target!r} " + "with nonzero init_qpos requires a DexSim authoring " + "API that applies initial joint poses before attachment " + "construction; the current model-initialization hook " + "runs too late. No scene has been committed." + ) + scene = manager._spawn_scene + if scene.builder.is_finalized: + return + seeds = tuple( + _InitialJointState( + f"{arena_name}/{uid}", + values, + mimic_stabilizer=articulations[ + uid + ].cfg.root_props.newton_mimic_stabilizer, + has_permanent_attachment=uid in attached_articulations, + ) + for uid, values in positions.items() + if values is None or values.size + for arena_name in scene.arena_names + ) + # Spawn only accepts additions to its pending control list. Refresh the + # group we own, so a failed build can remove or replace any articulation + # without duplicating or modifying caller-registered controls. + if self._joint_defaults is not None: + self._joint_defaults.replace(seeds) + elif seeds: + control = _SceneInitialJointStates(seeds) + scene.builder.add_runtime_control(control) + self._joint_defaults = control + + def _reset_scene_state( + self, env_ids: Sequence[int] | None, excluded_uids: set[str] + ) -> list[int] | None: + """Restore cable worlds through the public engine episode boundary.""" + manager = self._manager + cables = getattr(manager, "_cable_objects", {}) + if not cables: + return None + physical_uids = ( + set(manager._robots) + | set(manager._articulations) + | set(manager._rigid_objects) + | set(manager._rigid_object_groups) + | set(manager._deformable_objects) + | set(cables) + ) + if physical_uids.intersection(excluded_uids): + raise NotImplementedError( + "Cable episode reset requires every physical asset in selected " + "environments; no objects have been reset." + ) + cable = next(iter(cables.values())) + cable._require_bound() + ids = cable._resolve_env_ids(env_ids) + if ids: + result = manager.spawn_result + if result is None: + raise RuntimeError("Cable reset requires a prepared Spawn scene.") + arenas = [ + result.arenas.index(manager._spawn_scene.arena_names[i]) for i in ids + ] + result.reset_arenas(arenas) + return ids + def sync_render_state(self, result: "dexsim.scene.Scene") -> None: """Publish Newton state through DexSim's render bridge without stepping.""" if not self._manager.has_native_renderer: @@ -374,6 +474,11 @@ def get_scene(self): ) # -- capabilities --------------------------------------------------- # + @property + def supports_cables(self) -> bool: + """Newton supports cables with explicit DexUni on CUDA.""" + return True + @property def supports_volume_deformables(self) -> bool: return True diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index 215c6ebb6e..ad2d02d9b8 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -77,6 +77,7 @@ def _is_usd_path(path: object | None) -> bool: from dexsim.render import LightType, Windows from embodichain.lab.sim.objects import ( + CableObject, RigidObject, VisualObject, RigidObjectGroup, @@ -99,6 +100,7 @@ def _is_usd_path(path: object | None) -> bool: ) from embodichain.lab.sim.sensors.attachment import resolve_parent_nodes from embodichain.lab.sim.cfg import ( + CableObjectCfg, RenderCfg, PhysicsBackendCfg, GPUMemoryCfg, @@ -126,6 +128,7 @@ def _is_usd_path(path: object | None) -> bool: from embodichain.lab.sim.physics import make_physics_backend from embodichain.lab.sim.cfg.source import _asset_file_path from embodichain.lab.sim.spawn.descriptors import ( + cable_desc_from_cfg, articulation_desc_from_cfg, configure_articulation_desc, rigid_desc_from_cfg, @@ -733,6 +736,7 @@ def _initialize_simulation( self._rigid_objects: Dict[str, RigidObject] = dict() self._rigid_object_groups: Dict[str, RigidObjectGroup] = dict() self._deformable_objects: Dict[str, DeformableObject] = dict() + self._cable_objects: dict[str, CableObject] = {} self._articulations: Dict[str, Articulation] = dict() self._robots: Dict[str, Robot] = dict() @@ -1018,6 +1022,7 @@ def asset_uids(self) -> List[str]: uid_list.extend(list(self._rigid_object_groups.keys())) uid_list.extend(list(self._deformable_objects.keys())) uid_list.extend(list(self._fluid_objects.keys())) + uid_list.extend(getattr(self, "_cable_objects", {})) uid_list.extend(list(self._articulations.keys())) return uid_list @@ -1087,6 +1092,7 @@ def start_visualization(self) -> VisualizationRuntime | None: getattr(self, "_rigid_object_groups", {}), getattr(self, "_deformable_objects", {}), getattr(self, "_fluid_objects", {}), + getattr(self, "_cable_objects", {}), getattr(self, "_articulations", {}), getattr(self, "_robots", {}), getattr(self, "_sensors", {}), @@ -1856,6 +1862,7 @@ def prepare(self) -> None: and sensor.cfg.extrinsics.parent is not None ): sensor._detach_from_parent_nodes() + self.physics._prepare_scene_build() result = scene.commit() self._env = result.get_arena("default") self._arenas = [result.get_arena(name) for name in scene.arena_names] @@ -2502,13 +2509,14 @@ def get_asset( | RigidObjectGroup | DeformableObject | FluidObject + | CableObject | Articulation | None ): """Get an asset by its UID. - The asset can be a light, sensor, robot, visual object, rigid object, deformable, or - articulation. + The asset can be a light, sensor, robot, visual object, rigid object, deformable, + fluid, cable, or articulation. Args: uid (str): The UID of the asset. @@ -2532,6 +2540,8 @@ def get_asset( return self._deformable_objects[uid] if uid in self._fluid_objects: return self._fluid_objects[uid] + if uid in getattr(self, "_cable_objects", {}): + return self._cable_objects[uid] if uid in self._articulations: return self._articulations[uid] @@ -2759,10 +2769,9 @@ def add_visual_object(self, cfg: VisualObjectCfg) -> VisualObject: descriptor, materials = visual_desc_from_cfg(cfg) visual = VisualObject(cfg, self.device) was_materialized = self.spawn_result is not None - self._spawn_scene.builder.materials.update( - self._spawn_scene._resolve_materials(materials) + self._spawn_scene.declare( + "visual_object", cfg.uid, descriptor, facade=visual, materials=materials ) - self._spawn_scene.declare("visual_object", cfg.uid, descriptor, facade=visual) self._visual_objects[cfg.uid] = visual self.notify_visualization_topology_changed() if was_materialized: @@ -2910,11 +2919,14 @@ def _declare_rigid_objects( rigid_obj = RigidObject(cfg=cfg, device=self.device) declarations.append((cfg.uid, descriptor, materials, rigid_obj)) + for uid, _, _, _ in declarations: + if replace: + self._spawn_scene._require_removable(uid) + else: + self._spawn_scene._preflight_declaration("rigid_object", uid) + self._spawn_scene._register_material_batches(item[2] for item in declarations) was_materialized = self.spawn_result is not None for uid, descriptor, materials, rigid_obj in declarations: - self._spawn_scene.builder.materials.update( - self._spawn_scene._resolve_materials(materials) - ) if replace: self._spawn_scene.remove(uid) self._spawn_scene.declare("rigid_object", uid, descriptor, facade=rigid_obj) @@ -3015,6 +3027,7 @@ def add_fluid_object(self, cfg: FluidObjectCfg) -> FluidObject: raise ValueError( f"Fluid {key} conflicts with solver_cfg.sph_cfg.{key}." ) + self._spawn_scene._preflight_declaration("fluid_object", cfg.uid) points = cfg.spawn.sample(cfg.particle_spacing) pose = _pose_from_cfg(cfg) if cfg.spawn.reference_uid is not None: @@ -3070,6 +3083,70 @@ def get_fluid_diagnostics(self, uid: str) -> dict[str, object]: raise RuntimeError("Fluid diagnostics require prepare().") return self.physics.fluid_diagnostics(fluid) + def add_cable_object(self, cfg: CableObjectCfg) -> CableObject: + """Declare a cable before ``prepare()`` using DexSim's public Spawn API. + + Args: + cfg: Typed cable geometry, four-mode material and visual configuration. + + Returns: + A declared facade whose data becomes available after ``prepare()``. + + Raises: + NotImplementedError: Backend/device/solver/gradient mode is unsupported, + or the physical scene has already been finalized. + ValueError: Required configuration or the asset UID is invalid. + """ + if not isinstance(cfg, CableObjectCfg): + raise TypeError("Cable objects require CableObjectCfg.") + if not self.physics.supports_cables: + raise NotImplementedError("Cable objects require the Newton backend.") + if self.device.type != "cuda": + raise NotImplementedError("Cable objects require a CUDA device.") + if self._active_newton_solver_type != "dexuni": + raise NotImplementedError( + "Cable objects require the DexUni solver (solver_type='dexuni')." + ) + if self.sim_config.physics_cfg.requires_grad: + raise NotImplementedError( + "Cable objects do not support requires_grad=True." + ) + if self.spawn_result is not None: + raise NotImplementedError( + "Declare cable objects before scene finalization." + ) + descriptor, materials = cable_desc_from_cfg(cfg, per_env=True) + if descriptor.name in self.asset_uids or descriptor.name in self._spawn_scene: + raise ValueError( + f"Scene asset uid is already declared: {descriptor.name!r}." + ) + cable = CableObject(cfg, device=self.device) + self._spawn_scene.declare( + "cable_object", + descriptor.name, + descriptor, + facade=cable, + materials=materials, + ) + self._cable_objects[descriptor.name] = cable + self.notify_visualization_topology_changed() + return cable + + def get_cable_object(self, uid: str) -> CableObject | None: + """Return a cable by simulation UID, or None when absent. + + Args: + uid: Declared cable simulation UID. + + Returns: + The existing facade, including before it is Spawn-bound. + """ + return getattr(self, "_cable_objects", {}).get(uid) + + def get_cable_object_uid_list(self) -> list[str]: + """Return cable UIDs in declaration order.""" + return list(getattr(self, "_cable_objects", {})) + def add_deformable_object(self, cfg: DeformableObjectCfg) -> DeformableObject: """Declare a volume or surface deformable in the scene. @@ -3158,9 +3235,6 @@ def add_deformable_object(self, cfg: DeformableObjectCfg) -> DeformableObject: descriptor, materials = _deformable_desc_from_usd(cfg) else: descriptor, materials = descriptor_factory(cfg, per_env=True) - self._spawn_scene.builder.materials.update( - self._spawn_scene._resolve_materials(materials) - ) deformable = object_cls( cfg, device=self.device, @@ -3170,6 +3244,7 @@ def add_deformable_object(self, cfg: DeformableObjectCfg) -> DeformableObject: uid, descriptor, facade=deformable, + materials=materials, ) self._deformable_objects[uid] = deformable self.notify_visualization_topology_changed() @@ -3468,7 +3543,9 @@ def add_rigid_object_group(self, cfg: RigidObjectGroupCfg) -> RigidObjectGroup: "dynamic": ActorType.DYNAMIC, "kinematic": ActorType.KINEMATIC, }[cfg.body_type] + self._spawn_scene._preflight_declaration("rigid_object_group", uid) descriptors = [] + material_batches = [] for index, member in enumerate(cfg.rigid_objects.values()): member_cfg = deepcopy(member) member_cfg.uid = f"{uid}__member_{index}" @@ -3491,9 +3568,8 @@ def add_rigid_object_group(self, cfg: RigidObjectGroupCfg) -> RigidObjectGroup: f"Rigid object group member {index} has no rigid-body physics." ) descriptor.physics.actor_type = actor_type - self._spawn_scene.builder.materials.update( - self._spawn_scene._resolve_materials(materials) - ) + self._spawn_scene._require_available_uid(descriptor.name) + material_batches.append(materials) descriptors.append(descriptor) group = RigidObjectGroup( @@ -3502,6 +3578,7 @@ def add_rigid_object_group(self, cfg: RigidObjectGroupCfg) -> RigidObjectGroup: ) was_materialized = self.spawn_result is not None + self._spawn_scene._register_material_batches(material_batches) self._spawn_scene.declare( "rigid_object_group", uid, @@ -3725,15 +3802,13 @@ def _declare_spawn_articulation( prepare boundary. """ cfg = deepcopy(cfg) + materials = {} if _is_usd_path(_asset_file_path(cfg)): descriptor, materials = articulation_desc_from_usd( cfg, per_env=True, newton_solver_type=self._active_newton_solver_type, ) - self._spawn_scene.builder.materials.update( - self._spawn_scene._resolve_materials(materials) - ) else: descriptor = articulation_desc_from_cfg( cfg, @@ -3755,6 +3830,7 @@ def _declare_spawn_articulation( descriptor.name, descriptor, facade=facade, + materials=materials, configure_source=partial( configure_articulation_desc, cfg=cfg, @@ -4373,6 +4449,7 @@ def remove_asset(self, uid: str) -> bool: if uid == "default_plane": raise ValueError("The Spawn-owned default plane cannot be removed.") + scene._require_removable(uid) robot = self._robots.get(uid) if robot is not None and robot.cfg.enable_sensor: for sensor_cfg in robot.cfg.sensors: @@ -4408,6 +4485,7 @@ def remove_asset(self, uid: str) -> bool: self._rigid_objects.pop(uid, None) self._rigid_object_groups.pop(uid, None) self._deformable_objects.pop(uid, None) + getattr(self, "_cable_objects", {}).pop(uid, None) self._articulations.pop(uid, None) self._robots.pop(uid, None) self.notify_visualization_topology_changed() @@ -5327,6 +5405,11 @@ def reset_objects_state( ) -> None: """Reset the state of the simulated assets given the environment IDs and excluded UIDs. + Cable scenes restore creation state for every physical asset in selected + environments through one engine episode reset, including targets, loads + and supported controls. Physical exclusions are unsupported in this path; + runtime property/topology edits are not restored. Global time is kept. + Args: env_ids (Sequence[int] | None): The environment IDs to reset. If None, reset all environments. excluded_uids (Sequence[str] | None): List of asset UIDs to exclude from resetting. If None, reset all assets. @@ -5335,6 +5418,21 @@ def reset_objects_state( """ excluded_uids = set(excluded_uids) if excluded_uids is not None else set() excluded_sensor_uids = set(excluded_sensor_uids or ()) + reset_ids = self.physics._reset_scene_state(env_ids, excluded_uids) + if reset_ids is not None: + # Physical assets and solver history were reset atomically by the + # backend; render-only assets and sensors remain manager-owned. + if reset_ids: + for uid, visual in getattr(self, "_visual_objects", {}).items(): + if uid not in excluded_uids and visual.is_spawn_bound: + visual.reset(reset_ids) + for uid, light in self._lights.items(): + if uid not in excluded_uids: + light.reset(reset_ids) + for uid, sensor in self._sensors.items(): + if uid not in excluded_uids and uid not in excluded_sensor_uids: + sensor.reset(reset_ids) + return articulation_uids = tuple(self._robots) + tuple(self._articulations) reset_articulation_uids = tuple( uid for uid in articulation_uids if uid not in excluded_uids @@ -5587,6 +5685,7 @@ def _deferred_destroy(self) -> None: "_rigid_object_groups", "_deformable_objects", "_fluid_objects", + "_cable_objects", "_articulations", "_robots", ): @@ -5650,6 +5749,7 @@ def _sever_wrapper_refs(obj_registry): _sever_wrapper_refs("_rigid_object_groups") _sever_wrapper_refs("_deformable_objects") _sever_wrapper_refs("_fluid_objects") + _sever_wrapper_refs("_cable_objects") _sever_wrapper_refs("_articulations") _sever_wrapper_refs("_robots") _sever_wrapper_refs("_sensors") diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index e710f5d961..fd53048ed5 100644 --- a/embodichain/lab/sim/spawn/descriptors.py +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -69,6 +69,8 @@ from embodichain.lab.sim.cfg import ( _normalize_joint_target_mode, ArticulationCfg, + CableObjectCfg, + CablePhysicsCfg, CollisionPropertiesCfg, DefaultCollisionPropertiesCfg, DefaultRigidBodyPropertiesCfg, @@ -82,7 +84,12 @@ SurfaceDeformableObjectCfg, VolumeDeformableObjectCfg, ) -from embodichain.lab.sim.shapes import CubeCfg, MeshCfg, MeshCollisionCfg, SphereCfg +from embodichain.lab.sim.shapes import ( + CubeCfg, + MeshCfg, + MeshCollisionCfg, + SphereCfg, +) from embodichain.utils import logger from embodichain.utils.string import ( resolve_matching_names, @@ -90,6 +97,7 @@ ) if TYPE_CHECKING: + from dexsim.spawn import CableAttachmentDesc, CableDesc, CablePhysicsDesc from embodichain.lab.sim.material import VisualMaterialCfg from embodichain.lab.sim.cfg import VisualObjectCfg @@ -98,6 +106,7 @@ "configure_articulation_desc", "rigid_desc_from_cfg", "visual_desc_from_cfg", + "cable_desc_from_cfg", "surface_deformable_desc_from_cfg", "volume_deformable_desc_from_cfg", ] @@ -601,6 +610,118 @@ def _explicit_tet_mesh_from_cfg( } +def cable_desc_from_cfg( + cfg: CableObjectCfg, + *, + per_env: bool = True, +) -> tuple[CableDesc, dict[str, MaterialDesc]]: + """Lower cable authoring through existing pose/material/contact compilers. + + Args: + cfg: Cable configuration in EmbodiChain's xyzw convention. + per_env: Whether Spawn replicates this asset per environment. + + Returns: + The typed descriptor and material entries for the shared Spawn builder. + + Raises: + NotImplementedError: The installed DexSim build lacks cable descriptors. + """ + try: + from dexsim.spawn import CableAttachmentDesc, CableDesc, CablePhysicsDesc + except ImportError as exc: + raise NotImplementedError( + "Cable creation requires a DexSim build exposing CableDesc, " + "CablePhysicsDesc and CableAttachmentDesc." + ) from exc + cfg.validate() + uid = _required_uid(cfg.uid, "Cable object") + if not isinstance(cfg.attrs, CablePhysicsCfg): + raise TypeError("Cable attrs must be CablePhysicsCfg.") + if isinstance(cfg.attrs.collision_props, DefaultCollisionPropertiesCfg): + raise NotImplementedError( + "Cable contact properties require common or Newton configuration." + ) + contact = _resolve_rigid_physics( + RigidBodyPhysicsCfg( + collision_props=cfg.attrs.collision_props, + material_props=cfg.attrs.material_props, + ), + newton_solver_type="dexuni", + ) + collision = _compile_newton_collision( + contact, newton_solver_type="dexuni", author_shape_defaults=True + ) + quaternions = None + if cfg.segment_quaternions is not None: + values = np.asarray(cfg.segment_quaternions) + if values.ndim != 2 or values.shape[1] != 4: + raise ValueError("segment_quaternions must have shape (N - 1, 4).") + quaternions = values[:, [3, 0, 1, 2]].copy() # xyzw -> DexSim authoring wxyz + if cfg.visual_shape is not None and cfg.visual_material is not None: + raise ValueError("A custom visual_shape owns its visual_material.") + visual = cfg.visual_shape + if visual is not None and not isinstance(visual, MeshCfg): + raise TypeError("Cable visual_shape must be MeshCfg.") + if visual is not None and visual.compute_uv: + raise NotImplementedError( + "Cable handles do not expose projective UV writes; provide authored UVs." + ) + material_ref, material_entry = _compile_visual_material( + uid, cfg.visual_material if visual is None else visual.visual_material + ) + renders = [] + if visual is not None: + renders.append( + RenderDesc.from_geometry( + _mesh_geometry_from_cfg(visual, segment_name=uid), + load_option=_compile_load_option(visual), + material_ref=material_ref, + ) + ) + attachments = {} + for name in ("start_attachment", "end_attachment"): + value = getattr(cfg, name) + attachments[name] = ( + None + if value is None + else CableAttachmentDesc( + target=value.target, + position=np.asarray(value.position).copy(), + link_name=value.link_name, + ) + ) + descriptor = CableDesc( + name=uid, + points=np.asarray(cfg.points).copy(), + radius=cfg.radius, + pose=_pose_from_cfg(cfg), + segment_quaternions=quaternions, + closed=cfg.closed, + enable_collision=( + True if contact.collision_enabled is None else contact.collision_enabled + ), + fixed_segments=tuple(cfg.fixed_segments), + physics=CablePhysicsDesc( + **{ + item.name: getattr(cfg.attrs, item.name) + for item in fields(CablePhysicsDesc) + } + ), + collision=collision, + collision_filter_span=cfg.collision_filter_span, + renders=renders, + render_mode=cfg.render_mode, + render_material=( + None if material_entry is None or renders else material_entry[1] + ), + per_env=per_env, + **attachments, + ) + materials = {} if material_entry is None else {material_entry[0]: material_entry[1]} + return descriptor, materials + + def volume_deformable_desc_from_cfg( cfg: VolumeDeformableObjectCfg, *, diff --git a/embodichain/lab/sim/spawn/scene.py b/embodichain/lab/sim/spawn/scene.py index 0ffbd5c7f4..9935666e89 100644 --- a/embodichain/lab/sim/spawn/scene.py +++ b/embodichain/lab/sim/spawn/scene.py @@ -18,7 +18,7 @@ from __future__ import annotations -from collections.abc import Iterator +from collections.abc import Iterable, Iterator from contextlib import contextmanager from dataclasses import dataclass, fields, replace from typing import Any, Callable, Literal @@ -40,6 +40,7 @@ "soft_object", "cloth_object", "fluid_object", + "cable_object", ] @@ -178,6 +179,11 @@ def _bind_resource_materials(self, descriptor: Any) -> None: for member in descriptor: self._bind_resource_materials(member) return + render_material = getattr(descriptor, "render_material", None) + if isinstance(render_material, _AssetMaterialDesc): + _, descriptor.render_material = self._material_resources.resolve( + render_material + ) reference = getattr(descriptor, "material_ref", None) if reference in definitions: _, descriptor.material = self._material_resources.resolve( @@ -196,6 +202,65 @@ def arena_names(self) -> tuple[str, ...]: def __contains__(self, uid: str) -> bool: return uid in self._assets + def _require_available_uid(self, uid: str) -> None: + if uid in self._assets or any( + item.name == uid + for collection in ("mesh_objects", "scene_entities") + for item in getattr(self.builder, collection, ()) + ): + raise ValueError(f"Spawn asset uid is already declared: {uid!r}.") + + def _require_compatible_particles(self, *, cables: bool, particles: bool) -> None: + """Check both the incoming declaration and the native scene inventory.""" + if (cables and (particles or self._has_particles)) or ( + particles and self._has_cables + ): + raise NotImplementedError( + "Cable episode reset does not support fluid or deformable particles in the same arena." + ) + + @property + def _has_particles(self) -> bool: + return len(getattr(self.builder, "particle_sets", ())) > 0 or any( + item.kind in {"soft_object", "cloth_object", "fluid_object"} + for item in self._assets.values() + ) + + @property + def _has_cables(self) -> bool: + return len(getattr(self.builder, "cables", ())) > 0 or any( + item.kind == "cable_object" for item in self._assets.values() + ) + + def _preflight_declaration(self, kind: _AssetKind, uid: str) -> bool: + """Validate a declaration and return whether it is metadata-only.""" + self._require_available_uid(uid) + self._require_compatible_particles( + cables=kind == "cable_object", + particles=kind in {"soft_object", "cloth_object", "fluid_object"}, + ) + declaration_only = ( + kind == "visual_object" + and getattr(getattr(self, "_material_resources", None), "renderer", None) + == "no-render" + ) + if not declaration_only: + self._require_mutable_topology() + return declaration_only + + def _register_material_batches(self, batches: Iterable[dict[str, Any]]) -> None: + """Check all material definitions before publishing a declaration batch.""" + definitions = {} + for batch in batches: + for uid, desc in batch.items(): + if uid in definitions and definitions[uid] != desc: + raise ValueError( + f"Conflicting visual material definition for {uid!r}." + ) + definitions[uid] = desc + if definitions: + self.builder.materials.update(self._resolve_materials(definitions)) + def declare( self, kind: _AssetKind, @@ -203,11 +268,13 @@ def declare( descriptor: Any, *, facade: Any | None = None, + materials: dict[str, Any] | None = None, configure_source: Callable[[Any], None] | None = None, ) -> None: """Add a descriptor and associate it with an EmbodiChain facade.""" - if uid in self._assets: - raise ValueError(f"Spawn asset uid is already declared: {uid!r}.") + declaration_only = self._preflight_declaration(kind, uid) + if materials: + self.builder.materials.update(self._resolve_materials(materials)) self._bind_resource_materials(descriptor) self._initialize_facade_declaration(facade) declaration = _AssetDeclaration( @@ -215,13 +282,9 @@ def declare( descriptor=descriptor, facade=facade, source_configurator=configure_source, + declaration_only=declaration_only, ) - if ( - kind == "visual_object" - and getattr(getattr(self, "_material_resources", None), "renderer", None) - == "no-render" - ): - declaration.declaration_only = True + if declaration_only: self._assets[uid] = declaration return @@ -262,6 +325,7 @@ def declare( "articulation": "add_articulation", "soft_object": "add_soft_object", "cloth_object": "add_cloth_object", + "cable_object": "add_cable", }[kind] with self._native_material_scope(): declaration.descriptor = getattr(self.builder, add_name)(descriptor) @@ -324,15 +388,11 @@ def track( def remove(self, uid: str) -> None: """Remove a declared asset from its DexSim owner.""" + self._require_removable(uid) declaration = self._assets[uid] if getattr(declaration, "declaration_only", False): del self._assets[uid] return - if declaration.kind in {"soft_object", "cloth_object", "fluid_object"}: - raise NotImplementedError( - "DexSim Spawn does not yet expose pending removal for " - f"{declaration.kind.replace('_', ' ')}." - ) if declaration.kind == "rigid_object_group": for member in declaration.descriptor: self.builder.remove_object(member.name) @@ -352,6 +412,31 @@ def remove(self, uid: str) -> None: key: materials for key, materials in pending.items() if key[0] != uid } + def _require_removable(self, uid: str) -> None: + """Validate removal before mounted render resources are detached.""" + declaration = self._assets[uid] + if getattr(declaration, "declaration_only", False): + return + if declaration.kind in { + "soft_object", + "cloth_object", + "fluid_object", + "cable_object", + }: + raise NotImplementedError( + "DexSim Spawn does not yet expose pending removal for " + f"{declaration.kind.replace('_', ' ')}." + ) + self._require_mutable_topology() + + def _require_mutable_topology(self) -> None: + """Reject unsupported Newton rebuilds before changing scene resources.""" + if self.builder.is_finalized and (self._has_cables or self._has_particles): + raise NotImplementedError( + "DexSim does not support physical topology changes in finalized " + "cable or particle scenes; recreate the simulation." + ) + def commit(self) -> Any: """Finalize once or let the current ``Scene`` consume pending changes.""" if not self.builder.is_finalized: diff --git a/embodichain/lab/sim/spawn/usd_import.py b/embodichain/lab/sim/spawn/usd_import.py index 4256a1fa89..933b8fd573 100644 --- a/embodichain/lab/sim/spawn/usd_import.py +++ b/embodichain/lab/sim/spawn/usd_import.py @@ -105,6 +105,9 @@ def _compile_usd_import( raise NotImplementedError( f"Unsupported USD particle-set types: {unsupported_particles}." ) + manager._spawn_scene._require_compatible_particles( + cables=bool(source.cables), particles=bool(source.particle_sets) + ) resources = list(source.iter_entities()) source_paths_by_id = {id(item): _source_path(item) for item in resources} counts = Counter(source_paths_by_id.values()) diff --git a/examples/README.md b/examples/README.md index 8aed8ad827..63b755cadb 100644 --- a/examples/README.md +++ b/examples/README.md @@ -97,3 +97,33 @@ native and real-demonstration-driven experts through the shared engine. Its offl viewer pairs measured 3D playback with RGB-D, recipes, source clocks and explicit capability/acceptance results. Full and quick modes preserve unrun and failed cases; recovery and portability diagnostics keep their evidence separate from expert data. + + +## Newton cable runtime + +The [create-cable tutorial](../scripts/tutorials/sim/create_cable.py) creates a +procedural cable with one fixed segment, steps it under gravity, reads world +segment state and restores the episode creation state. Run it with the native +viewer or with two headless environments: + +```bash +python scripts/tutorials/sim/create_cable.py +python scripts/tutorials/sim/create_cable.py --headless --renderer no-render --num-envs 2 +``` + +The cable examples use CUDA and DexSim `0.6.0rc1` (engine commit +`1b968250ada2b06b210bc7aa08ef219769f01217`). The DexForce package index +provides Linux x86_64 wheels for Python 3.11 and 3.12. To install the pinned +engine in your EmbodiChain environment and check its build, run: + +```bash +python -m pip install --index-url https://pypi.org/simple \ + --extra-index-url http://pyp.open3dv.site:2345/simple/ --trusted-host pyp.open3dv.site \ + dexsim-engine==0.6.0rc1 +python -c "import dexsim; from dexsim.scene import SpawnedCable; print(dexsim.__version__, dexsim.__commit_id__)" +``` + +EmbodiChain defaults to DexUni when no solver is selected; +legacy `auto`/`AutoSolverCfg` selections must migrate to `dexuni` or the concrete +solver needed by the scene. Explicit `semi_implicit` remains required for +Newton gradient mode. diff --git a/scripts/tutorials/sim/create_cable.py b/scripts/tutorials/sim/create_cable.py new file mode 100644 index 0000000000..21da6a73fb --- /dev/null +++ b/scripts/tutorials/sim/create_cable.py @@ -0,0 +1,138 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Create a cable fixed at one end and let it bend under gravity with DexUni. + +Run ``python scripts/tutorials/sim/create_cable.py`` to open the native viewer. +Add ``--headless --renderer no-render`` for physics only, or ``--num-envs 2`` +to replicate the cable. Geometry is procedural and needs no downloaded assets. +""" + +from __future__ import annotations + +import argparse + +from embodichain.cli.sim import add_sim_args_to_parser + +__all__ = ["build_parser", "main"] + + +def build_parser() -> argparse.ArgumentParser: + """Return simulation options without initializing the engine.""" + parser = argparse.ArgumentParser(description=__doc__) + add_sim_args_to_parser(parser) + parser.set_defaults(device="cuda", physics="newton", renderer="fast-rt") + parser.add_argument( + "--steps", + type=int, + default=480, + help="Simulation steps before resetting and exiting.", + ) + return parser + + +def main(args: argparse.Namespace | None = None) -> None: + """Create, prepare, step, inspect and reset a replicated cable. + + Args: + args: Parsed options; None reads the command line. + """ + parser = build_parser() + if args is None: + args = parser.parse_args() + if args.physics != "newton" or not str(args.device).startswith("cuda"): + parser.error("Cables require --physics newton and a CUDA device.") + if args.num_envs < 1: + parser.error("--num-envs must be positive.") + if args.steps <= 0: + parser.error("--steps must be positive.") + + import torch + + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + from embodichain.lab.sim.cfg import ( + CableObjectCfg, + CablePhysicsCfg, + NewtonPhysicsCfg, + RenderCfg, + ) + from embodichain.lab.sim.material import VisualMaterialCfg + from embodichain.lab.visualization import visualization_cfg_from_args + + sim = SimulationManager( + SimulationManagerCfg( + device=args.device, + num_envs=args.num_envs, + arena_space=args.arena_space, + headless=args.headless, + gpu_id=args.gpu_id, + physics_dt=1 / 240, + render_cfg=RenderCfg(renderer=args.renderer), + physics_cfg=NewtonPhysicsCfg( + num_substeps=4, + collision_cfg=None, + solver_cfg={"solver_type": "dexuni"}, + ), + visualization=visualization_cfg_from_args(args), + ) + ) + cable = None + try: + # N + 1 local points define N segments. Fix the first segment in place. + cable = sim.add_cable_object( + CableObjectCfg( + uid="cable", + points=[(0.03 * index, 0, 0) for index in range(21)], + radius=0.006, + init_pos=(0, 0, 0.8), + fixed_segments=(0,), + attrs=CablePhysicsCfg( + stretch_stiffness=2.0e4, + stretch_damping=10.0, + bend_stiffness=0.01, + bend_damping=0.001, + ), + visual_material=VisualMaterialCfg(base_color=(0.15, 0.45, 0.9, 1)), + ) + ) + # Declaration becomes usable segment data at this boundary. + sim.prepare() + initial = cable.data.default_segment_state_w + print( + f"Created {cable.data.num_segments} cable segments per environment " + f"in {cable.num_instances} environments." + ) + if not args.headless: + sim.open_window() + for _ in range(args.steps): + sim.update(step=1) + # World XYZ, XYZW, COM linear velocity and angular velocity, (E, S, 13). + state = cable.data.segment_state_w + if not bool(torch.isfinite(state).all()): + raise RuntimeError("Cable state is not finite.") + print(f"Free segment height per environment: {state[:, -1, 2].tolist()} m") + # An episode reset also restores drive targets, loads and solver history. + sim.reset_objects_state() + torch.testing.assert_close(cable.data.segment_state_w, initial) + print("Restored the initial cable state without advancing physics.") + finally: + cable = None + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +if __name__ == "__main__": + main() diff --git a/tests/gym/envs/test_env_seed.py b/tests/gym/envs/test_env_seed.py index ed05be96ee..90e4a61ce0 100644 --- a/tests/gym/envs/test_env_seed.py +++ b/tests/gym/envs/test_env_seed.py @@ -30,6 +30,7 @@ import embodichain.lab.gym.envs.base_env as base_env_module from embodichain.lab.gym.envs import BaseEnv from embodichain.lab.sim import SimulationManager +from embodichain.lab.sim.physics import DefaultPhysicsBackend # This module calls one unbound manager method on registry stubs; no World is # constructed. Keep these reset-contract cases in the fast CPU CI lane. @@ -178,7 +179,7 @@ def test_reset_keeps_sensor_uid_exclusions_out_of_physical_registries( sensor = MagicMock(uid="shared") manager_sensor = MagicMock(uid="manager_only") env.sensors = {"observation": sensor} - env.sim.physics = SimpleNamespace(name="default") + env.sim.physics = DefaultPhysicsBackend(env.sim) env.sim._robots = {"shared": robot} for registry in ( "_articulations", diff --git a/tests/sim/objects/conftest.py b/tests/sim/objects/conftest.py new file mode 100644 index 0000000000..5b1e82fdb4 --- /dev/null +++ b/tests/sim/objects/conftest.py @@ -0,0 +1,45 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from pathlib import Path + +import pytest + + +@pytest.fixture +def volume_particle_usd(tmp_path: Path) -> Path: + """Author a single tetrahedron for CPU and isolated native USD imports.""" + from pxr import Sdf, Usd, UsdGeom + + path = tmp_path / "volume_particle.usda" + stage = Usd.Stage.CreateNew(str(path)) + UsdGeom.SetStageMetersPerUnit(stage, 1.0) + UsdGeom.SetStageUpAxis(stage, UsdGeom.Tokens.z) + UsdGeom.Xform.Define(stage, "/World") + soft = UsdGeom.Xform.Define(stage, "/World/Soft").GetPrim() + soft.AddAppliedSchema("PhysicsDeformableBodyAPI") + mesh = stage.DefinePrim("/World/Soft/sim", "TetMesh") + mesh.AddAppliedSchema("PhysicsVolumeDeformableSimAPI") + mesh.CreateAttribute("points", Sdf.ValueTypeNames.Point3fArray).Set( + [(0, 0, 1), (0.1, 0, 1), (0, 0.1, 1), (0, 0, 1.1)] + ) + mesh.CreateAttribute("tetVertexIndices", Sdf.ValueTypeNames.Int4Array).Set( + [(0, 1, 2, 3)] + ) + stage.GetRootLayer().Save() + return path diff --git a/tests/sim/objects/test_cable_object.py b/tests/sim/objects/test_cable_object.py new file mode 100644 index 0000000000..d8a4d98b36 --- /dev/null +++ b/tests/sim/objects/test_cable_object.py @@ -0,0 +1,830 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from pathlib import Path +from types import SimpleNamespace +from unittest.mock import Mock + +import numpy as np +import pytest +import torch +from dexsim.scene import Scene, SpawnedCable + +from embodichain.lab.sim import SimulationManager +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + CableAttachmentCfg, + CableObjectCfg, + NewtonPhysicsCfg, +) +from embodichain.lab.sim.objects import CableObject, CableObjectData +from embodichain.lab.sim.physics import NewtonPhysicsBackend, DefaultPhysicsBackend +from embodichain.lab.sim.spawn.scene import SpawnScene + +pytestmark = pytest.mark.no_sim + + +def _cfg() -> CableObjectCfg: + return CableObjectCfg( + uid="wire", points=[[0, 0, 0], [0, 0, 0.1], [0, 0, 0.2]], radius=0.01 + ) + + +class _CableBatch: + def __init__(self) -> None: + self.is_valid = True + # Quaternion-then-translation: deliberately non-identity orientation + # and different world origins distinguish both boundary mistakes. + self.pose = torch.tensor([0.1, 0.2, 0.3, 0.9, 10, 20, 30]).repeat(2, 2, 1) + self.pose[..., :4] /= torch.linalg.vector_norm( + self.pose[..., :4], dim=-1, keepdim=True + ) + self.pose[1, :, 4:] += 100 + self.velocity = torch.arange(24, dtype=torch.float32).reshape(2, 2, 6) + self.default_pose = self.pose.clone() + self.default_velocity = self.velocity.clone() + self.device = "cpu" + self.selection = [0, 1] + self.writes = 0 + self.step = 7 + self.reads = 0 + self.fail = False + + def fetch_state(self, pose, velocity) -> int: + if not self.is_valid: + raise RuntimeError("invalid Scene") + if self.fail: + self.fail = False + raise RuntimeError("batch read failed") + self.reads += 1 + pose.copy_(self.pose) + velocity.copy_(self.velocity) + return self.step + + def fetch_default_state(self, pose, velocity) -> int: + if self.fail: + self.fail = False + raise RuntimeError("batch read failed") + pose.copy_(self.default_pose) + velocity.copy_(self.default_velocity) + return 2 + + def select(self, ids): + self.selection = ids + return self + + def apply_state(self, *, pose, velocity): + self.writes += 1 + self.pose[self.selection] = pose + self.velocity[self.selection] = velocity + + +def _handles() -> list[Mock]: + handles = [Mock(spec=SpawnedCable), Mock(spec=SpawnedCable)] + for handle in handles: + handle.segment_count = 2 + handle.is_valid = True + return handles + + +def _bound_cable() -> tuple[CableObject, _CableBatch]: + batch = _CableBatch() + scene = Mock(spec=Scene) + scene.backend = "newton" + scene.world = object() + scene.create_cable_batch.return_value = batch + cable = CableObject(_cfg()) + cable._initialize_spawn_declaration(2) + cable.attach_spawn_handles(_handles()) + cable.bind_spawn(scene) + return cable, batch + + +def test_state_layout_world_coordinates_and_coherent_snapshot() -> None: + cable, batch = _bound_cable() + data = cable.data + before = batch.reads + state = data.segment_state_w + assert batch.reads == before + 1 + assert data.state_step == 7 + assert state.shape == (2, 2, 13) + torch.testing.assert_close(state[..., :3], batch.pose[..., 4:]) + torch.testing.assert_close(state[..., 3:7], batch.pose[..., :4]) + torch.testing.assert_close(state[..., 7:], batch.velocity) + + +def test_returned_and_default_states_are_independent_snapshots() -> None: + cable, batch = _bound_cable() + default = cable.data.default_segment_state_w + cable.data.segment_pose_w.zero_() + cable.data.segment_vel_w.zero_() + cable.data.default_segment_state_w.zero_() + batch.pose[..., 4] += 5 + assert torch.count_nonzero(cable.data.segment_pose_w) + torch.testing.assert_close(cable.data.default_segment_state_w, default) + assert not torch.equal(cable.data.segment_state_w, default) + + +def test_failed_binding_can_be_retried_without_partial_bound_state() -> None: + batch = _CableBatch() + batch.fail = True + scene = Mock(spec=Scene, backend="newton") + scene.world = object() + scene.create_cable_batch.return_value = batch + cable = CableObject(_cfg()) + cable._initialize_spawn_declaration(2) + cable.attach_spawn_handles(_handles()) + with pytest.raises(RuntimeError, match="batch read failed"): + cable.bind_spawn(scene) + assert cable.is_declared and cable.data is None + cable.bind_spawn(scene) + assert cable.is_spawn_bound + with pytest.raises(RuntimeError, match="already Spawn-bound"): + cable.bind_spawn(scene) + + +def test_data_rejects_mixed_topology() -> None: + handles = _handles() + handles[1].segment_count = 3 + with pytest.raises(ValueError, match="equal positive"): + CableObjectData(handles, Mock(spec=Scene), torch.device("cpu")) + + +def test_selected_load_rows_follow_environment_order() -> None: + cable, _ = _bound_cable() + force = np.array([[1, 2, 3], [4, 5, 6]]) + cable.set_force(force, segment_index=1, env_ids=[1, 0]) + np.testing.assert_allclose(cable._entities[1].set_force.call_args.args[0], force[0]) + np.testing.assert_allclose(cable._entities[0].set_force.call_args.args[0], force[1]) + cable.set_torque([1, 0, 0], env_ids=[1]) + cable._entities[0].set_torque.assert_not_called() + cable.clear_wrenches(env_ids=[0]) + cable._entities[1].clear_wrenches.assert_not_called() + + +@pytest.mark.parametrize( + "env_ids, force, segment", + [ + ([0, 0], [1, 2, 3], -1), + ([2], [1, 2, 3], -1), + ([True], [1, 2, 3], -1), + ([0], [1, float("nan"), 3], -1), + ([0], [[1, 2, 3], [4, 5, 6]], -1), + ([0], [1, 2, 3], 2), + ], +) +def test_invalid_load_does_not_mutate_any_environment(env_ids, force, segment) -> None: + cable, _ = _bound_cable() + with pytest.raises((ValueError, TypeError, IndexError)): + cable.set_force(force, env_ids=env_ids, segment_index=segment) + for handle in cable._entities: + handle.set_force.assert_not_called() + + +def test_released_scene_blocks_reads_and_loads() -> None: + cable, batch = _bound_cable() + batch.is_valid = False + with pytest.raises(RuntimeError, match="invalid Scene"): + _ = cable.data.segment_state_w + with pytest.raises(RuntimeError, match="released"): + cable.set_force([1, 0, 0]) + + +def test_invalid_handle_blocks_load_clearing_before_any_writes() -> None: + cable, _ = _bound_cable() + cable._entities[1].is_valid = False + with pytest.raises(RuntimeError, match="invalid cable handle"): + cable.clear_wrenches() + for handle in cable._entities: + handle.clear_wrenches.assert_not_called() + + +@pytest.mark.parametrize("operation", ["get_local_pose", "destroy"]) +def test_missing_engine_operations_are_explicit(operation) -> None: + cable, _ = _bound_cable() + with pytest.raises(NotImplementedError): + getattr(cable, operation)() + + +def _manager(solver="dexuni", device="cuda", requires_grad=False) -> SimulationManager: + sim = object.__new__(SimulationManager) + sim.physics = NewtonPhysicsBackend(sim) + sim.physics._configured_solver_type = solver + sim.device = torch.device(device) + sim.sim_config = SimpleNamespace( + physics_cfg=NewtonPhysicsCfg(requires_grad=requires_grad) + ) + sim._spawn_scene = object.__new__(SpawnScene) + sim._spawn_scene._num_envs = 2 + sim._spawn_scene._assets = {} + sim._spawn_scene._resolve_materials = lambda materials: materials + sim._spawn_scene.builder = SimpleNamespace( + is_finalized=False, + result=None, + materials={}, + add_cable=lambda desc: desc, + ) + sim._cable_objects = {} + for name in ( + "_robots", + "_articulations", + "_rigid_objects", + "_rigid_object_groups", + "_deformable_objects", + "_fluid_objects", + "_lights", + "_sensors", + ): + setattr(sim, name, {}) + sim.notify_visualization_topology_changed = Mock() + return sim + + +def test_manager_declaration_query_and_duplicate_uid() -> None: + sim = _manager() + cable = sim.add_cable_object(_cfg()) + assert cable.num_instances == 2 and cable.is_declared and cable.data is None + assert sim.get_cable_object("wire") is cable + assert sim.get_cable_object("missing") is None + assert sim.get_cable_object_uid_list() == ["wire"] + from embodichain.lab.sim.material import VisualMaterialCfg + + scene = sim._spawn_scene + del scene._resolve_materials + scene._usd_texture_files = SimpleNamespace(resolve=lambda materials: materials) + scene._resource_material_definitions = {} + scene._material_resources = SimpleNamespace(renderer="no-render") + duplicate = _cfg() + duplicate.visual_material = VisualMaterialCfg(uid="shared", base_color=[1, 0, 0, 1]) + with pytest.raises(ValueError, match="already declared"): + sim.add_cable_object(duplicate) + + other = _cfg() + other.uid = "other" + other.visual_material = VisualMaterialCfg(uid="shared", base_color=[0, 1, 0, 1]) + assert sim.add_cable_object(other) is sim.get_cable_object("other") + assert sim.get_cable_object("wire") is cable + assert cable.cfg.visual_material is None + + +@pytest.mark.parametrize("deformable_type", ["volume", "surface"]) +@pytest.mark.parametrize("cable_first", [True, False]) +def test_particle_and_cable_declarations_preserve_first_asset( + deformable_type: str, cable_first: bool +) -> None: + from embodichain.lab.sim.cfg import ( + SurfaceDeformableObjectCfg, + VolumeDeformableObjectCfg, + ) + + sim = _manager() + scene = sim._spawn_scene + scene.builder.add_soft_object = scene.builder.add_cloth_object = lambda desc: desc + cfg_type = ( + VolumeDeformableObjectCfg + if deformable_type == "volume" + else SurfaceDeformableObjectCfg + ) + mesh = { + "vertices": [[0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]], + "triangles": [[0, 2, 1], [0, 1, 3], [0, 3, 2], [1, 2, 3]], + } + deformable_cfg = cfg_type(uid="particles", shape=mesh) + add_first = sim.add_cable_object if cable_first else sim.add_deformable_object + add_second = sim.add_deformable_object if cable_first else sim.add_cable_object + first_cfg, second_cfg = ( + (_cfg(), deformable_cfg) if cable_first else (deformable_cfg, _cfg()) + ) + first = add_first(first_cfg) + with pytest.raises(NotImplementedError, match="same arena"): + add_second(second_cfg) + assert sim.get_asset(first_cfg.uid) is first + assert second_cfg.uid not in sim.asset_uids + assert second_cfg.uid not in scene + + +def _usd_import_manager() -> SimulationManager: + from dexsim.spawn import SceneBuilder, SceneDesc + + sim = _manager() + scene = sim._spawn_scene + # Keep the SDK's descriptor inventory and add_scene traversal without + # allocating a native World or particle solver. + builder = object.__new__(SceneBuilder) + builder.backend = "newton" + builder.scene_desc = SceneDesc(name="usd_particle_regression") + builder.result = None + builder._finalized = False + builder._adapter = SimpleNamespace(backend="newton", add=Mock()) + builder.replicate_plan = SimpleNamespace(env_names=lambda: ("arena_0", "arena_1")) + scene.builder = builder + del scene._resolve_materials + scene._usd_texture_files = SimpleNamespace(resolve=lambda materials: materials) + scene._resource_material_definitions = {} + scene._material_resources = SimpleNamespace(renderer="no-render") + scene._resolve_materials = Mock(wraps=scene._resolve_materials) + sim._visual_objects = {} + return sim + + +def _write_cable_usd(tmp_path: Path, *, particle_source: Path | None = None) -> Path: + source = ( + particle_source.read_text() if particle_source is not None else "#usda 1.0\n" + ) + path = tmp_path / "cables.usda" + path.write_text(source + """ +def BasisCurves "Cable" ( + prepend apiSchemas = ["PhysicsCurvesDeformableSimAPI"] +) +{ + uniform token type = "linear" + uniform token wrap = "nonperiodic" + int[] curveVertexCounts = [2] + point3f[] points = [(0, 0, 1), (0.1, 0, 1)] +} +""") + return path + + +@pytest.mark.parametrize("cable_first", [True, False]) +@pytest.mark.parametrize("cable_source", ["typed", "usd"]) +def test_usd_particles_and_cables_reject_mix_before_scene_mutation( + tmp_path: Path, volume_particle_usd: Path, cable_first: bool, cable_source: str +) -> None: + from dexsim.spawn import SoftBodyDesc + from embodichain.lab.sim.cfg import UsdImportCfg, UsdSourceCfg + from embodichain.lab.sim.material import VisualMaterialCfg + + sim = _usd_import_manager() + scene = sim._spawn_scene + builder = scene.builder + recipe = UsdImportCfg( + name="volume", source=UsdSourceCfg(file_path=str(volume_particle_usd)) + ) + cable_cfg = _cfg() + cable_cfg.visual_material = VisualMaterialCfg( + uid="cable_material", base_color=[1, 0, 0, 1] + ) + + def add_cable(): + if cable_source == "typed": + return sim.add_cable_object(cable_cfg) + imported = sim.add_usd( + UsdImportCfg( + name="cables", + source=UsdSourceCfg(file_path=str(_write_cable_usd(tmp_path))), + ) + ) + assert dict(imported) == {} + assert list(imported.resource_types.values()) == ["cable"] + assert len(builder.cables) == 1 + assert not sim._cable_objects and not scene._assets + return None + + if cable_first: + first = add_cable() + else: + imported = sim.add_usd(recipe) + first = imported["/World/Soft"] + assert len(builder.particle_sets) == 1 + assert isinstance(builder.particle_sets[0], SoftBodyDesc) + assert sim.get_asset(first.uid) is first + + initial_entities = [id(item) for item in builder.scene_desc.iter_entities()] + initial_materials = dict(builder.materials) + initial_definitions = dict(scene._resource_material_definitions) + initial_assets = dict(scene._assets) + scene._resolve_materials.reset_mock() + builder._adapter.add.reset_mock() + with pytest.raises(NotImplementedError, match="same arena"): + if cable_first: + sim.add_usd(recipe) + else: + add_cable() + + assert [id(item) for item in builder.scene_desc.iter_entities()] == initial_entities + assert builder.materials == initial_materials + assert scene._resource_material_definitions == initial_definitions + assert scene._assets == initial_assets + if first is not None: + assert sim.get_asset(first.uid) is first + scene._resolve_materials.assert_not_called() + builder._adapter.add.assert_not_called() + + +def test_one_usd_import_rejects_cables_and_particles_before_scene_mutation( + tmp_path: Path, + volume_particle_usd: Path, +) -> None: + from dexsim.kit.usd import parse_usd + from embodichain.lab.sim.cfg import UsdImportCfg, UsdSourceCfg + + source = _write_cable_usd(tmp_path, particle_source=volume_particle_usd) + parsed = parse_usd(str(source)) + assert len(parsed.cables) == len(parsed.particle_sets) == 1 + sim = _usd_import_manager() + scene = sim._spawn_scene + with pytest.raises(NotImplementedError, match="same arena"): + sim.add_usd( + UsdImportCfg(name="mixed", source=UsdSourceCfg(file_path=str(source))) + ) + assert list(scene.builder.scene_desc.iter_entities()) == [] + assert not scene._assets and not sim._deformable_objects + assert not scene.builder.materials and not scene._resource_material_definitions + scene._resolve_materials.assert_not_called() + scene.builder._adapter.add.assert_not_called() + + +@pytest.mark.parametrize("renderer", ["rt", "no-render"]) +def test_generated_cable_material_uses_scene_resource_resolution(renderer) -> None: + from embodichain.lab.sim._material_presets import _AssetMaterialDesc + from embodichain.lab.sim.material import VisualMaterialCfg + + sim = _manager() + scene = sim._spawn_scene + del scene._resolve_materials + scene._usd_texture_files = SimpleNamespace(resolve=lambda materials: materials) + scene._resource_material_definitions = {} + native_material = object() + scene._material_resources = SimpleNamespace( + renderer=renderer, resolve=Mock(return_value=(object(), native_material)) + ) + cfg = _cfg() + cfg.visual_material = VisualMaterialCfg(parameters={"transmission": 1.0}) + + sim.add_cable_object(cfg) + + descriptor = scene._assets["wire"].descriptor + assert scene.builder.materials == {} + if renderer == "no-render": + scene._material_resources.resolve.assert_not_called() + assert isinstance(descriptor.render_material, _AssetMaterialDesc) + else: + assert descriptor.render_material is native_material + + +@pytest.mark.parametrize( + "solver,device,grad", + [ + ("auto", "cuda", False), + ("vbd", "cuda", False), + ("dexuni", "cpu", False), + ("dexuni", "cuda", True), + ], +) +def test_manager_rejects_unsupported_runtime_before_declaration( + solver, device, grad +) -> None: + sim = _manager(solver, device, grad) + with pytest.raises(NotImplementedError): + sim.add_cable_object(_cfg()) + assert sim._spawn_scene._assets == {} and sim._cable_objects == {} + + +def test_manager_reset_rejects_before_robot_state_mutation_even_if_cable_excluded() -> ( + None +): + sim = _manager() + sim.add_cable_object(_cfg()) + robot = Mock() + sim._robots = {"arm": robot} + with pytest.raises(NotImplementedError, match="no objects have been reset"): + sim.reset_objects_state(env_ids=[0], excluded_uids=["wire"]) + robot.reset.assert_not_called() + + +def test_writes_convert_once_and_preserve_unselected_environment() -> None: + cable, batch = _bound_cable() + before = cable.data.segment_state_w + state = before[[1]].clone() + state[..., :3] += 0.25 + state[..., 7:] *= 0.5 + cable.write_segment_state_to_sim(state, env_ids=[1]) + torch.testing.assert_close(cable.data.segment_state_w[1], state[0]) + torch.testing.assert_close(cable.data.segment_state_w[0], before[0]) + assert batch.writes == 1 + cable.reset(env_ids=[1]) + torch.testing.assert_close(cable.data.segment_state_w, before) + cable._entities[1].clear_wrenches.assert_called_once() + cable._entities[0].clear_wrenches.assert_not_called() + + +@pytest.mark.parametrize("invalid", ["shape", "dtype", "grad", "stride", "ids"]) +def test_invalid_state_write_is_rejected_before_engine_mutation(invalid) -> None: + cable, batch = _bound_cable() + state = cable.data.segment_state_w + ids = None + if invalid == "shape": + state = state[:, :1].contiguous() + elif invalid == "dtype": + state = state.double() + elif invalid == "grad": + state.requires_grad_() + elif invalid == "stride": + state = torch.zeros(2, 2, 26)[..., ::2] + else: + ids = [True, 0] + with pytest.raises((ValueError, TypeError)): + cable.write_segment_state_to_sim(state, ids) + assert batch.writes == 0 + + +def test_defaults_are_engine_creation_state_even_after_prior_motion() -> None: + batch = _CableBatch() + batch.pose[..., 4:] += 20 + scene = Mock(spec=Scene) + scene.create_cable_batch.return_value = batch + data = CableObjectData(_handles(), scene, torch.device("cpu")) + torch.testing.assert_close( + data.default_segment_state_w[..., :3], batch.default_pose[..., 4:] + ) + assert not torch.equal(data.segment_state_w, data.default_segment_state_w) + + +def test_manager_cable_reset_resolves_arena_names_and_avoids_object_resets() -> None: + sim = _manager() + cable, _ = _bound_cable() + sim._cable_objects = {"wire": cable} + scene = Mock(spec=Scene) + scene.arenas = SimpleNamespace( + index=lambda name: {"arena_0": 2, "arena_1": 1}[name] + ) + sim._spawn_scene.builder.result = scene + sim._spawn_scene.builder.is_finalized = True + sim._spawn_scene.builder.replicate_plan = SimpleNamespace( + env_names=lambda: ("arena_0", "arena_1") + ) + robot, sensor = Mock(), Mock() + sim._robots = {"arm": robot} + sim._sensors = {"camera": sensor} + sim.reset_objects_state(env_ids=[1, 0], excluded_sensor_uids=["camera"]) + scene.reset_arenas.assert_called_once_with([1, 2]) + robot.reset.assert_not_called() + sensor.reset.assert_not_called() + + +def test_finalized_cable_scene_rejects_topology_changes_before_builder_mutation() -> ( + None +): + sim = _manager() + sim.add_cable_object(_cfg()) + builder = sim._spawn_scene.builder + builder.is_finalized = True + builder.add_object = Mock() + with pytest.raises(NotImplementedError, match="topology changes"): + sim._spawn_scene.declare("rigid_object", "box", SimpleNamespace()) + builder.add_object.assert_not_called() + with pytest.raises(NotImplementedError, match="pending removal"): + sim._spawn_scene.remove("wire") + + +def test_rejected_rigid_declaration_preserves_material_name_for_no_render_visual() -> ( + None +): + from embodichain.lab.sim.cfg import RigidObjectCfg, VisualObjectCfg + from embodichain.lab.sim.material import VisualMaterialCfg + from embodichain.lab.sim.shapes import CubeCfg + + sim = _manager(device="cpu") + sim._visual_objects = {} + scene = sim._spawn_scene + del scene._resolve_materials + scene._usd_texture_files = SimpleNamespace(resolve=lambda materials: materials) + scene._resource_material_definitions = {} + scene._material_resources = SimpleNamespace(renderer="no-render") + scene.track("cable_object", "wire", SimpleNamespace(name="wire")) + scene.builder.is_finalized = True + scene.builder.add_object = Mock() + with pytest.raises(NotImplementedError, match="topology changes"): + sim.add_rigid_object( + RigidObjectCfg( + uid="blocked", + shape=CubeCfg( + size=(0.1, 0.1, 0.1), + visual_material=VisualMaterialCfg( + uid="reserved", base_color=[1, 0, 0, 1] + ), + ), + ) + ) + + visual = sim.add_visual_object( + VisualObjectCfg( + uid="allowed", + shape=CubeCfg( + size=(0.1, 0.1, 0.1), + visual_material=VisualMaterialCfg( + uid="reserved", base_color=[0, 1, 0, 1] + ), + ), + ) + ) + assert sim.get_visual_object("allowed") is visual + assert list(visual.cfg.shape.visual_material.base_color) == [0, 1, 0, 1] + assert "blocked" not in scene and "blocked" not in sim._rigid_objects + assert scene._assets["allowed"].declaration_only + scene.builder.add_object.assert_not_called() + + +@pytest.mark.parametrize( + "fixed_kind", ["cable_object", "fluid_object", "cloth_object", "soft_object"] +) +@pytest.mark.parametrize("finalized", [False, True]) +@pytest.mark.parametrize("mounted_camera", [False, True]) +def test_robot_removal_preserves_mounted_resources_if_scene_is_immutable( + fixed_kind: str, finalized: bool, mounted_camera: bool +) -> None: + from embodichain.lab.sim.sensors import Camera, CameraCfg + from embodichain.lab.visualization.markers import ( + MarkerGroup, + MarkerGroupCfg, + MarkerPrototypeCfg, + ) + + sim = _manager() + scene = sim._spawn_scene + camera = Mock(spec=Camera) + camera.cfg = CameraCfg( + uid="mounted", extrinsics=CameraCfg.ExtrinsicsCfg(parent="robot/tool") + ) + scene.track(fixed_kind, "fixed", SimpleNamespace(name="fixed")) + robot = Mock( + cfg=SimpleNamespace( + enable_sensor=mounted_camera, + sensors=[camera.cfg] if mounted_camera else [], + ) + ) + scene.track("articulation", "robot", SimpleNamespace(name="robot"), facade=robot) + sim._robots["robot"] = robot + if mounted_camera: + sim._sensors["mounted"] = camera + parent_poses = np.array([[1, 0, 0, 0, 0, 0, 1]] * 2, dtype=np.float64) + group = MarkerGroup( + MarkerGroupCfg(name="attached", prototypes={"box": MarkerPrototypeCfg()}), + num_envs=2, + pose_resolver=lambda parent, link, ids: parent_poses[ids], + ) + group.update(translations=np.zeros((2, 1, 3))) + group.attach("robot") + sim._marker_groups = {group.name: group} + scene.builder.is_finalized = finalized + scene.builder.remove_articulation = Mock( + return_value=scene._assets["robot"].descriptor + ) + if finalized: + with pytest.raises(NotImplementedError, match="topology changes"): + sim.remove_asset("robot") + assert sim.get_asset("robot") is robot + if mounted_camera: + assert sim.get_sensor("mounted") is camera + camera.destroy.assert_not_called() + scene.builder.remove_articulation.assert_not_called() + parent_poses[:, 0] = 2 + group.update(colors=np.ones((2, 1, 4))) + np.testing.assert_allclose( + [marker.position for marker in group.snapshot()], [[2, 0, 0]] * 2 + ) + else: + sim._gizmos = {} + sim._disabled_robot_gizmos = set() + assert sim.remove_asset("robot") + assert "robot" not in scene and "mounted" not in sim.get_sensor_uid_list() + assert camera.destroy.call_count == int(mounted_camera) + parent_poses[:, 0] = 2 + group.update(colors=np.ones((2, 1, 4))) + np.testing.assert_allclose( + [marker.position for marker in group.snapshot()], [[1, 0, 0]] * 2 + ) + + +def test_default_backend_does_not_advertise_cables() -> None: + sim = _manager() + sim.physics = DefaultPhysicsBackend(SimpleNamespace()) + assert not sim.physics.supports_cables + with pytest.raises(NotImplementedError, match="Newton backend"): + sim.add_cable_object(_cfg()) + + +@pytest.mark.parametrize("endpoint", ["start_attachment", "end_attachment"]) +@pytest.mark.parametrize("qpos", [[0.025], torch.tensor([0.025])]) +def test_nonzero_attachment_is_rejected_before_commit_or_control_registration( + endpoint, qpos +): + sim = _manager() + cfg = _cfg() + setattr(cfg, endpoint, CableAttachmentCfg(target="tool", link_name="tip")) + sim.add_cable_object(cfg) + sim._articulations["tool"] = SimpleNamespace(cfg=ArticulationCfg(init_qpos=qpos)) + sim._spawn_scene.commit = Mock() + sim._spawn_scene.builder.add_runtime_control = Mock() + with pytest.raises(NotImplementedError, match="before attachment construction"): + sim.prepare() + sim._spawn_scene.commit.assert_not_called() + sim._spawn_scene.builder.add_runtime_control.assert_not_called() + + +def test_initial_joint_registration_is_retryable_after_preflight_and_idempotent(): + sim = _manager() + cfg = _cfg() + cfg.start_attachment = CableAttachmentCfg(target="tool", link_name="tip") + sim.add_cable_object(cfg) + sim._articulations["tool"] = SimpleNamespace(cfg=ArticulationCfg(init_qpos=[0.025])) + builder = sim._spawn_scene.builder + builder.add_runtime_control = Mock() + builder.replicate_plan = SimpleNamespace(env_names=lambda: ("arena_0", "arena_1")) + with pytest.raises(NotImplementedError): + sim.physics._prepare_scene_build() + sim._articulations["tool"].cfg.init_qpos = torch.tensor([0.0], requires_grad=True) + sim.physics._prepare_scene_build() + sim.physics._prepare_scene_build() + builder.add_runtime_control.assert_called_once() + controls = builder.add_runtime_control.call_args.args[0].seeds + assert [control.target for control in controls] == ["arena_0/tool", "arena_1/tool"] + for control in controls: + np.testing.assert_array_equal(control.positions, [[0.0]]) + + +def test_unattached_nonzero_joint_state_is_seeded_without_rejecting_other_anchors(): + sim = _manager() + cfg = _cfg() + cfg.start_attachment = CableAttachmentCfg(target="table") + sim.add_cable_object(cfg) + sim._articulations["tool"] = SimpleNamespace(cfg=ArticulationCfg(init_qpos=[0.025])) + builder = sim._spawn_scene.builder + builder.add_runtime_control = Mock() + builder.replicate_plan = SimpleNamespace(env_names=lambda: ("arena_0", "arena_1")) + sim.physics._prepare_scene_build() + builder.add_runtime_control.assert_called_once() + for seed in builder.add_runtime_control.call_args.args[0].seeds: + np.testing.assert_allclose(seed.positions, [[0.025]]) + + +def test_cable_scene_reset_restores_selected_visuals_lights_and_sensors_once(): + sim = _manager() + cable, _ = _bound_cable() + sim._cable_objects = {"wire": cable} + scene = Mock(spec=Scene) + scene.arenas = SimpleNamespace( + index=lambda name: {"arena_0": 1, "arena_1": 2}[name] + ) + sim._spawn_scene.builder.result = scene + sim._spawn_scene.builder.is_finalized = True + sim._spawn_scene.builder.replicate_plan = SimpleNamespace( + env_names=lambda: ("arena_0", "arena_1") + ) + light, sensor = Mock(), Mock() + visual = Mock(is_spawn_bound=True) + excluded_visual = Mock(is_spawn_bound=True) + unbound_visual = Mock(is_spawn_bound=False) + sim._visual_objects = { + "marker": visual, + "retained": excluded_visual, + "unbound": unbound_visual, + } + sim._lights = {"lamp": light} + sim._sensors = {"camera": sensor} + sim.reset_objects_state(env_ids=[1], excluded_uids=["retained"]) + scene.reset_arenas.assert_called_once_with([2]) + visual.reset.assert_called_once_with([1]) + excluded_visual.reset.assert_not_called() + unbound_visual.reset.assert_not_called() + light.reset.assert_called_once_with([1]) + sensor.reset.assert_called_once_with([1]) + scene.reset_arenas.reset_mock() + light.reset.reset_mock() + sensor.reset.reset_mock() + visual.reset.reset_mock() + sim.reset_objects_state(env_ids=[]) + scene.reset_arenas.assert_not_called() + light.reset.assert_not_called() + sensor.reset.assert_not_called() + visual.reset.assert_not_called() + + +def test_preflight_validates_all_initial_states_before_registering_controls(): + sim = _manager() + sim.add_cable_object(_cfg()) + sim._articulations = { + "valid": SimpleNamespace(cfg=ArticulationCfg(init_qpos=[0.025])), + "invalid": SimpleNamespace(cfg=ArticulationCfg(init_qpos=[float("nan")])), + } + builder = sim._spawn_scene.builder + builder.add_runtime_control = Mock() + with pytest.raises(ValueError, match="finite"): + sim.physics._prepare_scene_build() + builder.add_runtime_control.assert_not_called() diff --git a/tests/sim/objects/test_cable_object_cuda.py b/tests/sim/objects/test_cable_object_cuda.py new file mode 100644 index 0000000000..2a25e8594f --- /dev/null +++ b/tests/sim/objects/test_cable_object_cuda.py @@ -0,0 +1,467 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from pathlib import Path +import subprocess +import sys + +import numpy as np +import pytest +import torch +from scipy.spatial.transform import Rotation + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + CableAttachmentCfg, + CableObjectCfg, + CablePhysicsCfg, + FluidObjectCfg, + JointDrivePropertiesCfg, + NewtonPhysicsCfg, + RenderCfg, + RigidObjectCfg, + RobotCfg, + UsdImportCfg, + UsdSourceCfg, + VisualObjectCfg, +) +from embodichain.lab.sim.material import VisualMaterialCfg +from embodichain.lab.sim.shapes import CubeCfg + +pytestmark = [ + pytest.mark.subprocess_sim, + pytest.mark.gpu, + pytest.mark.skipif( + not torch.cuda.is_available(), reason="CableObject requires CUDA" + ), +] + + +def _volume_import(tmp_path: Path) -> UsdImportCfg: + path = tmp_path / "volume_particle.usda" + return UsdImportCfg(name="volume", source=UsdSourceCfg(file_path=str(path))) + + +class _CableObjectCUDAScenario: + """Replicated cable assertions executed only inside a fresh process.""" + + def setup_method(self, method) -> None: + if not torch.cuda.is_available(): + pytest.skip("CableObject requires CUDA") + solver = {"solver_type": "dexuni"} + if "expert_trajectory" in method.__name__: + solver.update( + { + "coupling": "two_way", + "mujoco_options": { + "enable_sleeping": True, + "update_data_interval": 1, + "iterations": 10, + "ls_iterations": 10, + }, + "vbd_options": {"iterations": 10, "cable_dahl_eps_max": 0.2}, + } + ) + cfg = SimulationManagerCfg( + width=128, + height=128, + headless=True, + device="cuda:0", + num_envs=2, + arena_space=3, + physics_dt=1 / 120, + enable_entity_gizmo=False, + robot_ik_gizmo=None, + render_cfg=RenderCfg(renderer="no-render"), + physics_cfg=NewtonPhysicsCfg( + solver_cfg=solver, + num_substeps=2, + ), + ) + self.sim = SimulationManager(cfg) + + def teardown_method(self) -> None: + if hasattr(self, "sim"): + self.sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + def test_replicated_state_force_and_cleanup(self, tmp_path: Path) -> None: + frames = Rotation.from_euler("z", [[37], [-19]], degrees=True).as_quat() + cable = self.sim.add_cable_object( + CableObjectCfg( + uid="wire", + points=[[0, 0, 0], [0, 0, 0.1], [0, 0, 0.2]], + radius=0.01, + init_pos=(0.5, 0.2, 1), + segment_quaternions=frames, + attrs=CablePhysicsCfg(bend_stiffness=0.1, twist_stiffness=0.1), + ) + ) + with pytest.raises(NotImplementedError, match="same arena"): + self.sim.add_fluid_object(FluidObjectCfg(uid="water")) + with pytest.raises(NotImplementedError, match="(?i)cable"): + self.sim.add_usd(_volume_import(tmp_path)) + assert "water" not in self.sim.asset_uids + assert cable.data is None and cable.num_instances == 2 + assert self.sim.get_asset("wire") is cable + assert "wire" in self.sim.asset_uids + self.sim.prepare() + assert cable.is_spawn_bound and cable.data.num_segments == 2 + with pytest.raises(NotImplementedError, match="topology changes"): + self.sim.add_rigid_object( + RigidObjectCfg( + uid="blocked", + shape=CubeCfg( + size=(0.1, 0.1, 0.1), + visual_material=VisualMaterialCfg( + uid="reserved", base_color=(1, 0, 0, 1) + ), + ), + ) + ) + visual = self.sim.add_visual_object( + VisualObjectCfg( + uid="allowed", + shape=CubeCfg( + size=(0.1, 0.1, 0.1), + visual_material=VisualMaterialCfg( + uid="reserved", base_color=(0, 1, 0, 1) + ), + ), + ) + ) + assert self.sim.get_visual_object("allowed") is visual + initial = cable.data.segment_state_w + assert initial.shape == (2, 2, 13) + assert torch.isfinite(initial).all() + scene = self.sim.spawn_result + for env_id, path in enumerate(cable.data.batch.paths): + handle = scene.get_cable(path) + matrices = handle.get_segment_poses() + np.testing.assert_allclose( + initial[env_id, :, :3].cpu(), matrices[:, :3, 3], atol=2e-6 + ) + np.testing.assert_allclose( + Rotation.from_quat(initial[env_id, :, 3:7].cpu()).as_matrix(), + matrices[:, :3, :3], + atol=2e-6, + ) + offsets = torch.tensor( + np.asarray(scene.arenas.root_offsets)[list(cable.data.batch.arena_indices)], + device=cable.device, + ) + torch.testing.assert_close( + initial[:, :, :3] - offsets[:, None, :], + torch.tensor( + [[0.5, 0.2, 1.05], [0.5, 0.2, 1.15]], device=cable.device + ).expand(2, 2, 3), + ) + self.sim.prepare() + for _ in range(3): + self.sim.update(step=1, render_final_step=False) + before = cable.data.segment_state_w + assert before[0, :, 2].mean() < initial[0, :, 2].mean() + cable.set_force([0, 0.1, 0], env_ids=[1]) + self.sim.update(step=1, render_final_step=False) + after = cable.data.segment_state_w + displacement = after[:, -1, 1] - before[:, -1, 1] + assert displacement[1] > displacement[0] + 1e-6 + cable.clear_wrenches(env_ids=[1]) + torch.testing.assert_close(cable.data.default_segment_state_w, initial) + self.sim.reset_objects_state(env_ids=[1]) + reset = cable.data.segment_state_w + torch.testing.assert_close(reset[0], after[0], rtol=0, atol=0) + torch.testing.assert_close(reset[1], initial[1], rtol=0, atol=0) + state = reset[[0]].clone() + state[..., 0] += 0.025 + cable.write_segment_state_to_sim(state, env_ids=[0]) + torch.testing.assert_close(cable.data.segment_state_w[0], state[0]) + torch.testing.assert_close( + cable.data.segment_state_w[1], initial[1], rtol=0, atol=0 + ) + with pytest.raises(ValueError, match="finite"): + cable.write_segment_state_to_sim(torch.full_like(state, float("nan")), [0]) + torch.testing.assert_close(cable.data.segment_state_w[0], state[0]) + data = cable.data + self.sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + assert cable.data is None + with pytest.raises(RuntimeError, match="invalid"): + _ = data.segment_state_w + del self.sim + + def test_imported_particles_survive_rejected_cable(self, tmp_path: Path) -> None: + imported = self.sim.add_usd(_volume_import(tmp_path)) + soft = imported["/World/Soft"] + with pytest.raises(NotImplementedError, match="(?i)cable"): + self.sim.add_cable_object( + CableObjectCfg(uid="wire", points=[[0, 0, 0], [0, 0, 0.1]], radius=0.01) + ) + assert self.sim.get_cable_object("wire") is None + assert self.sim.get_deformable_object(soft.uid) is soft + self.sim.prepare() + initial = soft.data.nodal_state_w.clone() + assert initial.shape == (2, 4, 6) + self.sim.update(step=3, render_final_step=False) + moved = soft.data.nodal_state_w.clone() + assert torch.all(moved[..., 2] < initial[..., 2]) + self.sim.reset_objects_state(env_ids=[1]) + reset = soft.data.nodal_state_w + torch.testing.assert_close(reset[0], moved[0], rtol=0, atol=0) + torch.testing.assert_close(reset[1], initial[1]) + + def test_attached_articulation_expert_trajectory_repeats_after_reset( + self, tmp_path: Path + ) -> None: + """Public commands, seeded initial state, and recorded repeated episodes.""" + urdf = tmp_path / "linear_tool.urdf" + urdf.write_text(""" + + + +""") + robot = self.sim.add_articulation( + ArticulationCfg( + uid="expert_tool", + fpath=str(urdf), + init_pos=(0, 0, 1), + init_qpos=[0.0], + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg( + stiffness=100.0, damping=5.0, target_mode="position" + ), + ) + ) + cable = self.sim.add_cable_object( + CableObjectCfg( + uid="wire", + points=[[0, 0, 0], [0.1, 0, 0], [0.2, 0, 0], [0.3, 0, 0]], + init_pos=(0, 0, 1), + radius=0.005, + start_attachment=CableAttachmentCfg( + target="expert_tool", link_name="tool" + ), + attrs=CablePhysicsCfg(bend_stiffness=0.01, twist_stiffness=0.01), + ) + ) + self.sim.prepare() + seed = 73 + generator = torch.Generator(device=cable.device).manual_seed(seed) + initial = cable.data.default_segment_state_w + # Small tail-only perturbation leaves the attached segment and anchor + # unchanged. Rest geometry remains engine-owned. + initial[:, 1:, :3] += 0.001 * ( + torch.rand((2, 2, 3), device=cable.device, generator=generator) - 0.5 + ) + initial[..., 7:] = 0 + frames = 40 + commands = ( + torch.linspace(0, 0.03, frames, device=cable.device)[:, None, None] + .expand(-1, 2, 1) + .contiguous() + ) + episodes, joint_episodes = [], [] + for _ in range(2): + self.sim.reset_objects_state() + cable.write_segment_state_to_sim(initial) + torch.testing.assert_close( + robot.get_qpos(), + torch.zeros_like(robot.get_qpos()), + atol=1e-7, + rtol=0, + ) + torch.testing.assert_close( + robot.get_qpos(target=True), + torch.zeros_like(robot.get_qpos()), + atol=1e-7, + rtol=0, + ) + states, joints = [], [] + for command in commands: + robot.set_qpos(command, target=True) + self.sim.update(step=1, render_final_step=False) + states.append(cable.data.segment_state_w) + joints.append(robot.get_qpos().clone()) + episodes.append(torch.stack(states)) + joint_episodes.append(torch.stack(joints)) + states = torch.stack(episodes) + joints = torch.stack(joint_episodes) + assert torch.isfinite(states).all() and torch.isfinite(joints).all() + assert joints[0, -1, :, 0].min() > 0.01 + assert states[0, -1, :, 0, 1].min() > 0.01 + torch.testing.assert_close(states[0], states[1], atol=1e-5, rtol=1e-5) + torch.testing.assert_close(joints[0], joints[1], atol=1e-6, rtol=1e-5) + np.savez_compressed( + tmp_path / "expert_trajectory.npz", + seed=seed, + physics_dt=self.sim.sim_config.physics_dt, + commands=commands.cpu().numpy(), + qpos=joints.cpu().numpy(), + cable_state_w=states.cpu().numpy(), + ) + + def test_nonzero_joint_creation_defaults_and_targets_survive_reset( + self, tmp_path: Path + ) -> None: + """An initialization hook must seed defaults without driving later frames.""" + urdf = tmp_path / "linear_tool.urdf" + urdf.write_text(""" + + + +""") + + from dexsim.engine.newton_physics.runtime_control import RuntimeControl + + initialized = [] + + class CallerControl(RuntimeControl): + graph_safe = True + + def initialize(self, context): + initialized.append(self) + + def __call__(self, context, substep_index, substep_count, substep_dt): + pass + + def prepare_episode_reset(self, arena_indices): + return lambda: None + + caller_control = CallerControl() + self.sim.add_runtime_control(caller_control) + # The discarded robot supplies two qpos values for a one-joint URDF. + # Retry with one UID removed and the other carrying a new initial pose. + self.sim.add_robot( + RobotCfg(uid="discarded", fpath=str(urdf), init_qpos=[0.09, 0.10]) + ) + self.sim.add_robot(RobotCfg(uid="tool", fpath=str(urdf), init_qpos=[0.05])) + self.sim.add_cable_object( + CableObjectCfg( + uid="wire", + points=[[0, 0, 0], [0.1, 0, 0]], + init_pos=(1, 0, 1), + radius=0.005, + ) + ) + with pytest.raises(ValueError, match="discarded.*expects 1 .*got 2"): + self.sim.prepare() + assert self.sim.remove_asset("discarded") + assert self.sim.remove_asset("tool") + robot = self.sim.add_robot( + RobotCfg( + uid="tool", + fpath=str(urdf), + init_pos=(0, 0, 1), + init_qpos=torch.tensor([0.025], device=self.sim.device), + asset_physics_mode="overlay", + joint_drive_props=JointDrivePropertiesCfg( + stiffness=100.0, damping=5.0, target_mode="position" + ), + ) + ) + self.sim.prepare() + assert initialized == [caller_control] + expected = torch.full_like(robot.get_qpos(), 0.025) + torch.testing.assert_close(robot.get_qpos(), expected, rtol=0, atol=1e-7) + torch.testing.assert_close( + robot.get_qpos(target=True), expected, rtol=0, atol=1e-7 + ) + self.sim.reset_objects_state() + torch.testing.assert_close(robot.get_qpos(), expected, rtol=0, atol=1e-7) + torch.testing.assert_close( + robot.get_qpos(target=True), expected, rtol=0, atol=1e-7 + ) + robot.set_qpos(torch.full_like(expected, 0.075), target=True) + for _ in range(15): + self.sim.update(step=1, render_final_step=False) + moved = robot.get_qpos().clone() + assert moved.min() > 0.03 + self.sim.reset_objects_state(env_ids=[0]) + torch.testing.assert_close(robot.get_qpos()[0], expected[0], rtol=0, atol=1e-7) + torch.testing.assert_close( + robot.get_qpos(target=True)[0], expected[0], rtol=0, atol=1e-7 + ) + torch.testing.assert_close(robot.get_qpos()[1], moved[1], rtol=0, atol=0) + + +def _run_cable_case(case_name: str, tmp_path: Path) -> None: + """Run one lifecycle case and release its native world even after failure.""" + scenario = _CableObjectCUDAScenario() + case = getattr(scenario, case_name) + try: + scenario.setup_method(case) + case(tmp_path) + finally: + scenario.teardown_method() + + +def _assert_cable_case_in_subprocess(case_name: str, tmp_path: Path) -> None: + """Keep process-wide renderer initialization outside the pytest worker.""" + script = ( + "import runpy, sys; from pathlib import Path; " + "module = runpy.run_path(sys.argv[1]); " + "module['_run_cable_case'](sys.argv[2], Path(sys.argv[3]))" + ) + result = subprocess.run( + [ + sys.executable, + "-c", + script, + str(Path(__file__).resolve()), + case_name, + str(tmp_path), + ], + capture_output=True, + text=True, + timeout=300, + ) + assert result.returncode == 0, result.stdout + result.stderr + + +class TestCableObjectCUDA: + """NoRender cable lifecycle coverage, with a fresh native process per case.""" + + def test_replicated_state_force_and_cleanup( + self, volume_particle_usd: Path + ) -> None: + _assert_cable_case_in_subprocess( + "test_replicated_state_force_and_cleanup", volume_particle_usd.parent + ) + + def test_imported_particles_survive_rejected_cable( + self, volume_particle_usd: Path + ) -> None: + _assert_cable_case_in_subprocess( + "test_imported_particles_survive_rejected_cable", volume_particle_usd.parent + ) + + @pytest.mark.usefixtures("requires_dexuni_two_way") + def test_attached_articulation_expert_trajectory_repeats_after_reset( + self, tmp_path: Path + ) -> None: + _assert_cable_case_in_subprocess( + "test_attached_articulation_expert_trajectory_repeats_after_reset", tmp_path + ) + + def test_nonzero_joint_creation_defaults_and_targets_survive_reset( + self, tmp_path: Path + ) -> None: + _assert_cable_case_in_subprocess( + "test_nonzero_joint_creation_defaults_and_targets_survive_reset", tmp_path + ) diff --git a/tests/sim/objects/test_cable_two_way_cuda.py b/tests/sim/objects/test_cable_two_way_cuda.py new file mode 100644 index 0000000000..7fc8905381 --- /dev/null +++ b/tests/sim/objects/test_cable_two_way_cuda.py @@ -0,0 +1,239 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Cable contact feedback through the public EmbodiChain CUDA interfaces.""" + +from __future__ import annotations + +import json +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 ( + ArticulationCfg, + CableObjectCfg, + CablePhysicsCfg, + NewtonCollisionPropertiesCfg, + NewtonJointDrivePropertiesCfg, + NewtonPhysicsCfg, + NewtonRigidBodyMaterialCfg, + RenderCfg, + RigidBodyPhysicsCfg, +) +from embodichain.lab.sim.sensors import ArticulationContactFilterCfg, ContactSensorCfg + +pytestmark = [ + pytest.mark.subprocess_sim, + pytest.mark.gpu, + pytest.mark.usefixtures("requires_dexuni_two_way", "requires_dexuni_contact_query"), +] + +# The finger moves along +X into the free middle segment of a clamped cable. +FINGER_START_X = -0.06 +FINGER_HALF_X = 0.01 +CABLE_RADIUS = 0.015 +CABLE_HEIGHT = 0.3 # Clear the default ground and isolate cable contacts. +TARGET_TRAVEL = 0.09 # Move the finger completely through the cable in one-way mode. +CONTACT_TRAVEL = -FINGER_START_X - FINGER_HALF_X - CABLE_RADIUS +POSITION_TOLERANCE = CABLE_RADIUS / 2 + + +def _write_finger(path: Path) -> None: + """Author one dynamic slider, with no other collision geometry.""" + path.write_text( + f""" + + + + + + + + + + + + +""", + encoding="utf-8", + ) + + +def _run_contact_case(path: Path, coupling: str) -> dict[str, float | int]: + """Push a deformable cable with one-way or two-way articulation coupling.""" + envelope = NewtonCollisionPropertiesCfg(margin=0.0, gap=0.0) + material = NewtonRigidBodyMaterialCfg(ke=2.0e4, kd=100.0, dynamic_friction=0.5) + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + device="cuda:0", + robot_ik_gizmo=None, + enable_entity_gizmo=False, + render_cfg=RenderCfg(renderer="no-render"), + physics_cfg=NewtonPhysicsCfg( + device="cuda:0", + gravity=(0.0, 0.0, 0.0), + physics_dt=1.0 / 60.0, + num_substeps=10, + use_cuda_graph=False, + collision_cfg=None, + solver_cfg={ + "solver_type": "dexuni", + "joint_mode": "dynamic", + "coupling": coupling, + "mujoco_articulation_paths": ("arena_0/finger",), + "vbd_options": {"iterations": 20}, + "collision_options": {"broad_phase": "nxn"}, + }, + ), + ) + ) + articulation = cable = sensor = None + try: + articulation = sim.add_articulation( + ArticulationCfg( + uid="finger", + fpath=str(path), + init_qpos=[0.0], + asset_physics_mode="overlay", + build_pk_chain=False, + attrs=RigidBodyPhysicsCfg( + collision_props=envelope, material_props=material + ), + joint_drive_props=NewtonJointDrivePropertiesCfg( + target_mode="position_velocity", + stiffness=1000.0, + damping=20.0, + passive_damping=50.0, + armature=0.3, + max_effort=10.0, + ), + ) + ) + cable = sim.add_cable_object( + CableObjectCfg( + uid="wire", + points=[(0.0, y, CABLE_HEIGHT) for y in (-0.09, -0.03, 0.03, 0.09)], + radius=CABLE_RADIUS, + fixed_segments=(0, 2), + attrs=CablePhysicsCfg( + density=1000.0, + stretch_stiffness=1.0e5, + shear_stiffness=1.0e5, + bend_stiffness=10.0, + twist_stiffness=10.0, + collision_props=envelope, + material_props=material, + ), + ) + ) + sim.prepare() + assert cable.data is not None + sensor = sim.add_sensor( + ContactSensorCfg( + uid="finger_contact", + articulation_cfg_list=[ + ArticulationContactFilterCfg( + articulation_uid="finger", link_name_list=["moving"] + ) + ], + filter_need_both_actor=False, + max_contacts_per_env=32, + ) + ) + articulation.set_qpos( + torch.tensor([[TARGET_TRAVEL]], device=sim.device), target=True + ) + contact_rows = 0 + peak_cable_deflection = 0.0 + # Two seconds allow the damped slider to settle at the contact surface. + for _ in range(120): + sim.update(step=1, render_final_step=False) + state = cable.data.segment_state_w + assert torch.isfinite(state).all() + peak_cable_deflection = max(peak_cable_deflection, float(state[0, 1, 0])) + sensor.update() + data = sensor.get_data() + valid = data["is_valid"] + if valid.any(): + # Cable shapes currently have no ContactActorInfo entry. With + # match-any on the finger, the native query still emits their + # rows. This isolated scene has no other possible counterpart. + assert sensor.contact_capabilities.geometry + assert not data["impulse_valid"][valid].any() + assert not data["friction_valid"][valid].any() + assert torch.count_nonzero(data["impulse"][valid]) == 0 + assert torch.count_nonzero(data["friction"][valid]) == 0 + assert torch.isfinite(data["position"][valid]).all() + assert torch.isfinite(data["normal"][valid]).all() + contact_rows += int(valid.sum()) + travel = float(articulation.get_qpos()[0, 0]) + return { + "travel": travel, + "peak_cable_deflection": peak_cable_deflection, + "geometry_contact_rows": contact_rows, + } + finally: + articulation = cable = sensor = None + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +def _assert_cable_feedback(path: Path) -> None: + """Compare identical scenes; only the articulation reaction policy differs.""" + _write_finger(path) + one_way = _run_contact_case(path, "one_way") + two_way = _run_contact_case(path, "two_way") + print(json.dumps({"one_way": one_way, "two_way": two_way}), flush=True) + assert abs(one_way["travel"] - TARGET_TRAVEL) < POSITION_TOLERANCE, one_way + assert abs(two_way["travel"] - CONTACT_TRAVEL) < POSITION_TOLERANCE, two_way + assert one_way["travel"] - two_way["travel"] > CABLE_RADIUS, (one_way, two_way) + assert two_way["geometry_contact_rows"] > 0, two_way + # The contacted segment is free, so the reaction must load the actual cable. + assert two_way["peak_cable_deflection"] > 1.0e-6, two_way + + +class TestCableTwoWayCUDA: + """Physical feedback and contact validity in an isolated native process.""" + + def test_cable_contact_blocks_dynamic_finger_with_two_way_coupling( + self, tmp_path: Path + ) -> None: + if not torch.cuda.is_available(): + pytest.skip("CUDA is required for cable two-way contact feedback.") + script = ( + "import runpy, sys; from pathlib import Path; " + "module = runpy.run_path(sys.argv[1]); " + "module['_assert_cable_feedback'](Path(sys.argv[2]))" + ) + result = subprocess.run( + [ + sys.executable, + "-c", + script, + str(Path(__file__).resolve()), + str(tmp_path / "finger.urdf"), + ], + capture_output=True, + text=True, + timeout=300, + ) + assert result.returncode == 0, result.stdout + result.stderr diff --git a/tests/sim/objects/test_fluid_runtime.py b/tests/sim/objects/test_fluid_runtime.py index 20e9822529..7247216fdc 100644 --- a/tests/sim/objects/test_fluid_runtime.py +++ b/tests/sim/objects/test_fluid_runtime.py @@ -27,6 +27,7 @@ import torch from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.cfg import ( + CableObjectCfg, FluidObjectCfg, FluidSpawnCfg, FluidRenderCfg, @@ -152,6 +153,15 @@ def _run_fluid_lifecycle(num_envs: int, automatic: bool) -> dict[str, object]: render=FluidRenderCfg(mode="none"), ) ) + with pytest.raises(NotImplementedError, match="same arena"): + sim.add_cable_object( + CableObjectCfg( + uid="wire", + points=[[0, 0, 0], [0, 0, 0.1], [0, 0, 0.2]], + radius=0.01, + ) + ) + assert "wire" not in sim.asset_uids with pytest.raises(RuntimeError, match="prepare"): _ = fluid.data sim.prepare() @@ -187,6 +197,10 @@ def _run_fluid_lifecycle(num_envs: int, automatic: bool) -> dict[str, object]: continued = fluid.data.particle_state_w torch.testing.assert_close(continued[0], first[0], rtol=1e-5, atol=1e-6) assert continued[1, :, 2].mean() < first[1, :, 2].mean() + sim.reset_objects_state(env_ids=[1]) + selected = fluid.data.particle_state_w + torch.testing.assert_close(selected[0], continued[0], rtol=0, atol=0) + torch.testing.assert_close(selected[1], original[1], rtol=0, atol=0) sim.reset_objects_state() torch.testing.assert_close(fluid.data.particle_state_w, original) sim.update(step=3, render_final_step=False) diff --git a/tests/sim/test_backend_parity.py b/tests/sim/test_backend_parity.py index b5be7e3609..5040f4dbdb 100644 --- a/tests/sim/test_backend_parity.py +++ b/tests/sim/test_backend_parity.py @@ -36,6 +36,7 @@ import torch import embodichain.lab.sim.sim_manager as sim_manager_module +from embodichain.lab.sim.cfg import CableObjectCfg from embodichain.lab.sim.physics import ( DefaultPhysicsBackend, NewtonPhysicsBackend, @@ -51,6 +52,7 @@ # feature -> {backend -> supported} BACKEND_CAPABILITIES: dict[str, dict[str, bool]] = { "robot": {"default": True, "newton": True}, + "cables": {"default": False, "newton": True}, "volume_deformables": {"default": False, "newton": True}, "surface_deformables": {"default": False, "newton": True}, "rigid_object_group": {"default": True, "newton": True}, @@ -68,6 +70,7 @@ # elsewhere rather than an add_* guard. CAPABILITY_TO_ADD_METHOD: dict[str, str | None] = { "robot": "add_robot", + "cables": "add_cable_object", "volume_deformables": "add_deformable_object", "surface_deformables": "add_deformable_object", "rigid_object_group": "add_rigid_object_group", @@ -76,10 +79,6 @@ } -# This module exercises capability guards using mock managers only. -pytestmark = pytest.mark.no_sim - - def _make_backend(name: str) -> PhysicsBackend: """Construct a backend with a minimal fake owning-manager back-ref.""" return BACKENDS[name](SimpleNamespace()) @@ -262,6 +261,15 @@ def test_add_method_guard_maps_to_capability( cfg = SimpleNamespace(uid=None) if feature in deformable_types: cfg.deformable_type = deformable_types[feature] + elif feature == "cables": + cfg = CableObjectCfg(uid=None, points=[[0, 0, 0], [0, 0, 0.2]], radius=0.01) + sim.device = torch.device("cuda:0") + sim.sim_config = SimpleNamespace( + physics_cfg=SimpleNamespace(requires_grad=False) + ) + sim._fluid_objects = {} + if supported: + backend._configured_solver_type = "dexuni" if supported: # Past the guard it will hit missing-world attrs; assert the failure is diff --git a/tests/sim/test_cable_cfg.py b/tests/sim/test_cable_cfg.py new file mode 100644 index 0000000000..12501c8de5 --- /dev/null +++ b/tests/sim/test_cable_cfg.py @@ -0,0 +1,188 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import pickle + +import numpy as np +import pytest +from scipy.spatial.transform import Rotation + +from embodichain.lab.sim.cfg import ( + CableAttachmentCfg, + CableObjectCfg, + CablePhysicsCfg, + NewtonCollisionPropertiesCfg, + NewtonRigidBodyMaterialCfg, + DefaultCollisionPropertiesCfg, +) +from embodichain.lab.sim.material import VisualMaterialCfg +from embodichain.lab.sim.shapes import MeshCfg +from embodichain.lab.sim.spawn.descriptors import cable_desc_from_cfg + +pytestmark = pytest.mark.no_sim + + +def _cfg(**kwargs) -> CableObjectCfg: + return CableObjectCfg( + uid="wire", points=[[0, 0, 0], [0, 0, 0.2]], radius=0.01, **kwargs + ) + + +def test_missing_engine_cable_descriptors_are_rejected_at_creation(monkeypatch): + """Optional cable support must not prevent ordinary core/config imports.""" + import dexsim.spawn + + monkeypatch.delattr(dexsim.spawn, "CableDesc") + cfg = _cfg() + with pytest.raises(NotImplementedError, match="DexSim build exposing CableDesc"): + cable_desc_from_cfg(cfg) + + +def test_nested_config_and_contact_schema_round_trip() -> None: + cfg = CableObjectCfg.from_dict( + { + "uid": "wire", + "points": [[0, 0, 0], [0, 0, 0.2]], + "radius": 0.01, + "init_pos": [0.4, 0.2, 1.0], + "fixed_segments": [0], + "attrs": { + "bend_stiffness": 0.0, + "twist_damping": 0.0, + "wall_thickness": 0.002, + "collision_props": {"backend": "newton", "margin": 0.002, "gap": 0.003}, + "material_props": {"backend": "newton", "dynamic_friction": 0.6}, + }, + "visual_material": {"uid": "wire_mat", "base_color": [1, 0, 0, 1]}, + } + ) + assert isinstance(cfg.attrs, CablePhysicsCfg) + assert isinstance(cfg.attrs.collision_props, NewtonCollisionPropertiesCfg) + assert isinstance(cfg.attrs.material_props, NewtonRigidBodyMaterialCfg) + restored = CableObjectCfg.from_dict(pickle.loads(pickle.dumps(cfg.to_dict()))) + desc, materials = cable_desc_from_cfg(restored) + assert desc.fixed_segments == (0,) + assert desc.physics.bend_stiffness == 0.0 and desc.physics.stretch_stiffness is None + assert desc.physics.twist_damping == 0.0 + assert desc.physics.wall_thickness == 0.002 + assert desc.collision.margin == 0.002 and desc.collision.gap == 0.003 + assert desc.collision.mu == 0.6 + assert materials["wire_mat"] is desc.render_material + np.testing.assert_allclose(desc.pose[:3, 3], [0.4, 0.2, 1.0]) + + +def test_optional_material_defaults_are_not_filled_by_embodichain() -> None: + desc, materials = cable_desc_from_cfg(_cfg()) + assert desc.collision is None and materials == {} + assert desc.physics.shear_stiffness is None + assert desc.physics.twist_stiffness is None + assert desc.physics.wall_thickness is None + + +def test_material_frames_convert_xyzw_once_without_mutating_input() -> None: + # An axial rotation about +Z distinguishes quaternion layouts while + # preserving the required segment tangent. + quaternions = Rotation.from_euler("z", [37], degrees=True).as_quat().reshape(1, 4) + saved = quaternions.copy() + cfg = _cfg(segment_quaternions=quaternions) + desc, _ = cable_desc_from_cfg(cfg) + np.testing.assert_allclose( + desc.segment_quaternions, saved[:, [3, 0, 1, 2]], atol=1e-7 + ) + np.testing.assert_array_equal(quaternions, saved) + + +def test_descriptor_owns_geometry_and_attachment_input() -> None: + points = np.array([[0, 0, 0], [0, 0, 0.2]], dtype=np.float32) + anchor = np.array([0, 0, 0.2], dtype=np.float32) + cfg = CableObjectCfg.from_dict( + { + "uid": "wire", + "points": points, + "radius": 0.01, + "end_attachment": {"target": "connector", "position": anchor}, + } + ) + assert isinstance(cfg.end_attachment, CableAttachmentCfg) + desc, _ = cable_desc_from_cfg(cfg) + points[:] = 10 + anchor[:] = 10 + np.testing.assert_allclose(desc.points[-1], [0, 0, 0.2]) + np.testing.assert_allclose(desc.end_attachment.position, [0, 0, 0.2]) + + +@pytest.mark.parametrize( + "overrides", + [ + {"radius": 0}, + {"points": [[0, 0, 0], [0, 0, 0]]}, + {"fixed_segments": [True]}, + {"closed": True}, + {"segment_quaternions": [[0, 0, 0]]}, + {"attrs": {"bend_stiffness": -1}}, + ], +) +def test_invalid_geometry_and_material_rejected_before_spawn(overrides) -> None: + data = {"uid": "wire", "points": [[0, 0, 0], [0, 0, 0.2]], "radius": 0.01} + data.update(overrides) + with pytest.raises((ValueError, TypeError)): + cable_desc_from_cfg(CableObjectCfg.from_dict(data)) + + +def test_required_geometry_is_validated() -> None: + with pytest.raises(TypeError, match="Missing values"): + cable_desc_from_cfg(CableObjectCfg(uid="wire")) + + +def test_custom_visual_uses_existing_mesh_and_material_compiler() -> None: + shape = MeshCfg( + vertices=np.array([[0, 0, 0], [0.01, 0, 0], [0, 0, 0.2]]), + triangles=np.array([[0, 1, 2]]), + visual_material=VisualMaterialCfg(uid="custom_mat"), + ) + desc, materials = cable_desc_from_cfg(_cfg(visual_shape=shape)) + assert len(desc.renders) == 1 + assert desc.renders[0].material_ref == "custom_mat" + assert "custom_mat" in materials and desc.render_material is None + with pytest.raises(ValueError, match="owns its visual_material"): + cable_desc_from_cfg( + _cfg(visual_shape=shape, visual_material=VisualMaterialCfg()) + ) + + +def test_collision_toggle_reaches_the_native_descriptor() -> None: + cfg = _cfg( + attrs=CablePhysicsCfg( + collision_props=NewtonCollisionPropertiesCfg(collision_enabled=False) + ) + ) + descriptor, _ = cable_desc_from_cfg(cfg) + assert descriptor.enable_collision is False + + +def test_default_native_collision_override_is_rejected() -> None: + cfg = _cfg(attrs=CablePhysicsCfg(collision_props=DefaultCollisionPropertiesCfg())) + with pytest.raises(NotImplementedError, match="common or Newton"): + cable_desc_from_cfg(cfg) + + +def test_custom_visual_does_not_silently_ignore_projective_uv_request() -> None: + with pytest.raises(NotImplementedError, match="authored UVs"): + cable_desc_from_cfg( + _cfg(visual_shape=MeshCfg(fpath="unused.obj", compute_uv=True)) + ) diff --git a/tests/sim/test_fluid_cfg.py b/tests/sim/test_fluid_cfg.py index 6a290e096d..0e0e099a5f 100644 --- a/tests/sim/test_fluid_cfg.py +++ b/tests/sim/test_fluid_cfg.py @@ -318,6 +318,7 @@ def _fluid_manager_for_renderer(renderer): import torch from embodichain.lab.sim import SimulationManagerCfg from embodichain.lab.sim.cfg import RenderCfg + from embodichain.lab.sim.spawn.scene import SpawnScene return SimpleNamespace( physics=SimpleNamespace( @@ -336,7 +337,7 @@ def _fluid_manager_for_renderer(renderer): asset_uids=[], _fluid_objects={}, _rigid_objects={}, - _spawn_scene=SimpleNamespace(declare=Mock()), + _spawn_scene=Mock(spec=SpawnScene), create_visual_material=Mock(), _invalidate_fluid_state=Mock(), notify_visualization_topology_changed=Mock(), diff --git a/tests/sim/test_runtime_controls.py b/tests/sim/test_runtime_controls.py index 531fba0800..a426c0fdc0 100644 --- a/tests/sim/test_runtime_controls.py +++ b/tests/sim/test_runtime_controls.py @@ -23,6 +23,7 @@ import numpy as np import pytest +import torch from embodichain.lab.sim._runtime_controls import ( _KinematicNodalTrajectoryControl, @@ -135,3 +136,303 @@ def test_kinematic_nodal_control_holds_last_unrated_sample() -> None: control(context, substep_index=0, substep_count=1, substep_dt=0.01) np.testing.assert_allclose(particle_set.positions[1], [3.0, 0.0, 0.0]) + + +def test_initial_joint_seed_uses_source_order_and_never_drives_runtime(monkeypatch): + from embodichain.lab.sim._runtime_controls import ( + _InitialJointState, + _SceneInitialJointStates, + ) + + initializer = MagicMock() + factory = MagicMock(return_value=initializer) + monkeypatch.setattr( + "dexsim.engine.newton_physics.KinematicJointTrajectoryControl", factory + ) + handle = SimpleNamespace( + get_actived_joint_names=lambda: ["source_a", "source_b"], + joint_dof_layout=[ + SimpleNamespace(name="source_b"), + SimpleNamespace(name="source_a"), + ], + get_mimic_info=lambda: SimpleNamespace( + mimic_id=[], mimic_parent=[], mimic_multiplier=[], mimic_offset=[] + ), + ) + batch = MagicMock() + result = SimpleNamespace( + get_articulation=lambda target: handle, + create_articulation_batch=MagicMock(return_value=batch), + ) + control = _SceneInitialJointStates( + (_InitialJointState("arena/tool", np.array([0.1, 0.2])),) + ) + control.initialize_model(object(), result) + np.testing.assert_allclose(factory.call_args.args[1], [[0.2, 0.1]]) + assert factory.call_args.kwargs["initialize_model_pose"] is True + control.initialize(SimpleNamespace(result=result)) + np.testing.assert_allclose( + batch.apply_state.call_args.kwargs["joint_target_position"], [[0.2, 0.1]] + ) + batch.reset_mock() + control(None, 0, 4, 1 / 240) + control.prepare_episode_reset((0,))() + batch.apply_state.assert_not_called() + control.close() + initializer.close.assert_called_once() + + +@pytest.mark.parametrize( + ("stabilizer", "solver_supports_stabilizer", "initial_qpos", "expected"), + [ + (False, True, [0.02, 0.0], [0.06, 0.02]), + (True, True, [0.02, 0.0], [0.0, 0.02]), + (True, False, [0.02, 0.0], [0.06, 0.02]), + (False, True, None, [0.1, 0.0]), + (True, True, None, [0.0, 0.0]), + (True, False, None, [0.1, 0.0]), + ], +) +def test_initial_mimic_defaults_and_targets_match_articulation_reset( + monkeypatch, stabilizer, solver_supports_stabilizer, initial_qpos, expected +) -> None: + from embodichain.lab.sim._runtime_controls import _InitialJointState + from embodichain.lab.sim.cfg import ArticulationCfg + from embodichain.lab.sim.objects.articulation import Articulation + + initializer = MagicMock() + factory = MagicMock(return_value=initializer) + monkeypatch.setattr( + "dexsim.engine.newton_physics.KinematicJointTrajectoryControl", factory + ) + monkeypatch.setattr( + "embodichain.lab.sim.objects.backends.newton._supports_newton_mimic_compliance", + lambda result: solver_supports_stabilizer, + ) + handle = SimpleNamespace( + get_dof=lambda: 2, + get_actived_joint_names=lambda: ["leader", "follower"], + joint_dof_layout=[ + SimpleNamespace(name="follower", dof_start=0), + SimpleNamespace(name="leader", dof_start=1), + ], + get_mimic_info=lambda: SimpleNamespace( + mimic_id=[1], mimic_parent=[0], mimic_multiplier=[-2.0], mimic_offset=[0.1] + ), + ) + batch = MagicMock() + result = SimpleNamespace( + get_articulation=lambda target: handle, + create_articulation_batch=lambda handles: batch, + ) + control = _InitialJointState( + "arena/tool", initial_qpos, mimic_stabilizer=stabilizer + ) + control.initialize_model(object(), result) + seeded_positions = factory.call_args.args[1].copy() + control.initialize(SimpleNamespace(result=result)) + for call in batch.apply_state.call_args_list: + if "joint_position" in call.kwargs: + seeded_positions = call.kwargs["joint_position"] + np.testing.assert_allclose(seeded_positions, [expected], atol=1e-7) + np.testing.assert_allclose( + batch.apply_state.call_args.kwargs["joint_target_position"], + [expected], + atol=1e-7, + ) + + articulation = object.__new__(Articulation) + # Binding resolves omitted initial positions to zeros before reset. + articulation.cfg = ArticulationCfg( + init_qpos=[0.0, 0.0] if initial_qpos is None else initial_qpos + ) + articulation.device = torch.device("cpu") + articulation._all_indices = np.array([0]) + articulation._entities = [handle] + articulation._data = SimpleNamespace( + is_newton_backend=True, articulation_view=MagicMock() + ) + articulation._mimic_info = SimpleNamespace( + mimic_id=np.array([0]), + mimic_parent=np.array([1]), + mimic_multiplier=np.array([-2.0]), + mimic_offset=np.array([0.1]), + ) + articulation._newton_mimic_compliance_configured = ( + stabilizer and solver_supports_stabilizer + ) + articulation._source_qpos_to_state_order = lambda qpos: qpos[:, [1, 0]] + articulation.restore_visual_material = MagicMock() + articulation._restore_default_physical_properties = MagicMock() + articulation.set_local_pose = MagicMock() + articulation.set_qpos = MagicMock() + articulation.reset(clear_dynamics=False) + for call in articulation.set_qpos.call_args_list: + np.testing.assert_allclose(call.args[0].numpy(), seeded_positions, atol=1e-7) + + +@pytest.mark.parametrize( + "value", + [[0.1, 0.2], np.array([0.1, 0.2]), torch.tensor([0.1, 0.2], requires_grad=True)], +) +def test_initial_joint_seed_copies_supported_inputs(value): + from embodichain.lab.sim._runtime_controls import _InitialJointState + + control = _InitialJointState("arena/tool", value) + np.testing.assert_allclose(control.positions, [[0.1, 0.2]]) + assert control.positions.dtype == np.float32 + if isinstance(value, torch.Tensor): + with torch.no_grad(): + value.zero_() + elif isinstance(value, np.ndarray): + value[:] = 0 + else: + value[:] = [0, 0] + np.testing.assert_allclose(control.positions, [[0.1, 0.2]]) + + +@pytest.mark.parametrize( + "value", [[[0.1]], [float("nan")], [float("inf")], [1e100], [True], ["0.1"]] +) +def test_initial_joint_seed_rejects_invalid_vectors(value): + from embodichain.lab.sim._runtime_controls import _InitialJointState + + with pytest.raises(ValueError, match="init_qpos"): + _InitialJointState("arena/tool", value) + + +def test_initial_joint_seed_accepts_bfloat16_tensor(): + from embodichain.lab.sim._runtime_controls import _InitialJointState + + control = _InitialJointState( + "arena/tool", torch.tensor([0.125, 0.25], dtype=torch.bfloat16) + ) + np.testing.assert_array_equal(control.positions, [[0.125, 0.25]]) + + +@pytest.mark.parametrize("stabilizer", [False, True]) +@pytest.mark.parametrize("initial_qpos", [None, [0.0, 0.0]]) +def test_mimic_offset_attachment_rejected_before_seeding_nonzero_pose( + monkeypatch, stabilizer, initial_qpos +) -> None: + from embodichain.lab.sim._runtime_controls import _InitialJointState + + initializer = MagicMock() + factory = MagicMock(return_value=initializer) + monkeypatch.setattr( + "dexsim.engine.newton_physics.KinematicJointTrajectoryControl", factory + ) + monkeypatch.setattr( + "embodichain.lab.sim.objects.backends.newton._supports_newton_mimic_compliance", + lambda result: False, + ) + handle = SimpleNamespace( + get_dof=lambda: 2, + get_actived_joint_names=lambda: ["leader", "follower"], + joint_dof_layout=[ + SimpleNamespace(name="leader", dof_start=0), + SimpleNamespace(name="follower", dof_start=1), + ], + get_mimic_info=lambda: SimpleNamespace( + mimic_id=[1], mimic_parent=[0], mimic_multiplier=[1.0], mimic_offset=[0.1] + ), + ) + batch = MagicMock() + result = SimpleNamespace( + get_articulation=lambda target: handle, + create_articulation_batch=lambda handles: batch, + ) + control = _InitialJointState( + "arena/tool", + initial_qpos, + mimic_stabilizer=stabilizer, + has_permanent_attachment=True, + ) + with pytest.raises(NotImplementedError, match="after mimic projection"): + control.initialize_model(object(), result) + control.initialize(SimpleNamespace(result=result)) + batch.apply_state.assert_not_called() + if not stabilizer: + factory.assert_not_called() + + +def test_cable_scene_seeds_omitted_joint_defaults_and_checks_attachment(monkeypatch): + from embodichain.lab.sim.cfg import ArticulationCfg + from embodichain.lab.sim.physics.newton import NewtonPhysicsBackend + + register = MagicMock() + manager = SimpleNamespace( + _robots={}, + _articulations={"tool": SimpleNamespace(cfg=ArticulationCfg())}, + _cable_objects={ + "cable": SimpleNamespace( + cfg=SimpleNamespace( + start_attachment=SimpleNamespace(target="tool"), + end_attachment=None, + ) + ) + }, + _spawn_scene=SimpleNamespace( + arena_names=("arena_0",), + builder=SimpleNamespace(is_finalized=False, add_runtime_control=register), + ), + ) + NewtonPhysicsBackend(manager)._prepare_scene_build() + register.assert_called_once() + control = register.call_args.args[0] + handle = SimpleNamespace( + get_dof=lambda: 2, + get_actived_joint_names=lambda: ["leader", "follower"], + joint_dof_layout=[ + SimpleNamespace(name="leader", dof_start=0), + SimpleNamespace(name="follower", dof_start=1), + ], + get_mimic_info=lambda: SimpleNamespace( + mimic_id=[1], mimic_parent=[0], mimic_multiplier=[1.0], mimic_offset=[0.1] + ), + ) + initializer = MagicMock() + monkeypatch.setattr( + "dexsim.engine.newton_physics.KinematicJointTrajectoryControl", initializer + ) + with pytest.raises(NotImplementedError, match="after mimic projection"): + control.initialize_model( + object(), SimpleNamespace(get_articulation=lambda target: handle) + ) + initializer.assert_not_called() + + +def test_omitted_initial_positions_allow_fixed_articulation(monkeypatch): + from embodichain.lab.sim._runtime_controls import _InitialJointState + + initializer = MagicMock() + monkeypatch.setattr( + "dexsim.engine.newton_physics.KinematicJointTrajectoryControl", initializer + ) + handle = SimpleNamespace( + get_dof=lambda: 0, + get_actived_joint_names=lambda: [], + joint_dof_layout=[], + get_mimic_info=lambda: SimpleNamespace( + mimic_id=[], mimic_parent=[], mimic_multiplier=[], mimic_offset=[] + ), + ) + result = SimpleNamespace( + get_articulation=lambda target: handle, + create_articulation_batch=MagicMock(), + ) + control = _InitialJointState("arena/tool", None) + control.initialize_model(object(), result) + control.initialize(SimpleNamespace(result=result)) + initializer.assert_not_called() + result.create_articulation_batch.assert_not_called() + + +@pytest.mark.gpu +def test_initial_joint_seed_accepts_cuda_tensor(): + from embodichain.lab.sim._runtime_controls import _InitialJointState + + if not torch.cuda.is_available(): + pytest.skip("CUDA is unavailable") + control = _InitialJointState("arena/tool", torch.tensor([0.025], device="cuda")) + np.testing.assert_allclose(control.positions, [[0.025]]) diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index 07a6ab7ce9..6ebb890b83 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -34,6 +34,7 @@ import embodichain.lab.visualization as visualization_module from embodichain.lab.sim.cfg import ( ArticulationCfg, + CableObjectCfg, DefaultPhysicsCfg, DenoisingCfg, DLSSCfg, @@ -2078,8 +2079,16 @@ def test_robot_sensor_preflight_precedes_resource_allocation(invalid: str) -> No @pytest.mark.gpu @pytest.mark.requires_sim -@pytest.mark.parametrize("backend", ["default", "newton"]) -@pytest.mark.parametrize("stereo", [False, True]) +@pytest.mark.parametrize( + "backend,stereo", + [ + ("default", False), + ("default", True), + ("newton", False), + ("newton", True), + ("newton_cable", False), + ], +) def test_robot_mounted_camera_backend_lifecycle( tmp_path: Path, backend: str, stereo: bool ) -> None: @@ -2107,7 +2116,9 @@ def test_robot_mounted_camera_backend_lifecycle( device="cuda:0", use_cuda_graph=False, num_substeps=1, - solver_cfg={"solver_type": "mujoco_warp"}, + solver_cfg={ + "solver_type": "dexuni" if backend == "newton_cable" else "mujoco_warp" + }, ) ) sim = SimulationManager( @@ -2134,6 +2145,15 @@ def test_robot_mounted_camera_backend_lifecycle( sensors=[camera_cfg], ) ) + if backend == "newton_cable": + sim.add_cable_object( + CableObjectCfg( + uid="wire", + points=[[0, 0, 0], [0, 0, 0.1], [0, 0, 0.2]], + radius=0.01, + init_pos=(0.3, 0, 1), + ) + ) sim.prepare() camera = sim.get_sensor("mounted") assert camera.is_attached @@ -2143,6 +2163,14 @@ def test_robot_mounted_camera_backend_lifecycle( camera.get_local_pose(to_matrix=True)[0].cpu(), camera_cfg.extrinsics.transformation, ) + if backend == "newton_cable": + with pytest.raises(NotImplementedError, match="topology changes"): + sim.remove_asset("robot") + assert sim.get_asset("robot") is robot + assert sim.get_sensor("mounted") is camera and camera.is_attached + camera.update() + assert camera.get_data()["color"].shape == (1, 24, 32, 4) + return before = camera.get_arena_pose(to_matrix=True).clone() robot.set_qpos(torch.tensor([[0.5]], device=sim.device), target=False) sim.sync_render_state() @@ -2791,7 +2819,9 @@ def _make_camera_attachment_manager(num_envs: int = 2) -> SimulationManager: bind=MagicMock(), ) sim.physics = SimpleNamespace( - prepare_spawn_runtime=MagicMock(), sync_render_state=MagicMock() + _prepare_scene_build=MagicMock(), + prepare_spawn_runtime=MagicMock(), + sync_render_state=MagicMock(), ) sim._default_plane = object() sim._ready_spawn_topology_revision = result.topology_revision @@ -3115,7 +3145,7 @@ def fake_save_window_record_worker( def test_reset_objects_state_includes_deformable_assets() -> None: sim = object.__new__(SimulationManager) - sim.physics = SimpleNamespace(name="default") + sim.physics = SimpleNamespace(name="default", _reset_scene_state=lambda *_: None) sim._robots = {} sim._articulations = {} sim._rigid_objects = {} @@ -3136,6 +3166,25 @@ def test_reset_objects_state_includes_deformable_assets() -> None: assert sim.simulation_time == 1.25 +def test_scene_reset_delegates_physical_ownership_to_backend() -> None: + sim = object.__new__(SimulationManager) + scene_reset = MagicMock(return_value=[1]) + sim.physics = SimpleNamespace(name="third_party", _reset_scene_state=scene_reset) + robot, cloth, light, camera = (MagicMock() for _ in range(4)) + sim._robots = {"robot": robot} + sim._deformable_objects = {"cloth": cloth} + sim._lights = {"light": light} + sim._sensors = {"camera": camera} + + sim.reset_objects_state(env_ids=[1], excluded_sensor_uids=["camera"]) + + scene_reset.assert_called_once_with([1], set()) + robot.reset.assert_not_called() + cloth.reset.assert_not_called() + light.reset.assert_called_once_with([1]) + camera.reset.assert_not_called() + + @pytest.mark.parametrize("solver_type", ["mujoco_warp", "dexuni"]) def test_newton_reset_clears_all_articulations_in_selected_worlds_together( solver_type, @@ -3150,7 +3199,9 @@ def test_newton_reset_clears_all_articulations_in_selected_worlds_together( "robot": ("robot_0", "robot_1"), "arm": ("arm_0", "arm_1"), } - sim.physics = SimpleNamespace(name="newton", solver_type=solver_type) + sim.physics = SimpleNamespace( + name="newton", solver_type=solver_type, _reset_scene_state=lambda *_: None + ) sim.num_envs = 2 sim._spawn_scene = SimpleNamespace( builder=SimpleNamespace(is_finalized=True, result=spawn_result), @@ -3180,7 +3231,9 @@ def test_newton_reset_rejects_excluding_one_articulation_from_selected_worlds( sim = object.__new__(SimulationManager) robot = MagicMock() articulation = MagicMock() - sim.physics = SimpleNamespace(name="newton", solver_type=solver_type) + sim.physics = SimpleNamespace( + name="newton", solver_type=solver_type, _reset_scene_state=lambda *_: None + ) sim._robots = {"robot": robot} sim._articulations = {"arm": articulation} sim._rigid_objects = {} @@ -3204,7 +3257,9 @@ def test_newton_non_mujoco_reset_allows_excluding_an_articulation() -> None: native_batch = MagicMock() spawn_result = SimpleNamespace(create_articulation_batch=MagicMock()) spawn_result.create_articulation_batch.return_value = native_batch - sim.physics = SimpleNamespace(name="newton", solver_type="xpbd") + sim.physics = SimpleNamespace( + name="newton", solver_type="xpbd", _reset_scene_state=lambda *_: None + ) sim.num_envs = 2 sim._spawn_scene = SimpleNamespace( builder=SimpleNamespace(is_finalized=True, result=spawn_result), @@ -3919,7 +3974,7 @@ def after_substep(_dt: float) -> None: def test_reset_fluid_selects_rows_and_honors_exclusions(): sim = object.__new__(SimulationManager) - sim.physics = SimpleNamespace(name="default") + sim.physics = NewtonPhysicsBackend(sim) sim.num_envs = 2 for name in ( "_robots", diff --git a/tests/test_release_metadata.py b/tests/test_release_metadata.py index b90ef0c62b..b4ee412065 100644 --- a/tests/test_release_metadata.py +++ b/tests/test_release_metadata.py @@ -178,6 +178,29 @@ def test_project_metadata_has_no_direct_dependencies() -> None: assert direct_dependencies == [] +@pytest.mark.parametrize( + ("version", "supported"), + [ + ("0.6.0.dev11491", False), + ("0.6.0rc1", True), + ], +) +def test_dexsim_dependency_selects_cable_release_wheel( + version: str, supported: bool +) -> None: + requirements = [ + Requirement(value) + for value in _read_project_metadata()["project"]["dependencies"] + ] + dexsim_requirement = next( + requirement + for requirement in requirements + if requirement.name.replace("_", "-") == "dexsim-engine" + ) + + assert dexsim_requirement.specifier.contains(version) is supported + + def test_wheel_metadata_accepts_index_dependencies(tmp_path: Path) -> None: wheel_path = _write_test_wheel( tmp_path,