diff --git a/agent_context/topics/simulation-system/deformables.md b/agent_context/topics/simulation-system/deformables.md index 165ad49df..db52a5268 100644 --- a/agent_context/topics/simulation-system/deformables.md +++ b/agent_context/topics/simulation-system/deformables.md @@ -108,8 +108,14 @@ Focused coverage: `tests/sim/test_deformable_cfg.py`, `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. +and binds the same facade in place during `prepare()`. `add_usd(UsdImportCfg)` +also returns a `CableObject` per imported cable under its source resource key, +with the same UID lookup, segment state and reset as a procedural declaration. +Cable geometry, placement and visuals come from the parsed `CableDesc`. +A sparse `asset_cfgs` entry keyed by the simulation curve prim can override +the UID, `fixed_segments`, `collision_filter_span` and individual `attrs` physics and +contact fields before declaration. Omitted fields retain the parsed values. +Cable declarations require DexUni on CUDA with gradient mode disabled. 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 diff --git a/embodichain/lab/sim/cfg/usd.py b/embodichain/lab/sim/cfg/usd.py index 72a9b6e7f..41ee08a67 100644 --- a/embodichain/lab/sim/cfg/usd.py +++ b/embodichain/lab/sim/cfg/usd.py @@ -74,7 +74,10 @@ class UsdImportCfg: In sparse mappings with ``auto`` placement, explicit ``init_pos`` or ``init_rot`` keys select the configured pose, including zero values. - Omitted pose fields retain source placement. + Omitted pose fields retain source placement. For cables, sparse mappings + accept ``uid``, ``fixed_segments``, ``collision_filter_span`` and fields of + ``CablePhysicsCfg`` under ``attrs``. Overrides apply to all curves on + the targeted prim, preserving its authored geometry and placement. """ robot_cfgs: dict[str, RobotCfg] = field(default_factory=dict) diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index 24bfa1d7c..358d4648b 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -3065,7 +3065,13 @@ def add_usd( ) -> ( UsdImportResult | dict[ - str, VisualObject | RigidObject | Articulation | Robot | DeformableObject + str, + VisualObject + | RigidObject + | Articulation + | Robot + | DeformableObject + | CableObject, ] ): """Declare a USD import through a validated configuration recipe. @@ -3094,7 +3100,8 @@ def add_usd( asset_cfgs: Legacy per-asset overrides; use ``cfg.asset_cfgs``. Returns: - Source-path mapping of declared facades and imported resource UIDs. + Source-path mapping of declared facades, including USD cables, + and imported resource UIDs. Legacy argument forms retain the mutable asset dictionary result. """ from .cfg.usd import _normalize_usd_import @@ -3146,6 +3153,7 @@ def add_usd( ), "soft_object": self._deformable_objects, "cloth_object": self._deformable_objects, + "cable_object": self._cable_objects, }[kind] registry[descriptor.name] = facade assets[source_path] = facade @@ -3644,22 +3652,8 @@ 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.") + def _require_cable_declaration(self) -> None: + """Check the shared runtime requirements for declaring cables.""" if not self.physics.supports_cables: raise NotImplementedError("Cable objects require the Newton backend.") if self.device.type != "cuda": @@ -3676,6 +3670,24 @@ def add_cable_object(self, cfg: CableObjectCfg) -> CableObject: raise NotImplementedError( "Declare cable objects before scene finalization." ) + + 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.") + self._require_cable_declaration() 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( diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index 6e78c429d..30b3a410f 100644 --- a/embodichain/lab/sim/spawn/descriptors.py +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -809,6 +809,22 @@ def _explicit_tet_mesh_from_cfg( } +def _cable_contact_spec(attrs: CablePhysicsCfg) -> _RigidPhysicsSpec: + if not isinstance(attrs, CablePhysicsCfg): + raise TypeError("Cable attrs must be CablePhysicsCfg.") + if isinstance(attrs.collision_props, DefaultCollisionPropertiesCfg): + raise NotImplementedError( + "Cable contact properties require common or Newton configuration." + ) + return _resolve_rigid_physics( + RigidBodyPhysicsCfg( + collision_props=attrs.collision_props, + material_props=attrs.material_props, + ), + newton_solver_type="dexuni", + ) + + def cable_desc_from_cfg( cfg: CableObjectCfg, *, @@ -835,19 +851,7 @@ def cable_desc_from_cfg( ) 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", - ) + contact = _cable_contact_spec(cfg.attrs) collision = _compile_newton_collision( contact, newton_solver_type="dexuni", author_shape_defaults=True ) diff --git a/embodichain/lab/sim/spawn/usd_import.py b/embodichain/lab/sim/spawn/usd_import.py index 4ca831078..6731eb18e 100644 --- a/embodichain/lab/sim/spawn/usd_import.py +++ b/embodichain/lab/sim/spawn/usd_import.py @@ -19,14 +19,16 @@ from __future__ import annotations from collections import Counter +from collections.abc import Mapping from copy import deepcopy -from dataclasses import dataclass, field, fields +from dataclasses import dataclass, field, fields, replace from typing import TYPE_CHECKING, Any import numpy as np from dexsim.spawn import ( ArticulationDesc, + CableDesc, ConstraintDesc, MeshObjectDesc, SoftBodyDesc, @@ -36,6 +38,9 @@ from embodichain.lab.sim.cfg import ( ArticulationCfg, ArticulationRootPropertiesCfg, + CableAttachmentCfg, + CableObjectCfg, + CablePhysicsCfg, RigidObjectCfg, RobotCfg, VisualObjectCfg, @@ -46,6 +51,7 @@ ) from embodichain.lab.sim.objects import ( Articulation, + CableObject, RigidObject, Robot, VisualObject, @@ -54,10 +60,17 @@ ) from embodichain.lab.sim.sensors import Camera, CameraCfg, StereoCamera, StereoCameraCfg from embodichain.lab.sim.sensors.registry import _resolve_sensor_factory -from .descriptors import _pose_from_cfg, configure_articulation_desc +from .descriptors import ( + _cable_contact_spec, + _compile_newton_collision, + _pose_from_cfg, + _required_uid, + configure_articulation_desc, +) from .usd import ( _configure_articulation_usd, _configure_rigid_usd, + _overlay_optional_properties, _parse_usd_source, _source_pose, _set_resolved_usd_pose, @@ -91,6 +104,67 @@ def _source_path(descriptor: Any) -> str: return usd.prim_path if usd is not None and usd.prim_path else descriptor.name +def _usd_cable_cfg( + descriptor: CableDesc, overrides: Mapping[str, Any] | None +) -> CableObjectCfg: + """Apply sparse cable properties before declaring the descriptor and its facade.""" + if overrides is None: + overrides = {} + if not isinstance(overrides, Mapping): + raise TypeError("USD cable overrides must be a sparse mapping.") + unknown = overrides.keys() - { + "uid", + "attrs", + "fixed_segments", + "collision_filter_span", + } + if unknown: + raise ValueError(f"Unknown USD cable override fields: {sorted(unknown)}.") + attrs = overrides.get("attrs", {}) + if not isinstance(attrs, Mapping): + raise TypeError("USD cable attrs must be a sparse mapping.") + parsed_physics = { + item.name: getattr(descriptor.physics, item.name) + for item in fields(descriptor.physics) + } + physics_cfg = CablePhysicsCfg(**{**parsed_physics, **attrs}) + if overrides: + if "uid" in overrides: + descriptor.name = _required_uid(overrides["uid"], "Cable object") + # Preserve the difference between an omitted coefficient and an explicit zero/None. + descriptor.physics = replace( + descriptor.physics, + **{key: attrs[key] for key in attrs.keys() & parsed_physics.keys()}, + ) + contact = _cable_contact_spec(physics_cfg) + descriptor.collision = _overlay_optional_properties( + descriptor.collision, + _compile_newton_collision(contact, newton_solver_type="dexuni"), + ) + if contact.collision_enabled is not None: + descriptor.enable_collision = contact.collision_enabled + for name in ("fixed_segments", "collision_filter_span"): + if name in overrides: + setattr(descriptor, name, overrides[name]) + # Revalidate indices, attachments and thickness against the source + # geometry before publishing any part of this freshly parsed scene. + descriptor.__post_init__() + + frames = descriptor.segment_quaternions + return CableObjectCfg( + uid=descriptor.name, + points=descriptor.points, + radius=descriptor.radius, + init_local_pose=descriptor.pose, + segment_quaternions=None if frames is None else frames[:, [1, 2, 3, 0]], + closed=descriptor.closed, + fixed_segments=descriptor.fixed_segments, + attrs=physics_cfg, + collision_filter_span=descriptor.collision_filter_span, + render_mode=descriptor.render_mode, + ) + + def _compile_usd_import( manager: SimulationManager | None, recipe: UsdImportCfg, @@ -143,6 +217,8 @@ def _compile_usd_import( manager._spawn_scene._require_compatible_particles( cables=bool(source.cables), particles=bool(source.particle_sets) ) + if source.cables: + manager._require_cable_declaration() 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()) @@ -163,7 +239,12 @@ def _compile_usd_import( if item.usd is not None and item.usd.prim_path else item.name ) - for item in (*source.mesh_objects, *source.articulations, *source.particle_sets) + for item in ( + *source.mesh_objects, + *source.articulations, + *source.particle_sets, + *source.cables, + ) } overlap = robot_cfgs.keys() & asset_cfgs.keys() missing = (robot_cfgs.keys() | asset_cfgs.keys()) - source_paths @@ -251,6 +332,21 @@ def _compile_usd_import( and descriptor.physics is None and not descriptor.collisions ) + if isinstance(descriptor, CableDesc): + if root_pose is not None: + descriptor.pose = root_pose @ descriptor.pose + cfg = _usd_cable_cfg(descriptor, configured) + if descriptor.name in occupied: + raise ValueError(f"USD asset UID already exists: {descriptor.name!r}.") + occupied.add(descriptor.name) + renamed[original_name] = descriptor.name + facade = ( + None if manager is None else CableObject(cfg, device=manager.device) + ) + declarations.append( + (resource_keys[id(descriptor)], "cable_object", descriptor, facade) + ) + continue if configured is not None and not isinstance( descriptor, (MeshObjectDesc, ArticulationDesc, SoftBodyDesc, ClothDesc) ): @@ -547,6 +643,23 @@ def renamed_endpoint(endpoint: str) -> str: owner, separator, suffix = endpoint.partition("/") return renamed.get(owner, owner) + separator + suffix + for _, kind, descriptor, facade in declarations: + if kind == "cable_object": + for name in ("start_attachment", "end_attachment"): + anchor = getattr(descriptor, name) + if anchor is not None: + anchor.target = renamed.get(anchor.target, anchor.target) + if facade is not None: + setattr( + facade.cfg, + name, + CableAttachmentCfg( + target=anchor.target, + position=anchor.position.copy(), + link_name=anchor.link_name, + ), + ) + for constraint in source.constraints: if constraint.body0 is not None: constraint.body0 = renamed_endpoint(constraint.body0) @@ -577,6 +690,7 @@ def renamed_endpoint(endpoint: str) -> str: public_kinds = { "soft_object": "volume_deformable", "cloth_object": "surface_deformable", + "cable_object": "cable", } resource_types = { path: "robot" if path in robot_paths else public_kinds.get(kind, kind) @@ -586,7 +700,6 @@ def renamed_endpoint(endpoint: str) -> str: ("cameras", "camera"), ("lights", "light"), ("records", "record"), - ("cables", "cable"), ("constraints", "constraint"), ): resource_types.update( diff --git a/embodichain/lab/sim/spawn/usd_result.py b/embodichain/lab/sim/spawn/usd_result.py index dd562cb59..198281319 100644 --- a/embodichain/lab/sim/spawn/usd_result.py +++ b/embodichain/lab/sim/spawn/usd_result.py @@ -25,6 +25,7 @@ if TYPE_CHECKING: from embodichain.lab.sim.objects import ( Articulation, + CableObject, DeformableObject, RigidObject, Robot, @@ -35,11 +36,15 @@ class UsdImportResult( - Mapping[str, "VisualObject | RigidObject | Articulation | Robot | DeformableObject"] + Mapping[ + str, + "VisualObject | RigidObject | Articulation | Robot | DeformableObject | CableObject", + ] ): """Map source resource keys to stable, declared EmbodiChain facades. - Facades bind in place at ``SimulationManager.prepare()``. Resource maps + Facades, including imported cables, bind in place at + ``SimulationManager.prepare()``. Resource maps include native-only scene resources and constraints; their values are logical Spawn UIDs, not native handles or builder indices. A unique source keeps its prim path as the resource key. When a source prim produces several @@ -51,7 +56,13 @@ class UsdImportResult( def __init__( self, assets: Mapping[ - str, VisualObject | RigidObject | Articulation | Robot | DeformableObject + str, + VisualObject + | RigidObject + | Articulation + | Robot + | DeformableObject + | CableObject, ], *, resource_uids: Mapping[str, str], @@ -83,7 +94,14 @@ def __init__( def __getitem__( self, source_path: str - ) -> VisualObject | RigidObject | Articulation | Robot | DeformableObject: + ) -> ( + VisualObject + | RigidObject + | Articulation + | Robot + | DeformableObject + | CableObject + ): return self._assets[source_path] def __iter__(self) -> Iterator[str]: diff --git a/scripts/validation/accept_usd_import.py b/scripts/validation/accept_usd_import.py index dd7b09a75..37429f7ec 100644 --- a/scripts/validation/accept_usd_import.py +++ b/scripts/validation/accept_usd_import.py @@ -169,7 +169,10 @@ def _accept_case(case: dict[str, Any], output: Path) -> dict[str, Any]: "An imported entity was lost or a source path is duplicated." ) expected_assets = ( - len(raw.mesh_objects) + len(raw.articulations) + len(raw.particle_sets) + len(raw.mesh_objects) + + len(raw.articulations) + + len(raw.particle_sets) + + len(raw.cables) ) record["resources"] = dict(imported.resource_uids) record["resource_source_paths"] = dict(imported.resource_source_paths) @@ -286,6 +289,12 @@ def check_state() -> None: ) elif type(asset).__name__ in ("Articulation", "Robot"): value = asset.get_qpos() + elif type(asset).__name__ == "CableObject": + value = asset.data.segment_state_w + if value.numel() == 0: + raise AssertionError( + f"Cable {asset.uid} has no simulation segments." + ) else: value = asset.get_local_pose() if not torch.isfinite(value).all(): diff --git a/tests/gym/envs/test_cable_recording_cuda.py b/tests/gym/envs/test_cable_recording_cuda.py index 81c39b5ea..41fd6750a 100644 --- a/tests/gym/envs/test_cable_recording_cuda.py +++ b/tests/gym/envs/test_cable_recording_cuda.py @@ -30,7 +30,7 @@ pytestmark = [pytest.mark.subprocess_sim, pytest.mark.gpu] -def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( +def test_usd_and_procedural_cables_record_and_replay_across_arenas( tmp_path: Path, ) -> None: # A fresh process preserves NoRender without the shared test engine's @@ -44,8 +44,32 @@ def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( """, encoding="utf-8", ) + usd = tmp_path / "wire.usda" + usd.write_text( + """#usda 1.0 +( + defaultPrim = "World" + metersPerUnit = 1 + upAxis = "Z" +) + +def Xform "World" +{ + def BasisCurves "wire" ( + prepend apiSchemas = ["PhysicsCurvesDeformableSimAPI"] + ) + { + int[] curveVertexCounts = [3] + point3f[] points = [(0, 0, 1), (0.1, 0, 1), (0.2, 0, 1)] + uniform token type = "linear" + uniform token wrap = "nonperiodic" + } +} +""", + encoding="utf-8", + ) result = subprocess.run( - [sys.executable, str(Path(__file__).resolve()), str(urdf)], + [sys.executable, str(Path(__file__).resolve()), str(urdf), str(usd)], env=os.environ.copy(), capture_output=True, text=True, @@ -54,7 +78,7 @@ def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( assert result.returncode == 0, result.stdout + result.stderr -def _run_recording(urdf: str) -> None: +def _run_recording(urdf: str, usd: str) -> None: import torch from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg @@ -66,10 +90,13 @@ def _run_recording(urdf: str) -> None: NewtonPhysicsCfg, RenderCfg, RobotCfg, + UsdImportCfg, + UsdSourceCfg, ) cfg = EmbodiedEnvCfg( num_envs=2, + enable_ground_plane=True, max_episode_steps=4, sim_steps_per_control=1, ignore_terminations=True, @@ -90,6 +117,11 @@ def _run_recording(urdf: str) -> None: solver_cfg={"solver_type": "dexuni"}, ), ), + usd_scene=UsdImportCfg( + name="scene", + source=UsdSourceCfg(file_path=usd), + asset_cfgs={"/World/wire": {"uid": "wire", "collision_filter_span": 2}}, + ), robot=RobotCfg( uid="slider", fpath=urdf, @@ -98,12 +130,11 @@ def _run_recording(urdf: str) -> None: ), cable_object=[ CableObjectCfg( - uid=uid, - points=[[0.1 * index, 0, 0] for index in range(segments + 1)], + uid="long_wire", + points=[[0.1 * index, 0, 0] for index in range(4)], radius=0.005, - init_pos=(0.0, y, 1.0), + init_pos=(0.0, 0.5, 1.0), ) - for uid, segments, y in (("wire", 2, 0.0), ("long_wire", 3, 0.5)) ], observations={ "wire": ObservationCfg( @@ -121,11 +152,16 @@ def _run_recording(urdf: str) -> None: env = None try: env = EmbodiedEnv(cfg) + cable = env.usd_scene_import["/World/wire"] + assert cable is env.sim.get_cable_object("wire") + assert cable.cfg.collision_filter_span == 2 assert not env.sim.has_native_renderer obs, _ = env.reset(seed=73) manager = env.observation_manager assert manager is not None and manager.has_history cables = {uid: env.sim.get_cable_object(uid) for uid in cfg.trajectory_uids} + assert cables["wire"].data.num_segments == 2 + assert cables["long_wire"].data.num_segments == 3 fields = {uid: f"cables/{uid}/segment_state_w" for uid in cables} episodes = env.episode_context.episode_ids expected = { @@ -225,6 +261,12 @@ def _run_recording(urdf: str) -> None: assert saved["meta"]["arena_origins_w"] == [source_origin.tolist()] for uid, field in fields.items(): assert saved["raw_observations"]["fields"][field]["values"].shape[1] == 4 + torch.testing.assert_close( + saved["terminal_states"]["cables", uid, "segment_state_w"].cpu(), + latest.values[field][[1]], + rtol=0, + atol=0, + ) env.close(exit_process=False) env = None @@ -258,4 +300,4 @@ def _run_recording(urdf: str) -> None: if __name__ == "__main__": faulthandler.enable() faulthandler.dump_traceback_later(120) - _run_recording(sys.argv[1]) + _run_recording(sys.argv[1], sys.argv[2]) diff --git a/tests/sim/objects/test_cable_object.py b/tests/sim/objects/test_cable_object.py index d8a4d98b3..8c78e4cc6 100644 --- a/tests/sim/objects/test_cable_object.py +++ b/tests/sim/objects/test_cable_object.py @@ -390,11 +390,10 @@ def add_cable(): source=UsdSourceCfg(file_path=str(_write_cable_usd(tmp_path))), ) ) - assert dict(imported) == {} + cable = imported["/Cable"] assert list(imported.resource_types.values()) == ["cable"] assert len(builder.cables) == 1 - assert not sim._cable_objects and not scene._assets - return None + return cable if cable_first: first = add_cable() diff --git a/tests/sim/objects/test_cable_object_cuda.py b/tests/sim/objects/test_cable_object_cuda.py index 2a25e8594..7904112f5 100644 --- a/tests/sim/objects/test_cable_object_cuda.py +++ b/tests/sim/objects/test_cable_object_cuda.py @@ -226,6 +226,92 @@ def test_imported_particles_survive_rejected_cable(self, tmp_path: Path) -> None torch.testing.assert_close(reset[0], moved[0], rtol=0, atol=0) torch.testing.assert_close(reset[1], initial[1]) + def test_usd_cable_state_selective_reset_and_viser(self, tmp_path: Path) -> None: + from embodichain.lab.visualization import SceneExporter, VisualizationCfg + + path = tmp_path / "wire.usda" + path.write_text( + """#usda 1.0 +( + defaultPrim = "World" + metersPerUnit = 1 + upAxis = "Z" +) +def Xform "World" { + def BasisCurves "Wire" ( + prepend apiSchemas = ["PhysicsCurvesDeformableSimAPI"] + ) { + int[] curveVertexCounts = [3] + point3f[] points = [(0, 0, 0), (0.1, 0, 0), (0.3, 0, 0)] + uniform token type = "linear" + uniform token wrap = "nonperiodic" + float[] physics:thicknesses = [0.02] + token physics:thicknesses:elementType = "constant" + double3 xformOp:translate = (0.25, 0.5, 1) + uniform token[] xformOpOrder = ["xformOp:translate"] + } +} +""", + encoding="utf-8", + ) + imported = self.sim.add_usd( + UsdImportCfg(name="wire", source=UsdSourceCfg(file_path=str(path))) + ) + key = "/World/Wire" + cable = imported[key] + uid = imported.resource_uids[key] + assert self.sim.get_asset(uid) is self.sim.get_cable_object(uid) is cable + assert cable.num_instances == 2 and cable.data is None + + self.sim.prepare() + initial = cable.data.default_segment_state_w + assert initial.shape == (2, 2, 13) + expected_centres = initial.new_tensor([[0.3, 0.5, 1], [0.45, 0.5, 1]]) + torch.testing.assert_close( + initial[..., :3] - self.sim.arena_offsets[:, None, :], + expected_centres.expand(2, 2, 3), + atol=1e-6, + rtol=0, + ) + + self.sim.update(step=3, render_final_step=False) + moved = cable.data.segment_state_w + assert torch.all(moved[..., 2] < initial[..., 2]) + self.sim.reset_objects_state(env_ids=[1]) + reset = cable.data.segment_state_w + torch.testing.assert_close(reset[0], moved[0], rtol=0, atol=0) + torch.testing.assert_close(reset[1], initial[1], rtol=0, atol=0) + + state = reset[[1]].clone() + state[..., 0] += 0.125 + cable.write_segment_state_to_sim(state, env_ids=[1]) + torch.testing.assert_close( + cable.data.segment_state_w[0], moved[0], rtol=0, atol=0 + ) + torch.testing.assert_close(cable.data.segment_state_w[[1]], state) + + exporter = SceneExporter( + self.sim, VisualizationCfg(backend="viser", env_ids=[1]) + ) + manifest = exporter.build_manifest() + frame = exporter.capture(sim_step=3, sim_time=self.sim.simulation_time).frame + nodes = [node for node in manifest.nodes if node.kind == "cable_segment"] + assert len(nodes) == 2 + geometries = { + geometry.geometry_id: geometry for geometry in manifest.geometries + } + for segment_index, node in enumerate(nodes): + assert node.env_id == 1 + assert exporter.resolve_node_target(node.node_id) == (uid, "cable") + index = frame.node_ids.index(node.node_id) + pose = state[0, segment_index].cpu().numpy() + np.testing.assert_allclose(frame.positions[index], pose[:3], atol=1e-6) + np.testing.assert_allclose(frame.wxyz[index], pose[[6, 3, 4, 5]], atol=1e-6) + vertices = geometries[node.geometry_id].vertices + np.testing.assert_allclose( + np.ptp(vertices, axis=0), [0.02, 0.02, (0.12, 0.22)[segment_index]] + ) + def test_attached_articulation_expert_trajectory_repeats_after_reset( self, tmp_path: Path ) -> None: @@ -451,6 +537,11 @@ def test_imported_particles_survive_rejected_cable( "test_imported_particles_survive_rejected_cable", volume_particle_usd.parent ) + def test_usd_cable_state_selective_reset_and_viser(self, tmp_path: Path) -> None: + _assert_cable_case_in_subprocess( + "test_usd_cable_state_selective_reset_and_viser", tmp_path + ) + @pytest.mark.usefixtures("requires_dexuni_two_way") def test_attached_articulation_expert_trajectory_repeats_after_reset( self, tmp_path: Path diff --git a/tests/sim/spawn/test_usd_scene_overrides.py b/tests/sim/spawn/test_usd_scene_overrides.py index 6df102db0..91b361018 100644 --- a/tests/sim/spawn/test_usd_scene_overrides.py +++ b/tests/sim/spawn/test_usd_scene_overrides.py @@ -18,6 +18,7 @@ from __future__ import annotations +from copy import deepcopy from types import SimpleNamespace from unittest.mock import MagicMock @@ -27,6 +28,9 @@ from pxr import Sdf, Usd, UsdGeom, UsdPhysics from dexsim.spawn import ( + CableDesc, + CablePhysicsDesc, + NewtonCollisionDesc, MeshObjectDesc, ConstraintDesc, RigidBodyPhysicsDesc, @@ -43,6 +47,7 @@ from embodichain.lab.sim.sim_manager import SimulationManager from embodichain.lab.sim.spawn.scene import SpawnScene from embodichain.lab.sim.cfg import UsdImportCfg, UsdSourceCfg +from embodichain.lab.sim.objects import CableObject pytestmark = pytest.mark.no_sim @@ -61,6 +66,7 @@ def _manager(): "_rigid_object_groups", "_deformable_objects", "_fluid_objects", + "_cable_objects", "_articulations", ): setattr(manager, name, {}) @@ -898,6 +904,11 @@ def test_curve_prim_resources_keep_all_uids_and_stable_source_keys( ) for namespace in (None, "copy"): manager = _manager() + manager.device = torch.device("cuda:0") + manager.physics = SimpleNamespace( + name="newton", solver_type="dexuni", supports_cables=True + ) + manager.sim_config.physics_cfg = SimpleNamespace(requires_grad=False) result = manager.add_usd( UsdImportCfg( name="scene", @@ -913,11 +924,81 @@ def test_curve_prim_resources_keep_all_uids_and_stable_source_keys( assert dict(result.resource_source_paths) == { key: "/World/Cable" for key in expected_keys } - assert len(result) == 0 + assert list(result) == expected_keys + for key in expected_keys: + cable = result[key] + uid = result.resource_uids[key] + assert isinstance(cable, CableObject) + assert cable is manager.get_cable_object(uid) is manager.get_asset(uid) + assert cable.is_declared and cable.data is None with pytest.raises(TypeError): result.resource_source_paths[expected_keys[0]] = "/Other" +def test_usd_cable_sparse_overrides_preserve_source_geometry_and_material(monkeypatch): + import dexsim.kit.usd + + source = _source() + key = "/World/Wire" + points = np.array([[0, 0, 1], [0.2, 0, 1], [0.55, 0, 1]], dtype=np.float32) + source.cables.append( + CableDesc( + name="wire", + points=points, + radius=0.015, + usd=UsdSourceDesc(prim_path=key), + physics=CablePhysicsDesc(density=321, twist_stiffness=7, bend_damping=2), + collision=NewtonCollisionDesc(mu=0.77), + ) + ) + monkeypatch.setattr(dexsim.kit.usd, "parse_usd", lambda *a, **k: deepcopy(source)) + manager = _manager() + manager.device = torch.device("cuda") + manager.physics = SimpleNamespace( + name="newton", solver_type="dexuni", supports_cables=True + ) + manager.sim_config.physics_cfg = SimpleNamespace(requires_grad=False) + recipe = UsdImportCfg( + name="scene", + source=UsdSourceCfg(file_path="scene.usda"), + asset_cfgs={key: {"fixed_segments": [2]}}, + ) + with pytest.raises(ValueError, match="fixed_segments"): + manager.add_usd(recipe) + manager._spawn_scene.builder.add_scene.assert_not_called() + + recipe.asset_cfgs[key] = { + "uid": "cable", + "fixed_segments": [0], + "collision_filter_span": 2, + "attrs": { + "stretch_stiffness": 1e5, + "bend_damping": 0, + "collision_props": {"contact_offset": 0.01, "rest_offset": 0.0}, + "material_props": {"backend": "newton", "ke": 10000, "kd": 0.1}, + }, + } + imported = manager.add_usd(recipe) + native = manager._spawn_scene.builder.add_scene.call_args.args[0].cables[0] + assert native.name == imported.resource_uids[key] == "cable" + facade = manager.get_cable_object("cable") + assert facade is imported[key] + np.testing.assert_array_equal(native.points, points) + assert native.radius == pytest.approx(0.015) + assert native.fixed_segments == facade.cfg.fixed_segments == (0,) + assert native.collision_filter_span == facade.cfg.collision_filter_span == 2 + for physics in (native.physics, facade.cfg.attrs): + assert physics.stretch_stiffness == 1e5 + assert physics.bend_damping == 0 + assert physics.density == 321 + assert physics.twist_stiffness == 7 + assert native.collision.mu == 0.77 + assert native.collision.margin == 0 + assert native.collision.gap == 0.01 + assert native.collision.ke == 10000 + assert native.collision.kd == 0.1 + + def test_portable_sparse_articulation_pose_matrix_is_decoded(monkeypatch): import json import dexsim.kit.usd