From 75d3c9a3561862c56e58fd676ab7e8c137b88a39 Mon Sep 17 00:00:00 2001 From: xiemenghong Date: Thu, 17 Sep 2026 09:48:58 +0800 Subject: [PATCH 01/27] feat(sim): default Newton rigid simulation to MJVBD V2 --- agent_context/MAP.yaml | 2 +- .../motion-planning/collision-worlds.md | 5 +- .../simulation-system/simulation-system.md | 96 ++++++++++++------- embodichain/lab/sim/cfg/robot.py | 4 +- embodichain/lab/sim/cfg/simulation.py | 32 ++++--- embodichain/lab/sim/objects/articulation.py | 2 +- .../lab/sim/objects/backends/collision.py | 4 +- .../lab/sim/objects/backends/newton.py | 15 ++- embodichain/lab/sim/physics/newton.py | 2 +- embodichain/lab/sim/sim_manager.py | 14 ++- embodichain/lab/sim/spawn/descriptors.py | 10 +- .../trajectory_open_loop_slow.yaml | 29 ++++++ .../manipulation/open_drawer/env.newton.yaml | 18 ++-- .../open_drawer/task.ur5.newton.yaml | 2 +- .../repeated_pick_place/env.newton.yaml | 18 ++-- .../repeated_pick_place/task.ur5.newton.yaml | 2 +- examples/sim/demo/grasp_cup_to_caffe.py | 8 +- examples/sim/demo/scoop_ice.py | 7 +- .../sim/motion/planners/curobo_planner.py | 12 ++- .../motion/trajectory_velocity_tracking.py | 3 +- .../atomic_action/move_held_object.py | 9 +- scripts/tutorials/atomic_action/pour.py | 12 +-- .../tutorials/atomic_action/tutorial_utils.py | 29 +++--- scripts/tutorials/gym/modular_env.py | 2 +- scripts/tutorials/sim/open_drawer.py | 25 +++-- tests/sim/test_cfg.py | 3 +- tests/sim/test_sim_manager_cfg.py | 16 ++-- 27 files changed, 241 insertions(+), 140 deletions(-) create mode 100644 embodichain_tasks/configs/components/execution_policies/trajectory_open_loop_slow.yaml diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 8086c4a5f8..24734f43a8 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -11,7 +11,7 @@ topics: aliases: [simulation system, simulation manager, Newton physics backend, motion module, sim motion, camera attachment, 运动能力模块, 仿真系统, 仿真管理器] keywords: [SimulationManager, SimulationManagerCfg, RenderCfg, DLSSCfg, DLSS, frame_time_delta_ms, offscreen_dlss_enabled, explicit physics stepping, arena, DexSim, PhysicsBackendCfg, - DefaultPhysicsCfg, NewtonPhysicsCfg, AutoSolverCfg, DexUniSolverCfg, DexUni, runtime control, deformable, particle set, + DefaultPhysicsCfg, NewtonPhysicsCfg, AutoSolverCfg, DexUniSolverCfg, DexUni, MJVBDV2SolverCfg, mjvbd_v2, mjvbd2, runtime control, deformable, particle set, ArticulationJointKinematics, get_parent_joint_chain, asset_physics_mode, joint_drive_props, enable_gravity, fixed-cadence playback, quaternion, xyzw] paths: [topics/simulation-system/simulation-system.md] diff --git a/agent_context/topics/motion-planning/collision-worlds.md b/agent_context/topics/motion-planning/collision-worlds.md index ac240779fb..b380bc8898 100644 --- a/agent_context/topics/motion-planning/collision-worlds.md +++ b/agent_context/topics/motion-planning/collision-worlds.md @@ -17,7 +17,10 @@ mapping key is the authoritative logical/source obstacle ID used by the content-cache key, `collision_world_entity_ids`, generated obstacle-name prefix, dynamic update key, and registry validation. Scene generation reads physical collision descriptors through `RigidObject.get_collision_shapes()` and emits a -mixed tensor-backed cuRobo scene. A compound source expands to physical names +mixed tensor-backed cuRobo scene. Geometry snapshots retain authored collision +shapes even when physical contact response is disabled, matching native shape +queries; a moving planner obstacle can therefore avoid exerting impulses on +the robot. A compound source expands to physical names such as `registry_id__shape_0`; cache and full-world identity remain keyed by the unexpanded `registry_id`, and dynamic updates fan out through the physical shapes' local poses. A registry mapping whose source lacks physical collision diff --git a/agent_context/topics/simulation-system/simulation-system.md b/agent_context/topics/simulation-system/simulation-system.md index 04c0a74f5a..8872a5dbd3 100644 --- a/agent_context/topics/simulation-system/simulation-system.md +++ b/agent_context/topics/simulation-system/simulation-system.md @@ -136,6 +136,8 @@ before closing the Spawn result, environment, and World. Default backends use the no-op hook. Newton synchronizes its resolved Warp CUDA device and clears its render bridge while Spawn still owns the parent skeletons, so cached link views cannot be destructed after their native parents. +Asset material and reset-baseline material caches are also cleared before +closing the Scene, so OptiX material wrappers cannot outlive the renderer. After backend materialization, dynamic `RigidObject`, `Articulation`, and `RigidObjectGroup` facades capture their resolved mass, inertia diagonal, and @@ -466,18 +468,52 @@ are both absent. Thus a config-backed launcher preserves the backend default, while an explicit CPU override remains authoritative even for Newton. Environment tensors derive from `sim.device`; do not introduce a second tensor-device selector. -Leaving `NewtonPhysicsCfg.solver_cfg=None` preserves DexSim's -`AutoSolverCfg` default. A DexSim build exporting `AutoSolverCfg` is required; -EmbodiChain does not substitute a concrete solver. DexSim resolves that -placeholder from the complete Spawn scene during finalization: rigid-only -scenes select XPBD, scenes with an articulation select MuJoCo Warp, and -supported particle families select their matching particle/deformable solver. -A mapping with `solver_type: auto` or -`class_type: AutoSolverCfg` is the explicit equivalent. Gradient mode must -still select `semi_implicit` explicitly because AutoSolver does not choose a -differentiable solver. Before finalization, EmbodiChain treats `auto` as -unresolved; after finalization, `NewtonPhysicsBackend.solver_type` reads the -concrete type from DexSim's World-owned backend. +`NewtonPhysicsCfg()` selects `mjvbd_v2` by default for rigid bodies and +articulations. V2 owns collision detection, so conversion omits the external +pipeline even when `collision_cfg` contains its default settings. Other +solvers retain the external pipeline unless explicitly disabled. +Explicit `solver_cfg=None`, `solver_type: auto`, or `class_type: AutoSolverCfg` +opts into DexSim's scene-aware selection: rigid-only scenes select XPBD, +articulation scenes select MuJoCo Warp, and supported particle families select +their matching solver. Gradient mode still requires explicit `semi_implicit`. +Before finalization, explicit `auto` is unresolved; after finalization, +`NewtonPhysicsBackend.solver_type` reads the concrete World-owned solver. +Explicit rigid/articulation V2 configurations select `mjvbd_v2` (aliases +`mjvbd2`, `mjvbdv2`, or `MJVBDV2SolverCfg`). Keep `joint_mode: dynamic` for +ordinary robot control, put existing MuJoCo options under `mujoco_options`, +and set `collision_cfg: null` because V2 owns collision detection. Joint +targets, state access, mimic compliance, and whole-world articulation reset +use the existing Scene/Batch path. V2's compact MuJoCo indices are translated +by DexSim for contact queries and by its index map for mimic equality rows. +Runtime FK updates only MuJoCo-owned articulation trees; replaying FK for +VBD-owned FREE joints would overwrite simulated rigid-body poses with stale +generalized coordinates. Reset completeness checks count Scene articulations, +not the internal FREE-joint trees used to represent standalone rigid bodies. +The pure MuJoCo branch exposes contact geometry, impulses, and friction on +CUDA. VBD contacts expose geometry without MuJoCo impulse reporting. + +V2 selects MuJoCo for articulations with active scalar joints and VBD for +unselected free rigid bodies. Its current mixed dynamic branch is one-way: +VBD contacts do not push back on the MuJoCo robot, and MuJoCo contacts are +disabled in that branch. Therefore switching an existing locomotion or +manipulation scene to V2 does not by itself guarantee equivalent contact +dynamics. In particular, adding a VBD-owned free rigid body switches a floating +robot out of the articulation-only branch and removes its MuJoCo ground +support, even if the new body is far away. Explicit `mujoco_articulations` or +`mujoco_articulation_paths` can select complete joint trees; these are ownership +choices, not automatic EmbodiChain rewrites. This integration does not extend +deformable support. + +The UR5 `repeated_pick_place` and `open_drawer` deployments provide +`task.ur5.newton.yaml` and matching `env.newton.yaml` components using V2. They share the +existing embodiment, controller gains, and Task Programs, with the timed +`trajectory_open_loop_slow.yaml` execution policy (80 control points). The +40-point drawer reference allocates only five samples to pulling; at 25 Hz its +0.2-second pull is shorter than the time needed for 18 cm at the drawer's +authored 0.35 m/s speed limit. Evaluate measured lift, placement, and drawer +travel separately from projected Task Program completion; a completed program +alone is not evidence of physical success. + Explicit coupled articulation/deformable configurations use `dexuni` or `DexUniSolverCfg`. DexUni owns collision detection and its particle-shape contacts, so explicit configurations set `collision_cfg=None` instead of @@ -491,28 +527,16 @@ The `open_drawer.py` tutorial combines this option with 20 Newton substeps per 10 ms control step, while keeping its authored robot gains, collision geometry, pull trajectory, success criteria, and push trajectory identical to Default. Atomic-action tutorials configure their shared Newton simulation in -`scripts/tutorials/atomic_action/tutorial_utils.py` with 20 solver substeps -per 10 ms physics step (0.5 ms solver intervals). They select `mujoco_warp` with -`use_mujoco_contacts=True` and set the external `collision_cfg` to `None`. -MuJoCo-Warp then generates and solves contacts internally at every solver -substep; external-pipeline settings such as `update_interval`, contact -reduction, and the external `rigid_contact_max` do not apply. DexSim derives -the native per-world contact capacity from the finalized scene, avoiding the -oversized fixed buffers formerly inherited from external-pipeline examples. -The shared factory leaves the Default backend configuration unchanged. For -parallel-jaw manipulation, the tutorial and packaged task components add a -Newton-only contact overlay (`condim=4`, `ke=4e4`, `kd=4e2`, torsional friction -`0.1`, rolling friction `0.01`) to the gripper and directly manipulated object -contact links. `condim=4` activates torsional friction in MuJoCo-Warp; rolling -friction is retained for solvers/`condim=6` that support it. This is a scoped -contact/force-closure candidate profile, not a replacement for Newton's scene-aware -`AutoSolverCfg`; rigid-only scenes still resolve to XPBD and articulation -scenes to MuJoCo-Warp. Keep -backend-specific overlays sparse so the Default adapter continues to use its -existing portable contact envelope. -The package dependency must identify the exact DexSim dev build containing -this API; a base `==0.4.3` requirement also accepts older local-version wheels -that do not export `AutoSolverCfg` and is therefore insufficient. +`scripts/tutorials/atomic_action/tutorial_utils.py` with `mjvbd_v2`, retaining +20 solver substeps per 10 ms physics step (0.5 ms solver intervals) and the +existing articulation tuning under `mujoco_options`. The direct drawer +tutorial and rigid manipulation examples use the same native V2 ownership. +The shared factory leaves the Default backend configuration unchanged. +Parallel-jaw manipulation keeps its Newton-only contact overlay (`condim=4`, +`ke=4e4`, `kd=4e2`, torsional friction `0.1`, rolling friction `0.01`) on the +gripper and manipulated objects. MuJoCo options affect articulation-owned +contacts; free rigid-body contacts use VBD and need not have identical effects. +Explicit cloth, softbody, and differentiable examples keep their selected solvers. Newton's `suppress_warp_kernel_logs=True` suppresses Warp's one-time runtime banner plus module compile/load chatter during manager startup, build, facade initialization, and physics updates, then restores the process-wide setting. @@ -721,6 +745,10 @@ only the generic `newton` and `default` alternatives are eligible; do not guess a solver-specific preset before DexSim has inspected the complete scene. This is the only robot preset selection boundary; do not add a second backend selector to robot configs. +For explicit `mjvbd_v2`, preset priority is `newton_mjvbd_v2`, then +`newton_mujoco_warp` / `newton_mjwarp`, then generic `newton` / `default`. +This preserves established articulation drive tuning while allowing a V2 +override. File-backed rigid objects and articulations share one source-independent physics policy: `asset_physics_mode="preserve"` keeps properties resolved from diff --git a/embodichain/lab/sim/cfg/robot.py b/embodichain/lab/sim/cfg/robot.py index c26dac1325..13f63395d4 100644 --- a/embodichain/lab/sim/cfg/robot.py +++ b/embodichain/lab/sim/cfg/robot.py @@ -349,7 +349,7 @@ def resolve( solver_type = str( solver_cfg.get("solver_type") or solver_cfg.get("class_type") - or "auto" + or "mjvbd_v2" ) else: solver_type = str(getattr(solver_cfg, "solver_type")) @@ -357,6 +357,8 @@ def resolve( solver_candidates = [] if solver_type != "auto": solver_candidates.append(f"newton_{solver_type}") + if solver_type == "mjvbd_v2": + solver_candidates.extend(("newton_mujoco_warp", "newton_mjwarp")) if solver_type == "mujoco_warp": solver_candidates.append("newton_mjwarp") candidates = (*solver_candidates, "newton", "default") diff --git a/embodichain/lab/sim/cfg/simulation.py b/embodichain/lab/sim/cfg/simulation.py index cb0803e1f6..5ace2c3ed1 100644 --- a/embodichain/lab/sim/cfg/simulation.py +++ b/embodichain/lab/sim/cfg/simulation.py @@ -530,14 +530,16 @@ class NewtonPhysicsCfg(PhysicsBackendCfg): Genuine Newton/Warp warnings and errors are not suppressed. """ - solver_cfg: Mapping[str, Any] | NewtonSolverCfg | None = None + solver_cfg: Mapping[str, Any] | NewtonSolverCfg | None = field( + default_factory=lambda: {"solver_type": "mjvbd_v2"} + ) """Optional Newton solver configuration. A mapping is converted to the matching DexSim Newton solver config. Include ``solver_type`` or ``class_type`` to select the solver, then add any - parameters accepted by that DexSim solver config. If omitted, EmbodiChain - preserves DexSim's scene-aware ``AutoSolverCfg`` default. A DexSim build - exporting ``AutoSolverCfg`` is required; no concrete-solver fallback is used. + parameters accepted by that DexSim solver config. The default is + ``mjvbd_v2`` for rigid bodies and articulations. Explicit ``None`` or + ``solver_type="auto"`` preserves DexSim's scene-aware ``AutoSolverCfg``. """ collision_cfg: NewtonCollisionPipelineCfg | Mapping[str, Any] | None = field( @@ -545,10 +547,10 @@ class NewtonPhysicsCfg(PhysicsBackendCfg): ) """Optional external collision-pipeline configuration. - The default preserves external contact generation for ordinary rigid-body - scenes. Set to ``None`` to avoid constructing or executing the external - pipeline, for example when particle solvers use rigid shapes solely as - one-way SDF boundaries. + MJVBD V2 owns collision detection and does not construct this pipeline. + Other solvers use these settings unless explicitly set to ``None``, for + example when particle solvers use rigid shapes solely as one-way SDF + boundaries. """ def __post_init__(self) -> None: @@ -566,6 +568,7 @@ def to_dexsim_cfg( DexUniSolverCfg, FeatherstoneSolverCfg, MJWarpSolverCfg, + MJVBDV2SolverCfg, NewtonCfg, NewtonCollisionPipelineCfg as DexsimNewtonCollisionPipelineCfg, SemiImplicitSolverCfg, @@ -586,6 +589,7 @@ def to_dexsim_cfg( "auto": AutoSolverCfg, "dexuni": DexUniSolverCfg, "mujoco_warp": MJWarpSolverCfg, + "mjvbd_v2": MJVBDV2SolverCfg, "xpbd": XPBDSolverCfg, "semi_implicit": SemiImplicitSolverCfg, "featherstone": FeatherstoneSolverCfg, @@ -606,7 +610,9 @@ def to_dexsim_cfg( ) collision_pipeline_cfg = None - if self.collision_cfg is not None: + if self.collision_cfg is not None and ( + solver_cfg is None or solver_cfg.solver_type != "mjvbd_v2" + ): collision_values = { item.name: getattr(self.collision_cfg, item.name) for item in fields(self.collision_cfg) @@ -655,6 +661,10 @@ def _normalize_newton_solver_type(solver_type: str) -> str: "mujocowarp": "mujoco_warp", "mujocowarpsolver": "mujoco_warp", "mujocowarpsolvercfg": "mujoco_warp", + "mjvbd2": "mjvbd_v2", + "mjvbdv2": "mjvbd_v2", + "mjvbd_v2": "mjvbd_v2", + "mjvbdv2solvercfg": "mjvbd_v2", "dexuni": "dexuni", "dexunisolver": "dexuni", "dexunisolvercfg": "dexuni", @@ -677,7 +687,7 @@ def _normalize_newton_solver_type(solver_type: str) -> str: if key not in aliases: logger.log_error( f"Unsupported Newton solver type '{solver_type}'. " - "Expected one of 'auto', 'dexuni', 'mjwarp', 'xpbd', 'semi_implicit', " + "Expected one of 'auto', 'dexuni', 'mjwarp', 'mjvbd_v2', 'xpbd', 'semi_implicit', " "'featherstone', or 'vbd'." ) return aliases[key] @@ -703,7 +713,7 @@ def _newton_solver_cfg_to_dexsim( configured_solver_type = ( solver_cfg_data.pop("solver_type", None) or solver_cfg_data.pop("class_type", None) - or "auto" + or "mjvbd_v2" ) normalized_solver_type = _normalize_newton_solver_type(str(configured_solver_type)) solver_cfg_type = solver_cfg_map[normalized_solver_type] diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index 23c78fcefa..a960831d12 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -1021,7 +1021,7 @@ def root_link_name(self) -> str: Returns: str: The name of the root link. """ - return self.entities[0].get_root_link_name() + return self._entities[0].get_root_link_name() @cached_property def joint_names(self) -> List[str]: diff --git a/embodichain/lab/sim/objects/backends/collision.py b/embodichain/lab/sim/objects/backends/collision.py index a39a35775a..1f2889669e 100644 --- a/embodichain/lab/sim/objects/backends/collision.py +++ b/embodichain/lab/sim/objects/backends/collision.py @@ -184,9 +184,9 @@ def _from_descriptor(entity: object) -> list[CollisionShapeDesc] | None: dtype=torch.float32, ).reshape(3) shapes: list[CollisionShapeDesc] = [] + # Match native shape queries: disabling contact response keeps geometry + # available to planners, including moving obstacles that exert no impulses. for shape_idx, collision in enumerate(collisions): - if getattr(collision, "enable_collision", True) is False: - continue geometry_type = getattr(collision, "geometry_type", None) geometry_type = getattr(geometry_type, "value", geometry_type) local_pose = torch.as_tensor( diff --git a/embodichain/lab/sim/objects/backends/newton.py b/embodichain/lab/sim/objects/backends/newton.py index 33b1644044..e6a8b94f26 100644 --- a/embodichain/lab/sim/objects/backends/newton.py +++ b/embodichain/lab/sim/objects/backends/newton.py @@ -204,11 +204,14 @@ def _configure_newton_mimic_compliance( backend = get_newton_backend(result.world) if ( backend is None - or backend.solver_type != "mujoco_warp" + or backend.solver_type not in {"mujoco_warp", "mjvbd_v2"} or backend.cfg.requires_grad or (backend.model is not None and backend.model.requires_grad) ): return False + solver = backend.mujoco_solver + if solver is None: + return False relation_names = [ (state_joint_names[child_id], state_joint_names[parent_id]) @@ -284,13 +287,21 @@ def _configure_newton_mimic_compliance( f"Newton model has no mimic constraint for joint pair {error.args[0]}." ) from error - solver = runtime.solver mapping = getattr(solver, "mjc_eq_to_newton_mimic", None) mjw_model = getattr(solver, "mjw_model", None) if mapping is None or mjw_model is None: raise RuntimeError("MuJoCo-Warp did not materialize Newton mimic rows.") mapping_values = np.asarray(mapping.numpy()) + if backend.solver_type == "mjvbd_v2": + from newton import Model + + scene_rows = backend.get_mujoco_index_map( + Model.AttributeFrequency.CONSTRAINT_MIMIC + ).numpy() + mapping_values = mapping_values.copy() + valid = mapping_values >= 0 + mapping_values[valid] = scene_rows[mapping_values[valid]] selected = np.isin(mapping_values, constraint_rows) if int(selected.sum()) != len(constraint_rows): raise RuntimeError( diff --git a/embodichain/lab/sim/physics/newton.py b/embodichain/lab/sim/physics/newton.py index 786ce98fb1..9fb74a16c0 100644 --- a/embodichain/lab/sim/physics/newton.py +++ b/embodichain/lab/sim/physics/newton.py @@ -204,6 +204,6 @@ def supports_contact_sensor(self) -> bool: if runtime_device is None: runtime_device = str(getattr(self._manager, "device", "")) return not ( - solver_type.endswith("mujoco_warp") + solver_type in {"mujoco_warp", "mjvbd_v2"} and runtime_device.lower().startswith("cpu") ) diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index 19d68cf313..3f70fc9dfc 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -537,7 +537,7 @@ def __init__( self._requested_renderer = sim_config.render_cfg.renderer solver_cfg = getattr(sim_config.physics_cfg, "solver_cfg", None) self._requested_solver = str( - solver_cfg.get("solver_type", solver_cfg.get("class_type", "auto")) + solver_cfg.get("solver_type", solver_cfg.get("class_type", "mjvbd_v2")) if isinstance(solver_cfg, Mapping) else getattr(solver_cfg, "solver_type", "auto") ) @@ -4427,6 +4427,7 @@ def reset_objects_state( None, "auto", "mujoco_warp", + "mjvbd_v2", } if ( use_coordinated_newton_clear @@ -4531,6 +4532,9 @@ def export_usd(self, fpath: str) -> bool: bool: True if export is successful, False otherwise. """ try: + # The optional USD kit registers the export methods on DexSim Env. + import dexsim.kit.usd + self._env.export_to_usd_file(fpath) logger.log_info(f"Simulation scene exported to USD file: {fpath}") return True @@ -4647,6 +4651,14 @@ def _deferred_destroy(self) -> None: "_robots", ): for asset in getattr(self, registry_name, {}).values(): + # Parsed material wrappers must die before their native renderer. + for material_attr in ( + "_visual_material", + "_original_visual_material", + "_original_visual_material_inst", + ): + if hasattr(asset, material_attr): + setattr(asset, material_attr, []) if hasattr(asset, "_data"): asset._data = None if hasattr(asset, "_spawn_result"): diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index beba18ddbc..ac596a0d62 100644 --- a/embodichain/lab/sim/spawn/descriptors.py +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -1168,9 +1168,13 @@ def _compile_joint_properties( if newton_solver_type is None else newton_solver_type.replace("-", "_").lower() ) - if normalized_solver not in {None, "auto", "mujoco_warp", "mjwarp"} and any( - mode == 1 for mode in joint_target_modes.values() - ): + if normalized_solver not in { + None, + "auto", + "mujoco_warp", + "mjwarp", + "mjvbd_v2", + } and any(mode == 1 for mode in joint_target_modes.values()): warnings.warn( f"Newton solver {newton_solver_type!r} does not consume " "joint_target_mode. POSITION is emulated with its configured " diff --git a/embodichain_tasks/configs/components/execution_policies/trajectory_open_loop_slow.yaml b/embodichain_tasks/configs/components/execution_policies/trajectory_open_loop_slow.yaml new file mode 100644 index 0000000000..3ab4ee0fa9 --- /dev/null +++ b/embodichain_tasks/configs/components/execution_policies/trajectory_open_loop_slow.yaml @@ -0,0 +1,29 @@ +policy_id: trajectory_open_loop_slow_v1 +preset_id: trajectory + +requires: + embodiment_contract: single_arm_parallel_gripper + +motion: + strategy: ik_interp + velocity_targets: auto + # Allow velocity-limited articulated objects to follow before releasing. + sample_count: 80 + dynamic_collision_mode: "off" + +tracking: + kind: timed + +recovery: + max_replans: 0 + max_action_retries: 0 + +workflow_recovery: + max_recovery_attempts: 0 + +runner: + minimum_cycle_time: 0.0 + hold_on_completion: false + hold_during_effect_verification: false + +effect_assurance: projected diff --git a/embodichain_tasks/configs/tasks/manipulation/open_drawer/env.newton.yaml b/embodichain_tasks/configs/tasks/manipulation/open_drawer/env.newton.yaml index 083537e0cc..fd5905b07c 100644 --- a/embodichain_tasks/configs/tasks/manipulation/open_drawer/env.newton.yaml +++ b/embodichain_tasks/configs/tasks/manipulation/open_drawer/env.newton.yaml @@ -13,15 +13,15 @@ physics_config: num_substeps: 20 collision_cfg: null solver_cfg: - solver_type: mujoco_warp - solver: newton - integrator: implicitfast - iterations: 20 - ls_iterations: 100 - cone: elliptic - impratio: 1000.0 - use_mujoco_contacts: true - enable_multiccd: true + solver_type: mjvbd_v2 + mujoco_options: + solver: newton + integrator: implicitfast + iterations: 20 + ls_iterations: 100 + cone: elliptic + impratio: 1000.0 + enable_multiccd: true simulation: light: diff --git a/embodichain_tasks/configs/tasks/manipulation/open_drawer/task.ur5.newton.yaml b/embodichain_tasks/configs/tasks/manipulation/open_drawer/task.ur5.newton.yaml index 9783c94c9f..c1890df4ba 100644 --- a/embodichain_tasks/configs/tasks/manipulation/open_drawer/task.ur5.newton.yaml +++ b/embodichain_tasks/configs/tasks/manipulation/open_drawer/task.ur5.newton.yaml @@ -6,7 +6,7 @@ environment: task_program: program: task_program/program.yaml integration: task_program/integration.yaml - execution_policy: ../../../components/execution_policies/trajectory_open_loop.yaml + execution_policy: ../../../components/execution_policies/trajectory_open_loop_slow.yaml embodiment: component: ../../../components/embodiments/ur5_dh_pgi_140_80.yaml diff --git a/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/env.newton.yaml b/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/env.newton.yaml index 2bf2375612..10209b420f 100644 --- a/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/env.newton.yaml +++ b/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/env.newton.yaml @@ -15,15 +15,15 @@ physics_config: num_substeps: 20 collision_cfg: null solver_cfg: - solver_type: mujoco_warp - solver: newton - integrator: implicitfast - iterations: 20 - ls_iterations: 100 - cone: elliptic - impratio: 1000.0 - use_mujoco_contacts: true - enable_multiccd: true + solver_type: mjvbd_v2 + mujoco_options: + solver: newton + integrator: implicitfast + iterations: 20 + ls_iterations: 100 + cone: elliptic + impratio: 1000.0 + enable_multiccd: true simulation: light: diff --git a/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.newton.yaml b/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.newton.yaml index 429264f5a1..166663208f 100644 --- a/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.newton.yaml +++ b/embodichain_tasks/configs/tasks/manipulation/repeated_pick_place/task.ur5.newton.yaml @@ -6,7 +6,7 @@ environment: task_program: program: task_program/program.yaml integration: task_program/integration.yaml - execution_policy: ../../../components/execution_policies/trajectory_open_loop.yaml + execution_policy: ../../../components/execution_policies/trajectory_open_loop_slow.yaml embodiment: component: ../../../components/embodiments/ur5_dh_pgi_140_80.yaml diff --git a/examples/sim/demo/grasp_cup_to_caffe.py b/examples/sim/demo/grasp_cup_to_caffe.py index aa3fff4636..b542d1967a 100644 --- a/examples/sim/demo/grasp_cup_to_caffe.py +++ b/examples/sim/demo/grasp_cup_to_caffe.py @@ -92,13 +92,9 @@ def initialize_simulation(args) -> SimulationManager: """ physics_cfg = physics_cfg_for_backend(args.physics) if args.physics == "newton": - # This contact-heavy URDF scene needs Newton's collision pipeline; - # MuJoCo's native contact path is not reliable for these convex meshes. physics_cfg.solver_cfg = { - "solver_type": "mujoco_warp", - "use_mujoco_contacts": False, - "nconmax": 16384, - "njmax": 65536, + "solver_type": "mjvbd_v2", + "mujoco_options": {"nconmax": 16384, "njmax": 65536}, } config = SimulationManagerCfg( diff --git a/examples/sim/demo/scoop_ice.py b/examples/sim/demo/scoop_ice.py index 8b05f43a0e..a809ad6f65 100644 --- a/examples/sim/demo/scoop_ice.py +++ b/examples/sim/demo/scoop_ice.py @@ -79,9 +79,10 @@ def initialize_simulation(args): """ physics_cfg = physics_cfg_for_backend(args.physics) if args.physics == "newton": - # Hundreds of free bodies make dense Newton Hessian factorization - # expensive. CG avoids that factorization while retaining contacts. - physics_cfg.solver_cfg = {"solver_type": "mujoco_warp", "solver": "cg"} + physics_cfg.solver_cfg = { + "solver_type": "mjvbd_v2", + "mujoco_options": {"solver": "cg"}, + } config = SimulationManagerCfg( headless=True, device=args.device, diff --git a/examples/sim/motion/planners/curobo_planner.py b/examples/sim/motion/planners/curobo_planner.py index 11b7e3f373..a983deb940 100644 --- a/examples/sim/motion/planners/curobo_planner.py +++ b/examples/sim/motion/planners/curobo_planner.py @@ -502,9 +502,9 @@ def _build_scene( init_rot=(0.0, 0.0, 0.0), ) ) + sim.prepare() finally: demo_block_mesh_path.unlink(missing_ok=True) - sim.prepare() if robot_type == "w1": # Keep the W1-specific IK diagnostic batched so it remains useful when @@ -861,7 +861,10 @@ def main(args: argparse.Namespace | None = None) -> None: binding, motion_policy, ), - ) + ), + context=engine.initial_context( + control_dt=sim.sim_config.physics_dt * args.step_repeat, + ), ) success = compiled.plan_success trajectory = compiled.trajectory.positions @@ -899,7 +902,10 @@ def main(args: argparse.Namespace | None = None) -> None: binding, motion_policy, ), - ) + ), + context=engine.initial_context( + control_dt=sim.sim_config.physics_dt * args.step_repeat, + ), ) success = compiled.plan_success trajectory = compiled.trajectory.positions diff --git a/examples/sim/motion/trajectory_velocity_tracking.py b/examples/sim/motion/trajectory_velocity_tracking.py index 9d1145ad12..8772c98f67 100644 --- a/examples/sim/motion/trajectory_velocity_tracking.py +++ b/examples/sim/motion/trajectory_velocity_tracking.py @@ -396,8 +396,7 @@ def main(args: argparse.Namespace | None = None) -> None: robot: Robot = sim.add_robot( FrankaPandaCfg.from_dict({"uid": "tracking_franka"}) ) - if sim.is_use_gpu_physics: - sim.init_gpu_physics() + sim.prepare() if not args.headless: sim.open_window() common_initial_qpos = robot.get_qpos().clone() diff --git a/scripts/tutorials/atomic_action/move_held_object.py b/scripts/tutorials/atomic_action/move_held_object.py index 4341110e04..e53ca5a1ad 100644 --- a/scripts/tutorials/atomic_action/move_held_object.py +++ b/scripts/tutorials/atomic_action/move_held_object.py @@ -27,6 +27,7 @@ sys.path.insert(0, str(_REPO_ROOT)) import torch +import trimesh from embodichain.data import get_data_path from embodichain.lab.sim.atomic_actions import ( @@ -82,11 +83,16 @@ def parse_arguments() -> argparse.Namespace: def create_pick_object(sim) -> RigidObject: """Create and settle the paper cup used by the tutorial.""" + mesh = trimesh.load_mesh(get_data_path(OBJECT_MESH_PATH), process=False) + # Author cup proportions in the mesh; physical body scale must be uniform. + mesh.apply_scale((0.75, 0.75, 1.0)) obj = sim.add_rigid_object( cfg=RigidObjectCfg( uid="paper_cup", shape=MeshCfg( - fpath=get_data_path(OBJECT_MESH_PATH), + vertices=mesh.vertices, + triangles=mesh.faces, + normals=mesh.vertex_normals, collision=MeshCollisionCfg( approximation="convex_decomposition", max_hulls=16, @@ -99,7 +105,6 @@ def create_pick_object(sim) -> RigidObject: newton_contact=sim.is_newton_backend, ), init_pos=[*OBJECT_XY, OBJECT_INITIAL_Z], - body_scale=(0.75, 0.75, 1.0), ) ) sim.prepare() diff --git a/scripts/tutorials/atomic_action/pour.py b/scripts/tutorials/atomic_action/pour.py index 34c2ef0e4c..56a39b75cd 100644 --- a/scripts/tutorials/atomic_action/pour.py +++ b/scripts/tutorials/atomic_action/pour.py @@ -39,7 +39,6 @@ PourOptions, ) from embodichain.lab.sim.motion.planners import ( - TrapezoidalPlanOptions, ToppraPlanOptions, TrajectorySampleMethod, ) @@ -103,14 +102,9 @@ def _create_pick_motion_policy( sample_method=TrajectorySampleMethod.QUANTITY, sample_interval=PICK_MOTION_SAMPLE_COUNT, ) - elif planner == "trapezoidal": - plan_opts = TrapezoidalPlanOptions( - sample_method=TrajectorySampleMethod.QUANTITY, - sample_interval=PICK_MOTION_SAMPLE_COUNT, - ) - elif planner == "curobo": - # cuRobo owns its native sampling and does not use the joint-space - # sample-count options above. + elif planner in ("trapezoidal", "curobo"): + # Preserve native waypoints before PickUp resamples its two motion + # segments. A fixed quantity can be smaller than the Cartesian IK path. plan_opts = None else: raise ValueError(f"Unsupported tutorial planner {planner!r}.") diff --git a/scripts/tutorials/atomic_action/tutorial_utils.py b/scripts/tutorials/atomic_action/tutorial_utils.py index 1167510b11..76e07f553b 100644 --- a/scripts/tutorials/atomic_action/tutorial_utils.py +++ b/scripts/tutorials/atomic_action/tutorial_utils.py @@ -267,10 +267,8 @@ def _tutorial_physics_cfg( ) -> PhysicsBackendCfg: """Build the shared physics configuration for atomic-action tutorials. - Newton tutorials intentionally use MuJoCo Warp's native collision path. - It generates contacts inside every solver substep, so an external Newton - collision pipeline would be unused and would only allocate unnecessary - contact buffers. + MJVBD V2 owns collision detection and retains MuJoCo articulation drives. + Free rigid bodies use VBD with the solver's native one-way coupling. """ physics_cfg = physics_cfg_for_backend(backend) if isinstance(physics_cfg, NewtonPhysicsCfg): @@ -279,20 +277,21 @@ def _tutorial_physics_cfg( # contacts and fast-changing manipulator loads. DexSim sizes # contact/constraint buffers from the finalized scene, including the # contact dimensions authored on gripper and object surfaces. - # MultiCCD retains up to four contacts per gripper-mesh/object pair, - # which prevents a marginal two-finger grasp from sliding away. + # Retain the existing MuJoCo articulation tuning. Free-body contacts + # remain VBD-owned and do not inherit these MuJoCo contact options. physics_cfg.num_substeps = 20 physics_cfg.collision_cfg = None physics_cfg.solver_cfg = { - "solver_type": "mujoco_warp", - "solver": "newton", - "integrator": "implicitfast", - "iterations": 20, - "ls_iterations": 100, - "cone": "elliptic", - "impratio": 1_000.0, - "use_mujoco_contacts": True, - "enable_multiccd": True, + "solver_type": "mjvbd_v2", + "mujoco_options": { + "solver": "newton", + "integrator": "implicitfast", + "iterations": 20, + "ls_iterations": 100, + "cone": "elliptic", + "impratio": 1_000.0, + "enable_multiccd": True, + }, } return physics_cfg diff --git a/scripts/tutorials/gym/modular_env.py b/scripts/tutorials/gym/modular_env.py index f7d33c5d75..e33bdc33c4 100644 --- a/scripts/tutorials/gym/modular_env.py +++ b/scripts/tutorials/gym/modular_env.py @@ -211,7 +211,7 @@ class ExampleCfg(EmbodiedEnvCfg): shape=MeshCfg( fpath=get_data_path("TableWare/tableware/fork/standard_fork_scale.ply"), ), - body_scale=(0.75, 0.75, 1.0), + body_scale=(0.75, 0.75, 0.75), init_pos=(0.8, 0, 1.0), ), ] diff --git a/scripts/tutorials/sim/open_drawer.py b/scripts/tutorials/sim/open_drawer.py index 58f893e342..99c0a55242 100644 --- a/scripts/tutorials/sim/open_drawer.py +++ b/scripts/tutorials/sim/open_drawer.py @@ -530,22 +530,21 @@ def _tutorial_physics_cfg( """Build the physics configuration used by this tutorial.""" physics_cfg = physics_cfg_for_backend(backend) if isinstance(physics_cfg, NewtonPhysicsCfg): - # Keep the same MuJoCo-Warp solver/contact profile as the atomic-action - # tutorial and packaged Newton task. The finer drawer substep cadence - # is intentional; per-world capacities remain auto-sized by DexSim - # from the finalized scene. + # Share the V2 articulation profile with the atomic-action tutorials. + # Keep the finer drawer substep cadence and native contact ownership. physics_cfg.num_substeps = 20 physics_cfg.collision_cfg = None physics_cfg.solver_cfg = { - "solver_type": "mujoco_warp", - "solver": "newton", - "integrator": "implicitfast", - "iterations": 20, - "ls_iterations": 100, - "cone": "elliptic", - "impratio": 1_000.0, - "use_mujoco_contacts": True, - "enable_multiccd": True, + "solver_type": "mjvbd_v2", + "mujoco_options": { + "solver": "newton", + "integrator": "implicitfast", + "iterations": 20, + "ls_iterations": 100, + "cone": "elliptic", + "impratio": 1_000.0, + "enable_multiccd": True, + }, } return physics_cfg diff --git a/tests/sim/test_cfg.py b/tests/sim/test_cfg.py index 13d74f2f72..523e1b6ef8 100644 --- a/tests/sim/test_cfg.py +++ b/tests/sim/test_cfg.py @@ -529,7 +529,7 @@ def test_robot_preset_accepts_newton_solver_alias() -> None: preset = _NewtonSolverAliasRobotPresetCfg() assert preset.resolve(DefaultPhysicsCfg()).uid == "fallback" - assert preset.resolve(NewtonPhysicsCfg()).uid == "fallback" + assert preset.resolve(NewtonPhysicsCfg()).uid == "mjwarp" assert ( preset.resolve(NewtonPhysicsCfg(solver_cfg={"solver_type": "mjwarp"})).uid == "mjwarp" @@ -812,6 +812,7 @@ def test_robot_cfg_merge_preserves_grouped_overrides() -> None: def test_newton_physics_inherits_common_gravity_and_collision_config() -> None: cfg = NewtonPhysicsCfg( + solver_cfg=None, gravity=[0.0, 0.0, -1.5], collision_cfg=NewtonCollisionPipelineCfg( broad_phase="sap", diff --git a/tests/sim/test_sim_manager_cfg.py b/tests/sim/test_sim_manager_cfg.py index a96f8e93cf..d34f11f2d4 100644 --- a/tests/sim/test_sim_manager_cfg.py +++ b/tests/sim/test_sim_manager_cfg.py @@ -124,15 +124,15 @@ def test_newton_physics_cfg_uses_device() -> None: @pytest.mark.no_sim -def test_newton_physics_cfg_preserves_dexsim_auto_solver_default() -> None: - from dexsim.engine.newton_physics import AutoSolverCfg +def test_newton_physics_cfg_defaults_to_mjvbd_v2_solver() -> None: + from dexsim.engine.newton_physics import MJVBDV2SolverCfg cfg = NewtonPhysicsCfg() dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) - assert isinstance(dexsim_cfg.solver_cfg, AutoSolverCfg) - assert dexsim_cfg.solver_cfg.solver_type == "auto" + assert isinstance(dexsim_cfg.solver_cfg, MJVBDV2SolverCfg) + assert dexsim_cfg.solver_cfg.solver_type == "mjvbd_v2" @pytest.mark.no_sim @@ -174,7 +174,7 @@ def test_newton_physics_cfg_requires_dexsim_auto_solver_api( @pytest.mark.no_sim def test_newton_gradient_mode_rejects_auto_solver() -> None: - cfg = NewtonPhysicsCfg(requires_grad=True) + cfg = NewtonPhysicsCfg(solver_cfg=None, requires_grad=True) with pytest.raises(RuntimeError, match="explicit.*semi_implicit"): cfg.to_dexsim_cfg(gpu_id=0) @@ -193,7 +193,9 @@ def test_newton_physics_cfg_passes_warp_log_suppression() -> None: def test_newton_physics_cfg_forwards_collision_pipeline_update_interval( update_interval: int | None, ) -> None: - cfg = NewtonPhysicsCfg(collision_cfg={"update_interval": update_interval}) + cfg = NewtonPhysicsCfg( + solver_cfg=None, collision_cfg={"update_interval": update_interval} + ) dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) @@ -296,7 +298,7 @@ def test_newton_backend_reports_scene_resolved_auto_solver( manager = SimpleNamespace(_world=world) backend = NewtonPhysicsBackend(manager) world_config = SimpleNamespace(newton_cfg=None) - sim_config = SimulationManagerCfg(physics_cfg=NewtonPhysicsCfg()) + sim_config = SimulationManagerCfg(physics_cfg=NewtonPhysicsCfg(solver_cfg=None)) monkeypatch.setattr( "dexsim.engine.newton_physics.backend_registry.get_newton_backend", lambda candidate: native_backend if candidate is world else None, From 731a20d032a49051942f3cafa3d41cde670ab00b Mon Sep 17 00:00:00 2001 From: xiemenghong Date: Fri, 18 Sep 2026 10:27:22 +0800 Subject: [PATCH 02/27] fix(sim): tune MJVBD V2 manipulation examples and record limitations --- .../topics/atomic-actions/atomic-actions.md | 29 ++ .../simulation-system/simulation-system.md | 62 +++- examples/sim/demo/grasp_cup_to_caffe.py | 3 +- examples/sim/demo/scoop_ice.py | 2 + outputs/validation/mjvbd_v2/MJVBDV2_issues.md | 350 ++++++++++++++++++ scripts/tutorials/atomic_action/axis_align.py | 11 +- .../atomic_action/coordinated_pickment.py | 32 +- .../atomic_action/coordinated_placement.py | 3 +- scripts/tutorials/atomic_action/hand_over.py | 53 ++- .../atomic_action/move_held_object.py | 29 +- scripts/tutorials/atomic_action/pickup.py | 11 +- scripts/tutorials/atomic_action/place.py | 38 +- scripts/tutorials/atomic_action/pour.py | 41 +- .../tutorials/atomic_action/scenario_utils.py | 4 +- .../tutorials/atomic_action/tutorial_utils.py | 44 ++- .../sim/atomic_actions/test_tutorial_utils.py | 48 +-- 16 files changed, 702 insertions(+), 58 deletions(-) create mode 100644 outputs/validation/mjvbd_v2/MJVBDV2_issues.md diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 6cfc208ed7..5694c474a6 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -92,6 +92,35 @@ repeated-pick/open-drawer environment components carry the same Newton-only overlays, while their declared Default backend continues to consume only the portable fields. +For UR5 with `mjvbd_v2`, the paper-cup `move_held_object.py` tutorial opts into +VBD contact history with `latest` collision matching, sliding friction 4 on the +gripper alone, and a 0.0115 m candidate close target. Transport orientation +is not yet repeatably aligned. The native contact alpha and the cup +and ground materials are unchanged. `create_tutorial_simulation` accepts +`newton_solver_options`, and `add_tutorial_robot` accepts +`newton_gripper_friction` and optional `arm_damping`; these are authored before +model construction. This +calibration also applies to the UR5-only `pour.py` tutorial with its 0.013 m +cube close target. The UR5 plastic-tray `coordinated_pickment.py` tutorial uses +the same contact settings with a 0.023 m close target; the dual-arm helper +`add_dual_tutorial_robot` also accepts `newton_gripper_friction`. +The UR5 vertical-can `hand_over.py` tutorial uses those contact settings with +left/right close targets of 0.007/0.0055 m and retains its original arm drives. +Its replay callback holds the current targets for one second immediately before +the compiled `receive_release` segment so the receiving arm can settle before +opening. The wait uses segment metadata, not a fixed waypoint index, and is +scoped to that same profile. This scene-specific calibration does not promise +identical final poses or stability across initial-state changes. Horizontal +objects and other robot families retain their prior configuration. +The UR5 V2 `place.py` profile uses a 0.013 m close target, gripper friction 4, +contact history with `latest` matching, contact alpha 0.5, and arm damping 1000. +The lower arm damping reduces tracking lag when the fingers open; finger +drives and authored release waypoints remain unchanged. Complete cached +replays preserve the grasp to release, but landing orientation still varies; +this is a scoped improvement, not a claim of full solver equivalence. +These explicit scene choices do not change world defaults +or establish a generic object-width-to-gripper-target mapping. + ## Semantic integration boundary `semantics` contains: diff --git a/agent_context/topics/simulation-system/simulation-system.md b/agent_context/topics/simulation-system/simulation-system.md index 8872a5dbd3..1f52ad696d 100644 --- a/agent_context/topics/simulation-system/simulation-system.md +++ b/agent_context/topics/simulation-system/simulation-system.md @@ -494,12 +494,19 @@ CUDA. VBD contacts expose geometry without MuJoCo impulse reporting. V2 selects MuJoCo for articulations with active scalar joints and VBD for unselected free rigid bodies. Its current mixed dynamic branch is one-way: -VBD contacts do not push back on the MuJoCo robot, and MuJoCo contacts are -disabled in that branch. Therefore switching an existing locomotion or -manipulation scene to V2 does not by itself guarantee equivalent contact -dynamics. In particular, adding a VBD-owned free rigid body switches a floating -robot out of the articulation-only branch and removes its MuJoCo ground -support, even if the new body is far away. Explicit `mujoco_articulations` or +VBD contacts do not push back on the MuJoCo robot. The local, uncommitted DexSim +patch enables a second MuJoCo-Warp collision/contact pipeline by default; explicitly setting +`mujoco_options.disable_contacts=True` still disables them. The compact +MuJoCo view contains owned links and world-attached static shapes, but excludes +VBD-owned free bodies and independent kinematic bodies. A stationary fixture +declared kinematic therefore does not provide robot contact reaction in this +view; use static for fixtures that never move. The stationary tables in +`examples/sim/demo/grasp_cup_to_caffe.py` and +`scripts/tutorials/atomic_action/coordinated_placement.py` use this declaration. +Moving kinematic obstacles and +dynamic-object reaction remain separate limitations. Mixed contact queries +still lack equivalent impulse reporting, even when robot ground support is +restored. Explicit `mujoco_articulations` or `mujoco_articulation_paths` can select complete joint trees; these are ownership choices, not automatic EmbodiChain rewrites. This integration does not extend deformable support. @@ -536,6 +543,33 @@ Parallel-jaw manipulation keeps its Newton-only contact overlay (`condim=4`, `ke=4e4`, `kd=4e2`, torsional friction `0.1`, rolling friction `0.01`) on the gripper and manipulated objects. MuJoCo options affect articulation-owned contacts; free rigid-body contacts use VBD and need not have identical effects. +The UR5 `pickup.py` and `axis_align.py` tutorials use a 0.013 m +DH-gripper close target for their 5 cm cube under V2 +(`MJVBD_V2_CUBE_CLOSE_QPOS` in `tutorial_utils.py`). The old +0.036 m target relies on contact feedback to stop the fingers; V2's one-way +proxies instead reach that target and squeeze the cube out. This calibration +is specific to that object and gripper, not a general grasp-width mapping. +These two cube tutorials retain native VBD defaults. Their calibrated target +does not make the original oversized command compatible with one-way contact. +The UR5 `move_held_object.py` paper-cup tutorial uses a 0.0115 m candidate close target, +gripper-only sliding friction 4, VBD contact history, and collision matching +`latest`, retaining the native contact alpha. The shared simulation helper +accepts scene-specific `newton_solver_options`; robot creation accepts +`newton_gripper_friction` before spawning. The cup candidate has not achieved repeatable orientation alignment. These are explicit tutorial choices, +not world defaults, and leave the cup and ground materials unchanged. The UR5-only `pour.py` tutorial uses the same contact history and gripper +friction with a 0.013 m cube close target; its V2 initial height includes the +ground's 1 mm contact margin. Other robot selections and physics solvers keep +their existing targets and materials. +The UR5 V2 `place.py` tutorial additionally uses contact alpha 0.5 and arm +damping 1000 with the same cube target, gripper friction, and contact history. +`add_tutorial_robot(arm_damping=...)` authors this drive override before spawn; +other callers retain their configured gains. Release waypoints are unchanged, +and landing orientation remains sensitive to contact variation. +The `examples/sim/demo/scoop_ice.py` scene sets +`vbd_options.rigid_body_contact_buffer_size=256` for dense ice contacts. +This per-body VBD capacity is independent of MuJoCo's `nconmax`/`njmax`; +overflow truncates the contacts used by each affected VBD body. The larger +scene-specific capacity does not establish stable friction grasping. Explicit cloth, softbody, and differentiable examples keep their selected solvers. Newton's `suppress_warp_kernel_logs=True` suppresses Warp's one-time runtime banner plus module compile/load chatter during manager startup, build, facade @@ -545,6 +579,10 @@ information is controlled separately by `dexsim_startup_info`. EmbodiChain-authored Newton collision shapes use a default margin and gap of `0.001 m` each only when no portable or Newton-native envelope is authored. +The Pour tutorial starts its cube center at half its height. Under V2, Pour +and Place add 1 mm initial clearance for the default ground's contact margin, +avoiding penetration recovery before grasp planning. The global ground +configuration stays unchanged to preserve the pure-MuJoCo locomotion path. `CollisionPropertiesCfg.contact_offset/rest_offset` are portable: Default uses them directly, while the Spawn compiler maps `rest_offset → margin` and `contact_offset - rest_offset → gap`. Both values must be present to derive a @@ -1049,3 +1087,15 @@ Default source-physics capture preserves each link's replication collision filter while filling mass/COM properties. Spawn articulation mass-property writes use the live handle's `set_link_inertia` and `set_link_com_pose`; native COM quaternions are converted from EmbodiChain `xyzw` to DexSim `wxyz`. + +## Current MJVBD V2 validation dependency + +The local mixed-scene validation uses an uncommitted DexSim patch that defaults +`mujoco_options.disable_contacts` to `False` while retaining +`use_mujoco_contacts=True`. This runs MuJoCo-Warp collision/contact solving for +its articulation/static compact view alongside Newton/VBD contacts. It differs +from native V2 mixed-mode contact configuration and is not an accepted final +architecture. Mixed locomotion alignment measurements depend on this patch; +they do not establish native single-pipeline mixed-mode equivalence. A second +local DexSim patch fixes convex mesh import coordinates. Neither patch is +contained in this EmbodiChain commit. diff --git a/examples/sim/demo/grasp_cup_to_caffe.py b/examples/sim/demo/grasp_cup_to_caffe.py index b542d1967a..7e07355534 100644 --- a/examples/sim/demo/grasp_cup_to_caffe.py +++ b/examples/sim/demo/grasp_cup_to_caffe.py @@ -213,7 +213,8 @@ def create_table(sim: SimulationManager) -> RigidObject: attrs=RigidBodyPhysicsCfg( mass_props=MassPropertiesCfg(mass=0.5), ), - body_type="kinematic", + # This fixture never moves; static shapes also enter V2's MuJoCo view. + body_type="static", init_pos=[1.1, -0.5, 0.08], init_rot=[0.0, 0.0, 0.0], ) diff --git a/examples/sim/demo/scoop_ice.py b/examples/sim/demo/scoop_ice.py index a809ad6f65..0f972e38c1 100644 --- a/examples/sim/demo/scoop_ice.py +++ b/examples/sim/demo/scoop_ice.py @@ -82,6 +82,8 @@ def initialize_simulation(args): physics_cfg.solver_cfg = { "solver_type": "mjvbd_v2", "mujoco_options": {"solver": "cg"}, + # Dense ice contacts exceed VBD's default per-body capacity of 64. + "vbd_options": {"rigid_body_contact_buffer_size": 256}, } config = SimulationManagerCfg( headless=True, diff --git a/outputs/validation/mjvbd_v2/MJVBDV2_issues.md b/outputs/validation/mjvbd_v2/MJVBDV2_issues.md new file mode 100644 index 0000000000..8f3e575569 --- /dev/null +++ b/outputs/validation/mjvbd_v2/MJVBDV2_issues.md @@ -0,0 +1,350 @@ +# MJVBDV2 接入问题记录 + +更新日期:2026-09-18 + +这份记录整理 MJVBDV2 接入 EmbodiChain 后遇到的问题:实际看到了什么、会影响什么、哪些已经修复、哪些还需要继续查。 + +这里的“与旧 solver 对齐”,指的是换 solver 后表现接近,不代表任务一定成功。例如,两边的抽屉都没有关上,仍可能是表现接近;两边程序都正常退出,也不代表物体的运动一致。文中的旧 solver 指 Newton 后端的 `mujoco_warp`。 + +本次保留 MJVBDV2 的物体所有权分工:带关节的机器人、门和抽屉由 MuJoCo 求解,自由运动的杯子、方块等刚体由 VBD 求解。布料、软体不在这次接入范围内。**但本轮 DexSim 补丁额外开启了混合模式的 MuJoCo 原生接触,实际运行两套碰撞管线,已经偏离原生混合模式的接触配置。详见文末更正。** + +本文链接的详细报告和原始数据是本地验证产物,未随这份问题记录提交到 GitHub。 + +**目前仍未完成全部动态刚体任务对齐。** 调参已恢复了一部分技能,但单次成功、停止后不掉落,都不能代表稳定替换。当前状态如下,后文保留问题的发现过程;详细试验以[第二轮调参记录](coupling_tuning/round2.md)为准。 + +| 场景 | 当前结论 | +| --- | --- | +| PickUp、Pour、Place、双臂抬托盘 | 已有完整改善结果和正式入口验证;仍缺跨初态稳定性证据。 | +| 纸杯 MoveHeldObject | 最新 11.5 mm 正式入口完整完成,位置误差 2.90 mm、姿态误差 11.32°;停止后基本稳定,但搬运转偏未解决。此前缓存的 3.57° 好结果未稳定复现。 | +| HandOver | 释放前等待一秒后,三次候选及正式入口均直立停稳;不代表新旧最终位姿完全一致,跨初态尚待验证。 | +| Assemble | alpha=0.5 的完整候选偏心 6.93 mm;重复及重新规划待结果。单纯延长释放等待无效,局部接触余量对照继续进行。 | +| 双臂放置 | 未恢复预期效果。已确认新旧 solver 在同轨迹下均被桌子挡住,右手偏差约 7–8 cm;安装位置对照待结果,抓持也仍需改善。 | +| 咖啡杯示例 | 新旧正常规划都没有稳定完成。已排队验证规划前自然落稳的影响,尚未证明等待有效。 | +| 舀冰 | 接触容量已修复;完整候选带出 5–6 块冰,但勺子姿态仍偏约 15–17°。摩擦、刚度对照继续运行;冰堆沉降差异也会影响结果,不能只比较一次带出数量。 | +| Locomotion | 已有约 1e-6 rad 量级的同动作状态对齐证据;混合模式结果依赖额外开启 MuJoCo 原生接触,不能证明原生单管线混合模式对齐,也不能替代完整训练验证。 | +| Twist | 按要求暂时跳过。 | + + +## 目前还有哪些问题 + +### 新发现:舀冰时 VBD 的接触列表装不下 + +**状态:容量 256 已完整运行,无溢出警告;舀冰例程已采用该容量,抓持稳定性继续调参。** + +抓持改善后的两次完整舀冰出现数千次接触溢出。一个动态刚体最多需要 170 / 183 个接触位置,但 VBD 默认只给每个刚体保留 64 个;超过容量的部分没有进入该刚体的接触求解。这与 MuJoCo 的接触、约束容量是两套设置,之前扩大 MuJoCo 容量不能解决它。 + +这是此前验证遗漏的一项,现予以更正:带出 1 / 3 块冰和滑转较大的结果真实存在,但接触已经被截断,不能据此判断单向耦合的能力上限。只把 VBD 容量提高到 256 后,已完整运行 3300 步,采样峰值 188、无溢出警告;最后带出 6 块冰,其中 5 块在保持前后采样中都位于勺碗范围。但勺子累计手内变化仍约 16.84 mm / 16.78°,说明排除截断后仍需改善抓持。随后原闭合目标也完整完成 3300 步,无溢出、采样峰值 192,末尾带出同一批 5 块冰;位置误差 22.62 mm、姿态误差 14.86°。原目标末尾保持更好,标定目标前段滑转更小,两者尚无全面胜出的配置。手部摩擦和接触刚度的首次独立对照都被临时记录器的法向量断言中断,尚无完整效果结论;记录器已改为保存异常原始值并明确标记不可计算的指标,两组按原物理参数重跑。对应 HandOver、双臂放置、咖啡杯和 Place 日志未发现同类警告,不能用这个问题解释所有未对齐任务。 + + +### 1. 混合场景的接触支持仍不完整,地面支撑已修复 + +**状态:本地小补丁已构建并安装到 embodichain 环境,恢复 MuJoCo 自有接触;接触力查询和动态物体反力仍有边界。补丁尚未提交或推送。** + +此前只有机器人和静态地面时,V2 使用纯 MuJoCo 分支,机器人可以正常行走。加入 VBD 动态刚体后切换到混合分支,却关闭了 MuJoCo 自身接触,连远处方块也会间接导致机器人失去地面支撑。这是已经确认的接入问题,不必通过重写双向耦合解决。 + +修正后,混合分支默认保留机器人与 MuJoCo 可见静态环境的接触,自由刚体仍归 VBD,反力仍不回传。直接加载补丁源码的四环境 GPU 对照完整运行 1500 步及五次重置,无容量溢出,关节最大差约 1.13e-6 rad,终止标志一致。随后正式 wheel 已安装,正常安装包入口的 CPU 混合落地及纯分支检查均通过。GPU 安装包任务回归仍在进行。 + +这项修正恢复地面支撑,但不代表混合场景的接触力查询已经与旧 MuJoCo 一致。若任务依赖动态物体把反力传给机器人,仍受单向耦合限制。 + +补充确认:静止的桌子如果被配置成独立的 `kinematic` 物体,也不会自动进入混合分支的 MuJoCo 碰撞模型。咖啡杯示例中,旧 MuJoCo 有桌子的 8 个碰撞几何,V2 的 MuJoCo 模型里没有它们。杯子仍会碰到桌子,但机器人不会收到这部分接触反力。示例中的桌子从不移动,已将它改为 static,运行时确认两边 MuJoCo 模型均有 8 个桌子碰撞几何。修正桌子后,最初的新旧咖啡杯完整任务仍失败,说明这不是全部原因;随后手部接触调参才出现完整改善候选。真正会运动的 kinematic 物体不能用这种场景修正替代支持。 + +### 部分接触参数在 VBD 中的作用与旧配置不同 + +**状态:已核对源码及 CPU 接触力;这不是所有剩余失败的原因证明。** + +- 当前默认的刚体硬接触不使用 `friction_epsilon`。只改变它不会调整这条分支的摩擦;原计划的纸杯候选已在启动前撤销。 +- 相同的夹爪接触余量也未必有相同效果:当前 UR5 手指的 1 mm margin 在 MuJoCo 原生碰撞中被清零,在 VBD 中仍保留。双臂放置的局部零余量候选仍在旧缓存的协作段甩出面包,已明确记录后提前停止,未修改全局默认;这不代表所有任务都不适合零余量,Assemble 的独立对照仍待结果。 +- 当前 VBD 刚体接触没有读取独立的扭转、滚动摩擦系数。旧 MuJoCo 中有效的扭转摩擦配置,不会自动得到等价作用。多个接触点仍可能通过切向力抵抗旋转,因此不能直接推断所有抓持都不适用。 +- 手指和物体的滑动摩擦取几何平均。例如手指设为 4、杯子为 0.97,接触对约为 1.97;刚度和阻尼则取两侧的算术平均。调参应比较最终接触对,而不只看某一个配置字段。 +- 接触阻尼 `kd` 在硬接触中有效,但只抵抗接触面相互接近的法向运动,不是通用的止滑、止转参数。安装包 CPU 核对中,增大它能提高接近时的法向阻力,却没有同步提高该瞬间的切向摩擦上限;法向不再接近时,这项阻尼不生效。不能因此断言某个任务失败由它造成,但不应把增大 `kd` 当作普遍增强抓持的手段。 + +单点纯扭转的 CPU 核对显示,提高滑动摩擦无法产生缺失的独立抗扭项。它只说明该力模型的边界;具体技能是否依赖这项能力,还需要实际接触几何和完整任务证据。详细记录及原始数据见[第二轮调参记录](coupling_tuning/round2.md)。 + +全程关闭接触点黏着重放也不是通用解法:HandOver 的独立候选在最终松手前就提前脱手,完整运行后侧倒。当前保留该设置,优先重复验证释放前等待的候选。这个失败不能证明只在释放阶段关闭重放也一定无效。 + +### 调参时要注意:刚体的旧阻尼字段不会改变 Newton 运动 + +**状态:已核对当前构建和运行时赋值路径,并用安装包 CPU 模型检查确认;不是 MJVBDV2 独有的新问题。** + +例程给杯子、勺子等刚体设置的 `linear_damping` / `angular_damping`,在当前 Newton 路径中只保留为属性记录。读回数值变了,不代表运动真的受到了更强阻尼。安装包检查将两项都从 0 改成 100,质量、惯量、速度、关节阻尼及接触材料等相关模型数组完全相同。 + +这与已经用于 Place、Assemble 调参的机械臂驱动阻尼不同;后者通过关节驱动配置实际进入求解。旧 `mujoco_warp` 和新 `mjvbd_v2` 共用上述刚体构建路径,因此不能据此解释两者所有差异,也不应通过放大这些无效字段来“修复”滑转。 + +DexSim 另有显式施加角阻尼力矩的运行时控制,但它需要单独注册,系数是力矩阻尼系数,不能直接照搬这里的旧数值。目前没有把它叠加到任务候选中,仍先完成接触摩擦与刚度的独立验证。证据见 `coupling_tuning/raw/r2_rigid_damping_mapping_cpu.json`。 + +### 2. 部分抓取、搬运任务与旧 solver 表现不同 + +**状态:本轮调参后部分恢复,尚不能整体认定对齐。** + +第二轮已经取得几项改善: + +- 纸杯的抓起和搬运已经能完整执行,但姿态精度不稳定。最新 11.5 mm 正式入口误差为 2.90 mm / 11.32°,而同闭合量此前缓存为 5.01 mm / 3.57°,旧 MuJoCo 独立规划为 3.69 mm / 2.88°。因此不能把缓存的一次改善写成已经对齐;当前精确缓存重复用于定位差异,未增加新的物理参数组合。 +- 同一接触配置用于 PickUp 后,最终差约 1.9 mm / 2.1°;Pour 完整 2933 步后差约 3.4 mm / 0.52°,均没有掉落。AxisAlign 恢复了对齐目标轴的效果,但旧对照自身掉落,暂不称为严格对齐。 +- 双臂托盘使用手指摩擦 4 和接触历史后,26、22.5、23 mm 闭合目标均完成搬运及额外 4.8 秒保持,22 mm 则只抬起了一部分。选择 23 mm 接入 UR5 塑料托盘入口;该候选最终与旧 MuJoCo 相差约 3.7 mm / 4.4°,距任务目标约 7.2 mm,旧 MuJoCo 为 9.3 mm。正式入口重新规划后也完整完成 888 步,得到一致结果;仍不能推广到其他尺寸及所有物体。 +- Place 需要额外调整:保留原 mimic,使用接触 alpha=0.5,并把机械臂阻尼降至 1000。四次完整回放均保持到释放,水平落点误差约 0.52–3.77 mm,其中一次直接使用新安装包。配置已接入 UR5 V2 教程;落地朝向约在 82.1–94.8° 之间波动,姿态差异尚未消除。正式入口重新规划随后完成 778 步,水平误差 2.35 mm,yaw 85.31°,与旧低阻尼对照姿态差约 0.37°,末尾稳定;仍未覆盖初态扰动。 + +Place 的诊断过程也排除了一个过于简单的解释:降低 mimic 阻尼虽然改善了手指跟随,却没有消除掉落;接触参数和机械臂驱动响应都参与了结果。详细试验保留在调参记录中。 + +换手原无等待配置不稳定,重复曾侧倒。保留左手 7 mm/右手 5.5 mm、手指摩擦 4、接触历史和原机械臂驱动,增加释放前一秒等待后,三次完整运行均直立停稳,水平误差约 13–16 mm。等待已接入 UR5 竖直罐子的教程,正式源代码入口随后完整完成 1776 步,水平误差 12.36 mm、直立倾斜 0.020°,最后两秒稳定。最终朝向仍有波动,重新规划生成的指令也与缓存几乎相同,尚未证明不同初态下的稳定性。 + +推物也得到改善:接近距离保持 30 mm,将推进补偿从 18 mm 调到 30 mm 后,完整任务的水平误差为 8.59 mm、姿态误差 0.82°,均优于旧对照的 10.35 mm / 1.78°。38 mm 补偿虽然进一步减小位置误差,却使姿态误差升至 6.44°,暂不采用。30 mm 候选随后三次完整运行的水平误差为 8.59 / 6.79 / 8.95 mm,姿态误差为 0.82 / 2.62 / 7.07°;其中一次重新规划,位置改善但姿态仍有波动,不能直接推广为所有物体的默认值。 + +这些是完整固定轨迹回放的证据,仍需正式入口实时重新规划和重复验证,不能推广为全部任务已经对齐。Place 的旧 MuJoCo 重复运行也会出现不同落点,需要同时报告技能效果和新旧轨迹差异,不能只挑一次成功结果比较。 + +Pour 还有已确认的初始化错误:5 cm 方块中心原来在 z=0,一半体积穿地,使规划器读到被顶起后的错误位置。现在改为半边长高度,V2 再留出 1 mm 初始间隙。**当前全局地面参数保持原值。** 之前清零地面余量的尝试因 locomotion 回归而撤回,上述最新 Pour 结果不依赖这项修改。详见 [第二轮调参记录](coupling_tuning/round2.md)。 + +下文是第一轮历史结果,并不代表所有候选的最新状态。 + +2026-09-17 补充:对 UR5 的 DH 夹爪,把 5 cm 方块的闭合目标从 0.036 m 改为 0.013 m 后,PickUp 和 AxisAlign 已通过完整动作及最终保持。PickUp 的最终物体中心与同轨迹 MuJoCo 对照相差约 1.9 mm;AxisAlign 最终目标轴误差约 0.75°。Place 的候选仍在搬运途中提前掉落,最终落点接近目标不能算成功,候选改动已撤回。单独增加迭代、启用接触历史或改变刚度等尝试没有恢复原来过大的闭合目标。MoveHeldObject 的 0.0115 m 候选完整跑完后没有掉落,但最终位置误差约 11.4 mm、姿态误差约 23.8°,尚未恢复目标位姿,未设为默认。 + +原因是旧 MuJoCo 的接触反力会把手指挡在约 0.012 m,V2 单向耦合则让手指真的闭合到 0.036 m。对已知物体标定闭合目标可以改善效果,但不能当作原始控制命令完全不变的替换。 + +补充几何核对:DH 夹爪原始碰撞网格在采样抓持区域的开口约 73.5–75.1 mm,不是规划模型填写的 100 mm。按关节运动计算,36 mm 目标会把间隙缩到约 1.5–3.1 mm,13 mm 目标则约为 47.5–49.1 mm。通用闭合适配必须使用实际夹爪标定,不能直接从规划模型最大开口换算。这是静态几何证据,尚未包含碰撞余量与求解过程。 + +第一轮 Pour 的无效固定轨迹在旧 MuJoCo 下也没抓住物体;第二轮已定位其初始化原因,并重新建立有效对照。第一轮筛选和验证边界见 [第一轮调参记录](coupling_tuning/report.md)。下面保留的是**上一轮沿用旧参数时的观察**,不是调参后的结果。 + +最常见的现象是:机器人按规划运动了,但物体没有被稳定夹住,抬起后滑落,或者没有到达预期位置。 + +| 场景 | MJVBDV2 的实际表现 | 旧 solver 的对照 | +| --- | --- | --- | +| PickUp | 方块短暂抬起后滑落,最终中心高度约 2.6 cm | 最终中心高度约 18.2 cm | +| MoveHeldObject | 杯子滑落,没有到达搬运目标 | 最终物体位置距目标约 3.7 mm | +| Pour | 方块没有保持夹持,倾倒动作未达到预期效果 | 对照中方块最终仍保持在约 18.1 cm 高处 | +| 双臂协作抬托盘 | 托盘只部分抬起,最终位置距目标约 6.6 cm | 距目标约 9.5 mm | + +这些场景涉及机器人与自由刚体的接触。V2 的求解方式和反力传递与旧 solver 不同,是需要重点检查的因素;目前没有逐项证明每个失败都只有这一个原因。 + +也不能据此认定所有 manipulation 都不能用。已有 UR5 抓放流程在一次运行中连续完成了三轮抓放,但那不是多次独立实验得出的成功率。 + +诊断时尽量固定初态和机械臂路径,逐项改变夹持指令、接触或驱动参数,观察物体从哪一阶段开始偏离。验收时再比较完整任务效果,并明确哪些参数或时序已经改变。放慢动作后成功可以是有效适配,但不能据此声称原时序直接替换已经对齐。 + +### 双臂放置的桌子:外观和碰撞形状没有对上 + +**状态:小补丁已构建并安装,正常安装入口检查通过,尚未提交或推送。** + +桌面看起来在 0.65 m 高,但实际碰撞网格最高只有约 0.553 m,横向也有偏移。旧 MuJoCo 和 V2 都受影响,面包与平底锅还没被抓住就掉下去了。这个现象不能用来判断 V2 的抓取能力。 + +原因是单凸包构建时重新用 Newton 读取 GLB,与 DexSim 渲染导入采用了不同坐标。改为使用已导入顶点后,桌子的碰撞坐标与外观恢复一致。面包还存在约 20 mm 的初始穿透;消除穿透后,两种 solver 的启动诊断都能让物体留在桌上。正式修正保持单凸包,不改变刚体所有权;没有增加新的仓库测试。几何修正后的旧 MuJoCo 完整动作已验证:前置抓取恢复,但面包仍在后续协作搬运中甩出;V2 候选继续验证。 协作放置源码中始终不动的桌子也已改成 static,确保它进入 V2 的 MuJoCo 接触模型;这不等于抓持问题已修复。 + +### 双臂放置:旧缓存不能代表正常规划,锅也没有准确到位 + +**状态:正常重新规划已完整运行,仍未达到预期落放效果;继续定位,不据此判定单向耦合不适用。** + +最新候选正常生成三段计划,前两段与旧缓存相同,第三段却明显不同:左手末端峰值速度约为旧缓存的六分之一。面包这次没有早早甩出,最后停在锅所在区域,但朝向几乎翻转,锅本身也偏离目标约 94 mm,不能只看“没有落地”就算成功。 + +释放前,右手离它应该到的位置约 78 mm,锅在手里的相对位移约 8 mm。这说明需要先查右臂为什么没走到位,继续提高夹爪摩擦并不对症。旧 MuJoCo 随后回放同一条新轨迹,右手同样偏离约 78 mm;协作阶段采样持续有右上臂与桌子的接触,驱动与约束力矩大幅对抗,两侧右臂实际关节最大差仅 2.15e-6 rad。这部分大误差并非 VBD 特有,需要先修正场景或路径。详细证据见[第二轮调参记录](coupling_tuning/round2.md)。 + +补充几何核对:右手目标姿态会让右上臂碰撞形状与桌子实体相交,并非只在桌下空隙中发生凸包假碰撞。这是需要先处理的场景/机械臂姿态问题,继续增大驱动力可能只会加深穿透。离线 IK 找到了其他不撞桌的终点姿态,但尚未验证完整路径;运行时接触对照已支持桌子阻挡这一判断;提高机器人安装位置的新旧正常规划对照仍待结果。当前没有为让示例通过而关闭桌子碰撞或改动规划框架。 + +仅把夹爪余量清零也没有解决双臂放置:面包在协作段被甩到工作区外约 4.9 m,确认剩余动作无法重新抓取后主动停止。该候选只运行了 2200 步,作为失败筛选记录,不计作完整验证。 + +### 3. Twist 是一个需要单独排查的例外 + +**状态:已观察到明显差异,原因尚未确定。** + +Twist 的默认场景是机器人与微波炉,主要对象都是带关节的结构,没有单独的动态自由刚体。因此,不能用“机器人和 VBD 物体之间缺少反力”来直接解释这个例子。 + +已有回放中,V2 的旋钮最终转角约为 **2.154 rad**,旧 solver 约为 **1.092 rad**,而且微波炉门也出现了不同程度的运动。 + +这说明“只要都是 articulation,就一定完全一致”还不能成立。不过,之前这组教程对照并没有严格锁定整段动作序列,规划和采样也不是完全受控的,所以目前记录的是待排查现象,还没有确定是哪一处物理实现出了问题。 + +本轮按要求跳过 Twist。若以后恢复排查,应使用同一段动作回放,并核对两边实际生效的接触、摩擦和关节参数。 + +### 4. Twist 启动时会打印大量 margin 警告,配置的 1 mm 没有生效 + +**状态:已确认,仍存在。警告背后的参数变化是真实的。** + +典型输出是: + +```text +authored margin=0.001... zeroed for NATIVECCD/MULTICCD compatibility +``` + +意思是:碰撞形状原本配置了约 1 mm 的接触提前量,但 solver 为兼容当前 MuJoCo-Warp 的原生接触算法,把它改成了 0。 + +Twist 教程启用了多接触点生成。夹爪有多个碰撞形状,每个相关形状都可能打印一条警告,因此启动时会看到很多行。仿真可以继续,但这 1 mm 的配置确实没有被保留,接触建立的时机可能受到影响。 + +这段逻辑在 MR 原始分支里就存在,不是本次 Contact Query 补丁新增的。目前也还没有证据证明它就是 Twist 新旧表现不同的原因。 + +**不能直接照警告建议,把 `use_mujoco_contacts` 改成 `False`。** 当前 V2 的纯 MuJoCo 分支要求这个选项为 `True`,直接修改会报错。警告里的建议适用于通用 MuJoCo solver,没有考虑 V2 的这一限制。 + +后续需要确认这类场景实际需要怎样的接触距离设置,并让警告给出适合 V2 的说明。只隐藏日志不会恢复被清零的参数。 + +### 5. Contact Query 能用,但混合场景还不能提供同样的力反馈 + +**状态:纯 MuJoCo 分支已接通并验证;混合分支的能力仍有限。** + +| 场景 | 当前能拿到什么 | +| --- | --- | +| 带关节机器人+静态地面,使用纯 MuJoCo 分支 | 接触对象、位置、法线、距离,以及法向冲量和摩擦冲量 | +| 包含 VBD 动态刚体的混合分支 | 接触几何信息;还没有与纯 MuJoCo 分支等价的接触冲量、力反馈 | + +因此,“能查到碰撞位置”和“能获得训练需要的接触力”需要分别确认。使用接触力作为 observation 或 reward 时,必须核对当前分支实际支持的字段。 + +此外,接口提供的冲量不是直接的力。本次足端力比较是用冲量除以对应的物理子步时间得到的,不能改用策略更新间隔来换算。 + +### 6. 位置控制下,`get_qf()` 不能当成实际电机输出力矩 + +**状态:已确认的现有 Newton 接口限制,不是本次证明出的 V2 独有问题。** + +在本次位置 PD 控制的行走验证中,`get_qf()` 读到的数组一直为 0。检查后确认,当前 Newton 路径读的是直接输入给关节的力矩命令,没有包含 PD 控制器实际产生的力矩。 + +**影响:不能拿这个全零数组来证明两边电机力矩一致,也不能直接用它计算实际力矩或能耗惩罚。** 如果训练任务需要这些奖励项,需要另行接通实际输出力矩,并确认它的含义。 + +## 接入过程中已经修复的问题 + +这些问题已经纳入补丁。下面保留记录,便于以后遇到类似现象时追查。 + +| 问题 | 当时的影响 | 当前处理 | +| --- | --- | --- | +| 接触查询和部分关节操作找不到 V2 内部的 MuJoCo solver | 原来直接访问 MuJoCo 的代码不能直接用于 V2,碰撞形状编号也需要转换 | 已接通内部 solver 的访问和编号转换;纯 MuJoCo 接触反馈已做数值对照 | +| 更新机器人姿态时,可能把自由刚体也写回旧位置 | VBD 已经算出的物体运动被旧关节数据覆盖 | 已限制更新范围,保留 VBD 计算出的自由刚体位置 | +| 瞬移物体后,物体会继续异常移动 | 零重力下把物体从 x=3 移到 x=4,之后两步却跑到 x=5 | 已同步求解器内部的历史状态;复测可以停在 x=4,并保留原有速度 | +| CUDA Graph 预热后,公开状态恢复了,内部历史没有恢复 | 正式仿真的第一步可能使用预热留下的状态,影响初始运动 | 已补齐内部状态恢复,并验证重建后继续步进 | +| VBD 中用于表示机器人的碰撞代理姿态处理不正确 | 代理可能使用回退后的姿态参与接触计算 | 已让不由 VBD 积分的代理保留传入姿态 | +| 混合分支关闭 MuJoCo 接触后,接触缓冲区容量为 0 | 某些摩擦计算路径会除零 | 已保留一个不使用的接触槽;没有因此重新启用 MuJoCo 接触 | +| reset 不能正确覆盖 V2,或把自由刚体的内部关节误算成机器人 | 环境复位可能报错或清理范围不正确 | 已适配;验证了单环境、部分环境和全部环境 reset | +| Gym reset 时删除并重建同名资产失败 | modular Gym 不能正常连续运行多个 episode | 已补齐同名替换:新对象使用新初值,其他对象保留原状态 | + +教程里还修复了几项与 solver 对接有关的问题,包括杯子缩放、Pour 的轨迹重采样、速度跟踪读取状态的时机,以及关闭物理碰撞后规划器丢失障碍物几何。这些修复让程序能正确执行,但不会自动解决抓取接触效果的差异。 + +## 行走已经确认到什么程度 + +在 **ANYmal-C+静态地面/平台** 范围内,当前适配版本与旧 solver 的行为接近。4 个环境分别执行前进、侧移和转弯,每个环境运行 30 秒,并覆盖了局部 reset。 + +相同动作输入下,正常行走时的关节位置最大差约为 **0.0000011 rad**,足端法向力分量最大差约为 **0.015 N**。旧 solver 自己重复运行也会有同量级差异;接触数量和接触/离地标记一致。 + +让两边各自运行策略时,转弯环境会出现步态相位差:机身位置最大差约 1.78 cm,旧 solver 自己重复运行约 1.92 cm。因此,可以说这组场景的行走效果接近,不能说逐帧完全一致。 + +把机器人强行移入地面的压力检查中,两种 solver 都出现了较大的接触力波动。这部分结果没有计入上面的正常行走指标。 + +**实际训练是否能达到相同效果还没有验证。** 本次没有进行 PPO 从零训练,也没有验证具体训练任务的完整 reward、done、域随机化或复杂地形。行走回放通过不等于学习曲线已经对齐。 + +## 一起发现、但不能归为 V2 新增的问题 + +- Press 的按钮行程在新旧 solver 中都出现越限;Slide 在两边都没有完全关上抽屉;OpenDoor 在两边都有打开后回摆。这些任务本身仍需改进,不能只按任务失败就判定 V2 不对齐。 +- 现有 Newton 后端尚未支持通用刚体约束,旧 solver 路径也没有这项能力。 +- USD 导出可以生成文件,但还没有完整写出 Newton 的刚体、关节等物理信息,不能当成可完整还原的仿真场景。 +- NeuralPlanner 缺少匹配的模型资源,尚未完成对应验证。 +- 有些入口不加 `--physics newton` 时会运行默认后端。比较结果前需要确认实际 backend 和 solver,不能只根据“安装了哪个 DexSim”判断运行路径。 + +## 这份记录对应哪一版 + +- EmbodiChain:[draft PR #645](https://github.com/DexForce/EmbodiChain/pull/645),代码提交 `75d3c9a3`。 +- DexSim:[MR !1345](http://192.168.3.16/Engine/dexsim/-/merge_requests/1345),适配补丁已提交到 `MJVBDV2` 分支,提交 `f7d60c2a2`。 +- 验证使用的是包含这些补丁的本地构建,不代表未经适配的早期分支也具备同样能力。 + +此前覆盖了 15 项内置原子技能、18 个原子教程和另外 26 个 Newton 示例入口,但没有穷举所有机器人、参数和规划器组合。最后一轮补丁后,只对受影响场景进行了针对性复测,没有把全部入口再跑一遍。操作任务的多数结果来自单次回放,不能当成统计成功率。 + +详细数据见[示例与原子技能验证报告](/home/oem/code/EmbodiChain/outputs/validation/mjvbd_v2/report.md)和[行走对齐验证报告](/home/oem/code/EmbodiChain/outputs/validation/mjvbd_v2/locomotion_alignment/report.md)。 + +## 最新补充:舀冰旧基线已经跑通 + +旧 MuJoCo 的接触和约束容量增到 8192 / 16384 后,完整完成了 3300 步舀冰与末尾保持,没有 CUDA 错误或容量溢出。勺子没有脱手,最后距目标约 5.6 mm / 1.7°,11 块冰块处于勺斗范围内并已抬升约 29–34 cm。之前“没有有效旧基线”的状态因此更新;具体指标及限制见第二轮记录。V2 的相同种子完整对照尚待运行,不能先宣布舀冰已经对齐。 + +### 2026-09-18 补充:舀冰的完整失败证据 + +新旧 solver 都完整运行到 3300 步,包含全部舀冰动作和末尾保持。旧 MuJoCo 保持勺子,并在勺碗区域带出 11 块冰;当前 MJVBDV2 在抬升时脱手,最终勺子落地,没有带出冰块。两侧手指、勺子的材料输入一致,但接触求解方式不同。此结果确认当前配置没有恢复任务效果,不足以证明单向耦合永远无法完成该任务。 + +正在验证的改进仅涉及手指摩擦、接触历史、闭合姿态和分段闭合指令。没有改变刚体归属,没有附着物体,没有增加仓库测试或 benchmark。分段闭合仍占原来的 100 个物理步,避免用额外等待掩盖差异。 + +### 交接的重复性仍不合格(2026-09-18 补充) + +交接不能只看最好的一次。正式安装包入口两次完整运行的水平落点误差分别为 31.60 mm 和 15.37 mm,两次指令几乎一致。此前表现较好的缓存候选再次完整运行时还出现侧倒,误差 45.91 mm。因此,当前夹爪和接触配置仍不足以稳定替换旧 solver,教程内的参数只能视为本地候选。 + +后续分别检查“机械臂尚未到位就松手”和接触历史中的黏着锚点。先前数据已把主要差异放大阶段定位到释放和落地,但尚未证明是哪一项机制造成失败;不能用未经验证的原因解释掉这些失败。 + +### 舀冰的旧对照也需要重新核实(2026-09-18 补充) + +当前安装包的旧 MuJoCo 补充运行在第 868 步出现冰块非有限位姿,接近阶段已可见飞散,未完成任务。此前成功的旧侧 3300 步记录早于本轮补丁安装,不能直接当作当前版本的严格基线。正在核对单凸包网格导入并补当前安装包的完整旧对照;尚无证据断言该发散由网格补丁造成。V2 此前抬升时丢勺子的失败仍然有效,并未因旧对照异常而撤销。 + +### 舀冰抓持已有改善,但没有提前判定任务通过(2026-09-18) + +正常安装包下,只将手部摩擦设为 4 并启用接触历史,原闭合目标不变,V2 完成了前 1200 步抓持和抬升:勺子抬高 33.7 cm,没有掉落。原闭合目标下相对手部仍有约 3.4 mm / 5.7° 的滑转;随后小幅标定六个手指目标,完整抬升的相对变化降至约 1.14 mm / 1.49°。两组均未脱手,但完整进入冰堆、舀出冰块和保持尚待验证。该结果支持继续调参,不能据此宣称舀冰已对齐。 + +另外,一个代表冰块在新旧网格导入路径下的凸包顶点、尺寸、体积,以及同一 CPU 算法计算的质量、质心和惯量均完全一致。目前没有证据把旧侧发散归因于该冰块的外形或惯量被补丁改坏。 + +### 舀冰盖子驱动是一个新的待验证因素 + +盖子初始是打开的,但配置的驱动没有生效,机械臂接近阶段盖子会明显转动。旧侧冰块飞散也发生在这一阶段,尚不能证明两者是因果关系;V2 中盖子同样转动,冰块却保持稳定。已准备新旧共同的对照,用有限力矩的位置驱动保持盖子打开,仍保留动态关节和碰撞。结果待运行,未把它当作已经修复的问题。 + +### 完整舀冰、双臂放置仍有实质差距 + +舀冰标定候选已经完整完成 3300 步,能带出 3 块冰;只改摩擦和接触历史、保留原闭合目标时为 1 块。但历史旧侧能带出 11 块,标定候选末尾目标误差仍有约 22.69 mm / 14.76°,抓持和末尾保持还不够稳定。分段闭合在修正诊断脚本后完成了阶段验证,反而比闭合姿态标定滑转更大,暂不采用。 + +双臂放置的右夹口修正后,抬锅明显稳定了,但面包仍在协作阶段脱手。放慢第三阶段两倍没有得到成功结果;其中 V2 在前面未改变时序的抓取阶段就掉面包,不能把这次失败归咎于减速。 + +保持容器盖子打开的旧侧舀冰对照已完整运行,没有再次数值发散,却没有带出冰块。因此,“不崩溃”和“完成任务”在这组场景中也是两回事,盖子驱动尚不是完整修复。 + + +### 舀冰旧基线也不稳定,同步开关不是修复(2026-09-18 补充) + +当前安装包中,旧 MuJoCo 显式关闭 CUDA 同步后完整跑完 3300 步,最后带出 5 块冰,但勺子在手里已滑转约 20.8°;显式开启同步的同配置运行则在第 700 步出现冰块非有限位姿。历史上开启同步曾带出 11 块,说明不能把这个开关当作解决办法,也不能把历史最好的一次当作稳定基准。两组仅各一次,尚不足以确定同步开关与失败的因果关系。 + +V2 扩大容量后完整带出 5–6 块冰的结果有效,但不能因为本次旧侧退化就宣布对齐。当前继续完成手部摩擦、刚度的独立验证,并保留完整动作和保持阶段的评价。详见第二轮记录中的同步对照及原始日志。 + + +### 张开夹爪后,接触余量仍可能妨碍释放(2026-09-18 补充) + +Assemble 中,张手后额外等一秒仍留下约 17.82 mm 偏心。等待结束时手指已基本全开,但罐子维持约 9.85° 倾斜;记录的手指接触点间隙约 0.81–0.94 mm,仍处于 VBD 的 1 mm 接触余量内,相关接触的法向约束乘子也没有归零。继续等待并没有让它脱离这些接触。 + +只改接触 alpha=0.5 的另一遍完整运行则正常落到底座,偏心 6.93 mm 并停稳,正在安排重复验证。另增加只改夹爪 margin 的完整对照,检查原 MuJoCo 中被清零、VBD 中仍保留的余量是否影响释放;尚未证明清零就能解决,未改正式默认值。这个线索来自已有接触点的几何与乘子记录,不是精确碰撞重算或完整力测量。 + + +### alpha 的改善不能跨任务直接推广(2026-09-18 补充) + +Assemble 使用 alpha=0.5 的单次结果改善到 6.93 mm 偏心,但同样调整在双臂放置中使面包第一次抬升就失败,最后仍留在桌上;锅的抓持反而更稳定。完整 3032 步正常退出也不能算任务成功。当前不将 alpha=0.5 设为通用默认,继续用完整任务验证每个候选。 + + +### 纸杯的较好参数仍未证明稳定(2026-09-18 补充) + +11.5 mm 闭合量曾在完整缓存回放中取得约 5.01 mm / 3.57° 的目标误差,但正式入口正常重新规划的一次运行中,杯子在手内的转动明显更大。两次记录到的材料、驱动和初态一致,机械臂命令几乎相同;到第 4587 步,杯子姿态已相差约 8.09°。本次仍在运行,不能当作完整失败,但之前一次好结果也不足以验收这套参数。 + +目前需要确认的是抓持对微小差异是否敏感,以及是否存在尚未记录的模型或执行差别。尚未证明是 GPU 非确定性,也未证明必须改双向耦合。详细对照见第二轮记录的纸杯 11.5 mm 小节。 + + +### 少量接触条目的法向量为零,部分诊断数值需作废(2026-09-18 补充) + +装配的一次完整运行中,三个采样点出现零法向接触。原诊断程序据此算出的“1 mm 穿透”没有物理意义,现已标为不可计算并保留原始数据。张手等待结束时的四个接触法向正常,因此先前关于释放余量的线索仍成立,但不能借这些零法向数据推断夹持力或穿透。 + +这是一项待定位的接触数据问题,尚未证明源于碰撞生成、记录时机或 solver 内部处理,也未证明是任务失败的原因。后续接触记录会显式标识有效性,避免继续误读。 + + +### 纸杯最新完整结果:停止后抓得住,搬运转偏尚未解决(2026-09-18 补充) + +11.5 mm 正式入口现已完整跑完 7460 步,最终位置误差 2.90 mm、姿态误差 11.32°;最后 4.8 秒只变化约 0.63 mm / 0.15°。主要偏差在搬运过程中形成,停止后没有继续明显滑动,也没有恢复正确朝向。 + +此前同闭合量缓存回放的姿态误差只有 3.57°,旧 MuJoCo 独立规划约 2.88°。因此当前不能认为纸杯已稳定对齐,也不能把单次缓存的好结果作为参数已经标定成功的证明。精确缓存重复已排队,仍未确定根因。 + + +### 双臂放置有明确的机器人—桌子冲突,不能全归因于 VBD(2026-09-18 补充) + +旧 MuJoCo 回放与 V2 相同的新轨迹后,右手同样偏离目标约 7–8 cm。协作阶段记录持续出现右上臂与桌子的接触,驱动和约束产生很大的反向力矩;两侧右臂实际关节最大差仅约 2.15e-6 rad。此前几何核对也发现目标姿态与桌子相交。 + +这说明应先处理安装位置或运动路径,继续加摩擦不能解决被桌子挡住的问题。提高安装位置的新旧完整对照已排队,仍需验证整段路径和最终任务效果。本次旧侧使用了和 V2 一样的标定夹爪命令,是定位原因的对照,不是原教程的成功基线;面包抓持问题也尚未解决。 + + +### 舀冰也已记录到真实零法向接触(2026-09-18 补充) + +观测器修复后的摩擦 8 重跑中,第 2360 步起保存到了勺子与手掌、手指之间的零法向接触,并带有非零约束乘子。新记录器不再据此中断任务,也不把这些乘子当成有效的法向/切向力,完整运行仍在继续。 + +这个现象需要继续定位,不能通过增加摩擦来解释或修复数据本身。但当前还不能证明它是滑转原因,更不能据此断言 MJVBDV2 不适合舀冰;已安排的碰撞生成后观测将帮助区分异常出现的环节。 + + +### 混合模式启用了两套碰撞管线,之前的对齐结论需要限定(2026-09-18 更正) + +本轮为了恢复机器人与地面的接触,把 DexSim 混合模式中的 `mujoco_options.disable_contacts` 从强制 `True` 改为默认 `False`,同时仍使用 `use_mujoco_contacts=True`。这会同时运行 MuJoCo-Warp 和 Newton 两套碰撞管线,并不是单纯保留关节求解。此前把它笼统称为“接触修复”不够准确:它改变了原生 V2 混合模式的接触分工。 + +当前修改后的分工是:MuJoCo-Warp 检测并求解其紧凑模型内的 articulation 与静态环境、允许的 articulation 之间的接触;Newton 检测 VBD 自由刚体相关接触,由 VBD 求解。自由刚体没有进入 MuJoCo 模型,机器人在 VBD 侧只是无动力学响应的运动代理,因此两套管线并不意味着同一物体被重复施加接触响应。不过,双管线的开销、接触参数差异和数据查询范围仍需分别处理。 + +原生混合模式关闭 MuJoCo 接触,又不把 VBD 反力传回机器人。这意味着 Newton 检测到机器人与地面接触,也不能仅靠这条单向路径给机器人提供支撑力。若要求混合模式只用 Newton 碰撞,同时保留动态 articulation 的接触响应,需要将合适的 Newton 接触映射给 MuJoCo 求解;当前 V2 没有接通这条路径,构造器还明确拒绝 `use_mujoco_contacts=False`。这不是只改一个开关就完成的适配。 + +因此,混合场景 locomotion 的约 1e-6 rad 级同动作对齐,是在额外开启 MuJoCo 原生接触之后得到的,不能当作原生 V2 单向混合模式已经对齐的证据。纯 articulation 路线的结果需要单独看待;两者都不能替代完整训练收敛验证。双臂放置中机器人受桌子阻挡的对照,同样依赖这项开启接触的修改。 + +本次提交保留现有实验结果和配置,但不把双管线方案视为已确认的最终架构。DexSim 的这项补丁及凸包导入坐标修复仍是本轮环境的额外依赖,未包含在 EmbodiChain 的 GitHub 提交中。仅拉取 EmbodiChain 代码,不能保证复现本轮结果。 + +### 本轮提交范围与参数适用边界(2026-09-18 补充) + +本轮提交包含按场景选择的夹爪闭合目标、部分夹爪摩擦系数 4、接触历史与 latest 匹配;Place 额外使用 alpha=0.5 和机械臂 damping=1000;HandOver 在释放前等待 1 秒;Pour/Place 修正初始高度;固定桌子使用 static;Scoop 将 VBD 单刚体接触容量提高到 256。夹爪数值是关节位置目标,不是夹口宽度。 + +这些调整有机器人和物体范围,尚不是通用参数。其中纸杯 0.0115 闭合目标仍是候选,完整运行的姿态误差达到 11.32°,不能称为稳定标定成功。提高摩擦、刚度、安装高度及额外沉降等待等临时对照没有全部写进正式示例。未新增仓库测试或 benchmark 文件,已有测试随配置修改;原始实验日志和临时诊断脚本未纳入本次提交。 diff --git a/scripts/tutorials/atomic_action/axis_align.py b/scripts/tutorials/atomic_action/axis_align.py index 912f73934d..c805e4d2fe 100644 --- a/scripts/tutorials/atomic_action/axis_align.py +++ b/scripts/tutorials/atomic_action/axis_align.py @@ -42,6 +42,8 @@ from embodichain.lab.sim.shapes import CubeCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( + DEFAULT_GRIPPER_CLOSE_QPOS, + MJVBD_V2_CUBE_CLOSE_QPOS, add_ur5_gripper_robot, clone_local_pose_from_first_env, create_antipodal_semantics, @@ -149,7 +151,14 @@ def main() -> None: sim = create_tutorial_simulation(args) robot = add_ur5_gripper_robot(sim, tcp_z=0.15) obj = create_align_object(sim) - hand_open, hand_close = get_hand_open_close_qpos(robot) + hand_open, hand_close = get_hand_open_close_qpos( + robot, + close_qpos=( + MJVBD_V2_CUBE_CLOSE_QPOS + if args.robot == "ur5" and sim.physics.solver_type == "mjvbd_v2" + else DEFAULT_GRIPPER_CLOSE_QPOS + ), + ) initialize_pre_pick_robot_pose(robot, obj, hand_open) motion_gen = create_toppra_motion_generator( robot, diff --git a/scripts/tutorials/atomic_action/coordinated_pickment.py b/scripts/tutorials/atomic_action/coordinated_pickment.py index 33c62d5e03..9169edc36b 100644 --- a/scripts/tutorials/atomic_action/coordinated_pickment.py +++ b/scripts/tutorials/atomic_action/coordinated_pickment.py @@ -161,6 +161,8 @@ class PickmentObjectPreset: PICKMENT_HAND_INTERP_STEPS = 10 PICKMENT_HOLD_STEPS = 4 ROBOTIQ_2F_140_CLOSE_QPOS = 0.7 +# Calibrated for the plastic tray and UR5/DH PGI one-way contact. +MJVBD_V2_TRAY_CLOSE_QPOS = 0.023 TRAJECTORY_SIM_STEPS = 4 @@ -189,6 +191,8 @@ def parse_arguments() -> argparse.Namespace: def create_dual_robot( sim: SimulationManager, robot_type: TutorialRobot, + *, + newton_gripper_friction: float | None = None, ) -> Robot: """Create the selected dual-arm robot with its matching grippers.""" return add_dual_tutorial_robot( @@ -198,6 +202,7 @@ def create_dual_robot( urdf_name=f"dual_{robot_type}_coordinated_pickment", tcp_z=GRIPPER_TCP_Z, solver="pytorch", + newton_gripper_friction=newton_gripper_friction, ) @@ -395,6 +400,12 @@ def run_coordinated_pickment_demo( hand_close_qpos = ( ROBOTIQ_2F_140_CLOSE_QPOS if args.robot == "ur10" else preset.hand_close_qpos ) + if ( + args.robot == "ur5" + and args.object == "plastic_tray" + and sim.physics.solver_type == "mjvbd_v2" + ): + hand_close_qpos = MJVBD_V2_TRAY_CLOSE_QPOS left_open, left_close = get_hand_open_close_qpos( robot, hand_control_part="left_hand", @@ -527,8 +538,27 @@ def main() -> None: sim = create_tutorial_simulation( args, arena_space=3.0, + newton_solver_options=( + { + "vbd_options": {"rigid_contact_history": True}, + "collision_options": {"contact_matching": "latest"}, + } + if args.physics == "newton" + and args.robot == "ur5" + and args.object == "plastic_tray" + else None + ), + ) + use_vbd_grasp = ( + args.robot == "ur5" + and args.object == "plastic_tray" + and sim.physics.solver_type == "mjvbd_v2" + ) + robot = create_dual_robot( + sim, + args.robot, + newton_gripper_friction=4.0 if use_vbd_grasp else None, ) - robot = create_dual_robot(sim, args.robot) run_coordinated_pickment_demo(args, sim, robot) diff --git a/scripts/tutorials/atomic_action/coordinated_placement.py b/scripts/tutorials/atomic_action/coordinated_placement.py index 6bd7c08e3f..362ef304a3 100644 --- a/scripts/tutorials/atomic_action/coordinated_placement.py +++ b/scripts/tutorials/atomic_action/coordinated_placement.py @@ -235,7 +235,8 @@ def create_table(sim: SimulationManager) -> RigidObject: static_friction=0.95, restitution=0.01, ), - body_type="kinematic", + # This fixture never moves; static shapes also enter V2's MuJoCo view. + body_type="static", init_pos=list(TABLE_INIT_POS), init_rot=list(TABLE_INIT_ROT), ) diff --git a/scripts/tutorials/atomic_action/hand_over.py b/scripts/tutorials/atomic_action/hand_over.py index 8b06150de8..8a401fb1a6 100644 --- a/scripts/tutorials/atomic_action/hand_over.py +++ b/scripts/tutorials/atomic_action/hand_over.py @@ -19,6 +19,7 @@ from __future__ import annotations import argparse +import math import sys from pathlib import Path @@ -88,6 +89,9 @@ # --------------------------------------------------------------------------- HAND_CLOSE_QPOS = 0.04 +MJVBD_V2_CAN_LEFT_CLOSE_QPOS = 0.007 +MJVBD_V2_CAN_RIGHT_CLOSE_QPOS = 0.0055 +MJVBD_V2_RELEASE_SETTLE_DURATION = 1.0 HANDOVER_SAMPLE_INTERVAL = 220 HANDOVER_HAND_INTERP_STEPS = 10 HANDOVER_PRE_GRASP_DISTANCE = 0.08 @@ -119,6 +123,8 @@ def parse_arguments() -> argparse.Namespace: def create_dual_robot( sim: SimulationManager, robot_type: TutorialRobot, + *, + newton_gripper_friction: float | None = None, ) -> Robot: """Create the selected dual-arm robot with its matching grippers.""" return add_dual_tutorial_robot( @@ -131,6 +137,7 @@ def create_dual_robot( hand_stiffness=1e3, hand_damping=1e2, hand_max_effort=1e4, + newton_gripper_friction=newton_gripper_friction, ) @@ -204,11 +211,20 @@ def run_handover_demo( planner=getattr(args, "planner", "trapezoidal"), ) + use_vbd_grasp = ( + args.robot == "ur5" + and not args.is_horizontal + and sim.physics.solver_type == "mjvbd_v2" + ) left_open, left_close = get_hand_open_close_qpos( - robot, hand_control_part="left_hand", close_qpos=HAND_CLOSE_QPOS + robot, + hand_control_part="left_hand", + close_qpos=MJVBD_V2_CAN_LEFT_CLOSE_QPOS if use_vbd_grasp else HAND_CLOSE_QPOS, ) right_open, right_close = get_hand_open_close_qpos( - robot, hand_control_part="right_hand", close_qpos=HAND_CLOSE_QPOS + robot, + hand_control_part="right_hand", + close_qpos=MJVBD_V2_CAN_RIGHT_CLOSE_QPOS if use_vbd_grasp else HAND_CLOSE_QPOS, ) final_pose = torch.eye(4, dtype=torch.float32) @@ -285,6 +301,17 @@ def run_handover_demo( if wait_for_user: input("Press Enter to execute the handover...") + release_start = compiled.segment(0, "receive_release").start + + def settle_before_release(step_index: int, _total_steps: int) -> None: + if step_index == release_start - 1: + # Let the receiving arm stop before opening the gripper. + sim.update( + step=math.ceil( + MJVBD_V2_RELEASE_SETTLE_DURATION / sim.sim_config.physics_dt + ) + ) + replay_trajectory( sim, robot, @@ -293,6 +320,7 @@ def run_handover_demo( video_prefix="handover_auto_play", hold_steps=0, trajectory_sim_steps=TRAJECTORY_SIM_STEPS, + on_trajectory_step=settle_before_release if use_vbd_grasp else None, look_at=HANDOVER_RECORD_LOOK_AT, ) if wait_for_user: @@ -305,8 +333,27 @@ def main() -> None: sim = create_tutorial_simulation( args, arena_space=3.0, + newton_solver_options=( + { + "vbd_options": {"rigid_contact_history": True}, + "collision_options": {"contact_matching": "latest"}, + } + if args.physics == "newton" + and args.robot == "ur5" + and not args.is_horizontal + else None + ), + ) + use_vbd_grasp = ( + args.robot == "ur5" + and not args.is_horizontal + and sim.physics.solver_type == "mjvbd_v2" + ) + robot = create_dual_robot( + sim, + args.robot, + newton_gripper_friction=4.0 if use_vbd_grasp else None, ) - robot = create_dual_robot(sim, args.robot) run_handover_demo(args, sim, robot) serve_tutorial_scene(sim, args) diff --git a/scripts/tutorials/atomic_action/move_held_object.py b/scripts/tutorials/atomic_action/move_held_object.py index e53ca5a1ad..9c164e630a 100644 --- a/scripts/tutorials/atomic_action/move_held_object.py +++ b/scripts/tutorials/atomic_action/move_held_object.py @@ -44,6 +44,7 @@ from embodichain.lab.sim.shapes import MeshCfg, MeshCollisionCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( + DEFAULT_GRIPPER_CLOSE_QPOS, add_tutorial_robot, broadcast_pose_batch, clone_local_pose_from_first_env, @@ -70,6 +71,8 @@ MOVE_HELD_OBJECT_SAMPLE_INTERVAL = 120 HAND_INTERP_STEPS = 12 POST_TRAJECTORY_STEPS = 240 +# Candidate for this cup and UR5/DH PGI; transport orientation remains unaligned. +MJVBD_V2_CUP_CLOSE_QPOS = 0.0115 def parse_arguments() -> argparse.Namespace: @@ -129,15 +132,35 @@ def make_object_target_pose(device: torch.device) -> torch.Tensor: def main() -> None: """Plan MoveEndEffector -> PickUp -> MoveHeldObject.""" args = parse_arguments() - sim = create_tutorial_simulation(args) - robot = add_tutorial_robot(sim, args.robot) + sim = create_tutorial_simulation( + args, + newton_solver_options=( + { + "vbd_options": {"rigid_contact_history": True}, + "collision_options": {"contact_matching": "latest"}, + } + if args.physics == "newton" and args.robot == "ur5" + else None + ), + ) + use_vbd_grasp = args.robot == "ur5" and sim.physics.solver_type == "mjvbd_v2" + robot = add_tutorial_robot( + sim, + args.robot, + newton_gripper_friction=4.0 if use_vbd_grasp else None, + ) obj = create_pick_object(sim) sim.prepare() motion_gen = create_curobo_motion_generator( robot, planner=getattr(args, "planner", "trapezoidal"), ) - hand_open, hand_close = get_hand_open_close_qpos(robot) + hand_open, hand_close = get_hand_open_close_qpos( + robot, + close_qpos=( + MJVBD_V2_CUP_CLOSE_QPOS if use_vbd_grasp else DEFAULT_GRIPPER_CLOSE_QPOS + ), + ) engine = create_simulation_atomic_action_engine( motion_generator=motion_gen, diff --git a/scripts/tutorials/atomic_action/pickup.py b/scripts/tutorials/atomic_action/pickup.py index e35bc902a5..1313bd3cbe 100644 --- a/scripts/tutorials/atomic_action/pickup.py +++ b/scripts/tutorials/atomic_action/pickup.py @@ -40,6 +40,8 @@ from embodichain.lab.sim.shapes import CubeCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( + DEFAULT_GRIPPER_CLOSE_QPOS, + MJVBD_V2_CUBE_CLOSE_QPOS, add_tutorial_robot, clone_local_pose_from_first_env, create_antipodal_semantics, @@ -130,7 +132,14 @@ def main() -> None: robot = add_tutorial_robot(sim, args.robot, tcp_z=0.15) obj = create_pick_object(sim) sim.prepare() - hand_open, hand_close = get_hand_open_close_qpos(robot) + hand_open, hand_close = get_hand_open_close_qpos( + robot, + close_qpos=( + MJVBD_V2_CUBE_CLOSE_QPOS + if args.robot == "ur5" and sim.physics.solver_type == "mjvbd_v2" + else DEFAULT_GRIPPER_CLOSE_QPOS + ), + ) initialize_pre_pick_robot_pose(robot, obj, hand_open) motion_gen = create_curobo_motion_generator( robot, diff --git a/scripts/tutorials/atomic_action/place.py b/scripts/tutorials/atomic_action/place.py index f943bd2e13..973d999657 100644 --- a/scripts/tutorials/atomic_action/place.py +++ b/scripts/tutorials/atomic_action/place.py @@ -42,6 +42,8 @@ from embodichain.lab.sim.shapes import CubeCfg from embodichain.utils import logger from scripts.tutorials.atomic_action.tutorial_utils import ( + DEFAULT_GRIPPER_CLOSE_QPOS, + MJVBD_V2_CUBE_CLOSE_QPOS, add_tutorial_robot, broadcast_pose_batch, broadcast_waypoint_pose_batch, @@ -81,6 +83,8 @@ def parse_arguments() -> argparse.Namespace: def create_pick_object(sim) -> RigidObject: """Create a settled cube for the PickUp and Place sequence.""" + # Start outside the default ground's 1 mm VBD contact margin. + ground_clearance = 0.001 if sim.physics.solver_type == "mjvbd_v2" else 0.0 obj = sim.add_rigid_object( cfg=RigidObjectCfg( uid="cube", @@ -93,7 +97,7 @@ def create_pick_object(sim) -> RigidObject: angular_damping=0.2, newton_contact=sim.is_newton_backend, ), - init_pos=[*OBJECT_XY, 0.5 * OBJECT_SIZE[2]], + init_pos=[*OBJECT_XY, 0.5 * OBJECT_SIZE[2] + ground_clearance], ) ) sim.prepare() @@ -125,15 +129,41 @@ def make_place_eef_poses(device: torch.device) -> torch.Tensor: def main() -> None: """Plan and replay PickUp followed by a multi-waypoint Place.""" args = parse_arguments() - sim = create_tutorial_simulation(args) - robot = add_tutorial_robot(sim, args.robot, tcp_z=0.15) + sim = create_tutorial_simulation( + args, + newton_solver_options=( + { + "vbd_options": { + "rigid_contact_history": True, + "rigid_avbd_contact_alpha": 0.5, + }, + "collision_options": {"contact_matching": "latest"}, + } + if args.physics == "newton" and args.robot == "ur5" + else None + ), + ) + use_vbd_grasp = sim.physics.solver_type == "mjvbd_v2" and args.robot == "ur5" + robot = add_tutorial_robot( + sim, + args.robot, + tcp_z=0.15, + newton_gripper_friction=4.0 if use_vbd_grasp else None, + # Reach the release waypoint before the fingers finish opening. + arm_damping=1000.0 if use_vbd_grasp else None, + ) obj = create_pick_object(sim) sim.prepare() motion_gen = create_curobo_motion_generator( robot, planner=getattr(args, "planner", "trapezoidal"), ) - hand_open, hand_close = get_hand_open_close_qpos(robot) + hand_open, hand_close = get_hand_open_close_qpos( + robot, + close_qpos=( + MJVBD_V2_CUBE_CLOSE_QPOS if use_vbd_grasp else DEFAULT_GRIPPER_CLOSE_QPOS + ), + ) initialize_pre_pick_robot_pose(robot, obj, hand_open) engine = create_simulation_atomic_action_engine( diff --git a/scripts/tutorials/atomic_action/pour.py b/scripts/tutorials/atomic_action/pour.py index 56a39b75cd..e70fbe0123 100644 --- a/scripts/tutorials/atomic_action/pour.py +++ b/scripts/tutorials/atomic_action/pour.py @@ -44,11 +44,14 @@ ) from embodichain.utils import logger from scripts.tutorials.atomic_action.axis_align import ( + OBJECT_SIZE, create_align_object, create_axis_align_semantics, ) from scripts.tutorials.atomic_action.tutorial_utils import ( - add_ur5_gripper_robot, + DEFAULT_GRIPPER_CLOSE_QPOS, + MJVBD_V2_CUBE_CLOSE_QPOS, + add_tutorial_robot, create_parallel_jaw_grasp_pose_generator, create_toppra_motion_generator, create_tutorial_argument_parser, @@ -70,7 +73,8 @@ HAND_INTERP_STEPS = 12 POST_TRAJECTORY_STEPS = 240 PICK_MOTION_SAMPLE_COUNT = PICK_SAMPLE_INTERVAL - HAND_INTERP_STEPS -OBJ_POSITION = (-0.5, 0.0, 0.0) +# Start above the ground so penetration recovery does not move the grasp target. +OBJ_POSITION = (-0.5, 0.0, 0.5 * OBJECT_SIZE[2]) RECORD_LOOK_AT = ( (-1.5, 0.2, 1.2), (-0.4, 0.0, 0.4), @@ -118,13 +122,38 @@ def _create_pick_motion_policy( def main() -> None: """Plan and replay PickUp followed by Pour.""" args = parse_arguments() - sim = create_tutorial_simulation(args) - robot = add_ur5_gripper_robot(sim, tcp_z=0.15) + sim = create_tutorial_simulation( + args, + newton_solver_options=( + { + "vbd_options": {"rigid_contact_history": True}, + "collision_options": {"contact_matching": "latest"}, + } + if args.physics == "newton" + else None + ), + ) + use_vbd_grasp = sim.physics.solver_type == "mjvbd_v2" + robot = add_tutorial_robot( + sim, + "ur5", + tcp_z=0.15, + newton_gripper_friction=4.0 if use_vbd_grasp else None, + ) + obj_position = list(OBJ_POSITION) + if use_vbd_grasp: + # Start outside the default ground's 1 mm VBD contact margin. + obj_position[2] += 0.001 obj = create_align_object( sim, - obj_position=OBJ_POSITION, + obj_position=obj_position, + ) + hand_open, hand_close = get_hand_open_close_qpos( + robot, + close_qpos=( + MJVBD_V2_CUBE_CLOSE_QPOS if use_vbd_grasp else DEFAULT_GRIPPER_CLOSE_QPOS + ), ) - hand_open, hand_close = get_hand_open_close_qpos(robot) initialize_pre_pick_robot_pose(robot, obj, hand_open) planner = getattr(args, "planner", "trapezoidal") motion_gen = create_toppra_motion_generator(robot, planner=planner) diff --git a/scripts/tutorials/atomic_action/scenario_utils.py b/scripts/tutorials/atomic_action/scenario_utils.py index d52e6bd996..10cf327425 100644 --- a/scripts/tutorials/atomic_action/scenario_utils.py +++ b/scripts/tutorials/atomic_action/scenario_utils.py @@ -234,6 +234,7 @@ def add_dual_tutorial_robot( hand_stiffness: float = 1e3, hand_damping: float = 1e2, hand_max_effort: float = 1e4, + newton_gripper_friction: float | None = None, ) -> Robot: """Add a supported dual-arm tutorial robot to a simulation. @@ -254,6 +255,7 @@ def add_dual_tutorial_robot( hand_stiffness: Hand joint drive stiffness. hand_damping: Hand joint drive damping. hand_max_effort: Hand joint maximum effort. + newton_gripper_friction: Optional Newton gripper sliding friction. Returns: The added dual-arm robot instance. @@ -274,7 +276,7 @@ def add_dual_tutorial_robot( hand_damping=hand_damping, hand_max_effort=hand_max_effort, ) - configure_newton_gripper_contacts(sim, robot_cfg) + configure_newton_gripper_contacts(sim, robot_cfg, friction=newton_gripper_friction) return sim.add_robot(cfg=robot_cfg) diff --git a/scripts/tutorials/atomic_action/tutorial_utils.py b/scripts/tutorials/atomic_action/tutorial_utils.py index 76e07f553b..28fa6910c1 100644 --- a/scripts/tutorials/atomic_action/tutorial_utils.py +++ b/scripts/tutorials/atomic_action/tutorial_utils.py @@ -111,6 +111,9 @@ palm_depth=0.096, ) DEFAULT_GRIPPER_CLOSE_QPOS = 0.036 +# Calibrated for the UR5's DH-PGI gripper holding the tutorial's 5 cm cube. +# V2's one-way contacts cannot stop the fingers at an oversized close target. +MJVBD_V2_CUBE_CLOSE_QPOS = 0.013 NEWTON_GRASP_CONTACT_STIFFNESS = 4.0e4 NEWTON_GRASP_CONTACT_DAMPING = 4.0e2 # MuJoCo-Warp's default contact dimension (3) has no torsional friction. A @@ -301,6 +304,7 @@ def create_tutorial_simulation( *, arena_space: float = 2.5, sun_direction: Sequence[float] = DEFAULT_TUTORIAL_SUN_DIRECTION, + newton_solver_options: Mapping[str, object] | None = None, ) -> SimulationManager: """Create the shared simulation setup used by atomic-action tutorials. @@ -310,10 +314,17 @@ def create_tutorial_simulation( arena_space: Spacing between parallel simulation arenas in meters. sun_direction: Direction of the single global sun light. The vector points from the light toward the scene. + newton_solver_options: Scene-specific overrides for the Newton solver + configuration, such as VBD contact history and collision matching. Returns: A simulation manager with the tutorial key light configured. """ + physics_cfg = _tutorial_physics_cfg(getattr(args, "physics", "default")) + if newton_solver_options is not None: + if not isinstance(physics_cfg, NewtonPhysicsCfg): + raise ValueError("Newton solver options require the Newton backend.") + physics_cfg.solver_cfg = {**physics_cfg.solver_cfg, **newton_solver_options} sim = SimulationManager( SimulationManagerCfg( width=VIEWER_WIDTH, @@ -321,7 +332,7 @@ def create_tutorial_simulation( headless=True, num_envs=args.num_envs, device=args.device, - physics_cfg=_tutorial_physics_cfg(getattr(args, "physics", "default")), + physics_cfg=physics_cfg, render_cfg=RenderCfg(renderer=args.renderer), physics_dt=1.0 / 100.0, arena_space=arena_space, @@ -406,6 +417,9 @@ def add_tutorial_robot( robot_type: TutorialRobot, init_pos: Sequence[float] = (0.0, 0.0, 0.0), init_qpos: Sequence[float] | None = None, + *, + newton_gripper_friction: float | None = None, + arm_damping: float | None = None, **kwargs, ) -> Robot: """Add a selected tutorial robot with the shared PGI gripper. @@ -415,6 +429,9 @@ def add_tutorial_robot( robot_type: Tutorial robot family to construct. init_pos: Root position of the robot in its arena. init_qpos: Optional full robot joint configuration. + newton_gripper_friction: Optional sliding-friction coefficient for the + Newton gripper collision links; object materials are unaffected. + arm_damping: Optional drive damping for the arm control part. Returns: The added robot instance. @@ -428,7 +445,9 @@ def add_tutorial_robot( init_qpos=init_qpos, **kwargs, ) - configure_newton_gripper_contacts(sim, robot_cfg) + if arm_damping is not None: + robot_cfg.joint_drive_props.damping["arm"] = arm_damping + configure_newton_gripper_contacts(sim, robot_cfg, friction=newton_gripper_friction) return sim.add_robot(cfg=robot_cfg) @@ -617,9 +636,19 @@ def configure_newton_link_contacts( def configure_newton_gripper_contacts( sim: SimulationManager, robot_cfg: RobotCfg, + *, + friction: float | None = None, ) -> None: - """Configure Newton gripper contacts and recompute their source inertia.""" + """Configure Newton gripper contacts and recompute their source inertia. + + Args: + sim: Simulation manager that owns the robot. + robot_cfg: Robot configuration to update before spawning. + friction: Optional static and dynamic sliding-friction coefficient. + """ if not sim.is_newton_backend: + if friction is not None: + raise ValueError("Newton gripper friction requires the Newton backend.") return configure_newton_link_contacts( @@ -628,9 +657,11 @@ def configure_newton_gripper_contacts( group_name="newton_gripper_contacts", link_names_expr=[_GRIPPER_CONTACT_LINK_PATTERN], ) - robot_cfg.link_attrs["newton_gripper_contacts"].attrs.mass_props = ( - MassPropertiesCfg(recompute_inertia=True) - ) + attrs = robot_cfg.link_attrs["newton_gripper_contacts"].attrs + attrs.mass_props = MassPropertiesCfg(recompute_inertia=True) + if friction is not None: + attrs.material_props.static_friction = friction + attrs.material_props.dynamic_friction = friction def create_trapezoidal_motion_generator( @@ -1544,6 +1575,7 @@ def create_tutorial_robot_cfg( "DEFAULT_AXIS_LEN", "DEFAULT_AXIS_SIZE", "DEFAULT_GRIPPER_CLOSE_QPOS", + "MJVBD_V2_CUBE_CLOSE_QPOS", "DEFAULT_TUTORIAL_SUN_DIRECTION", "DEFAULT_TUTORIAL_SUN_INTENSITY", "GRIPPER_HAND_JOINT_PATTERN", diff --git a/tests/sim/atomic_actions/test_tutorial_utils.py b/tests/sim/atomic_actions/test_tutorial_utils.py index c967391725..1649196956 100644 --- a/tests/sim/atomic_actions/test_tutorial_utils.py +++ b/tests/sim/atomic_actions/test_tutorial_utils.py @@ -268,7 +268,7 @@ def _run_obstacle_animation(*, pace_wall_time: bool) -> tuple[MagicMock, MagicMo return obstacle, adapter -def test_atomic_action_tutorial_uses_native_mujoco_contact_settings() -> None: +def test_atomic_action_tutorial_uses_native_mjvbd_v2_settings() -> None: module = importlib.import_module("scripts.tutorials.atomic_action.tutorial_utils") default_cfg = module._tutorial_physics_cfg("default") @@ -279,29 +279,24 @@ def test_atomic_action_tutorial_uses_native_mujoco_contact_settings() -> None: assert newton_cfg.num_substeps == 20 assert newton_cfg.collision_cfg is None assert newton_cfg.solver_cfg == { - "solver_type": "mujoco_warp", - "solver": "newton", - "integrator": "implicitfast", - "iterations": 20, - "ls_iterations": 100, - "cone": "elliptic", - "impratio": 1_000.0, - "use_mujoco_contacts": True, - "enable_multiccd": True, + "solver_type": "mjvbd_v2", + "mujoco_options": { + "solver": "newton", + "integrator": "implicitfast", + "iterations": 20, + "ls_iterations": 100, + "cone": "elliptic", + "impratio": 1_000.0, + "enable_multiccd": True, + }, } dexsim_cfg = newton_cfg.to_dexsim_cfg(gpu_id=0) - assert dexsim_cfg.solver_cfg.solver_type == "mujoco_warp" - assert dexsim_cfg.solver_cfg.solver == "newton" - assert dexsim_cfg.solver_cfg.integrator == "implicitfast" - assert dexsim_cfg.solver_cfg.iterations == 20 - assert dexsim_cfg.solver_cfg.ls_iterations == 100 - assert dexsim_cfg.solver_cfg.nconmax is None - assert dexsim_cfg.solver_cfg.njmax is None - assert dexsim_cfg.solver_cfg.cone == "elliptic" - assert dexsim_cfg.solver_cfg.impratio == pytest.approx(1_000.0) - assert dexsim_cfg.solver_cfg.use_mujoco_contacts is True - assert dexsim_cfg.solver_cfg.enable_multiccd is True + assert dexsim_cfg.solver_cfg.solver_type == "mjvbd_v2" + assert ( + dexsim_cfg.solver_cfg.mujoco_options == newton_cfg.solver_cfg["mujoco_options"] + ) + assert dexsim_cfg.solver_cfg.vbd_options == {} assert dexsim_cfg.collision_pipeline_cfg is None @@ -1023,6 +1018,7 @@ def test_place_tutorial_registers_pick_object_with_simulation_engine_factory() - n_sample=1, no_vis_eef_axis=True, robot="ur5", + physics="default", ) sim = MagicMock() sim.device = torch.device("cpu") @@ -1316,9 +1312,13 @@ def test_pour_tutorial_uses_configured_pickup_and_local_rotation_axis() -> None: assert module.POUR_INTERNAL_AXIS == (1.0, 0.0, 0.0) pick_policy = module._create_pick_motion_policy() assert pick_policy.sample_count == module.PICK_SAMPLE_INTERVAL - assert isinstance(pick_policy.plan_opts, module.TrapezoidalPlanOptions) - assert pick_policy.plan_opts.sample_method is module.TrajectorySampleMethod.QUANTITY - assert pick_policy.plan_opts.sample_interval == module.PICK_MOTION_SAMPLE_COUNT + assert pick_policy.plan_opts is None + toppra_policy = module._create_pick_motion_policy("toppra") + assert isinstance(toppra_policy.plan_opts, module.ToppraPlanOptions) + assert ( + toppra_policy.plan_opts.sample_method is module.TrajectorySampleMethod.QUANTITY + ) + assert toppra_policy.plan_opts.sample_interval == module.PICK_MOTION_SAMPLE_COUNT def test_replay_timed_trajectory_uses_arrival_intervals() -> None: From 51af3224ac4c56dd586c32f965e66eed1e34e297 Mon Sep 17 00:00:00 2001 From: xiemenghong Date: Sun, 20 Sep 2026 17:00:33 +0800 Subject: [PATCH 03/27] fix(tutorial): align drawer damping across physics backends --- scripts/tutorials/atomic_action/slide.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/scripts/tutorials/atomic_action/slide.py b/scripts/tutorials/atomic_action/slide.py index 2970504f70..ddba7af78b 100644 --- a/scripts/tutorials/atomic_action/slide.py +++ b/scripts/tutorials/atomic_action/slide.py @@ -95,10 +95,16 @@ def create_drawer( init_pos=DRAWER_POSITION, init_rot=DRAWER_ORIENTATION, init_qpos=(0.0,), - joint_drive_props=JointDrivePropertiesCfg(drive_type="none"), + joint_drive_props=JointDrivePropertiesCfg( + drive_type="force", + target_mode="velocity", + stiffness=0.0, + damping=5.0, + ), attrs=create_tutorial_rigid_body_physics( static_friction=1.0, dynamic_friction=1.0, + linear_damping=0.0, ), ) configure_newton_link_contacts( From bc5369b089a332328867c87e6cf63340b815bb4b Mon Sep 17 00:00:00 2001 From: xiemenghong Date: Sun, 20 Sep 2026 17:50:02 +0800 Subject: [PATCH 04/27] docs(sim): clarify MJVBD V2 coupling and control limitations --- outputs/validation/mjvbd_v2/MJVBDV2_issues.md | 376 ++++-------------- 1 file changed, 70 insertions(+), 306 deletions(-) diff --git a/outputs/validation/mjvbd_v2/MJVBDV2_issues.md b/outputs/validation/mjvbd_v2/MJVBDV2_issues.md index 8f3e575569..56245a63af 100644 --- a/outputs/validation/mjvbd_v2/MJVBDV2_issues.md +++ b/outputs/validation/mjvbd_v2/MJVBDV2_issues.md @@ -1,350 +1,114 @@ # MJVBDV2 接入问题记录 -更新日期:2026-09-18 +更新日期:2026-09-20 -这份记录整理 MJVBDV2 接入 EmbodiChain 后遇到的问题:实际看到了什么、会影响什么、哪些已经修复、哪些还需要继续查。 +本文记录影响 MJVBDV2 替换现有 solver 的机制问题、证据和验证边界。布料、软体不在本次范围内。“对齐”指更换 solver 后行为接近,不等于程序正常退出,也不等于某个任务偶然成功。旧 solver 指 Newton 后端的 `mujoco_warp`;Default 后端的对照会单独注明。 -这里的“与旧 solver 对齐”,指的是换 solver 后表现接近,不代表任务一定成功。例如,两边的抽屉都没有关上,仍可能是表现接近;两边程序都正常退出,也不代表物体的运动一致。文中的旧 solver 指 Newton 后端的 `mujoco_warp`。 +**当前接入已打通主要接口和状态生命周期,但尚不能作为保持原有抓取控制方式不变的通用替代。最关键的限制是:VBD 物体的接触反力不回传给 articulation。** 针对物体标定夹爪目标可以改善部分任务,不能补齐这一机制。 -本次保留 MJVBDV2 的物体所有权分工:带关节的机器人、门和抽屉由 MuJoCo 求解,自由运动的杯子、方块等刚体由 VBD 求解。布料、软体不在这次接入范围内。**但本轮 DexSim 补丁额外开启了混合模式的 MuJoCo 原生接触,实际运行两套碰撞管线,已经偏离原生混合模式的接触配置。详见文末更正。** +## 1. 单向耦合改变了位置夹紧的工作方式 -本文链接的详细报告和原始数据是本地验证产物,未随这份问题记录提交到 GitHub。 +MJVBDV2 混合模式中,MuJoCo 推进机器人关节,VBD 推进自由刚体。机器人在 VBD 中是按给定姿态运动的碰撞代理,VBD 对物体计算的反力不会传回机器人。 -**目前仍未完成全部动态刚体任务对齐。** 调参已恢复了一部分技能,但单次成功、停止后不掉落,都不能代表稳定替换。当前状态如下,后文保留问题的发现过程;详细试验以[第二轮调参记录](coupling_tuning/round2.md)为准。 +在具有双向接触响应的系统中,夹爪位置驱动允许把目标设得比物体允许的开口更闭合。手指被物体挡住,位置误差继续产生驱动力,驱动力与接触反力形成平衡。因此,“没有到达夹爪目标”可以是正常夹紧状态,并非控制失败。 -| 场景 | 当前结论 | -| --- | --- | -| PickUp、Pour、Place、双臂抬托盘 | 已有完整改善结果和正式入口验证;仍缺跨初态稳定性证据。 | -| 纸杯 MoveHeldObject | 最新 11.5 mm 正式入口完整完成,位置误差 2.90 mm、姿态误差 11.32°;停止后基本稳定,但搬运转偏未解决。此前缓存的 3.57° 好结果未稳定复现。 | -| HandOver | 释放前等待一秒后,三次候选及正式入口均直立停稳;不代表新旧最终位姿完全一致,跨初态尚待验证。 | -| Assemble | alpha=0.5 的完整候选偏心 6.93 mm;重复及重新规划待结果。单纯延长释放等待无效,局部接触余量对照继续进行。 | -| 双臂放置 | 未恢复预期效果。已确认新旧 solver 在同轨迹下均被桌子挡住,右手偏差约 7–8 cm;安装位置对照待结果,抓持也仍需改善。 | -| 咖啡杯示例 | 新旧正常规划都没有稳定完成。已排队验证规划前自然落稳的影响,尚未证明等待有效。 | -| 舀冰 | 接触容量已修复;完整候选带出 5–6 块冰,但勺子姿态仍偏约 15–17°。摩擦、刚度对照继续运行;冰堆沉降差异也会影响结果,不能只比较一次带出数量。 | -| Locomotion | 已有约 1e-6 rad 量级的同动作状态对齐证据;混合模式结果依赖额外开启 MuJoCo 原生接触,不能证明原生单管线混合模式对齐,也不能替代完整训练验证。 | -| Twist | 按要求暂时跳过。 | - - -## 目前还有哪些问题 - -### 新发现:舀冰时 VBD 的接触列表装不下 - -**状态:容量 256 已完整运行,无溢出警告;舀冰例程已采用该容量,抓持稳定性继续调参。** - -抓持改善后的两次完整舀冰出现数千次接触溢出。一个动态刚体最多需要 170 / 183 个接触位置,但 VBD 默认只给每个刚体保留 64 个;超过容量的部分没有进入该刚体的接触求解。这与 MuJoCo 的接触、约束容量是两套设置,之前扩大 MuJoCo 容量不能解决它。 - -这是此前验证遗漏的一项,现予以更正:带出 1 / 3 块冰和滑转较大的结果真实存在,但接触已经被截断,不能据此判断单向耦合的能力上限。只把 VBD 容量提高到 256 后,已完整运行 3300 步,采样峰值 188、无溢出警告;最后带出 6 块冰,其中 5 块在保持前后采样中都位于勺碗范围。但勺子累计手内变化仍约 16.84 mm / 16.78°,说明排除截断后仍需改善抓持。随后原闭合目标也完整完成 3300 步,无溢出、采样峰值 192,末尾带出同一批 5 块冰;位置误差 22.62 mm、姿态误差 14.86°。原目标末尾保持更好,标定目标前段滑转更小,两者尚无全面胜出的配置。手部摩擦和接触刚度的首次独立对照都被临时记录器的法向量断言中断,尚无完整效果结论;记录器已改为保存异常原始值并明确标记不可计算的指标,两组按原物理参数重跑。对应 HandOver、双臂放置、咖啡杯和 Place 日志未发现同类警告,不能用这个问题解释所有未对齐任务。 - - -### 1. 混合场景的接触支持仍不完整,地面支撑已修复 - -**状态:本地小补丁已构建并安装到 embodichain 环境,恢复 MuJoCo 自有接触;接触力查询和动态物体反力仍有边界。补丁尚未提交或推送。** - -此前只有机器人和静态地面时,V2 使用纯 MuJoCo 分支,机器人可以正常行走。加入 VBD 动态刚体后切换到混合分支,却关闭了 MuJoCo 自身接触,连远处方块也会间接导致机器人失去地面支撑。这是已经确认的接入问题,不必通过重写双向耦合解决。 - -修正后,混合分支默认保留机器人与 MuJoCo 可见静态环境的接触,自由刚体仍归 VBD,反力仍不回传。直接加载补丁源码的四环境 GPU 对照完整运行 1500 步及五次重置,无容量溢出,关节最大差约 1.13e-6 rad,终止标志一致。随后正式 wheel 已安装,正常安装包入口的 CPU 混合落地及纯分支检查均通过。GPU 安装包任务回归仍在进行。 - -这项修正恢复地面支撑,但不代表混合场景的接触力查询已经与旧 MuJoCo 一致。若任务依赖动态物体把反力传给机器人,仍受单向耦合限制。 - -补充确认:静止的桌子如果被配置成独立的 `kinematic` 物体,也不会自动进入混合分支的 MuJoCo 碰撞模型。咖啡杯示例中,旧 MuJoCo 有桌子的 8 个碰撞几何,V2 的 MuJoCo 模型里没有它们。杯子仍会碰到桌子,但机器人不会收到这部分接触反力。示例中的桌子从不移动,已将它改为 static,运行时确认两边 MuJoCo 模型均有 8 个桌子碰撞几何。修正桌子后,最初的新旧咖啡杯完整任务仍失败,说明这不是全部原因;随后手部接触调参才出现完整改善候选。真正会运动的 kinematic 物体不能用这种场景修正替代支持。 - -### 部分接触参数在 VBD 中的作用与旧配置不同 - -**状态:已核对源码及 CPU 接触力;这不是所有剩余失败的原因证明。** - -- 当前默认的刚体硬接触不使用 `friction_epsilon`。只改变它不会调整这条分支的摩擦;原计划的纸杯候选已在启动前撤销。 -- 相同的夹爪接触余量也未必有相同效果:当前 UR5 手指的 1 mm margin 在 MuJoCo 原生碰撞中被清零,在 VBD 中仍保留。双臂放置的局部零余量候选仍在旧缓存的协作段甩出面包,已明确记录后提前停止,未修改全局默认;这不代表所有任务都不适合零余量,Assemble 的独立对照仍待结果。 -- 当前 VBD 刚体接触没有读取独立的扭转、滚动摩擦系数。旧 MuJoCo 中有效的扭转摩擦配置,不会自动得到等价作用。多个接触点仍可能通过切向力抵抗旋转,因此不能直接推断所有抓持都不适用。 -- 手指和物体的滑动摩擦取几何平均。例如手指设为 4、杯子为 0.97,接触对约为 1.97;刚度和阻尼则取两侧的算术平均。调参应比较最终接触对,而不只看某一个配置字段。 -- 接触阻尼 `kd` 在硬接触中有效,但只抵抗接触面相互接近的法向运动,不是通用的止滑、止转参数。安装包 CPU 核对中,增大它能提高接近时的法向阻力,却没有同步提高该瞬间的切向摩擦上限;法向不再接近时,这项阻尼不生效。不能因此断言某个任务失败由它造成,但不应把增大 `kd` 当作普遍增强抓持的手段。 - -单点纯扭转的 CPU 核对显示,提高滑动摩擦无法产生缺失的独立抗扭项。它只说明该力模型的边界;具体技能是否依赖这项能力,还需要实际接触几何和完整任务证据。详细记录及原始数据见[第二轮调参记录](coupling_tuning/round2.md)。 - -全程关闭接触点黏着重放也不是通用解法:HandOver 的独立候选在最终松手前就提前脱手,完整运行后侧倒。当前保留该设置,优先重复验证释放前等待的候选。这个失败不能证明只在释放阶段关闭重放也一定无效。 - -### 调参时要注意:刚体的旧阻尼字段不会改变 Newton 运动 - -**状态:已核对当前构建和运行时赋值路径,并用安装包 CPU 模型检查确认;不是 MJVBDV2 独有的新问题。** - -例程给杯子、勺子等刚体设置的 `linear_damping` / `angular_damping`,在当前 Newton 路径中只保留为属性记录。读回数值变了,不代表运动真的受到了更强阻尼。安装包检查将两项都从 0 改成 100,质量、惯量、速度、关节阻尼及接触材料等相关模型数组完全相同。 - -这与已经用于 Place、Assemble 调参的机械臂驱动阻尼不同;后者通过关节驱动配置实际进入求解。旧 `mujoco_warp` 和新 `mjvbd_v2` 共用上述刚体构建路径,因此不能据此解释两者所有差异,也不应通过放大这些无效字段来“修复”滑转。 - -DexSim 另有显式施加角阻尼力矩的运行时控制,但它需要单独注册,系数是力矩阻尼系数,不能直接照搬这里的旧数值。目前没有把它叠加到任务候选中,仍先完成接触摩擦与刚度的独立验证。证据见 `coupling_tuning/raw/r2_rigid_damping_mapping_cpu.json`。 - -### 2. 部分抓取、搬运任务与旧 solver 表现不同 - -**状态:本轮调参后部分恢复,尚不能整体认定对齐。** - -第二轮已经取得几项改善: - -- 纸杯的抓起和搬运已经能完整执行,但姿态精度不稳定。最新 11.5 mm 正式入口误差为 2.90 mm / 11.32°,而同闭合量此前缓存为 5.01 mm / 3.57°,旧 MuJoCo 独立规划为 3.69 mm / 2.88°。因此不能把缓存的一次改善写成已经对齐;当前精确缓存重复用于定位差异,未增加新的物理参数组合。 -- 同一接触配置用于 PickUp 后,最终差约 1.9 mm / 2.1°;Pour 完整 2933 步后差约 3.4 mm / 0.52°,均没有掉落。AxisAlign 恢复了对齐目标轴的效果,但旧对照自身掉落,暂不称为严格对齐。 -- 双臂托盘使用手指摩擦 4 和接触历史后,26、22.5、23 mm 闭合目标均完成搬运及额外 4.8 秒保持,22 mm 则只抬起了一部分。选择 23 mm 接入 UR5 塑料托盘入口;该候选最终与旧 MuJoCo 相差约 3.7 mm / 4.4°,距任务目标约 7.2 mm,旧 MuJoCo 为 9.3 mm。正式入口重新规划后也完整完成 888 步,得到一致结果;仍不能推广到其他尺寸及所有物体。 -- Place 需要额外调整:保留原 mimic,使用接触 alpha=0.5,并把机械臂阻尼降至 1000。四次完整回放均保持到释放,水平落点误差约 0.52–3.77 mm,其中一次直接使用新安装包。配置已接入 UR5 V2 教程;落地朝向约在 82.1–94.8° 之间波动,姿态差异尚未消除。正式入口重新规划随后完成 778 步,水平误差 2.35 mm,yaw 85.31°,与旧低阻尼对照姿态差约 0.37°,末尾稳定;仍未覆盖初态扰动。 - -Place 的诊断过程也排除了一个过于简单的解释:降低 mimic 阻尼虽然改善了手指跟随,却没有消除掉落;接触参数和机械臂驱动响应都参与了结果。详细试验保留在调参记录中。 - -换手原无等待配置不稳定,重复曾侧倒。保留左手 7 mm/右手 5.5 mm、手指摩擦 4、接触历史和原机械臂驱动,增加释放前一秒等待后,三次完整运行均直立停稳,水平误差约 13–16 mm。等待已接入 UR5 竖直罐子的教程,正式源代码入口随后完整完成 1776 步,水平误差 12.36 mm、直立倾斜 0.020°,最后两秒稳定。最终朝向仍有波动,重新规划生成的指令也与缓存几乎相同,尚未证明不同初态下的稳定性。 +当前单向路径没有这个平衡:被抓取的 VBD 物体无法阻止手指闭合。手指仍受自身驱动、力矩限制和 MuJoCo 可见接触影响,但缺少这件物体的反力。当两指间隙小于物体尺寸时,VBD 只能尝试移动或转动物体,可能出现穿透、挤出、滑转或抖动。 -推物也得到改善:接近距离保持 30 mm,将推进补偿从 18 mm 调到 30 mm 后,完整任务的水平误差为 8.59 mm、姿态误差 0.82°,均优于旧对照的 10.35 mm / 1.78°。38 mm 补偿虽然进一步减小位置误差,却使姿态误差升至 6.44°,暂不采用。30 mm 候选随后三次完整运行的水平误差为 8.59 / 6.79 / 8.95 mm,姿态误差为 0.82 / 2.62 / 7.07°;其中一次重新规划,位置改善但姿态仍有波动,不能直接推广为所有物体的默认值。 +已有直接证据:同一抓取命令下,旧 MuJoCo 的手指被物体挡在约 0.012 m,新路径则继续到约 0.036 m。这些数值是关节位置,不是夹口宽度。调小闭合目标后能够恢复部分抓取,但控制命令已经改变。 -这些是完整固定轨迹回放的证据,仍需正式入口实时重新规划和重复验证,不能推广为全部任务已经对齐。Place 的旧 MuJoCo 重复运行也会出现不同落点,需要同时报告技能效果和新旧轨迹差异,不能只挑一次成功结果比较。 +**影响与边界:** -Pour 还有已确认的初始化错误:5 cm 方块中心原来在 z=0,一半体积穿地,使规划器读到被顶起后的错误位置。现在改为半边长高度,V2 再留出 1 mm 初始间隙。**当前全局地面参数保持原值。** 之前清零地面余量的尝试因 locomotion 回归而撤回,上述最新 Pour 结果不依赖这项修改。详见 [第二轮调参记录](coupling_tuning/round2.md)。 +- 依赖“过闭合目标+接触反力”的位置夹紧控制,不能直接沿用;物体负载对机械臂的反作用也缺失。 +- 增加摩擦、迭代或减小步长,可以改善现有接触求解,无法产生缺失的反力回传。 +- 按尺寸标定闭合目标只是一种场景适配,会对几何、姿态、碰撞余量和控制误差敏感,不能视为通用闭合策略。 +- 若要求保留原有接触驱动的控制行为,需要让这类接触具有双向动力学响应,例如由同一求解器处理,或采用经过稳定性验证的双向耦合。简单延迟一帧回传力并不自动保证稳定。 +- 根据接触主动停止闭合属于另一种控制策略,也不能完整恢复物体对机械臂的负载反馈。 -下文是第一轮历史结果,并不代表所有候选的最新状态。 +这是已确认的结构性限制,但不能据此把所有任务失败都归为同一个原因;场景几何和规划问题仍需独立排查。 -2026-09-17 补充:对 UR5 的 DH 夹爪,把 5 cm 方块的闭合目标从 0.036 m 改为 0.013 m 后,PickUp 和 AxisAlign 已通过完整动作及最终保持。PickUp 的最终物体中心与同轨迹 MuJoCo 对照相差约 1.9 mm;AxisAlign 最终目标轴误差约 0.75°。Place 的候选仍在搬运途中提前掉落,最终落点接近目标不能算成功,候选改动已撤回。单独增加迭代、启用接触历史或改变刚度等尝试没有恢复原来过大的闭合目标。MoveHeldObject 的 0.0115 m 候选完整跑完后没有掉落,但最终位置误差约 11.4 mm、姿态误差约 23.8°,尚未恢复目标位姿,未设为默认。 +## 2. 动态刚体的加入会改变 articulation 的接触路径 -原因是旧 MuJoCo 的接触反力会把手指挡在约 0.012 m,V2 单向耦合则让手指真的闭合到 0.036 m。对已知物体标定闭合目标可以改善效果,但不能当作原始控制命令完全不变的替换。 +纯 articulation 与静态地面场景可以走纯 MuJoCo 分支。加入 VBD 动态刚体后,原生混合分支关闭 MuJoCo 接触,同时又不回传 VBD 反力,因此即使新增物体在远处,也可能让机器人失去地面支撑。这是求解路径变化,不是机器人控制参数突然失效。 -补充几何核对:DH 夹爪原始碰撞网格在采样抓持区域的开口约 73.5–75.1 mm,不是规划模型填写的 100 mm。按关节运动计算,36 mm 目标会把间隙缩到约 1.5–3.1 mm,13 mm 目标则约为 47.5–49.1 mm。通用闭合适配必须使用实际夹爪标定,不能直接从规划模型最大开口换算。这是静态几何证据,尚未包含碰撞余量与求解过程。 +本地实验补丁把混合模式的 `disable_contacts` 默认改为 `False`,保留 `use_mujoco_contacts=True`,恢复了 MuJoCo 紧凑模型内的机器人、静态环境及允许的 articulation 间接触。**它实际启用了 MuJoCo-Warp 和 Newton 两套碰撞管线,偏离原生 V2 混合模式的接触配置。** -第一轮 Pour 的无效固定轨迹在旧 MuJoCo 下也没抓住物体;第二轮已定位其初始化原因,并重新建立有效对照。第一轮筛选和验证边界见 [第一轮调参记录](coupling_tuning/report.md)。下面保留的是**上一轮沿用旧参数时的观察**,不是调参后的结果。 +自由刚体仍不进入 MuJoCo 模型,VBD 侧机器人仍不接收反力,所以双管线不等于同一对接触重复施力,也没有解决第 1 节的夹紧限制。不过,两套管线带来的开销、参数差异和查询范围需要分别处理;本方案尚不是确认后的最终架构。 -最常见的现象是:机器人按规划运动了,但物体没有被稳定夹住,抬起后滑落,或者没有到达预期位置。 +若要求只使用 Newton 碰撞,同时保留 articulation 的接触响应,需要把合适的接触映射给 MuJoCo 求解。当前 V2 尚未接通此路径,直接设 `use_mujoco_contacts=False` 会被拒绝。 -| 场景 | MJVBDV2 的实际表现 | 旧 solver 的对照 | -| --- | --- | --- | -| PickUp | 方块短暂抬起后滑落,最终中心高度约 2.6 cm | 最终中心高度约 18.2 cm | -| MoveHeldObject | 杯子滑落,没有到达搬运目标 | 最终物体位置距目标约 3.7 mm | -| Pour | 方块没有保持夹持,倾倒动作未达到预期效果 | 对照中方块最终仍保持在约 18.1 cm 高处 | -| 双臂协作抬托盘 | 托盘只部分抬起,最终位置距目标约 6.6 cm | 距目标约 9.5 mm | +另外,独立 kinematic 物体不会自动进入上述 MuJoCo 接触模型。固定设施可按真实用途声明为 static;真正运动的 kinematic 物体不能用这种替换来解决支持缺口。 -这些场景涉及机器人与自由刚体的接触。V2 的求解方式和反力传递与旧 solver 不同,是需要重点检查的因素;目前没有逐项证明每个失败都只有这一个原因。 +## 3. 同名接触参数并不代表相同的物理作用 -也不能据此认定所有 manipulation 都不能用。已有 UR5 抓放流程在一次运行中连续完成了三轮抓放,但那不是多次独立实验得出的成功率。 +切换求解器后,必须核对参数实际被谁消费,不能只看配置值相同: -诊断时尽量固定初态和机械臂路径,逐项改变夹持指令、接触或驱动参数,观察物体从哪一阶段开始偏离。验收时再比较完整任务效果,并明确哪些参数或时序已经改变。放慢动作后成功可以是有效适配,但不能据此声称原时序直接替换已经对齐。 - -### 双臂放置的桌子:外观和碰撞形状没有对上 - -**状态:小补丁已构建并安装,正常安装入口检查通过,尚未提交或推送。** - -桌面看起来在 0.65 m 高,但实际碰撞网格最高只有约 0.553 m,横向也有偏移。旧 MuJoCo 和 V2 都受影响,面包与平底锅还没被抓住就掉下去了。这个现象不能用来判断 V2 的抓取能力。 - -原因是单凸包构建时重新用 Newton 读取 GLB,与 DexSim 渲染导入采用了不同坐标。改为使用已导入顶点后,桌子的碰撞坐标与外观恢复一致。面包还存在约 20 mm 的初始穿透;消除穿透后,两种 solver 的启动诊断都能让物体留在桌上。正式修正保持单凸包,不改变刚体所有权;没有增加新的仓库测试。几何修正后的旧 MuJoCo 完整动作已验证:前置抓取恢复,但面包仍在后续协作搬运中甩出;V2 候选继续验证。 协作放置源码中始终不动的桌子也已改成 static,确保它进入 V2 的 MuJoCo 接触模型;这不等于抓持问题已修复。 - -### 双臂放置:旧缓存不能代表正常规划,锅也没有准确到位 - -**状态:正常重新规划已完整运行,仍未达到预期落放效果;继续定位,不据此判定单向耦合不适用。** - -最新候选正常生成三段计划,前两段与旧缓存相同,第三段却明显不同:左手末端峰值速度约为旧缓存的六分之一。面包这次没有早早甩出,最后停在锅所在区域,但朝向几乎翻转,锅本身也偏离目标约 94 mm,不能只看“没有落地”就算成功。 - -释放前,右手离它应该到的位置约 78 mm,锅在手里的相对位移约 8 mm。这说明需要先查右臂为什么没走到位,继续提高夹爪摩擦并不对症。旧 MuJoCo 随后回放同一条新轨迹,右手同样偏离约 78 mm;协作阶段采样持续有右上臂与桌子的接触,驱动与约束力矩大幅对抗,两侧右臂实际关节最大差仅 2.15e-6 rad。这部分大误差并非 VBD 特有,需要先修正场景或路径。详细证据见[第二轮调参记录](coupling_tuning/round2.md)。 - -补充几何核对:右手目标姿态会让右上臂碰撞形状与桌子实体相交,并非只在桌下空隙中发生凸包假碰撞。这是需要先处理的场景/机械臂姿态问题,继续增大驱动力可能只会加深穿透。离线 IK 找到了其他不撞桌的终点姿态,但尚未验证完整路径;运行时接触对照已支持桌子阻挡这一判断;提高机器人安装位置的新旧正常规划对照仍待结果。当前没有为让示例通过而关闭桌子碰撞或改动规划框架。 - -仅把夹爪余量清零也没有解决双臂放置:面包在协作段被甩到工作区外约 4.9 m,确认剩余动作无法重新抓取后主动停止。该候选只运行了 2200 步,作为失败筛选记录,不计作完整验证。 - -### 3. Twist 是一个需要单独排查的例外 - -**状态:已观察到明显差异,原因尚未确定。** - -Twist 的默认场景是机器人与微波炉,主要对象都是带关节的结构,没有单独的动态自由刚体。因此,不能用“机器人和 VBD 物体之间缺少反力”来直接解释这个例子。 - -已有回放中,V2 的旋钮最终转角约为 **2.154 rad**,旧 solver 约为 **1.092 rad**,而且微波炉门也出现了不同程度的运动。 - -这说明“只要都是 articulation,就一定完全一致”还不能成立。不过,之前这组教程对照并没有严格锁定整段动作序列,规划和采样也不是完全受控的,所以目前记录的是待排查现象,还没有确定是哪一处物理实现出了问题。 - -本轮按要求跳过 Twist。若以后恢复排查,应使用同一段动作回放,并核对两边实际生效的接触、摩擦和关节参数。 - -### 4. Twist 启动时会打印大量 margin 警告,配置的 1 mm 没有生效 - -**状态:已确认,仍存在。警告背后的参数变化是真实的。** - -典型输出是: - -```text -authored margin=0.001... zeroed for NATIVECCD/MULTICCD compatibility -``` - -意思是:碰撞形状原本配置了约 1 mm 的接触提前量,但 solver 为兼容当前 MuJoCo-Warp 的原生接触算法,把它改成了 0。 - -Twist 教程启用了多接触点生成。夹爪有多个碰撞形状,每个相关形状都可能打印一条警告,因此启动时会看到很多行。仿真可以继续,但这 1 mm 的配置确实没有被保留,接触建立的时机可能受到影响。 - -这段逻辑在 MR 原始分支里就存在,不是本次 Contact Query 补丁新增的。目前也还没有证据证明它就是 Twist 新旧表现不同的原因。 - -**不能直接照警告建议,把 `use_mujoco_contacts` 改成 `False`。** 当前 V2 的纯 MuJoCo 分支要求这个选项为 `True`,直接修改会报错。警告里的建议适用于通用 MuJoCo solver,没有考虑 V2 的这一限制。 - -后续需要确认这类场景实际需要怎样的接触距离设置,并让警告给出适合 V2 的说明。只隐藏日志不会恢复被清零的参数。 - -### 5. Contact Query 能用,但混合场景还不能提供同样的力反馈 - -**状态:纯 MuJoCo 分支已接通并验证;混合分支的能力仍有限。** - -| 场景 | 当前能拿到什么 | +| 参数或机制 | 当前差异及影响 | | --- | --- | -| 带关节机器人+静态地面,使用纯 MuJoCo 分支 | 接触对象、位置、法线、距离,以及法向冲量和摩擦冲量 | -| 包含 VBD 动态刚体的混合分支 | 接触几何信息;还没有与纯 MuJoCo 分支等价的接触冲量、力反馈 | - -因此,“能查到碰撞位置”和“能获得训练需要的接触力”需要分别确认。使用接触力作为 observation 或 reward 时,必须核对当前分支实际支持的字段。 - -此外,接口提供的冲量不是直接的力。本次足端力比较是用冲量除以对应的物理子步时间得到的,不能改用策略更新间隔来换算。 +| 接触余量 margin | MuJoCo 原生 NATIVECCD/MULTICCD 路径会把相关形状的非零 margin 清零,VBD 仍保留;接触建立和释放时机可能不同。启动警告反映真实参数变化,隐藏警告不等于修复。 | +| 扭转、滚动摩擦 | 当前 VBD 刚体接触不读取独立的扭转、滚动摩擦系数,不能照搬 MuJoCo 的相应约束。多个接触点的切向力仍可产生抗转力矩,不等于完全没有抗转能力。 | +| 滑动摩擦、刚度与阻尼 | VBD 接触对的滑动摩擦取几何平均,刚度和阻尼取算术平均;应比较最终接触对,而非只比较手指字段。 | +| 接触阻尼 kd | 当前硬接触中主要抵抗相互接近的法向运动,不是通用的切向止滑或角阻尼。 | +| friction_epsilon | 当前默认刚体硬接触路径不消费该项,修改它不会改变这条路径的摩擦。 | +| 刚体 linear_damping / angular_damping | 现有 Newton 路径中的这些旧属性只保留记录,没有等价施加到运动上;关节驱动 damping 则实际参与求解。此限制也影响旧 Newton MuJoCo,不能归为 V2 独有回归。 | -### 6. 位置控制下,`get_qf()` 不能当成实际电机输出力矩 +这解释了为什么复制旧参数不能保证行为相同,但不意味着全部差异都能靠调参消除。尤其是摩擦和接触阻尼无法替代双向反力。 -**状态:已确认的现有 Newton 接口限制,不是本次证明出的 V2 独有问题。** +## 4. 接触观测尚未形成统一的力反馈接口 -在本次位置 PD 控制的行走验证中,`get_qf()` 读到的数组一直为 0。检查后确认,当前 Newton 路径读的是直接输入给关节的力矩命令,没有包含 PD 控制器实际产生的力矩。 - -**影响:不能拿这个全零数组来证明两边电机力矩一致,也不能直接用它计算实际力矩或能耗惩罚。** 如果训练任务需要这些奖励项,需要另行接通实际输出力矩,并确认它的含义。 - -## 接入过程中已经修复的问题 - -这些问题已经纳入补丁。下面保留记录,便于以后遇到类似现象时追查。 - -| 问题 | 当时的影响 | 当前处理 | -| --- | --- | --- | -| 接触查询和部分关节操作找不到 V2 内部的 MuJoCo solver | 原来直接访问 MuJoCo 的代码不能直接用于 V2,碰撞形状编号也需要转换 | 已接通内部 solver 的访问和编号转换;纯 MuJoCo 接触反馈已做数值对照 | -| 更新机器人姿态时,可能把自由刚体也写回旧位置 | VBD 已经算出的物体运动被旧关节数据覆盖 | 已限制更新范围,保留 VBD 计算出的自由刚体位置 | -| 瞬移物体后,物体会继续异常移动 | 零重力下把物体从 x=3 移到 x=4,之后两步却跑到 x=5 | 已同步求解器内部的历史状态;复测可以停在 x=4,并保留原有速度 | -| CUDA Graph 预热后,公开状态恢复了,内部历史没有恢复 | 正式仿真的第一步可能使用预热留下的状态,影响初始运动 | 已补齐内部状态恢复,并验证重建后继续步进 | -| VBD 中用于表示机器人的碰撞代理姿态处理不正确 | 代理可能使用回退后的姿态参与接触计算 | 已让不由 VBD 积分的代理保留传入姿态 | -| 混合分支关闭 MuJoCo 接触后,接触缓冲区容量为 0 | 某些摩擦计算路径会除零 | 已保留一个不使用的接触槽;没有因此重新启用 MuJoCo 接触 | -| reset 不能正确覆盖 V2,或把自由刚体的内部关节误算成机器人 | 环境复位可能报错或清理范围不正确 | 已适配;验证了单环境、部分环境和全部环境 reset | -| Gym reset 时删除并重建同名资产失败 | modular Gym 不能正常连续运行多个 episode | 已补齐同名替换:新对象使用新初值,其他对象保留原状态 | - -教程里还修复了几项与 solver 对接有关的问题,包括杯子缩放、Pour 的轨迹重采样、速度跟踪读取状态的时机,以及关闭物理碰撞后规划器丢失障碍物几何。这些修复让程序能正确执行,但不会自动解决抓取接触效果的差异。 - -## 行走已经确认到什么程度 - -在 **ANYmal-C+静态地面/平台** 范围内,当前适配版本与旧 solver 的行为接近。4 个环境分别执行前进、侧移和转弯,每个环境运行 30 秒,并覆盖了局部 reset。 - -相同动作输入下,正常行走时的关节位置最大差约为 **0.0000011 rad**,足端法向力分量最大差约为 **0.015 N**。旧 solver 自己重复运行也会有同量级差异;接触数量和接触/离地标记一致。 - -让两边各自运行策略时,转弯环境会出现步态相位差:机身位置最大差约 1.78 cm,旧 solver 自己重复运行约 1.92 cm。因此,可以说这组场景的行走效果接近,不能说逐帧完全一致。 - -把机器人强行移入地面的压力检查中,两种 solver 都出现了较大的接触力波动。这部分结果没有计入上面的正常行走指标。 - -**实际训练是否能达到相同效果还没有验证。** 本次没有进行 PPO 从零训练,也没有验证具体训练任务的完整 reward、done、域随机化或复杂地形。行走回放通过不等于学习曲线已经对齐。 - -## 一起发现、但不能归为 V2 新增的问题 - -- Press 的按钮行程在新旧 solver 中都出现越限;Slide 在两边都没有完全关上抽屉;OpenDoor 在两边都有打开后回摆。这些任务本身仍需改进,不能只按任务失败就判定 V2 不对齐。 -- 现有 Newton 后端尚未支持通用刚体约束,旧 solver 路径也没有这项能力。 -- USD 导出可以生成文件,但还没有完整写出 Newton 的刚体、关节等物理信息,不能当成可完整还原的仿真场景。 -- NeuralPlanner 缺少匹配的模型资源,尚未完成对应验证。 -- 有些入口不加 `--physics newton` 时会运行默认后端。比较结果前需要确认实际 backend 和 solver,不能只根据“安装了哪个 DexSim”判断运行路径。 - -## 这份记录对应哪一版 - -- EmbodiChain:[draft PR #645](https://github.com/DexForce/EmbodiChain/pull/645),代码提交 `75d3c9a3`。 -- DexSim:[MR !1345](http://192.168.3.16/Engine/dexsim/-/merge_requests/1345),适配补丁已提交到 `MJVBDV2` 分支,提交 `f7d60c2a2`。 -- 验证使用的是包含这些补丁的本地构建,不代表未经适配的早期分支也具备同样能力。 - -此前覆盖了 15 项内置原子技能、18 个原子教程和另外 26 个 Newton 示例入口,但没有穷举所有机器人、参数和规划器组合。最后一轮补丁后,只对受影响场景进行了针对性复测,没有把全部入口再跑一遍。操作任务的多数结果来自单次回放,不能当成统计成功率。 - -详细数据见[示例与原子技能验证报告](/home/oem/code/EmbodiChain/outputs/validation/mjvbd_v2/report.md)和[行走对齐验证报告](/home/oem/code/EmbodiChain/outputs/validation/mjvbd_v2/locomotion_alignment/report.md)。 - -## 最新补充:舀冰旧基线已经跑通 - -旧 MuJoCo 的接触和约束容量增到 8192 / 16384 后,完整完成了 3300 步舀冰与末尾保持,没有 CUDA 错误或容量溢出。勺子没有脱手,最后距目标约 5.6 mm / 1.7°,11 块冰块处于勺斗范围内并已抬升约 29–34 cm。之前“没有有效旧基线”的状态因此更新;具体指标及限制见第二轮记录。V2 的相同种子完整对照尚待运行,不能先宣布舀冰已经对齐。 - -### 2026-09-18 补充:舀冰的完整失败证据 - -新旧 solver 都完整运行到 3300 步,包含全部舀冰动作和末尾保持。旧 MuJoCo 保持勺子,并在勺碗区域带出 11 块冰;当前 MJVBDV2 在抬升时脱手,最终勺子落地,没有带出冰块。两侧手指、勺子的材料输入一致,但接触求解方式不同。此结果确认当前配置没有恢复任务效果,不足以证明单向耦合永远无法完成该任务。 - -正在验证的改进仅涉及手指摩擦、接触历史、闭合姿态和分段闭合指令。没有改变刚体归属,没有附着物体,没有增加仓库测试或 benchmark。分段闭合仍占原来的 100 个物理步,避免用额外等待掩盖差异。 - -### 交接的重复性仍不合格(2026-09-18 补充) - -交接不能只看最好的一次。正式安装包入口两次完整运行的水平落点误差分别为 31.60 mm 和 15.37 mm,两次指令几乎一致。此前表现较好的缓存候选再次完整运行时还出现侧倒,误差 45.91 mm。因此,当前夹爪和接触配置仍不足以稳定替换旧 solver,教程内的参数只能视为本地候选。 - -后续分别检查“机械臂尚未到位就松手”和接触历史中的黏着锚点。先前数据已把主要差异放大阶段定位到释放和落地,但尚未证明是哪一项机制造成失败;不能用未经验证的原因解释掉这些失败。 - -### 舀冰的旧对照也需要重新核实(2026-09-18 补充) - -当前安装包的旧 MuJoCo 补充运行在第 868 步出现冰块非有限位姿,接近阶段已可见飞散,未完成任务。此前成功的旧侧 3300 步记录早于本轮补丁安装,不能直接当作当前版本的严格基线。正在核对单凸包网格导入并补当前安装包的完整旧对照;尚无证据断言该发散由网格补丁造成。V2 此前抬升时丢勺子的失败仍然有效,并未因旧对照异常而撤销。 - -### 舀冰抓持已有改善,但没有提前判定任务通过(2026-09-18) - -正常安装包下,只将手部摩擦设为 4 并启用接触历史,原闭合目标不变,V2 完成了前 1200 步抓持和抬升:勺子抬高 33.7 cm,没有掉落。原闭合目标下相对手部仍有约 3.4 mm / 5.7° 的滑转;随后小幅标定六个手指目标,完整抬升的相对变化降至约 1.14 mm / 1.49°。两组均未脱手,但完整进入冰堆、舀出冰块和保持尚待验证。该结果支持继续调参,不能据此宣称舀冰已对齐。 - -另外,一个代表冰块在新旧网格导入路径下的凸包顶点、尺寸、体积,以及同一 CPU 算法计算的质量、质心和惯量均完全一致。目前没有证据把旧侧发散归因于该冰块的外形或惯量被补丁改坏。 - -### 舀冰盖子驱动是一个新的待验证因素 - -盖子初始是打开的,但配置的驱动没有生效,机械臂接近阶段盖子会明显转动。旧侧冰块飞散也发生在这一阶段,尚不能证明两者是因果关系;V2 中盖子同样转动,冰块却保持稳定。已准备新旧共同的对照,用有限力矩的位置驱动保持盖子打开,仍保留动态关节和碰撞。结果待运行,未把它当作已经修复的问题。 - -### 完整舀冰、双臂放置仍有实质差距 - -舀冰标定候选已经完整完成 3300 步,能带出 3 块冰;只改摩擦和接触历史、保留原闭合目标时为 1 块。但历史旧侧能带出 11 块,标定候选末尾目标误差仍有约 22.69 mm / 14.76°,抓持和末尾保持还不够稳定。分段闭合在修正诊断脚本后完成了阶段验证,反而比闭合姿态标定滑转更大,暂不采用。 - -双臂放置的右夹口修正后,抬锅明显稳定了,但面包仍在协作阶段脱手。放慢第三阶段两倍没有得到成功结果;其中 V2 在前面未改变时序的抓取阶段就掉面包,不能把这次失败归咎于减速。 - -保持容器盖子打开的旧侧舀冰对照已完整运行,没有再次数值发散,却没有带出冰块。因此,“不崩溃”和“完成任务”在这组场景中也是两回事,盖子驱动尚不是完整修复。 - - -### 舀冰旧基线也不稳定,同步开关不是修复(2026-09-18 补充) - -当前安装包中,旧 MuJoCo 显式关闭 CUDA 同步后完整跑完 3300 步,最后带出 5 块冰,但勺子在手里已滑转约 20.8°;显式开启同步的同配置运行则在第 700 步出现冰块非有限位姿。历史上开启同步曾带出 11 块,说明不能把这个开关当作解决办法,也不能把历史最好的一次当作稳定基准。两组仅各一次,尚不足以确定同步开关与失败的因果关系。 - -V2 扩大容量后完整带出 5–6 块冰的结果有效,但不能因为本次旧侧退化就宣布对齐。当前继续完成手部摩擦、刚度的独立验证,并保留完整动作和保持阶段的评价。详见第二轮记录中的同步对照及原始日志。 - - -### 张开夹爪后,接触余量仍可能妨碍释放(2026-09-18 补充) - -Assemble 中,张手后额外等一秒仍留下约 17.82 mm 偏心。等待结束时手指已基本全开,但罐子维持约 9.85° 倾斜;记录的手指接触点间隙约 0.81–0.94 mm,仍处于 VBD 的 1 mm 接触余量内,相关接触的法向约束乘子也没有归零。继续等待并没有让它脱离这些接触。 - -只改接触 alpha=0.5 的另一遍完整运行则正常落到底座,偏心 6.93 mm 并停稳,正在安排重复验证。另增加只改夹爪 margin 的完整对照,检查原 MuJoCo 中被清零、VBD 中仍保留的余量是否影响释放;尚未证明清零就能解决,未改正式默认值。这个线索来自已有接触点的几何与乘子记录,不是精确碰撞重算或完整力测量。 - - -### alpha 的改善不能跨任务直接推广(2026-09-18 补充) - -Assemble 使用 alpha=0.5 的单次结果改善到 6.93 mm 偏心,但同样调整在双臂放置中使面包第一次抬升就失败,最后仍留在桌上;锅的抓持反而更稳定。完整 3032 步正常退出也不能算任务成功。当前不将 alpha=0.5 设为通用默认,继续用完整任务验证每个候选。 - - -### 纸杯的较好参数仍未证明稳定(2026-09-18 补充) - -11.5 mm 闭合量曾在完整缓存回放中取得约 5.01 mm / 3.57° 的目标误差,但正式入口正常重新规划的一次运行中,杯子在手内的转动明显更大。两次记录到的材料、驱动和初态一致,机械臂命令几乎相同;到第 4587 步,杯子姿态已相差约 8.09°。本次仍在运行,不能当作完整失败,但之前一次好结果也不足以验收这套参数。 - -目前需要确认的是抓持对微小差异是否敏感,以及是否存在尚未记录的模型或执行差别。尚未证明是 GPU 非确定性,也未证明必须改双向耦合。详细对照见第二轮记录的纸杯 11.5 mm 小节。 - - -### 少量接触条目的法向量为零,部分诊断数值需作废(2026-09-18 补充) - -装配的一次完整运行中,三个采样点出现零法向接触。原诊断程序据此算出的“1 mm 穿透”没有物理意义,现已标为不可计算并保留原始数据。张手等待结束时的四个接触法向正常,因此先前关于释放余量的线索仍成立,但不能借这些零法向数据推断夹持力或穿透。 - -这是一项待定位的接触数据问题,尚未证明源于碰撞生成、记录时机或 solver 内部处理,也未证明是任务失败的原因。后续接触记录会显式标识有效性,避免继续误读。 - - -### 纸杯最新完整结果:停止后抓得住,搬运转偏尚未解决(2026-09-18 补充) - -11.5 mm 正式入口现已完整跑完 7460 步,最终位置误差 2.90 mm、姿态误差 11.32°;最后 4.8 秒只变化约 0.63 mm / 0.15°。主要偏差在搬运过程中形成,停止后没有继续明显滑动,也没有恢复正确朝向。 +| 路径 | 已确认能力 | +| --- | --- | +| 纯 MuJoCo | 已适配形状编号映射,可查询接触几何、法向及摩擦冲量,并做过行走数值对照。 | +| 混合模式 | 可查询接触几何,但尚未统一合并 MuJoCo 与 VBD 接触,也未提供与纯分支等价的冲量、力反馈。占位零值不能解释为真实无力。 | -此前同闭合量缓存回放的姿态误差只有 3.57°,旧 MuJoCo 独立规划约 2.88°。因此当前不能认为纸杯已稳定对齐,也不能把单次缓存的好结果作为参数已经标定成功的证明。精确缓存重复已排队,仍未确定根因。 +“查到碰撞”不等于“有可用于控制或奖励的接触力”。冲量换算为力需要使用对应物理子步时长,不能使用策略更新间隔。即使未来能查询 VBD 接触力,查询接口本身也不会使它回传给 articulation。 +另有两项观测限制: -### 双臂放置有明确的机器人—桌子冲突,不能全归因于 VBD(2026-09-18 补充) +- 当前 Newton 的 `get_qf()` 在位置驱动下读的是直接输入的力矩命令,不包含 PD 实际输出。全零结果不能证明电机力矩一致,也不能直接用于能耗奖励;这不是 V2 独有问题。 +- 部分混合接触记录出现零法向且伴有非零约束乘子。这些条目不能用于推导穿透或物理力;尚未确定异常产生于碰撞生成、读取时机还是内部处理,也未证明它是任务失败的原因。 -旧 MuJoCo 回放与 V2 相同的新轨迹后,右手同样偏离目标约 7–8 cm。协作阶段记录持续出现右上臂与桌子的接触,驱动和约束产生很大的反向力矩;两侧右臂实际关节最大差仅约 2.15e-6 rad。此前几何核对也发现目标姿态与桌子相交。 +## 5. 接触容量与碰撞成本影响正确性和可用性 -这说明应先处理安装位置或运动路径,继续加摩擦不能解决被桌子挡住的问题。提高安装位置的新旧完整对照已排队,仍需验证整段路径和最终任务效果。本次旧侧使用了和 V2 一样的标定夹爪命令,是定位原因的对照,不是原教程的成功基线;面包抓持问题也尚未解决。 +VBD 单刚体接触容量与 MuJoCo 的接触、约束容量是不同配置。容量不足会截断参与求解的接触,不只是降低性能;仅扩大 MuJoCo 缓冲区不能解决 VBD 溢出。密集接触验证已观察到单刚体需求超过默认 64,扩大到 256 后完整运行无溢出,但受载滑转仍存在。因此不能用发生截断的结果推断单向耦合的能力上限,也不能把扩容当作夹持问题的完整修复。 +性能方面,在双臂、桌子和两个动态物体的初始场景中,关闭窗口、排除规划与录像并完成预热后,同样 20 个子步下,推进 0.08 秒仿真:旧 MuJoCo 约需 0.10 秒,V2 约需 0.38 秒。CUDA Graph 已启用。阶段测量显示主要瓶颈为 Newton 的 GJK/MPR 凸体精细碰撞,每个子步重复执行会放大成本。 -### 舀冰也已记录到真实零法向接触(2026-09-18 补充) +这是特定场景的步进测量,不是全任务平均值或所有场景的固定倍率。优先方向是检查碰撞对过滤和凸体复杂度;降低子步数需要重新验证接触效果。本地已准备该 demo 从 20 降到 5 的改动,但尚未完成完整速度和效果复验,不在本次文档提交内。 -观测器修复后的摩擦 8 重跑中,第 2360 步起保存到了勺子与手掌、手指之间的零法向接触,并带有非零约束乘子。新记录器不再据此中断任务,也不把这些乘子当成有效的法向/切向力,完整运行仍在继续。 +## 6. 资产、规划与执行差异必须从 solver 差异中分离 -这个现象需要继续定位,不能通过增加摩擦来解释或修复数据本身。但当前还不能证明它是滑转原因,更不能据此断言 MJVBDV2 不适合舀冰;已安排的碰撞生成后观测将帮助区分异常出现的环节。 +已有对照发现以下问题会同时影响新旧 solver: +- 单凸包重新读取 GLB 时采用了不同坐标约定,造成外观和碰撞几何不一致。本地导入补丁改为使用已导入顶点;仅检查画面不够,还需核对实际碰撞形状。 +- 初始穿透、物体尚未落稳就规划,以及目标姿态或路径与桌子相交,会改变抓取初态或阻挡机械臂。双臂同轨迹对照已确认新旧都出现桌子阻挡,不能把此类偏差全归为 VBD。 +- 缓存轨迹、重新规划、不同夹爪命令、释放等待和播放时序会改变实验条件。增加等待或改变目标后成功,是适配结果,不能宣称原控制命令直接替换已对齐。 -### 混合模式启用了两套碰撞管线,之前的对齐结论需要限定(2026-09-18 更正) +因此验证应分别报告:相同输入下的物理差异、适配后的完整任务效果,以及重复运行或初态变化下的稳定性。两边都失败不代表自动对齐,一次成功也不代表稳定兼容。 -本轮为了恢复机器人与地面的接触,把 DexSim 混合模式中的 `mujoco_options.disable_contacts` 从强制 `True` 改为默认 `False`,同时仍使用 `use_mujoco_contacts=True`。这会同时运行 MuJoCo-Warp 和 Newton 两套碰撞管线,并不是单纯保留关节求解。此前把它笼统称为“接触修复”不够准确:它改变了原生 V2 混合模式的接触分工。 +## 已修复的接入问题 -当前修改后的分工是:MuJoCo-Warp 检测并求解其紧凑模型内的 articulation 与静态环境、允许的 articulation 之间的接触;Newton 检测 VBD 自由刚体相关接触,由 VBD 求解。自由刚体没有进入 MuJoCo 模型,机器人在 VBD 侧只是无动力学响应的运动代理,因此两套管线并不意味着同一物体被重复施加接触响应。不过,双管线的开销、接触参数差异和数据查询范围仍需分别处理。 +以下补丁解决了接口或状态同步错误,不改变单向耦合的基本性质: -原生混合模式关闭 MuJoCo 接触,又不把 VBD 反力传回机器人。这意味着 Newton 检测到机器人与地面接触,也不能仅靠这条单向路径给机器人提供支撑力。若要求混合模式只用 Newton 碰撞,同时保留动态 articulation 的接触响应,需要将合适的 Newton 接触映射给 MuJoCo 求解;当前 V2 没有接通这条路径,构造器还明确拒绝 `use_mujoco_contacts=False`。这不是只改一个开关就完成的适配。 +| 类别 | 已完成处理 | +| --- | --- | +| 内部 MuJoCo 访问与编号 | 接通内部 solver,适配 mimic、接触形状编号和 reset。 | +| 状态所有权 | 限制 FK 更新范围,避免旧关节状态覆盖 VBD 自由刚体位置;修正运动代理姿态保留。 | +| 历史状态 | 瞬移和 CUDA Graph 预热回滚时同步内部历史,避免物体异常续移。 | +| 接触缓冲区 | 关闭 MuJoCo 接触时保留必要的非零容量,避免特定摩擦路径除零;这与后续额外开启接触是两项不同改动。 | +| 场景生命周期 | 适配部分/全部 reset,以及同名资产删除重建,支持连续 episode。 | -因此,混合场景 locomotion 的约 1e-6 rad 级同动作对齐,是在额外开启 MuJoCo 原生接触之后得到的,不能当作原生 V2 单向混合模式已经对齐的证据。纯 articulation 路线的结果需要单独看待;两者都不能替代完整训练收敛验证。双臂放置中机器人受桌子阻挡的对照,同样依赖这项开启接触的修改。 +## 验证结论与未覆盖范围 -本次提交保留现有实验结果和配置,但不把双管线方案视为已确认的最终架构。DexSim 的这项补丁及凸包导入坐标修复仍是本轮环境的额外依赖,未包含在 EmbodiChain 的 GitHub 提交中。仅拉取 EmbodiChain 代码,不能保证复现本轮结果。 +- **行走:** ANYmal-C 与静态地面/平台已有正常行走和接触反馈对照,同动作关节位置最大差约 1.1e-6 rad,足端法向力分量最大差约 0.015 N,与旧 solver 自身重复差异同量级。纯 MuJoCo 路线与混合路线必须区分;混合地面支撑结果依赖额外开启 MuJoCo 原生接触,不能作为原生混合配置已对齐的证据。未验证 PPO 从零训练收敛、完整生产 reward/done 或复杂地形。 +- **操作:** 抓起、倾倒、放置和托盘搬运已有完整改善结果;纸杯搬运、装配、双臂放置、舀冰等仍未完成稳定对齐。剩余问题涉及夹持与释放机制、几何/路径和基线稳定性,不能概括为“几个 demo 参数没调好”。 +- **Default 对照:** Slide 采用两后端相同的有效关节阻尼后已通过用户效果验收;这不是旧 Newton MuJoCo 与 V2 的严格数值对照。 +- **暂未完成:** Twist 按要求跳过,其早期结果未严格固定完整动作序列;NeuralPlanner 缺少匹配模型。通用刚体约束和 USD 完整物理导出属于既有 Newton 支持缺口。 -### 本轮提交范围与参数适用边界(2026-09-18 补充) +历史覆盖包括 15 项内置原子技能、18 个原子教程和 26 个其他 Newton 示例入口。不是所有入口都完成任务,也没有覆盖所有机器人、初态与规划器组合。后续补丁只做了针对性复测,不能把早期覆盖数量当作最新版本的全量通过率。 -本轮提交包含按场景选择的夹爪闭合目标、部分夹爪摩擦系数 4、接触历史与 latest 匹配;Place 额外使用 alpha=0.5 和机械臂 damping=1000;HandOver 在释放前等待 1 秒;Pour/Place 修正初始高度;固定桌子使用 static;Scoop 将 VBD 单刚体接触容量提高到 256。夹爪数值是关节位置目标,不是夹口宽度。 +## 版本与复现依赖 -这些调整有机器人和物体范围,尚不是通用参数。其中纸杯 0.0115 闭合目标仍是候选,完整运行的姿态误差达到 11.32°,不能称为稳定标定成功。提高摩擦、刚度、安装高度及额外沉降等待等临时对照没有全部写进正式示例。未新增仓库测试或 benchmark 文件,已有测试随配置修改;原始实验日志和临时诊断脚本未纳入本次提交。 +- EmbodiChain:[draft PR #645](https://github.com/DexForce/EmbodiChain/pull/645)。本文整理截至 2026-09-20 的证据;本次文档更新不代表重新完成全部实验。 +- 已推送 DexSim 适配补丁:[MR !1345](http://192.168.3.16/Engine/dexsim/-/merge_requests/1345),`MJVBDV2` 分支提交 `f7d60c2a2`。 +- **后续验证另依赖两项仍未提交、未推送的 DexSim 本地代码补丁:混合模式开启 MuJoCo 原生接触,以及单凸包导入坐标修复。仅检出上述 DexSim 提交与 EmbodiChain PR,不能保证复现这些后续结果。** 这些代码已用于本地安装包,但不在 EmbodiChain 仓库提交范围内。 +- 没有新增仓库测试或 benchmark。历史参数筛选、测量和详细日志保存在本地 `outputs/validation/mjvbd_v2/` 下的 `report.md`、`locomotion_alignment/report.md` 和 `coupling_tuning/round2.md`;这些产物未随 PR 发布。本文保留综合结论,逐次实验历史仍可从本地记录与 Git 历史追溯。 From 5f19889062c406ba12f764a4f270983c325f624d Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 29 Sep 2026 13:22:43 +0800 Subject: [PATCH 05/27] fix(sim): adapt examples to DexUni solver config Migrate DexUni example settings to the current namespaced solver interface and preserve the existing cloth contact values. Update the Newton config regression test and document the new mapping contract. --- embodichain/lab/sim/cfg/simulation.py | 4 +++ examples/sim/demo/fold_tshirt.py | 29 +++++++++++--------- examples/sim/demo/pick_up_cloth.py | 39 +++++++++++++-------------- examples/sim/demo/press_softbody.py | 32 ++++++++++++---------- tests/sim/test_sim_manager_cfg.py | 8 +++--- 5 files changed, 61 insertions(+), 51 deletions(-) diff --git a/embodichain/lab/sim/cfg/simulation.py b/embodichain/lab/sim/cfg/simulation.py index 9451364a8c..10d55635c4 100644 --- a/embodichain/lab/sim/cfg/simulation.py +++ b/embodichain/lab/sim/cfg/simulation.py @@ -561,6 +561,10 @@ class NewtonPhysicsCfg(PhysicsBackendCfg): parameters accepted by that DexSim solver config. If omitted, EmbodiChain preserves DexSim's scene-aware ``AutoSolverCfg`` default. A DexSim build exporting ``AutoSolverCfg`` is required; no concrete-solver fallback is used. + For ``solver_type='dexuni'``, the current DexSim contract keeps expert + options under ``vbd_options``, ``mujoco_options`` and + ``collision_options``; use ``joint_mode`` and ``contact_mode`` for the + coupled dispatch policy. """ collision_cfg: NewtonCollisionPipelineCfg | Mapping[str, Any] | None = field( diff --git a/examples/sim/demo/fold_tshirt.py b/examples/sim/demo/fold_tshirt.py index 50f0f975ba..c0424a795e 100644 --- a/examples/sim/demo/fold_tshirt.py +++ b/examples/sim/demo/fold_tshirt.py @@ -354,19 +354,22 @@ def initialize_simulation( use_cuda_graph=use_cuda_graph, solver_cfg={ "solver_type": "dexuni", - "iterations": SOLVER_ITERATIONS, - "particle_enable_self_contact": True, - "particle_self_contact_radius": 0.002, - "particle_self_contact_margin": 0.002, - "particle_topological_contact_filter_threshold": 1, - "particle_rest_shape_contact_exclusion_radius": 0.005, - "particle_vertex_contact_buffer_size": 96, - "particle_edge_contact_buffer_size": 128, - "particle_collision_detection_interval": -1, - "self_contact_bvh_rebuild_interval_frames": 15, - "rigid_contact_max": 0, - "step_rigid_bodies": False, - "soft_contact_margin": 0.008, + "joint_mode": "kinematic", + "contact_mode": "soft", + "vbd_options": { + "iterations": SOLVER_ITERATIONS, + "particle_enable_self_contact": True, + "particle_self_contact_radius": 0.002, + "particle_self_contact_margin": 0.002, + "particle_topological_contact_filter_threshold": 1, + "particle_rest_shape_contact_exclusion_radius": 0.005, + "particle_vertex_contact_buffer_size": 96, + "particle_edge_contact_buffer_size": 128, + "particle_collision_detection_interval": -1, + }, + "collision_options": { + "soft_contact_margin": 0.008, + }, "soft_contact_ke": 3.0e5, "soft_contact_kd": 5.0e-2, "soft_contact_mu": 0.5, diff --git a/examples/sim/demo/pick_up_cloth.py b/examples/sim/demo/pick_up_cloth.py index 1b4839e06c..e9360db754 100644 --- a/examples/sim/demo/pick_up_cloth.py +++ b/examples/sim/demo/pick_up_cloth.py @@ -437,29 +437,28 @@ def main(args: argparse.Namespace | None = None) -> None: num_substeps=12, solver_cfg={ "solver_type": "dexuni", - "iterations": 24, - "particle_enable_self_contact": True, - "particle_self_contact_radius": 0.002, - "particle_self_contact_margin": 0.002, - "particle_topological_contact_filter_threshold": 1, - "particle_rest_shape_contact_exclusion_radius": 0.005, - "particle_vertex_contact_buffer_size": 96, - "particle_edge_contact_buffer_size": 128, - "particle_collision_detection_interval": -1, - "particle_enable_tile_solve": True, - "soft_contact_margin": 0.008, + "joint_mode": "kinematic", + "contact_mode": "soft", + "vbd_options": { + "iterations": 24, + "particle_enable_self_contact": True, + "particle_self_contact_radius": 0.002, + "particle_self_contact_margin": 0.002, + "particle_topological_contact_filter_threshold": 1, + "particle_rest_shape_contact_exclusion_radius": 0.005, + "particle_vertex_contact_buffer_size": 96, + "particle_edge_contact_buffer_size": 128, + "particle_collision_detection_interval": -1, + "particle_enable_tile_solve": True, + "rigid_contact_k_start": CLOTH_RIGID_CONTACT_KE, + "rigid_body_particle_contact_buffer_size": 512, + }, + "collision_options": { + "soft_contact_margin": 0.008, + }, "soft_contact_ke": CLOTH_RIGID_CONTACT_KE, "soft_contact_kd": CLOTH_RIGID_CONTACT_KD, "soft_contact_mu": CLOTH_RIGID_CONTACT_MU, - # Use the mixed material stiffness immediately instead of - # ramping new contacts from the low DexUni default. - "rigid_contact_k_start": CLOTH_RIGID_CONTACT_KE, - "rigid_body_particle_contact_buffer_size": 512, - "rigid_contact_max": 0, - # The registered runtime control advances and interpolates the - # robot kinematically at every Newton substep. - "step_rigid_bodies": False, - "self_contact_bvh_rebuild_interval_frames": 1, }, # DexUni owns its particle-shape contacts and collision detection. collision_cfg=None, diff --git a/examples/sim/demo/press_softbody.py b/examples/sim/demo/press_softbody.py index e5b7209677..0405fee8b4 100644 --- a/examples/sim/demo/press_softbody.py +++ b/examples/sim/demo/press_softbody.py @@ -48,7 +48,7 @@ def parse_arguments(argv: list[str] | None = None) -> argparse.Namespace: import numpy as np import torch -from dexsim.utility.path import get_resources_data_path +from embodichain.data import get_data_path from embodichain.lab.sim import SimulationManager, SimulationManagerCfg from embodichain.lab.sim.cfg import ( NewtonPhysicsCfg, @@ -98,22 +98,26 @@ def initialize_simulation(args: argparse.Namespace) -> SimulationManager: num_substeps=NUM_SUBSTEPS, solver_cfg={ "solver_type": "dexuni", - "iterations": SOLVER_ITERATIONS, - "particle_enable_self_contact": False, - "particle_self_contact_radius": 0.005, - "particle_self_contact_margin": 0.005, - "particle_topological_contact_filter_threshold": 3, - "particle_enable_tile_solve": True, - "rigid_body_particle_contact_buffer_size": 512, - "rigid_contact_k_start": SOFT_CONTACT_KE, - "rigid_contact_max": 0, - "soft_contact_margin": SOFT_CONTACT_MARGIN, + # DexUni keeps solver-specific expert options in namespaced + # mappings. The kinematic trajectory owns the robot joints. + "joint_mode": "kinematic", + "contact_mode": "soft", + "vbd_options": { + "iterations": SOLVER_ITERATIONS, + "particle_enable_self_contact": False, + "particle_self_contact_radius": 0.005, + "particle_self_contact_margin": 0.005, + "particle_topological_contact_filter_threshold": 3, + "particle_enable_tile_solve": True, + "rigid_body_particle_contact_buffer_size": 512, + "rigid_contact_k_start": SOFT_CONTACT_KE, + }, + "collision_options": { + "soft_contact_margin": SOFT_CONTACT_MARGIN, + }, "soft_contact_ke": SOFT_CONTACT_KE, "soft_contact_kd": SOFT_CONTACT_KD, "soft_contact_mu": SOFT_CONTACT_MU, - # The registered trajectory updates the robot kinematically at - # every Newton substep; DexUni only needs to solve the soft body. - "step_rigid_bodies": False, }, # DexUni owns its particle-shape contacts and collision detection. collision_cfg=None, diff --git a/tests/sim/test_sim_manager_cfg.py b/tests/sim/test_sim_manager_cfg.py index 47abd7f7fb..825247dd2f 100644 --- a/tests/sim/test_sim_manager_cfg.py +++ b/tests/sim/test_sim_manager_cfg.py @@ -461,8 +461,8 @@ def test_newton_physics_cfg_accepts_dexuni_solver_mapping( cfg = NewtonPhysicsCfg( solver_cfg={ config_key: solver_name, - "iterations": 12, - "step_rigid_bodies": False, + "joint_mode": "kinematic", + "vbd_options": {"iterations": 12}, } ) @@ -470,8 +470,8 @@ def test_newton_physics_cfg_accepts_dexuni_solver_mapping( assert isinstance(dexsim_cfg.solver_cfg, DexUniSolverCfg) assert dexsim_cfg.solver_cfg.solver_type == "dexuni" - assert dexsim_cfg.solver_cfg.iterations == 12 - assert dexsim_cfg.solver_cfg.step_rigid_bodies is False + assert dexsim_cfg.solver_cfg.joint_mode == "kinematic" + assert dexsim_cfg.solver_cfg.vbd_options["iterations"] == 12 @pytest.mark.no_sim From aa9f0b6b7fd46a1c4aebdb9781bffbd3576e9764 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 29 Sep 2026 23:35:00 +0800 Subject: [PATCH 06/27] feat(sim): expose DexUni contact sensor data Allow geometry-only ContactQuery data for DexUni VBD paths and full impulse/friction data for pure MuJoCo. Add capability tests and GPU end-to-end coverage. --- .../topics/sensor-system/lifecycle.md | 7 + embodichain/lab/sim/physics/newton.py | 28 +++- embodichain/lab/sim/sensors/contact_sensor.py | 4 +- tests/sim/sensors/test_contact_newton_e2e.py | 146 +++++++++++++++++- tests/sim/test_backend_parity.py | 33 +++- 5 files changed, 208 insertions(+), 10 deletions(-) diff --git a/agent_context/topics/sensor-system/lifecycle.md b/agent_context/topics/sensor-system/lifecycle.md index 8a83ede7d3..fce171f43a 100644 --- a/agent_context/topics/sensor-system/lifecycle.md +++ b/agent_context/topics/sensor-system/lifecycle.md @@ -40,6 +40,13 @@ support. Do not infer force support from sensor construction alone. Inspect backend/solver/device matrix: Newton AutoSolver may change the answer during preparation, so the manager checks again afterwards. +DexUni's `pure_mujoco` path reports rigid-contact geometry, normal impulse and +tangential impulse through the MuJoCo contact buffer, except when MuJoCo uses +its CPU implementation. Other DexUni paths expose available rigid-contact +geometry with `impulse=False` and `friction=False`; zero-valued output columns +there are placeholders, not measured zero forces. Particle-shape contacts do +not enter this rigid ContactQuery. + The sensor retains backend-emitted rows rather than reducing an actor pair to one contact. Force-reporting paths apply the query's positive-impulse filtering; geometry-only paths may provide valid rows with zero impulse. Solver settings diff --git a/embodichain/lab/sim/physics/newton.py b/embodichain/lab/sim/physics/newton.py index a567463eb5..1e0c5640aa 100644 --- a/embodichain/lab/sim/physics/newton.py +++ b/embodichain/lab/sim/physics/newton.py @@ -199,19 +199,33 @@ def supports_rigid_object_group(self) -> bool: @property def supports_contact_sensor(self) -> bool: # ContactSensor consumes the backend-neutral Scene ContactQuery API, - # but not every Newton solver/device pair currently publishes that - # query. Keep this runtime-sensitive so ``solver_type='auto'`` can be - # rechecked after Spawn finalization resolves the concrete solver. + # but CPU MuJoCo cannot publish its force report. Keep this runtime- + # sensitive so ``auto`` and DexUni's scene-selected solver path are + # rechecked after Spawn finalization. solver_type = self.solver_type if solver_type is None: return True solver_type = solver_type.lower() - if solver_type.endswith("dexuni"): - return False + if solver_type == "dexuni": + world = getattr(self._manager, "_world", None) + if world is None: + return True + from dexsim.engine.newton_physics.backend_registry import ( + get_newton_backend, + ) + + backend = get_newton_backend(world) + solver = None if backend is None else backend.solver + features = None if solver is None else getattr(solver, "features", None) + if getattr(features, "backend", None) == "pure_mujoco": + mujoco_solver = backend.mujoco_solver + return mujoco_solver is not None and not mujoco_solver.use_mujoco_cpu + # Other DexUni paths expose rigid-contact geometry through + # ContactQuery, which marks impulse and friction unavailable. + return True runtime_device = self._runtime_device if runtime_device is None: runtime_device = str(getattr(self._manager, "device", "")) return not ( - solver_type in {"mujoco_warp", "dexuni"} - and runtime_device.lower().startswith("cpu") + solver_type == "mujoco_warp" and runtime_device.lower().startswith("cpu") ) diff --git a/embodichain/lab/sim/sensors/contact_sensor.py b/embodichain/lab/sim/sensors/contact_sensor.py index a08b5a1216..31909ac03f 100644 --- a/embodichain/lab/sim/sensors/contact_sensor.py +++ b/embodichain/lab/sim/sensors/contact_sensor.py @@ -110,7 +110,9 @@ class ContactSensor(BaseSensor): The sensor preserves every backend-emitted contact row that passes the backend's positive-impulse filter. Geometry-only Newton solvers retain all - candidate contact rows. + candidate rigid-contact rows. Inspect :attr:`contact_capabilities` before + interpreting ``impulse`` or ``friction``: DexUni paths other than + ``pure_mujoco`` do not report either quantity. """ SUPPORTED_DATA_TYPES = [ diff --git a/tests/sim/sensors/test_contact_newton_e2e.py b/tests/sim/sensors/test_contact_newton_e2e.py index 94b1816d33..e3dcca2590 100644 --- a/tests/sim/sensors/test_contact_newton_e2e.py +++ b/tests/sim/sensors/test_contact_newton_e2e.py @@ -18,6 +18,7 @@ from __future__ import annotations import pytest +import torch import warp as wp from embodichain.lab.sim.cfg import ( @@ -26,9 +27,13 @@ RigidBodyPhysicsCfg, RigidObjectCfg, ) -from embodichain.lab.sim.sensors import ContactSensorCfg +from embodichain.lab.sim.sensors import ( + ArticulationContactFilterCfg, + ContactSensorCfg, +) from embodichain.lab.sim.shapes import CubeCfg from embodichain.lab.sim.sim_manager import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.robots import URRobotCfg pytestmark = [pytest.mark.requires_sim, pytest.mark.gpu] @@ -103,3 +108,142 @@ def test_newton_contact_sensor_reports_each_arena() -> None: from dexsim.engine.newton_physics import teardown_newton_physics teardown_newton_physics() + + +def test_dexuni_pure_mujoco_contact_sensor_reports_impulse() -> None: + """Pure DexUni/MuJoCo exposes geometry, normal and friction impulses.""" + wp.init() + if not wp.is_cuda_available(): + pytest.skip("CUDA is required for the DexUni contact contract.") + + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + device="cuda:0", + physics_cfg=NewtonPhysicsCfg( + device="cuda:0", + num_substeps=1, + use_cuda_graph=False, + solver_cfg={ + "solver_type": "dexuni", + "joint_mode": "dynamic", + "contact_mode": "auto", + }, + ), + ) + ) + try: + sim.add_rigid_object( + RigidObjectCfg( + uid="ground", + shape=CubeCfg(size=[3.0, 3.0, 0.1]), + attrs=RigidBodyPhysicsCfg(), + body_type="static", + init_pos=(0.0, 0.0, -0.05), + ) + ) + sim.add_robot( + URRobotCfg.from_dict( + { + "robot_type": "ur10", + "uid": "robot", + "init_pos": (0.0, 0.0, 0.0), + } + ) + ) + sensor = sim.add_sensor( + ContactSensorCfg( + uid="contacts", + rigid_uid_list=["ground"], + articulation_cfg_list=[ + ArticulationContactFilterCfg(articulation_uid="robot") + ], + filter_need_both_actor=False, + max_contacts_per_env=64, + ) + ) + + sim.update(step=10) + sensor.update() + + assert sensor.contact_capabilities.geometry + assert sensor.contact_capabilities.impulse + assert sensor.contact_capabilities.friction + data = sensor.get_data() + valid = data["is_valid"] + assert valid.any() + assert torch.isfinite(data["position"][valid]).all() + assert torch.isfinite(data["normal"][valid]).all() + assert (data["impulse"][valid] > 1.0e-7).any() + finally: + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + from dexsim.engine.newton_physics import teardown_newton_physics + + teardown_newton_physics() + + +def test_dexuni_vbd_contact_sensor_exposes_geometry_only() -> None: + """VBD-owned rigid contacts keep geometry but disable force fields.""" + wp.init() + if not wp.is_cuda_available(): + pytest.skip("CUDA is required for the DexUni contact contract.") + + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + device="cuda:0", + physics_cfg=NewtonPhysicsCfg( + device="cuda:0", + num_substeps=1, + use_cuda_graph=False, + solver_cfg={"solver_type": "dexuni"}, + ), + ) + ) + try: + sim.add_rigid_object( + RigidObjectCfg( + uid="ground", + shape=CubeCfg(size=[1.0, 1.0, 0.1]), + attrs=RigidBodyPhysicsCfg(), + body_type="static", + ) + ) + sim.add_rigid_object( + RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=[0.2, 0.2, 0.2]), + attrs=RigidBodyPhysicsCfg(mass_props=MassPropertiesCfg(mass=1.0)), + body_type="dynamic", + init_pos=(0.0, 0.0, 0.4), + ) + ) + sensor = sim.add_sensor( + ContactSensorCfg( + uid="contacts", + rigid_uid_list=["cube"], + filter_need_both_actor=False, + max_contacts_per_env=16, + ) + ) + + sim.update(step=120) + sensor.update() + + assert sensor.contact_capabilities.geometry + assert not sensor.contact_capabilities.impulse + assert not sensor.contact_capabilities.friction + data = sensor.get_data() + valid = data["is_valid"] + assert valid.any() + assert torch.isfinite(data["position"][valid]).all() + assert torch.isfinite(data["normal"][valid]).all() + assert torch.count_nonzero(data["impulse"][valid]) == 0 + assert torch.count_nonzero(data["friction"][valid]) == 0 + finally: + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + from dexsim.engine.newton_physics import teardown_newton_physics + + teardown_newton_physics() diff --git a/tests/sim/test_backend_parity.py b/tests/sim/test_backend_parity.py index c5fabe1ce2..1581e38fc5 100644 --- a/tests/sim/test_backend_parity.py +++ b/tests/sim/test_backend_parity.py @@ -123,7 +123,7 @@ def test_contact_sensor_guard_uses_backend_capability() -> None: [ ("mujoco_warp", "cuda:0", True), ("mujoco_warp", "cpu", False), - ("dexuni", "cuda:0", False), + ("dexuni", "cuda:0", True), ("xpbd", "cpu", True), ], ) @@ -139,6 +139,37 @@ def test_newton_contact_sensor_capability_depends_on_runtime( assert backend.supports_contact_sensor is expected +@pytest.mark.parametrize( + ("solver_path", "use_mujoco_cpu", "expected"), + [ + ("pure_mujoco", False, True), + ("pure_mujoco", True, False), + ("pure_vbd", False, True), + ("coupled", False, True), + ("mjvbd_kinematic_soft", False, True), + ], +) +def test_dexuni_contact_sensor_capability_follows_resolved_path( + monkeypatch: pytest.MonkeyPatch, + solver_path: str, + use_mujoco_cpu: bool, + expected: bool, +) -> None: + world = object() + native = SimpleNamespace( + solver_type="dexuni", + solver=SimpleNamespace(features=SimpleNamespace(backend=solver_path)), + mujoco_solver=SimpleNamespace(use_mujoco_cpu=use_mujoco_cpu), + ) + monkeypatch.setattr( + "dexsim.engine.newton_physics.backend_registry.get_newton_backend", + lambda candidate: native if candidate is world else None, + ) + backend = NewtonPhysicsBackend(SimpleNamespace(_world=world)) + + assert backend.supports_contact_sensor is expected + + def test_contact_sensor_capability_is_rechecked_after_prepare( monkeypatch: pytest.MonkeyPatch, ) -> None: From 1832e5676ffbdffe1010ccdb4e8aee5d8e2a128a Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 29 Sep 2026 23:52:25 +0800 Subject: [PATCH 07/27] test(sim): cross-check DexUni contact impulses --- tests/sim/sensors/test_contact_newton_e2e.py | 29 ++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/tests/sim/sensors/test_contact_newton_e2e.py b/tests/sim/sensors/test_contact_newton_e2e.py index e3dcca2590..2a4cd96666 100644 --- a/tests/sim/sensors/test_contact_newton_e2e.py +++ b/tests/sim/sensors/test_contact_newton_e2e.py @@ -17,6 +17,8 @@ from __future__ import annotations +import math + import pytest import torch import warp as wp @@ -175,6 +177,33 @@ def test_dexuni_pure_mujoco_contact_sensor_reports_impulse() -> None: assert torch.isfinite(data["position"][valid]).all() assert torch.isfinite(data["normal"][valid]).all() assert (data["impulse"][valid] > 1.0e-7).any() + + # Cross-check the adapter against DexUni's native force report. The + # sensor's normal impulse is the normal component of that force over + # the solver substep, so this catches axis/sign/unit regressions that + # finite-value assertions cannot detect. + from dexsim.engine.newton_physics.backend_registry import get_newton_backend + from newton import Contacts + + backend = get_newton_backend(sim._world) + assert backend.solver.features.backend == "pure_mujoco" + report = Contacts( + backend.mujoco_solver.get_max_contact_count(), + 0, + device=backend.model.device, + requested_attributes={"force"}, + ) + backend.mujoco_solver.update_contacts(report, backend.state_0) + raw_count = int(report.rigid_contact_count.numpy()[0]) + raw_forces = report.force.numpy()[:raw_count, :3] + raw_normals = report.rigid_contact_normal.numpy()[:raw_count] + substep_dt = float(backend.cfg.dt) / int(backend.cfg.num_substeps) + raw_normal_impulse = abs((raw_forces * raw_normals).sum(axis=1)) * substep_dt + raw_normal_impulse = raw_normal_impulse[raw_normal_impulse > 1.0e-7] + sensor_total = float(data["impulse"][valid].sum().item()) + raw_total = float(raw_normal_impulse.sum()) + assert raw_total > 0.0 + assert math.isclose(sensor_total, raw_total, rel_tol=2.0e-3, abs_tol=2.0e-4) finally: sim.destroy(exit_process=False) SimulationManager.flush_cleanup_queue() From c763aad3d4484e71c5e19db05ff4e7385f2bd55c Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 29 Sep 2026 23:54:28 +0800 Subject: [PATCH 08/27] test(sim): assert DexUni contact invariants --- tests/sim/sensors/test_contact_newton_e2e.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/tests/sim/sensors/test_contact_newton_e2e.py b/tests/sim/sensors/test_contact_newton_e2e.py index 2a4cd96666..c523fd4024 100644 --- a/tests/sim/sensors/test_contact_newton_e2e.py +++ b/tests/sim/sensors/test_contact_newton_e2e.py @@ -176,6 +176,15 @@ def test_dexuni_pure_mujoco_contact_sensor_reports_impulse() -> None: assert valid.any() assert torch.isfinite(data["position"][valid]).all() assert torch.isfinite(data["normal"][valid]).all() + assert torch.isfinite(data["friction"][valid]).all() + assert torch.isfinite(data["impulse"][valid]).all() + assert torch.isfinite(data["distance"][valid]).all() + normal = data["normal"][valid] + normal_norm = torch.linalg.vector_norm(normal, dim=-1) + assert torch.allclose(normal_norm, torch.ones_like(normal_norm), atol=1.0e-4) + tangent_residual = (data["friction"][valid] * normal).sum(dim=-1).abs() + assert tangent_residual.max() < 1.0e-4 + assert (data["user_ids"][valid] >= 0).all() assert (data["impulse"][valid] > 1.0e-7).any() # Cross-check the adapter against DexUni's native force report. The @@ -268,6 +277,7 @@ def test_dexuni_vbd_contact_sensor_exposes_geometry_only() -> None: assert valid.any() assert torch.isfinite(data["position"][valid]).all() assert torch.isfinite(data["normal"][valid]).all() + assert torch.isfinite(data["distance"][valid]).all() assert torch.count_nonzero(data["impulse"][valid]) == 0 assert torch.count_nonzero(data["friction"][valid]) == 0 finally: From 7049fcfad6be3a071b9cb68c80909314f5975807 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 3 Oct 2026 23:32:27 +0800 Subject: [PATCH 09/27] fix(sim): update DexUni migration validation --- .../task_program/test_task_vertical_slices.py | 19 +++++++++-------- ...test_newton_collision_pipeline_schedule.py | 12 ++++++++++- tests/sim/test_open_drawer_tutorial.py | 21 ++++++++++--------- tests/sim/test_scoop_ice_demo.py | 9 +++++++- tests/test_task_program_package_data.py | 1 + 5 files changed, 41 insertions(+), 21 deletions(-) diff --git a/tests/gym/envs/task_program/test_task_vertical_slices.py b/tests/gym/envs/task_program/test_task_vertical_slices.py index 5fcf845f47..c8a5e50265 100644 --- a/tests/gym/envs/task_program/test_task_vertical_slices.py +++ b/tests/gym/envs/task_program/test_task_vertical_slices.py @@ -402,15 +402,16 @@ def test_newton_contact_overlays_are_scoped_in_packaged_manipulation_configs() - assert cube_physics.num_substeps == 20 assert cube_physics.collision_cfg is None assert cube_physics.solver_cfg == { - "solver_type": "mujoco_warp", - "solver": "newton", - "integrator": "implicitfast", - "iterations": 20, - "ls_iterations": 100, - "cone": "elliptic", - "impratio": 1000.0, - "use_mujoco_contacts": True, - "enable_multiccd": True, + "solver_type": "dexuni", + "mujoco_options": { + "solver": "newton", + "integrator": "implicitfast", + "iterations": 20, + "ls_iterations": 100, + "cone": "elliptic", + "impratio": 1000.0, + "enable_multiccd": True, + }, } cube_collision = cube_cfg.rigid_object[0].attrs.collision_props assert isinstance(cube_collision, NewtonCollisionPropertiesCfg) diff --git a/tests/sim/atomic_actions/test_newton_collision_pipeline_schedule.py b/tests/sim/atomic_actions/test_newton_collision_pipeline_schedule.py index 2ea7c730b5..9f977d3de1 100644 --- a/tests/sim/atomic_actions/test_newton_collision_pipeline_schedule.py +++ b/tests/sim/atomic_actions/test_newton_collision_pipeline_schedule.py @@ -34,4 +34,14 @@ def test_atomic_action_tutorial_disables_external_collision_pipeline() -> None: assert physics_cfg.collision_cfg is None dexsim_cfg = physics_cfg.to_dexsim_cfg(gpu_id=0) assert dexsim_cfg.collision_pipeline_cfg is None - assert dexsim_cfg.solver_cfg.use_mujoco_contacts is True + assert dexsim_cfg.solver_cfg.solver_type == "dexuni" + assert dexsim_cfg.solver_cfg.mujoco_options == { + "solver": "newton", + "integrator": "implicitfast", + "iterations": 20, + "ls_iterations": 100, + "cone": "elliptic", + "impratio": 1_000.0, + "enable_multiccd": True, + } + assert dexsim_cfg.solver_cfg.vbd_options == {} diff --git a/tests/sim/test_open_drawer_tutorial.py b/tests/sim/test_open_drawer_tutorial.py index 35e2c2fe72..a761e9d357 100644 --- a/tests/sim/test_open_drawer_tutorial.py +++ b/tests/sim/test_open_drawer_tutorial.py @@ -115,7 +115,7 @@ def add_articulation(self, cfg): assert drawer_cfg.link_attrs is None -def test_tutorial_newton_physics_cfg_uses_shared_mujoco_warp_profile() -> None: +def test_tutorial_newton_physics_cfg_uses_shared_dexuni_profile() -> None: tutorial = _load_tutorial_module() cfg = tutorial._tutorial_physics_cfg("newton") @@ -123,15 +123,16 @@ def test_tutorial_newton_physics_cfg_uses_shared_mujoco_warp_profile() -> None: assert cfg.num_substeps == 20 assert cfg.collision_cfg is None assert cfg.solver_cfg == { - "solver_type": "mujoco_warp", - "solver": "newton", - "integrator": "implicitfast", - "iterations": 20, - "ls_iterations": 100, - "cone": "elliptic", - "impratio": 1_000.0, - "use_mujoco_contacts": True, - "enable_multiccd": True, + "solver_type": "dexuni", + "mujoco_options": { + "solver": "newton", + "integrator": "implicitfast", + "iterations": 20, + "ls_iterations": 100, + "cone": "elliptic", + "impratio": 1_000.0, + "enable_multiccd": True, + }, } diff --git a/tests/sim/test_scoop_ice_demo.py b/tests/sim/test_scoop_ice_demo.py index 74a28daa8e..12495f7702 100644 --- a/tests/sim/test_scoop_ice_demo.py +++ b/tests/sim/test_scoop_ice_demo.py @@ -196,4 +196,11 @@ def manager(cfg): monkeypatch.setattr(demo, "SimulationManager", manager) demo.initialize_simulation(demo.build_parser().parse_args(["--physics", "newton"])) - assert configs[0].physics_cfg.solver_cfg["solver"] == "cg" + assert configs[0].physics_cfg.solver_cfg["solver_type"] == "dexuni" + assert configs[0].physics_cfg.solver_cfg["mujoco_options"]["solver"] == "cg" + assert ( + configs[0].physics_cfg.solver_cfg["vbd_options"][ + "rigid_body_contact_buffer_size" + ] + == 256 + ) diff --git a/tests/test_task_program_package_data.py b/tests/test_task_program_package_data.py index 2e7aac9ead..6bd464b563 100644 --- a/tests/test_task_program_package_data.py +++ b/tests/test_task_program_package_data.py @@ -109,6 +109,7 @@ Path("tasks/manipulation/tableware/stack_cups/README.md"), Path("components/execution_policies/motion_gen_verified.yaml"), Path("components/execution_policies/trajectory_open_loop.yaml"), + Path("components/execution_policies/trajectory_open_loop_slow.yaml"), Path("components/execution_policies/trajectory_open_loop_dense.yaml"), Path("components/expansion_policies/task_program_episode.yaml"), Path("components/randomization_profiles/cube_initial_pose.yaml"), From a09bd7edc9ae2837072ee45501b7f72c3a0092da Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sun, 4 Oct 2026 00:53:57 +0800 Subject: [PATCH 10/27] docs(sim): use DexUni terminology consistently --- agent_context/topics/atomic-actions/atomic-actions.md | 2 +- examples/sim/demo/grasp_cup_to_caffe.py | 2 +- scripts/tutorials/atomic_action/coordinated_placement.py | 2 +- scripts/tutorials/atomic_action/tutorial_utils.py | 2 +- scripts/tutorials/sim/open_drawer.py | 2 +- tests/sim/test_backend_parity.py | 2 +- 6 files changed, 6 insertions(+), 6 deletions(-) diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 9fbc9784d4..94275ec9a6 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -112,7 +112,7 @@ opening. The wait uses segment metadata, not a fixed waypoint index, and is scoped to that same profile. This scene-specific calibration does not promise identical final poses or stability across initial-state changes. Horizontal objects and other robot families retain their prior configuration. -The UR5 V2 `place.py` profile uses a 0.013 m close target, gripper friction 4, +The UR5 DexUni `place.py` profile uses a 0.013 m close target, gripper friction 4, contact history with `latest` matching, contact alpha 0.5, and arm damping 1000. The lower arm damping reduces tracking lag when the fingers open; finger drives and authored release waypoints remain unchanged. Complete cached diff --git a/examples/sim/demo/grasp_cup_to_caffe.py b/examples/sim/demo/grasp_cup_to_caffe.py index 3364041946..a7aed78525 100644 --- a/examples/sim/demo/grasp_cup_to_caffe.py +++ b/examples/sim/demo/grasp_cup_to_caffe.py @@ -213,7 +213,7 @@ def create_table(sim: SimulationManager) -> RigidObject: attrs=RigidBodyPhysicsCfg( mass_props=MassPropertiesCfg(mass=0.5), ), - # This fixture never moves; static shapes also enter V2's MuJoCo view. + # This fixture never moves; static shapes also enter DexUni's MuJoCo view. body_type="static", init_pos=[1.1, -0.5, 0.08], init_rot=[0.0, 0.0, 0.0], diff --git a/scripts/tutorials/atomic_action/coordinated_placement.py b/scripts/tutorials/atomic_action/coordinated_placement.py index 362ef304a3..b8049f5ca4 100644 --- a/scripts/tutorials/atomic_action/coordinated_placement.py +++ b/scripts/tutorials/atomic_action/coordinated_placement.py @@ -235,7 +235,7 @@ def create_table(sim: SimulationManager) -> RigidObject: static_friction=0.95, restitution=0.01, ), - # This fixture never moves; static shapes also enter V2's MuJoCo view. + # This fixture never moves; static shapes also enter DexUni's MuJoCo view. body_type="static", init_pos=list(TABLE_INIT_POS), init_rot=list(TABLE_INIT_ROT), diff --git a/scripts/tutorials/atomic_action/tutorial_utils.py b/scripts/tutorials/atomic_action/tutorial_utils.py index 592d45b3f5..b88cf90ebb 100644 --- a/scripts/tutorials/atomic_action/tutorial_utils.py +++ b/scripts/tutorials/atomic_action/tutorial_utils.py @@ -115,7 +115,7 @@ ) DEFAULT_GRIPPER_CLOSE_QPOS = 0.036 # Calibrated for the UR5's DH-PGI gripper holding the tutorial's 5 cm cube. -# V2's one-way contacts cannot stop the fingers at an oversized close target. +# DexUni's one-way contacts cannot stop the fingers at an oversized close target. DEXUNI_CUBE_CLOSE_QPOS = 0.013 NEWTON_GRASP_CONTACT_STIFFNESS = 4.0e4 NEWTON_GRASP_CONTACT_DAMPING = 4.0e2 diff --git a/scripts/tutorials/sim/open_drawer.py b/scripts/tutorials/sim/open_drawer.py index c9a162ed2a..ecd40c17b9 100644 --- a/scripts/tutorials/sim/open_drawer.py +++ b/scripts/tutorials/sim/open_drawer.py @@ -530,7 +530,7 @@ def _tutorial_physics_cfg( """Build the physics configuration used by this tutorial.""" physics_cfg = physics_cfg_for_backend(backend) if isinstance(physics_cfg, NewtonPhysicsCfg): - # Share the V2 articulation profile with the atomic-action tutorials. + # Share the DexUni articulation profile with the atomic-action tutorials. # Keep the finer drawer substep cadence and native contact ownership. physics_cfg.num_substeps = 20 physics_cfg.collision_cfg = None diff --git a/tests/sim/test_backend_parity.py b/tests/sim/test_backend_parity.py index 1581e38fc5..9b4f156806 100644 --- a/tests/sim/test_backend_parity.py +++ b/tests/sim/test_backend_parity.py @@ -146,7 +146,7 @@ def test_newton_contact_sensor_capability_depends_on_runtime( ("pure_mujoco", True, False), ("pure_vbd", False, True), ("coupled", False, True), - ("mjvbd_kinematic_soft", False, True), + ("dexuni_kinematic_soft", False, True), ], ) def test_dexuni_contact_sensor_capability_follows_resolved_path( From ad9ba877cf8d0baf1724b402ef53c3d13000dee1 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sun, 4 Oct 2026 16:46:23 +0800 Subject: [PATCH 11/27] wip --- embodichain/lab/sim/utility/dynamic_pybind.py | 10 +++++++++- tests/sim/utility/test_dynamic_pybind.py | 6 ++++-- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/embodichain/lab/sim/utility/dynamic_pybind.py b/embodichain/lab/sim/utility/dynamic_pybind.py index 8946a1fd68..b1e395e776 100644 --- a/embodichain/lab/sim/utility/dynamic_pybind.py +++ b/embodichain/lab/sim/utility/dynamic_pybind.py @@ -61,7 +61,15 @@ def set_projective_uv(self: RenderBody, proj_direct: np.ndarray | None = None) - start_idx = 0 for i in range(n_mesh): mesh_vert_uvs = vert_uvs[start_idx : start_idx + n_vert_list[i], :] - self.set_uv_mapping(uvs=mesh_vert_uvs, mesh_id=i) + # DexSim's binding accepts a column-major Eigen matrix while the + # generated UV array is indexed as row-major ``(vertex, coord)``. + # Repack the same values so the native binding receives each ``(u, v)`` + # pair in the intended order without requiring a native rebuild. + native_uvs = np.asarray( + mesh_vert_uvs.ravel(order="C").reshape(mesh_vert_uvs.shape, order="F"), + dtype=np.float32, + ) + self.set_uv_mapping(uvs=native_uvs, mesh_id=i) start_idx += n_vert_list[i] diff --git a/tests/sim/utility/test_dynamic_pybind.py b/tests/sim/utility/test_dynamic_pybind.py index 0f5c9bd5e2..93422761e7 100644 --- a/tests/sim/utility/test_dynamic_pybind.py +++ b/tests/sim/utility/test_dynamic_pybind.py @@ -69,5 +69,7 @@ def fake_get_mesh_auto_uv(mesh, received_direction): set_projective_uv(render_body, project_direction) - np.testing.assert_array_equal(render_body.uv_mappings[0], generated_uv[:3]) - np.testing.assert_array_equal(render_body.uv_mappings[1], generated_uv[3:]) + expected_first = generated_uv[:3].ravel(order="C").reshape((3, 2), order="F") + expected_second = generated_uv[3:].ravel(order="C").reshape((3, 2), order="F") + np.testing.assert_array_equal(render_body.uv_mappings[0], expected_first) + np.testing.assert_array_equal(render_body.uv_mappings[1], expected_second) From 94280206ac5007fd5943aed82e9ab29950d3a2f3 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 10 Oct 2026 23:35:21 +0800 Subject: [PATCH 12/27] feat(physics): support current DexUni two-way coupling contracts --- .../articulation-adapters.md | 11 ++ .../physics-configuration.md | 17 +- .../embodichain/embodichain.lab.sim.cfg.rst | 42 +++++ embodichain/lab/sim/cfg/articulation.py | 36 +++- embodichain/lab/sim/cfg/physics.py | 6 +- embodichain/lab/sim/spawn/descriptors.py | 60 ++++++ embodichain/lab/sim/spawn/usd.py | 4 + pyproject.toml | 2 +- tests/conftest.py | 9 + tests/sim/spawn/test_descriptors.py | 137 ++++++++++++++ tests/sim/test_cfg.py | 1 + tests/sim/test_dexuni_two_way.py | 178 ++++++++++++++++++ tests/sim/test_sim_manager_cfg.py | 51 +++++ 13 files changed, 545 insertions(+), 9 deletions(-) create mode 100644 tests/sim/test_dexuni_two_way.py diff --git a/agent_context/topics/simulation-system/articulation-adapters.md b/agent_context/topics/simulation-system/articulation-adapters.md index 2d449a0c08..88bcdd95f6 100644 --- a/agent_context/topics/simulation-system/articulation-adapters.md +++ b/agent_context/topics/simulation-system/articulation-adapters.md @@ -21,12 +21,21 @@ the native articulation before Direct GPU initialization and initial reset. Iteration counts are configured together because the native setter is atomic. Applying them only during facade binding is too late for captured GPU settings. +`root_props.newton_gravity_compensation` is Newton creation intent applied in +both asset physics modes; omission preserves the source/default policy. It +compensates articulation link weight without removing payload gravity or +contact reactions. DexSim checks the resolved solver's MuJoCo support. + `joint_drive_props` is the sole joint-property entry point; sparse fields retain source values. `drive_type` describes the response and `target_mode` the active command components. Lowering masks inactive gains: none/effort clear both, velocity clears position gain. Newton rejects active acceleration drives; non-MuJoCo position mode is gain-based emulation with zero velocity target. Exact field options and defaults remain in `cfg/articulation.py`. +Newton-specific `joint_drive_props.passive_damping` lowers to passive DOF +damping, separately from the drive's velocity gain. It retains the same +exact-name/regex/control-part matching and remains active for passive or effort +target modes. Applied Newton-only settings are rejected on Default. ## State order and batched writes @@ -122,3 +131,5 @@ FREE-joint synchronization, mesh export and no-op root writes; `tests/sim/objects/test_articulation.py` for topology/FK. Source configuration and root timing are covered by `tests/sim/spawn/test_descriptors.py` and `tests/sim/test_sim_manager.py`. +`tests/sim/test_dexuni_two_way.py` compares contact-limited dynamic fingers +against the same one-way commands, including CUDA Graph and reset. diff --git a/agent_context/topics/simulation-system/physics-configuration.md b/agent_context/topics/simulation-system/physics-configuration.md index 2d243afe7f..c09260466e 100644 --- a/agent_context/topics/simulation-system/physics-configuration.md +++ b/agent_context/topics/simulation-system/physics-configuration.md @@ -6,7 +6,7 @@ Return to the [simulation overview](simulation-system.md) for lifecycle entry. ## Backend, solver and device resolution `SimulationManagerCfg.physics_cfg` selects the backend by config type. -`cfg/simulation.py` owns `PhysicsBackendCfg`, `DefaultPhysicsCfg` and +`cfg/physics.py` owns `PhysicsBackendCfg`, `DefaultPhysicsCfg` and `NewtonPhysicsCfg`; do not add another backend string to the manager config. Gym's file-owned selector and composition rules belong to [environment configuration](../env-framework/configuration.md). @@ -26,7 +26,7 @@ The concrete solver is resolved from the complete Spawn scene, then exposed through `NewtonPhysicsBackend.solver_type`. Do not guess a solver-specific robot preset while it is unresolved. Gradient mode requires an explicit `semi_implicit` solver. Read exact solver options/defaults in -`cfg/simulation.py` and the installed DexSim config types; do not substitute a +`cfg/physics.py` and the installed DexSim config types; do not substitute a hardcoded fallback when an expected DexSim API is absent. `collision_cfg` owns the external Newton contact pipeline. Its @@ -38,10 +38,19 @@ external-pipeline tuning does not configure internal contact generation. The manual differentiable trajectory requires the external pipeline; see [differentiable environment](../differentiable-env/differentiable-env.md). +DexUni preserves its one-way contact policy unless `coupling="two_way"` is +explicit. Two-way feedback requires dynamically integrated MuJoCo joints and +rejects full-surface rigid-soft contacts. Prescribed kinematic trajectories +cannot respond to contact reactions. Forward solver expert options through +`NewtonPhysicsCfg.solver_cfg`; DexSim owns their validation and dispatch. +Articulation gravity compensation and passive damping are separate creation +settings; see [articulation adapters](articulation-adapters.md). + Capability checks remain explicit: Default owns native rigid constraints and Newton deformables are separate. Contact support is rechecked after Newton -solver resolution; DexUni and MuJoCo-Warp on CPU do not expose the supported -ContactQuery path. See `physics/` and [sensors](../sensor-system/sensor-system.md). +solver resolution; capabilities depend on the concrete DexUni contact path +and whether MuJoCo runs on CUDA. See `physics/` and the +[contact capability contract](../sensor-system/lifecycle.md#contact-queries). ## Sparse source physics 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 8ad718a951..f9ba36283a 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst @@ -135,6 +135,13 @@ For manager-owned Newton steps, ``sync_to_renderer=None`` or ``False`` publishes only for visual consumers; ``True`` additionally requests publication on steps without consumers. +DexUni accepts ``coupling="two_way"`` with ``joint_mode="dynamic"`` to feed +contact reactions back to articulations. Keep ``collision_cfg=None`` when +DexUni owns collision detection. ``coupling_options`` configures the native +feedback policy; full-surface rigid-soft contacts are unsupported in this mode. +Articulation gravity compensation and passive joint damping are configured +separately below. + .. currentmodule:: embodichain.lab.sim.cfg.physics .. autosummary:: @@ -148,6 +155,41 @@ without consumers. physics_backend_from_cfg validate_physics_cfg +Articulation configuration module +--------------------------------- + +Articulation types live in ``embodichain.lab.sim.cfg.articulation`` and remain +available from the ``embodichain.lab.sim.cfg`` facade. Root creation settings +are independent of sparse link and joint physics overlays. +``root_props.newton_gravity_compensation`` compensates articulation link +weight on MuJoCo-backed Newton solvers while retaining payload gravity and +contact reactions. Omission preserves source intent. + +``NewtonJointDrivePropertiesCfg.passive_damping`` selects passive DOF damping +through the same exact-name, regex and control-part rules as the drive gains. +It is independent of the drive velocity gain ``damping`` and stays active for +passive and effort target modes. In dictionaries, select this subtype with +``joint_drive_props.backend="newton"``. Both settings require Newton. + +.. currentmodule:: embodichain.lab.sim.cfg.articulation + +.. autosummary:: + + ArticulationRootPropertiesCfg + LinkPhysicsOverrideCfg + link_attrs_from_dict + JointDrivePropertiesCfg + NewtonJointDrivePropertiesCfg + ArticulationCfg + +.. autoclass:: ArticulationRootPropertiesCfg + :members: + :no-index: + +.. autoclass:: NewtonJointDrivePropertiesCfg + :members: + :no-index: + Compatibility facade -------------------- diff --git a/embodichain/lab/sim/cfg/articulation.py b/embodichain/lab/sim/cfg/articulation.py index 9f5f34f657..c63ce6622e 100644 --- a/embodichain/lab/sim/cfg/articulation.py +++ b/embodichain/lab/sim/cfg/articulation.py @@ -34,6 +34,15 @@ _rigid_body_physics_from_dict, ) +__all__ = [ + "ArticulationRootPropertiesCfg", + "LinkPhysicsOverrideCfg", + "link_attrs_from_dict", + "JointDrivePropertiesCfg", + "NewtonJointDrivePropertiesCfg", + "ArticulationCfg", +] + def _normalize_joint_target_mode(value: object) -> int: """Normalize a portable joint target mode to its backend integer value.""" @@ -94,6 +103,16 @@ class ArticulationRootPropertiesCfg: min_velocity_iters: int | None = None """Default-only minimum root velocity-solver iterations (0 to 255).""" + newton_gravity_compensation: float | None = None + """MuJoCo link-weight compensation fraction in [0, 1], on Newton only. + + ``None`` preserves the source/default policy; zero retains gravity and one + compensates every link in this articulation. Payload gravity and contact + reactions remain active. This creation setting applies in both asset + physics modes, independently of DexUni's coupling policy. The finalized + Newton solver must support MuJoCo gravity compensation. + """ + def __post_init__(self) -> None: """Require the two values consumed by the atomic Default setter.""" if (self.min_position_iters is None) != (self.min_velocity_iters is None): @@ -376,12 +395,23 @@ def to_dict(self) -> dict[str, Any]: @configclass class NewtonJointDrivePropertiesCfg(JointDrivePropertiesCfg): - """Compatibility subtype for serialized Newton joint-drive configs. + """Joint-drive configuration with Newton-specific passive dynamics. ``target_mode`` is now portable and lives on :class:`JointDrivePropertiesCfg`. The subtype remains so existing - ``backend="newton"`` dictionaries and round trips retain their type; new - robot definitions should use the common class. + ``backend="newton"`` dictionaries and round trips retain their type. Use + the common class when no Newton-specific dynamics are required. + """ + + passive_damping: dict[str, float] | float | None = None + """Nonnegative passive joint damping, independent of drive damping. + + Uses the same joint-name, regex and robot control-part rules as the drive + gains. Apply it with ``asset_physics_mode='overlay'``; ``None`` preserves + source damping. Unlike :attr:`damping`, this + lowers to Newton's ``JointDofConfig.damping`` and remains active when the + drive target mode is ``none`` or ``effort``. It requires Newton and is + consumed by MuJoCo-backed solvers, including dynamic DexUni. """ diff --git a/embodichain/lab/sim/cfg/physics.py b/embodichain/lab/sim/cfg/physics.py index 0514fa6d79..7734507670 100644 --- a/embodichain/lab/sim/cfg/physics.py +++ b/embodichain/lab/sim/cfg/physics.py @@ -352,7 +352,11 @@ class NewtonPhysicsCfg(PhysicsBackendCfg): For ``solver_type='dexuni'``, the current DexSim contract keeps expert options under ``vbd_options``, ``mujoco_options`` and ``collision_options``; use ``joint_mode`` and ``contact_mode`` for the - coupled dispatch policy. + coupled dispatch policy. ``coupling='two_way'`` enables contact reactions + on dynamically integrated articulations. Optional ``coupling_options`` + accepts ``mass_scale``, ``proxy_relaxation`` and ``iterations``. Two-way + coupling requires ``joint_mode='dynamic'`` and does not support full-surface + rigid-soft contacts; DexSim validates these combinations. """ collision_cfg: NewtonCollisionPipelineCfg | Mapping[str, Any] | None = field( diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index 4164d530a5..8152b347b8 100644 --- a/embodichain/lab/sim/spawn/descriptors.py +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -71,6 +71,7 @@ DefaultRigidBodyPropertiesCfg, MassPropertiesCfg, NewtonCollisionPropertiesCfg, + NewtonJointDrivePropertiesCfg, NewtonRigidBodyMaterialCfg, RigidBodyMaterialCfg, RigidBodyPhysicsCfg, @@ -621,6 +622,10 @@ def articulation_desc_from_cfg( newton_solver_type=newton_solver_type, ) fixed_base, self_collision_enabled = _articulation_root_values(cfg) + compensation = _articulation_gravity_compensation(cfg, newton_solver_type) + newton_options = ( + {} if compensation is None else {"newton_gravity_compensation": compensation} + ) return ArticulationDesc( name=_articulation_uid(cfg.uid, str(path)), pose=_pose_from_cfg(cfg), @@ -635,9 +640,32 @@ def articulation_desc_from_cfg( urdf_read_inertia=True, per_env=per_env, body_scale=_vector3(cfg.body_scale, field_name="body_scale"), + **newton_options, ) +def _articulation_gravity_compensation( + cfg: ArticulationCfg, newton_solver_type: str | None +) -> float | None: + """Validate explicit Newton articulation creation intent.""" + value = cfg.root_props.newton_gravity_compensation + if value is None: + return None + if ( + not isinstance(value, numbers.Real) + or not np.isfinite(value) + or not 0 <= value <= 1 + ): + raise ValueError("newton_gravity_compensation must be finite and in [0, 1].") + if newton_solver_type is None: + raise ValueError("newton_gravity_compensation requires the Newton backend.") + if not hasattr(ArticulationDesc, "newton_gravity_compensation"): + raise RuntimeError( + "newton_gravity_compensation requires DexSim dev f32785317 or newer." + ) + return float(value) + + def _validate_articulation_rigid_physics( cfg: ArticulationCfg, *, @@ -734,6 +762,9 @@ def configure_articulation_desc( f"Articulation source {desc.name!r} must be resolved before " "configuration." ) + compensation = _articulation_gravity_compensation(cfg, newton_solver_type) + if compensation is not None: + desc.newton_gravity_compensation = compensation preserve_source_physics = cfg.resolve_asset_physics_mode() == "preserve" setattr(desc, "_embodichain_preserve_source_physics", preserve_source_physics) for link in desc.links: @@ -895,6 +926,14 @@ def configure_articulation_desc( newton=newton_desc, newton_target_mode=joint_target_modes.get(joint_name), ) + if ( + newton_desc.damping is not None + and desc.get_joint_desc(joint_name).newton.damping != newton_desc.damping + ): + raise RuntimeError( + "DexSim did not retain passive_damping; install DexSim dev " + "f32785317 or newer." + ) _apply_articulation_newton_gap_default(desc, newton_solver_type) return desc @@ -1151,6 +1190,27 @@ def _compile_joint_properties( setattr(default_desc, default_field, scalar) setattr(newton_desc, newton_field, scalar) + if ( + isinstance(cfg.joint_drive_props, NewtonJointDrivePropertiesCfg) + and cfg.joint_drive_props.passive_damping is not None + ): + if newton_solver_type is None: + raise ValueError("passive_damping requires the Newton backend.") + matches = _joint_property_matches( + cfg.joint_drive_props.passive_damping, + joint_names, + property_name="passive_damping", + control_parts=control_parts, + ) + for joint_name, value in matches: + if ( + not isinstance(value, numbers.Real) + or not np.isfinite(value) + or value < 0 + ): + raise ValueError("passive_damping must be finite and nonnegative.") + joint_properties[joint_name][1].damping = float(value) + # Solvers that ignore Newton's target-mode enum still consume drive gains. # Masking inactive components makes NONE, EFFORT, and VELOCITY deterministic # across the currently supported solver set. diff --git a/embodichain/lab/sim/spawn/usd.py b/embodichain/lab/sim/spawn/usd.py index 77aa4632f3..3302733044 100644 --- a/embodichain/lab/sim/spawn/usd.py +++ b/embodichain/lab/sim/spawn/usd.py @@ -39,6 +39,7 @@ _compile_rigid_physics, _compile_visual_material, _articulation_root_values, + _articulation_gravity_compensation, _pose_from_cfg, _required_uid, _resolve_rigid_physics, @@ -178,6 +179,7 @@ def articulation_desc_from_usd( newton_solver_type: str | None = None, ) -> tuple[ArticulationDesc, dict[str, MaterialDesc]]: """Select the sole articulation in a USD stage.""" + compensation = _articulation_gravity_compensation(cfg, newton_solver_type) preserve_asset_physics = cfg.resolve_asset_physics_mode() == "preserve" if not preserve_asset_physics: _validate_articulation_rigid_physics( @@ -206,6 +208,8 @@ def articulation_desc_from_usd( fixed_base_default=bool(desc.fixed_base), self_collision_default=desc.enable_self_collision, ) + if compensation is not None: + desc.newton_gravity_compensation = compensation return desc, materials diff --git a/pyproject.toml b/pyproject.toml index bef5415c44..0a74511358 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -25,7 +25,7 @@ dynamic = ["version"] # Core install dependencies (kept from requirements.txt). dependencies = [ - "dexsim_engine==0.5.1rc1", + "dexsim_engine>=0.5.1rc1,<0.6", "setuptools>=78.1.1", "gymnasium>=0.29.1", "huggingface_hub>=0.35.3,<1.0", diff --git a/tests/conftest.py b/tests/conftest.py index 44b48cae8c..ab4c6550b0 100644 --- a/tests/conftest.py +++ b/tests/conftest.py @@ -26,6 +26,15 @@ os.environ.setdefault("EMBODICHAIN_SIM_EXIT_PROCESS", "0") +@pytest.fixture +def requires_dexuni_two_way() -> None: + """Qualify dev-only contracts without excluding released-engine coverage.""" + from dexsim.engine.newton_physics import DexUniSolverCfg + + if "coupling" not in inspect.signature(DexUniSolverCfg).parameters: + pytest.skip("DexUni two-way coupling requires DexSim dev f32785317 or newer.") + + @pytest.fixture(scope="session") def _discover_task_packages(): """Discover all installed task packages once per test session. diff --git a/tests/sim/spawn/test_descriptors.py b/tests/sim/spawn/test_descriptors.py index 11b57a2a08..a4edf0e669 100644 --- a/tests/sim/spawn/test_descriptors.py +++ b/tests/sim/spawn/test_descriptors.py @@ -1138,6 +1138,143 @@ def test_articulation_constructor_defers_newton_properties_until_configure() -> assert contact.ke is None +@pytest.mark.parametrize("mode", ["preserve", "overlay"]) +@pytest.mark.usefixtures("requires_dexuni_two_way") +def test_articulation_newton_gravity_compensation_is_creation_intent(mode) -> None: + cfg = ArticulationCfg.from_dict( + { + "uid": "robot", + "fpath": "robot.urdf", + "asset_physics_mode": mode, + "root_props": {"newton_gravity_compensation": 1.0}, + } + ) + descriptor = articulation_desc_from_cfg(cfg, newton_solver_type="dexuni") + assert descriptor.newton_gravity_compensation == 1.0 + + resolved = _resolved_articulation_desc() + configure_articulation_desc(resolved, cfg, newton_solver_type="dexuni") + assert resolved.newton_gravity_compensation == 1.0 + + +def test_articulation_unconfigured_gravity_compensation_preserves_source() -> None: + descriptor = _resolved_articulation_desc() + descriptor.newton_gravity_compensation = 0.5 + configure_articulation_desc( + descriptor, ArticulationCfg(), newton_solver_type="dexuni" + ) + assert descriptor.newton_gravity_compensation == 0.5 + + +@pytest.mark.parametrize("mode", ["preserve", "overlay"]) +@pytest.mark.usefixtures("requires_dexuni_two_way") +def test_usd_articulation_forwards_gravity_compensation(monkeypatch, mode) -> None: + source = _resolved_articulation_desc() + monkeypatch.setattr( + "embodichain.lab.sim.spawn.usd._parse_singleton", + lambda *_args: (SimpleNamespace(materials={}), source), + ) + cfg = ArticulationCfg.from_dict( + { + "uid": "robot", + "fpath": "robot.usd", + "asset_physics_mode": mode, + "root_props": {"newton_gravity_compensation": 0.5}, + } + ) + descriptor, _ = articulation_desc_from_usd(cfg, newton_solver_type="dexuni") + assert descriptor.newton_gravity_compensation == 0.5 + + +@pytest.mark.parametrize("value", [-0.1, 1.1, float("nan"), float("inf")]) +def test_articulation_gravity_compensation_rejects_invalid_values(value) -> None: + cfg = ArticulationCfg() + cfg.root_props.newton_gravity_compensation = value + with pytest.raises(ValueError, match="newton_gravity_compensation"): + articulation_desc_from_cfg( + cfg, source_path="robot.urdf", newton_solver_type="dexuni" + ) + + +def test_articulation_gravity_compensation_rejects_default_backend() -> None: + cfg = ArticulationCfg() + cfg.root_props.newton_gravity_compensation = 1.0 + with pytest.raises(ValueError, match="Newton backend"): + articulation_desc_from_cfg(cfg, source_path="robot.urdf") + + +def test_gravity_compensation_requires_native_descriptor_support(monkeypatch) -> None: + monkeypatch.delattr(ArticulationDesc, "newton_gravity_compensation", raising=False) + cfg = ArticulationCfg() + cfg.root_props.newton_gravity_compensation = 1.0 + with pytest.raises(RuntimeError, match="requires DexSim dev"): + articulation_desc_from_cfg( + cfg, source_path="robot.urdf", newton_solver_type="dexuni" + ) + + +@pytest.mark.parametrize("target_mode", ["position_velocity", "none"]) +@pytest.mark.usefixtures("requires_dexuni_two_way") +def test_newton_passive_damping_is_independent_of_drive_gains(target_mode) -> None: + cfg = ArticulationCfg.from_dict( + { + "asset_physics_mode": "overlay", + "joint_drive_props": { + "backend": "newton", + "target_mode": target_mode, + "stiffness": 12.0, + "damping": 4.0, + "passive_damping": {"arm_.*": 50.0}, + }, + } + ) + descriptor = _resolved_articulation_desc() + configure_articulation_desc(descriptor, cfg, newton_solver_type="dexuni") + joint = descriptor.get_joint_desc("arm_joint") + assert joint.newton.damping == 50.0 + expected_kd = 0.0 if target_mode == "none" else 4.0 + assert joint.newton.target_kd == expected_kd + assert joint.dexsim.damping == expected_kd + round_trip = JointDrivePropertiesCfg.from_dict(cfg.joint_drive_props.to_dict()) + assert round_trip.passive_damping == {"arm_.*": 50.0} + + +@pytest.mark.parametrize("value", [-1.0, float("nan"), float("inf")]) +def test_newton_passive_damping_rejects_invalid_values(value) -> None: + cfg = ArticulationCfg(asset_physics_mode="overlay") + cfg.joint_drive_props = NewtonJointDrivePropertiesCfg() + cfg.joint_drive_props.passive_damping = value + with pytest.raises(ValueError, match="passive_damping"): + configure_articulation_desc( + _resolved_articulation_desc(), cfg, newton_solver_type="dexuni" + ) + + +def test_newton_passive_damping_rejects_default_backend() -> None: + cfg = ArticulationCfg(asset_physics_mode="overlay") + cfg.joint_drive_props = NewtonJointDrivePropertiesCfg() + cfg.joint_drive_props.passive_damping = 50.0 + with pytest.raises(ValueError, match="Newton backend"): + configure_articulation_desc(_resolved_articulation_desc(), cfg) + + +def test_passive_damping_rejects_native_merges_that_discard_it(monkeypatch) -> None: + cfg = ArticulationCfg( + asset_physics_mode="overlay", + joint_drive_props=NewtonJointDrivePropertiesCfg(passive_damping=50.0), + ) + descriptor = _resolved_articulation_desc() + set_properties = descriptor.set_joint_properties + + def discard_damping(*args, **kwargs): + set_properties(*args, **kwargs) + descriptor.get_joint_desc("arm_joint").newton.damping = None + + monkeypatch.setattr(descriptor, "set_joint_properties", discard_damping) + with pytest.raises(RuntimeError, match="did not retain passive_damping"): + configure_articulation_desc(descriptor, cfg, newton_solver_type="dexuni") + + @pytest.mark.parametrize("solver_type", [None, "auto", "mujoco_warp"]) @pytest.mark.parametrize("mode", ["preserve", "overlay"]) @pytest.mark.parametrize("source_gap", [None, 0.0, 0.025]) diff --git a/tests/sim/test_cfg.py b/tests/sim/test_cfg.py index 4bf64db16f..c53d24754f 100644 --- a/tests/sim/test_cfg.py +++ b/tests/sim/test_cfg.py @@ -1004,6 +1004,7 @@ def test_backend_joint_and_articulation_configs_round_trip() -> None: "sleep_threshold": None, "min_position_iters": None, "min_velocity_iters": None, + "newton_gravity_compensation": None, } assert type(restored_root) is ArticulationRootPropertiesCfg diff --git a/tests/sim/test_dexuni_two_way.py b/tests/sim/test_dexuni_two_way.py new file mode 100644 index 0000000000..a1b2a218be --- /dev/null +++ b/tests/sim/test_dexuni_two_way.py @@ -0,0 +1,178 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Physical acceptance of DexUni contact feedback through EmbodiChain.""" + +from __future__ import annotations + +from pathlib import Path + +import pytest +import torch + +from embodichain.lab.sim.cfg import ( + ArticulationCfg, + ArticulationRootPropertiesCfg, + MassPropertiesCfg, + NewtonCollisionPropertiesCfg, + NewtonJointDrivePropertiesCfg, + NewtonPhysicsCfg, + NewtonRigidBodyMaterialCfg, + RenderCfg, + RigidBodyPhysicsCfg, + RigidObjectCfg, +) +from embodichain.lab.sim.shapes import CubeCfg +from embodichain.lab.sim.sim_manager import SimulationManager, SimulationManagerCfg + +pytestmark = [ + pytest.mark.requires_sim, + pytest.mark.gpu, + pytest.mark.usefixtures("requires_dexuni_two_way"), +] + +FINGER_START = 0.08 +FINGER_HALF = 0.01 +BOX_HALF = 0.025 +TARGET = FINGER_START - FINGER_HALF +CONTACT_TRAVEL = FINGER_START - FINGER_HALF - BOX_HALF + + +def _gripper_urdf(path: Path) -> None: + """Write two light, effort-limited fingers without downloadable assets.""" + fingers = [] + for name, sign in (("left", -1.0), ("right", 1.0)): + fingers.append(f""" + + + + + + + + + """) + path.write_text( + '' + "".join(fingers) + "", + encoding="utf-8", + ) + + +def _grip(path: Path, coupling: str, use_cuda_graph: bool) -> torch.Tensor: + material = RigidBodyPhysicsCfg( + collision_props=NewtonCollisionPropertiesCfg(margin=0.0, gap=0.0), + material_props=NewtonRigidBodyMaterialCfg( + ke=2.0e4, kd=100.0, dynamic_friction=1.0 + ), + ) + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + render_cfg=RenderCfg(renderer="no-render"), + physics_cfg=NewtonPhysicsCfg( + device="cuda:0", + physics_dt=1.0 / 60.0, + num_substeps=10, + use_cuda_graph=use_cuda_graph, + collision_cfg=None, + solver_cfg={ + "solver_type": "dexuni", + "joint_mode": "dynamic", + "coupling": coupling, + "vbd_options": {"iterations": 10}, + "collision_options": {"broad_phase": "nxn"}, + }, + ), + ) + ) + try: + articulation = sim.add_articulation( + ArticulationCfg( + uid="gripper", + fpath=str(path), + asset_physics_mode="overlay", + build_pk_chain=False, + attrs=material, + root_props=ArticulationRootPropertiesCfg( + newton_gravity_compensation=1.0 + ), + 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, + ), + ) + ) + sim.add_rigid_object( + RigidObjectCfg( + uid="floor", + shape=CubeCfg(size=(1.0, 1.0, 0.1)), + body_type="static", + init_pos=(0.0, 0.0, -0.05), + attrs=material, + ) + ) + box_material = material.copy() + box_material.mass_props = MassPropertiesCfg(mass=0.0625) + box = sim.add_rigid_object( + RigidObjectCfg( + uid="box", + shape=CubeCfg(size=(2 * BOX_HALF,) * 3), + init_pos=(0.0, 0.0, BOX_HALF), + attrs=box_material, + ) + ) + sim.prepare() + articulation.set_qpos( + torch.full((1, 2), TARGET, device=sim.device), target=True + ) + sim.update(step=120) + qpos = articulation.get_qpos().detach().cpu() + assert torch.isfinite(qpos).all() + assert torch.isfinite(box.get_local_pose()).all() + if use_cuda_graph: + assert sim.physics.cuda_graph_status == "captured" + + sim.reset_objects_state() + torch.testing.assert_close( + articulation.get_qpos(), torch.zeros((1, 2), device=sim.device) + ) + return qpos + finally: + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +@pytest.mark.parametrize("use_cuda_graph", [False, True]) +def test_dexuni_two_way_contact_blocks_dynamic_finger_targets( + tmp_path: Path, use_cuda_graph: bool +) -> None: + if not torch.cuda.is_available(): + pytest.skip("CUDA is required for the DexUni two-way contract.") + path = tmp_path / "gripper.urdf" + _gripper_urdf(path) + one_way = _grip(path, "one_way", use_cuda_graph) + two_way = _grip(path, "two_way", use_cuda_graph) + + # Without feedback, the same effort-limited fingers reach the commanded + # overlapping pose. Feedback stops them at the box's physical surface. + assert torch.all(one_way > TARGET - 0.005), one_way + assert torch.all(two_way < CONTACT_TRAVEL + 0.01), two_way + assert torch.all(two_way > CONTACT_TRAVEL - 0.01), two_way diff --git a/tests/sim/test_sim_manager_cfg.py b/tests/sim/test_sim_manager_cfg.py index 128d01efd8..fef7fd2529 100644 --- a/tests/sim/test_sim_manager_cfg.py +++ b/tests/sim/test_sim_manager_cfg.py @@ -501,3 +501,54 @@ def test_newton_physics_cfg_directly_accepts_dexsim_solver_cfg_object() -> None: assert isinstance(dexsim_cfg.solver_cfg, XPBDSolverCfg) assert dexsim_cfg.solver_cfg.iterations == 8 assert dexsim_cfg.solver_cfg.enable_restitution is True + + +@pytest.mark.no_sim +@pytest.mark.usefixtures("requires_dexuni_two_way") +def test_newton_physics_cfg_forwards_two_way_coupling() -> None: + cfg = NewtonPhysicsCfg( + collision_cfg=None, + solver_cfg={ + "solver_type": "dexuni", + "joint_mode": "dynamic", + "coupling": "two_way", + "coupling_options": { + "mass_scale": 4.0, + "proxy_relaxation": 0.5, + "iterations": 2, + }, + "mujoco_articulation_paths": ("env_0/robot",), + "vbd_options": {"iterations": 10}, + }, + ) + native = cfg.to_dexsim_cfg(gpu_id=0) + assert native.collision_pipeline_cfg is None + assert native.solver_cfg.joint_mode == "dynamic" + assert native.solver_cfg.coupling == "two_way" + assert native.solver_cfg.coupling_options == cfg.solver_cfg["coupling_options"] + assert native.solver_cfg.mujoco_articulation_paths == ("env_0/robot",) + assert native.solver_cfg.vbd_options == {"iterations": 10} + + +@pytest.mark.no_sim +@pytest.mark.parametrize( + ("options", "error"), + [ + ({"joint_mode": "kinematic"}, "requires joint_mode='dynamic'"), + ({"coupling_options": {"mass_scale": 0.0}}, "mass_scale"), + ({"coupling_options": {"proxy_relaxation": 1.1}}, "proxy_relaxation"), + ({"coupling_options": {"iterations": 0}}, "iterations"), + ({"coupling_options": {"unknown": 1.0}}, "Unsupported"), + ( + {"collision_options": {"enable_rigid_soft_full_surface_contact": True}}, + "full-surface", + ), + ], +) +@pytest.mark.usefixtures("requires_dexuni_two_way") +def test_newton_physics_cfg_preserves_native_two_way_validation(options, error) -> None: + cfg = NewtonPhysicsCfg( + solver_cfg={"solver_type": "dexuni", "coupling": "two_way", **options} + ) + with pytest.raises(ValueError, match=error): + cfg.to_dexsim_cfg(gpu_id=0) From 6e60b8e76f704648c56018e504c288f2c3aba69a Mon Sep 17 00:00:00 2001 From: yuecideng Date: Wed, 21 Oct 2026 01:53:32 +0800 Subject: [PATCH 13/27] fix(dexsim): adapt latest 0.5.1 runtime contracts --- .../topics/sensor-system/contact-history.md | 8 +- .../topics/sensor-system/lifecycle.md | 7 +- .../topics/simulation-system/lifecycle.md | 5 +- .../scene_engine/pipeline/utils/scene_usd.py | 6 +- embodichain/lab/sim/physics/newton.py | 23 +++- embodichain/lab/sim/sensors/_warp/contact.py | 21 +++- .../lab/sim/sensors/_warp/contact_history.py | 44 +++++--- .../lab/sim/sensors/contact_history.py | 18 +++- embodichain/lab/sim/sensors/contact_sensor.py | 61 ++++++++++- embodichain/lab/sim/sim_manager.py | 1 + .../rl/experimental/newton/planar_reach.py | 4 +- pyproject.toml | 3 +- tests/gen_sim/scene_engine/test_scene_usd.py | 20 ++++ tests/sim/sensors/test_contact_history.py | 31 ++++++ tests/sim/sensors/test_contact_newton_e2e.py | 38 +++++-- .../sim/sensors/test_contact_query_sensor.py | 100 ++++++++++++++++++ .../sim/sensors/test_contact_sensor_graph.py | 30 ++++++ tests/sim/test_backend_parity.py | 58 +++++++++- tests/sim/test_sim_manager.py | 14 ++- 19 files changed, 449 insertions(+), 43 deletions(-) diff --git a/agent_context/topics/sensor-system/contact-history.md b/agent_context/topics/sensor-system/contact-history.md index 91049ba8d6..733e79cfe6 100644 --- a/agent_context/topics/sensor-system/contact-history.md +++ b/agent_context/topics/sensor-system/contact-history.md @@ -16,7 +16,13 @@ process valid contact rows, using precomputed sorted environment/actor indices; no environment-by-contact-by-body tensor is allocated. Per-body force, contact flags and timing buffers persist between samples. `current_air_time` measures an unfinished flight; `last_air_time` retains its completed duration on landing. -Selective reset clears only the selected rows. +Selective reset clears only the selected rows. Force reduction uses only +components marked available by the sample's normal/friction validity flags. +Geometry-only rows can count as contact when no force threshold is requested; +they do not create measured force or satisfy a positive force threshold. +Native friction is the tangential impulse on actor 0, while the normal points +toward actor 1: actor-0 force is `(-normal * impulse + friction) / dt` and +actor 1 receives the opposite force. `dropped_contacts` reports query loss plus separately counted scatter overflow across the control interval. Query counts stay on the device until this diagnostic diff --git a/agent_context/topics/sensor-system/lifecycle.md b/agent_context/topics/sensor-system/lifecycle.md index 8a83ede7d3..746b8b2869 100644 --- a/agent_context/topics/sensor-system/lifecycle.md +++ b/agent_context/topics/sensor-system/lifecycle.md @@ -35,7 +35,12 @@ Spawn handles and creates one query. Preserve these query/buffer contracts: - Only validity/counts reset each update. Data outside valid rows is undefined. `contact_capabilities` distinguishes geometry from normal/friction impulse -support. Do not infer force support from sensor construction alone. Inspect +support for the whole query. Current DexSim DexUni reports may mix geometry-only +and measured-force rows; `impulse_valid` and `friction_valid` preserve their +per-row quantity availability through sensor scatter, filtering and reset. +Unavailable values are placeholders, not measured zeros. Older buffers without +per-row flags use the query capabilities; DexUni queries require the newer +validity contract. Do not infer force support from sensor construction alone. Inspect `supports_contact_sensor` in `embodichain/lab/sim/physics/` for the current backend/solver/device matrix: Newton AutoSolver may change the answer during preparation, so the manager checks again afterwards. diff --git a/agent_context/topics/simulation-system/lifecycle.md b/agent_context/topics/simulation-system/lifecycle.md index 7031d6320e..d3b0de0c5d 100644 --- a/agent_context/topics/simulation-system/lifecycle.md +++ b/agent_context/topics/simulation-system/lifecycle.md @@ -56,8 +56,9 @@ rows before episode initialization events run. Manager reset restores all selected Newton articulations before one complete Scene batch clears drive targets, dynamics, warm-start state, kinematics and -external wrenches. MuJoCo-Warp cannot safely clear a selected world's solver -state with only some of its articulations; excluding a subset is rejected. +external wrenches. MuJoCo-Warp and DexUni cannot safely clear a selected world's +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. `destroy(exit_process=False)` queues native cleanup. Flush with diff --git a/embodichain/gen_sim/scene_engine/pipeline/utils/scene_usd.py b/embodichain/gen_sim/scene_engine/pipeline/utils/scene_usd.py index 5c9f20bf66..7f2888e7d2 100644 --- a/embodichain/gen_sim/scene_engine/pipeline/utils/scene_usd.py +++ b/embodichain/gen_sim/scene_engine/pipeline/utils/scene_usd.py @@ -950,7 +950,11 @@ def _export_material_without_texture( material.CreateSurfaceOutput().ConnectToSource(shader.ConnectableAPI(), "surface") _set_safe_material_input( source_material=source_material, - getter_name="get_base_color", + getter_name=( + "get_base_color_linear" + if callable(getattr(source_material, "get_base_color_linear", None)) + else "get_base_color" + ), shader=shader, input_name="diffuseColor", value_type=Sdf.ValueTypeNames.Color3f, diff --git a/embodichain/lab/sim/physics/newton.py b/embodichain/lab/sim/physics/newton.py index cbb37d11bf..fdbe269d7d 100644 --- a/embodichain/lab/sim/physics/newton.py +++ b/embodichain/lab/sim/physics/newton.py @@ -209,7 +209,28 @@ def supports_contact_sensor(self) -> bool: return True solver_type = solver_type.lower() if solver_type.endswith("dexuni"): - return False + from dexsim.scene import ContactBuffer + + # Current DexSim exposes mixed DexUni/MuJoCo contact reports with + # per-row quantity validity. Older wheels lack that contract. + if not hasattr(ContactBuffer, "impulse_valid"): + return False + world = getattr(self._manager, "_world", None) + if world is not None: + from dexsim.engine.newton_physics.backend_registry import get_newton_backend + + backend = get_newton_backend(world) + mujoco_solver = getattr(backend, "mujoco_solver", None) + if mujoco_solver is not None and mujoco_solver.use_mujoco_cpu: + features = getattr(getattr(backend, "solver", None), "features", None) + # One-way DexUni disables MuJoCo contacts and keeps the VBD + # geometry-only source. Other CPU MuJoCo paths cannot report + # contacts, even when the Newton model itself lives on CUDA. + return bool( + solver_type.endswith("dexuni") + and getattr(features, "backend", None) == "coupled" + and not getattr(features, "two_way_coupling_enabled", False) + ) runtime_device = self._runtime_device if runtime_device is None: runtime_device = str(getattr(self._manager, "device", "")) diff --git a/embodichain/lab/sim/sensors/_warp/contact.py b/embodichain/lab/sim/sensors/_warp/contact.py index f5924f3f21..8ff5de50ee 100644 --- a/embodichain/lab/sim/sensors/_warp/contact.py +++ b/embodichain/lab/sim/sensors/_warp/contact.py @@ -41,7 +41,7 @@ def _scatter_contact_row( ): env_id = env_ids[i] if env_id < 0 or env_id >= num_contacts_per_env.shape[0]: - return + return wp.vec2i(-1, -1) # Atomically increment contact counter for this environment contact_idx = wp.atomic_add(num_contacts_per_env, env_id, 1) @@ -51,7 +51,7 @@ def _scatter_contact_row( wp.atomic_add(dropped, env_id, 1) # Decrement counter since we didn't write this contact wp.atomic_sub(num_contacts_per_env, env_id, 1) - return + return wp.vec2i(-1, -1) # Extract contact data columns x = contact_data[i, 0] @@ -86,6 +86,7 @@ def _scatter_contact_row( user_ids_out[env_id, contact_idx, 1] = user_ids[i, 1] is_valid[env_id, contact_idx] = True + return wp.vec2i(env_id, contact_idx) @wp.kernel(enable_backward=False) @@ -141,11 +142,17 @@ def scatter_contact_rows( is_valid: wp.array(dtype=wp.bool, ndim=2), counts: wp.array(dtype=wp.int32), dropped: wp.array(dtype=wp.int32), + quantity_valid: wp.array(dtype=wp.bool, ndim=2), + has_quantity_valid: bool, + impulse_available: bool, + friction_available: bool, + impulse_valid: wp.array(dtype=wp.bool, ndim=2), + friction_valid: wp.array(dtype=wp.bool, ndim=2), ): """Scatter valid device-counted rows and record per-environment overflow.""" i = wp.tid() if i < counts[0]: - _scatter_contact_row( + row = _scatter_contact_row( i, contact_data, user_ids, @@ -162,3 +169,11 @@ def scatter_contact_rows( True, dropped, ) + if row[0] >= 0: + normal_valid = impulse_available + tangent_valid = friction_available + if has_quantity_valid: + normal_valid = quantity_valid[i, 0] + tangent_valid = quantity_valid[i, 1] + impulse_valid[row[0], row[1]] = normal_valid + friction_valid[row[0], row[1]] = tangent_valid diff --git a/embodichain/lab/sim/sensors/_warp/contact_history.py b/embodichain/lab/sim/sensors/_warp/contact_history.py index d788468d3b..9617b79add 100644 --- a/embodichain/lab/sim/sensors/_warp/contact_history.py +++ b/embodichain/lab/sim/sensors/_warp/contact_history.py @@ -84,6 +84,10 @@ def reduce_contact_rows( actors: wp.array(dtype=wp.int32, ndim=2), env_ids: wp.array(dtype=wp.int32), counts: wp.array(dtype=wp.int32), + quantity_valid: wp.array(dtype=wp.bool, ndim=2), + has_quantity_valid: bool, + impulse_available: bool, + friction_available: bool, keys: wp.array(dtype=wp.int64), rows: wp.array(dtype=wp.int32), counterparts: wp.array(dtype=wp.int64), @@ -101,14 +105,19 @@ def reduce_contact_rows( env = env_ids[i] if env < 0 or env >= hits.shape[0] or hits.shape[1] == 0: return - force = ( - wp.vec3( - data[i, 3] * data[i, 9] + data[i, 6], - data[i, 4] * data[i, 9] + data[i, 7], - data[i, 5] * data[i, 9] + data[i, 8], - ) - / dt - ) + normal_valid = impulse_available + tangent_valid = friction_available + if has_quantity_valid: + normal_valid = quantity_valid[i, 0] + tangent_valid = quantity_valid[i, 1] + # reduce_pair negates this vector for actor 0. Native friction already + # describes the impulse on actor 0, so subtract it here. + force = wp.vec3(0.0) + if normal_valid: + force = wp.vec3(data[i, 3], data[i, 4], data[i, 5]) * data[i, 9] + if tangent_valid: + force = force - wp.vec3(data[i, 6], data[i, 7], data[i, 8]) + force = force / dt reduce_pair( env, actors[i, 0], @@ -134,6 +143,8 @@ def reduce_contact_batch( normal: wp.array(dtype=wp.float32, ndim=3), friction: wp.array(dtype=wp.float32, ndim=3), impulse: wp.array(dtype=wp.float32, ndim=2), + impulse_valid: wp.array(dtype=wp.bool, ndim=2), + friction_valid: wp.array(dtype=wp.bool, ndim=2), keys: wp.array(dtype=wp.int64), rows: wp.array(dtype=wp.int32), counterparts: wp.array(dtype=wp.int64), @@ -148,14 +159,17 @@ def reduce_contact_batch( env, i = wp.tid() if not valid[env, i] or hits.shape[1] == 0: return - force = ( - wp.vec3( - normal[env, i, 0] * impulse[env, i] + friction[env, i, 0], - normal[env, i, 1] * impulse[env, i] + friction[env, i, 1], - normal[env, i, 2] * impulse[env, i] + friction[env, i, 2], + force = wp.vec3(0.0) + if impulse_valid[env, i]: + force = ( + wp.vec3(normal[env, i, 0], normal[env, i, 1], normal[env, i, 2]) + * impulse[env, i] ) - / dt - ) + if friction_valid[env, i]: + force = force - wp.vec3( + friction[env, i, 0], friction[env, i, 1], friction[env, i, 2] + ) + force = force / dt reduce_pair( env, actors[env, i, 0], diff --git a/embodichain/lab/sim/sensors/contact_history.py b/embodichain/lab/sim/sensors/contact_history.py index 259b5c6670..5700ae484e 100644 --- a/embodichain/lab/sim/sensors/contact_history.py +++ b/embodichain/lab/sim/sensors/contact_history.py @@ -160,9 +160,17 @@ def _finish_sample(self, dt: float) -> None: ], ) - def _update_from_query(self, buffer: ContactBuffer, dt: float) -> None: + def _update_from_query( + self, + buffer: ContactBuffer, + dt: float, + *, + impulse_available: bool = True, + friction_available: bool = True, + ) -> None: """Reduce compact query rows using its device-resident valid-row count.""" args = self._start_sample(dt) + quantity_valid = getattr(buffer, "quantity_valid", None) self._launch( reduce_contact_rows, buffer.capacity, @@ -171,6 +179,12 @@ def _update_from_query(self, buffer: ContactBuffer, dt: float) -> None: wp.from_torch(buffer.actor_ids), wp.from_torch(buffer.env_ids), wp.from_torch(buffer.count_device), + wp.from_torch( + quantity_valid if quantity_valid is not None else self.contact + ), + quantity_valid is not None, + impulse_available, + friction_available, *args, ], ) @@ -200,6 +214,8 @@ def update(self, data: Mapping[str, torch.Tensor], dt: float) -> None: wp.from_torch(data[name]) for name in ("is_valid", "normal", "friction", "impulse") ], + wp.from_torch(data.get("impulse_valid", data["is_valid"])), + wp.from_torch(data.get("friction_valid", data["is_valid"])), *args, ], ) diff --git a/embodichain/lab/sim/sensors/contact_sensor.py b/embodichain/lab/sim/sensors/contact_sensor.py index 08ce036eab..10764c9cfd 100644 --- a/embodichain/lab/sim/sensors/contact_sensor.py +++ b/embodichain/lab/sim/sensors/contact_sensor.py @@ -122,6 +122,8 @@ class ContactSensor(BaseSensor): "distance", "user_ids", "is_valid", + "impulse_valid", + "friction_valid", ] def __init__( @@ -287,6 +289,16 @@ def _build_sensor_from_config( dtype=torch.bool, device=device, ), + "impulse_valid": torch.zeros( + (num_envs, config.max_contacts_per_env), + dtype=torch.bool, + device=device, + ), + "friction_valid": torch.zeros( + (num_envs, config.max_contacts_per_env), + dtype=torch.bool, + device=device, + ), }, batch_size=[num_envs, config.max_contacts_per_env], device=device, @@ -357,8 +369,11 @@ def _read_contacts(self) -> ContactBuffer: def _scatter_contacts(self, buffer: ContactBuffer) -> None: assert self._num_contacts_per_env is not None self._num_contacts_per_env.zero_() - self._data_buffer["is_valid"].zero_() + for name in ("is_valid", "impulse_valid", "friction_valid"): + self._data_buffer[name].zero_() self._sample_scatter_dropped_count.zero_() + quantity_valid = getattr(buffer, "quantity_valid", None) + capabilities = self.contact_capabilities wp.launch( kernel=scatter_contact_rows, dim=buffer.capacity, @@ -382,6 +397,16 @@ def _scatter_contacts(self, buffer: ContactBuffer) -> None: ], wp.from_torch(buffer.count_device), wp.from_torch(self._sample_scatter_dropped_count), + wp.from_torch( + quantity_valid + if quantity_valid is not None + else self._data_buffer["is_valid"] + ), + quantity_valid is not None, + capabilities.impulse, + capabilities.friction, + wp.from_torch(self._data_buffer["impulse_valid"]), + wp.from_torch(self._data_buffer["friction_valid"]), ], device=str(self.device), stream=current_warp_stream(self.device), @@ -439,11 +464,17 @@ def get_data(self) -> TensorDict: Returns: Batched contact data. ``position`` is in the Arena frame; - ``normal`` points from actor 0 toward actor 1; ``friction`` and - ``impulse`` are impulses; ``distance`` is signed separation; + ``normal`` points from actor 0 toward actor 1; ``friction`` is + tangential impulse on actor 0; ``impulse`` preserves the native + scalar magnitude (total for Default CPU, normal otherwise); + ``distance`` is signed separation; ``user_ids`` contains backend-neutral contact actor IDs; and ``is_valid`` marks rows populated by the latest update. Values in invalid rows are unspecified and may come from an earlier update. + ``impulse_valid`` and ``friction_valid`` mark measured quantities + per row; unavailable values are placeholders, not measured zeros. + These flags may be true even when the aggregate capability is false + for a mixed DexUni report. """ return self._data_buffer @@ -546,6 +577,9 @@ def update_physics_step(self, dt: float) -> None: return key = ( id(buffer), + id(getattr(buffer, "quantity_valid", None)), + self.contact_capabilities.impulse, + self.contact_capabilities.friction, dt, self.cfg.max_contacts_per_env, tuple( @@ -589,7 +623,12 @@ def _sample_contacts(self, buffer: ContactBuffer, dt: float) -> None: self._query_dropped_count.add_(buffer.dropped_count_device) self._scatter_dropped_count.add_(self._sample_scatter_dropped_count) for history in self._histories.values(): - history._update_from_query(buffer, dt) + history._update_from_query( + buffer, + dt, + impulse_available=self.contact_capabilities.impulse, + friction_available=self.contact_capabilities.friction, + ) def reset(self, env_ids: Sequence[int] | None = None) -> None: """Clear contact samples and histories only for the selected rows. @@ -606,6 +645,8 @@ def reset(self, env_ids: Sequence[int] | None = None) -> None: ) for value in ( self._data_buffer["is_valid"], + self._data_buffer["impulse_valid"], + self._data_buffer["friction_valid"], self._num_contacts_per_env, self._scatter_dropped_count, self._sample_scatter_dropped_count, @@ -647,6 +688,12 @@ def filter_by_user_ids( "distance": self._data_buffer["distance"][env_ids_tensor].flatten(0, 1), "user_ids": self._data_buffer["user_ids"][env_ids_tensor].flatten(0, 1), "is_valid": self._data_buffer["is_valid"][env_ids_tensor].flatten(0, 1), + "impulse_valid": self._data_buffer["impulse_valid"][env_ids_tensor].flatten( + 0, 1 + ), + "friction_valid": self._data_buffer["friction_valid"][ + env_ids_tensor + ].flatten(0, 1), } # Create valid mask (only slots up to _num_contacts_per_env are valid) @@ -685,6 +732,12 @@ def filter_by_user_ids( (0, 2), dtype=torch.int32, device=self.device ), "is_valid": torch.empty((0,), dtype=torch.bool, device=self.device), + "impulse_valid": torch.empty( + (0,), dtype=torch.bool, device=self.device + ), + "friction_valid": torch.empty( + (0,), dtype=torch.bool, device=self.device + ), }, batch_size=[0], device=self.device, diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index 2e0fcbb207..d6dcdc9bf0 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -4873,6 +4873,7 @@ def reset_objects_state( None, "auto", "mujoco_warp", + "dexuni", } if ( use_coordinated_newton_clear diff --git a/embodichain/learning/rl/experimental/newton/planar_reach.py b/embodichain/learning/rl/experimental/newton/planar_reach.py index 41c3e5adf9..81d3f2f04c 100644 --- a/embodichain/learning/rl/experimental/newton/planar_reach.py +++ b/embodichain/learning/rl/experimental/newton/planar_reach.py @@ -351,7 +351,7 @@ def close(self) -> None: """No-op; Warp/Newton resources are released with the Python object.""" def _build_model(self) -> tuple[Any, wp.array]: - template = newton.ModelBuilder(gravity=0.0, up_axis=newton.Axis.Y) + template = newton.ModelBuilder(gravity=(0.0, 0.0, 0.0), up_axis=newton.Axis.Y) first_link = template.add_link(label="planar_first_link") template.add_shape_sphere(first_link, radius=0.01) second_link = template.add_link(label="planar_end_link") @@ -388,7 +388,7 @@ def _build_model(self) -> tuple[Any, wp.array]: ) template.add_articulation([first_joint, second_joint]) - builder = newton.ModelBuilder(gravity=0.0, up_axis=newton.Axis.Y) + builder = newton.ModelBuilder(gravity=(0.0, 0.0, 0.0), up_axis=newton.Axis.Y) builder.replicate( template, world_count=self.num_envs, diff --git a/pyproject.toml b/pyproject.toml index 20930f944e..966af111e9 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -25,7 +25,8 @@ dynamic = ["version"] # Core install dependencies (kept from requirements.txt). dependencies = [ - "dexsim_engine==0.5.1rc1", + # Accept audited 0.5.1 source/final builds alongside the published RC wheel. + "dexsim_engine>=0.5.1rc1,<0.5.2", "shapely>=2.0", "setuptools>=78.1.1", "gymnasium>=0.29.1", diff --git a/tests/gen_sim/scene_engine/test_scene_usd.py b/tests/gen_sim/scene_engine/test_scene_usd.py index 4bfc291f9a..68e28d32b6 100644 --- a/tests/gen_sim/scene_engine/test_scene_usd.py +++ b/tests/gen_sim/scene_engine/test_scene_usd.py @@ -186,3 +186,23 @@ def test_usd_source_texture_retains_native_orientation(tmp_path: Path) -> None: reader = UsdShade.Shader(texture.GetInput("st").GetConnectedSource()[0].GetPrim()) assert reader.GetIdAttr().Get() == "UsdPrimvarReader_float2" assert reader.GetInput("varname").Get() == "st" + + +def test_scalar_material_export_prefers_scene_linear_base_color(): + stage = Usd.Stage.CreateInMemory() + native = _native_material() + native.get_base_color = lambda: (0.5, 0.6, 0.7) + linear_color = (0.21404114, 0.31854678, 0.44798842) + native.get_base_color_linear = lambda: linear_color + material = _export_material_without_texture(native, stage, Sdf.Path("/World")) + shader = material.ComputeSurfaceSource()[0] + np.testing.assert_allclose(shader.GetInput("diffuseColor").Get(), linear_color) + + +def test_scalar_material_export_keeps_legacy_base_color_getter(): + stage = Usd.Stage.CreateInMemory() + native = _native_material() + native.get_base_color = lambda: (0.1, 0.2, 0.3) + material = _export_material_without_texture(native, stage, Sdf.Path("/World")) + shader = material.ComputeSurfaceSource()[0] + np.testing.assert_allclose(shader.GetInput("diffuseColor").Get(), (0.1, 0.2, 0.3)) diff --git a/tests/sim/sensors/test_contact_history.py b/tests/sim/sensors/test_contact_history.py index 1e25471f15..221c8e3f7a 100644 --- a/tests/sim/sensors/test_contact_history.py +++ b/tests/sim/sensors/test_contact_history.py @@ -124,6 +124,8 @@ def test_sparse_rows_use_device_count_and_environment_qualified_actor_ids(device counterpart_ids=torch.tensor([[0], [0]], device=device), ) buffer = ContactBuffer.allocate(6, device) + if hasattr(buffer, "quantity_valid"): + buffer.quantity_valid.fill_(True) buffer.data.fill_(float("nan")) buffer.data[:4] = torch.tensor( [ @@ -146,3 +148,32 @@ def test_sparse_rows_use_device_count_and_environment_qualified_actor_ids(device ) assert history.contact.tolist() == [[True, False], [False, True]] assert history.contact_count.tolist() == [1.0, 1.0] + + +@pytest.mark.parametrize("threshold,found", [(0.0, True), (1.0, False)]) +def test_geometry_only_rows_do_not_create_force_or_force_threshold_events( + threshold, found +): + history = ContactHistory(torch.tensor([[10]]), force_threshold=threshold) + data = sample([[[0, 10]]]) + data["impulse_valid"] = torch.zeros((1, 1), dtype=torch.bool) + data["friction_valid"] = torch.zeros((1, 1), dtype=torch.bool) + data["impulse"].fill_(float("nan")) + data["friction"].fill_(float("nan")) + history.update(data, 0.1) + assert not history.force.any() + assert history.found.item() == found + + +@pytest.mark.parametrize( + "pair,expected", [((10, 0), (5.0, 0.0, -20.0)), ((0, 10), (-5.0, 0.0, 20.0))] +) +def test_force_preserves_friction_impulse_on_actor_zero( + pair: tuple[int, int], expected: tuple[float, float, float] +) -> None: + history = ContactHistory(torch.tensor([[10]])) + data = sample([[pair]]) + data["impulse"][0, 0] = 2.0 + data["friction"][0, 0] = torch.tensor([0.5, 0.0, 0.0]) + history.update(data, 0.1) + torch.testing.assert_close(history.force[0, 0], torch.tensor(expected)) diff --git a/tests/sim/sensors/test_contact_newton_e2e.py b/tests/sim/sensors/test_contact_newton_e2e.py index 94b1816d33..41069f3eee 100644 --- a/tests/sim/sensors/test_contact_newton_e2e.py +++ b/tests/sim/sensors/test_contact_newton_e2e.py @@ -33,7 +33,12 @@ pytestmark = [pytest.mark.requires_sim, pytest.mark.gpu] -def test_newton_contact_sensor_reports_each_arena() -> None: +@pytest.mark.parametrize("solver_type", ["mujoco_warp", "dexuni"]) +def test_newton_contact_sensor_reports_each_arena(solver_type: str) -> None: + from dexsim.scene import ContactBuffer + + if solver_type == "dexuni" and not hasattr(ContactBuffer, "impulse_valid"): + pytest.skip("DexUni contacts require DexSim's current dev API.") wp.init() if not wp.is_cuda_available(): pytest.skip("CUDA is required for the Newton contact E2E contract.") @@ -50,8 +55,12 @@ def test_newton_contact_sensor_reports_each_arena() -> None: # Exercise a multi-point manifold so query-side reductions # cannot satisfy this regression contract accidentally. solver_cfg={ - "solver_type": "mujoco_warp", - "enable_multiccd": True, + "solver_type": solver_type, + **( + {"enable_multiccd": True} + if solver_type == "mujoco_warp" + else {} + ), }, ), ) @@ -86,12 +95,29 @@ def test_newton_contact_sensor_reports_each_arena() -> None: sensor.update() assert sensor.contact_capabilities.geometry - assert sensor.contact_capabilities.impulse counts = sensor._num_contacts_per_env.cpu().tolist() - assert counts == [4, 4] data = sensor.get_data() assert data["is_valid"].sum(dim=1).cpu().tolist() == counts - assert (data["impulse"][data["is_valid"]] > 1.0e-7).all() + if solver_type == "mujoco_warp": + assert sensor.contact_capabilities.impulse + assert counts == [4, 4] + assert data["impulse_valid"][data["is_valid"]].all() + assert data["friction_valid"][data["is_valid"]].all() + else: + assert all(count > 0 for count in counts) + # DexUni includes geometry-only VBD rows. Its aggregate capability + # does not describe every row in a combined VBD/MuJoCo report. + assert not sensor.contact_capabilities.impulse + raw = sensor._query.fetch() + for env_id in range(sim.num_envs): + selected = raw.env_ids[: raw.count] == env_id + for name in ("impulse_valid", "friction_valid"): + assert ( + data[name][env_id].sum() + == getattr(raw, name)[: raw.count][selected].sum() + ) + measured = data["is_valid"] & data["impulse_valid"] + assert (data["impulse"][measured] > 1.0e-7).all() actor_ids = data["user_ids"][data["is_valid"]] assert all( sensor.get_actor_info(actor_id).path.endswith(("/ground", "/cube")) diff --git a/tests/sim/sensors/test_contact_query_sensor.py b/tests/sim/sensors/test_contact_query_sensor.py index 3072117ca2..7cf6a39cb5 100644 --- a/tests/sim/sensors/test_contact_query_sensor.py +++ b/tests/sim/sensors/test_contact_query_sensor.py @@ -18,6 +18,7 @@ from types import SimpleNamespace +import pytest import torch import warp as wp @@ -37,6 +38,8 @@ def __init__(self) -> None: self.actor_info_calls = 0 self.buffer = ContactBuffer.allocate(4, "cpu") self.buffer.count = 2 + if hasattr(self.buffer, "quantity_valid"): + self.buffer.quantity_valid.fill_(True) self.buffer.data[0] = torch.tensor( [1.0, 2.0, 3.0, 0.0, 0.0, 1.0, 0.1, 0.2, 0.3, 0.4, -0.01] ) @@ -195,3 +198,100 @@ def test_scatter_overflow_is_counted_separately_and_reset_by_selected_row(): assert sensor.dropped_contacts == 3 sensor.reset() assert sensor.dropped_contacts == 0 + + +@pytest.mark.parametrize( + "device", ["cpu", pytest.param("cuda:0", marks=pytest.mark.gpu)] +) +def test_mixed_contact_quantities_survive_scatter_filter_and_history(device): + query = _FakeQuery() + if not hasattr(query.buffer, "quantity_valid"): + pytest.skip("Per-row contact quantities require DexSim's current dev API.") + query.capabilities = ContactQueryCapabilities(True, False, False) + query.buffer.quantity_valid[:2] = torch.tensor([[True, False], [False, True]]) + # Poison unsupported quantities; neither the observation mask nor the + # history reducer may promote them to measured forces. + query.buffer.data[0, 6:9] = float("nan") + query.buffer.data[1, 9] = float("nan") + query.buffer.data[1, 6:9] = torch.tensor([0.0, 0.5, 0.0]) + if device != "cpu": + old = query.buffer + query.buffer = ContactBuffer.allocate(old.capacity, device) + for name in ("data", "actor_ids", "env_ids", "quantity_valid"): + getattr(query.buffer, name).copy_(getattr(old, name)) + query.buffer.count = old.count + query.buffer.device_counts[0] = old.count + query.fetch_async = query.fetch + handles = ( + SimpleNamespace(path="arena_0/cube"), + SimpleNamespace(path="arena_1/cube"), + ) + owner = SimpleNamespace( + num_envs=2, + arena_offsets=torch.zeros((2, 3), device=device), + _spawn_scene=SimpleNamespace(handles=lambda uid: handles), + spawn_result=SimpleNamespace( + create_contact_query=lambda *args, **kwargs: query + ), + ) + sensor = ContactSensor( + ContactSensorCfg( + uid="contacts", + rigid_uid_list=["cube"], + filter_need_both_actor=False, + max_contacts_per_env=2, + ), + torch.device(device), + owner=owner, + ) + history = sensor.create_history( + "forces", torch.tensor([[4], [7]], device=device), force_threshold=1.0 + ) + sensor.update_physics_step(0.1) + data = sensor.get_data() + assert data["impulse_valid"].cpu().tolist() == [[True, False], [False, False]] + assert data["friction_valid"].cpu().tolist() == [[False, False], [True, False]] + torch.testing.assert_close( + history.force.cpu(), torch.tensor([[[0.0, 0.0, -4.0]], [[0.0, -5.0, 0.0]]]) + ) + assert history.contact.all() + filtered = sensor.filter_by_user_ids(torch.tensor([4])) + assert filtered["impulse_valid"].tolist() == [True] + assert filtered["friction_valid"].tolist() == [False] + sensor.reset([0]) + assert not data["impulse_valid"].any() + assert data["friction_valid"][1, 0] + query.buffer.count = 0 + query.buffer.device_counts.zero_() + sensor.update() + assert not data["is_valid"].any() + assert not data["friction_valid"].any() + + +def test_legacy_contact_buffer_uses_query_quantity_capabilities(): + query = _FakeQuery() + old = query.buffer + query.buffer = SimpleNamespace( + data=old.data, + actor_ids=old.actor_ids, + env_ids=old.env_ids, + device_counts=old.device_counts, + count_device=old.count_device, + dropped_count_device=old.dropped_count_device, + capacity=old.capacity, + count=old.count, + dropped_count=old.dropped_count, + ) + query.capabilities = ContactQueryCapabilities(True, False, False) + query.buffer.data[:2, 6:10] = float("nan") + sensor = _sensor_for_query(query) + history = sensor.create_history( + "forces", torch.tensor([[4], [7]]), force_threshold=1.0 + ) + sensor.update_physics_step(0.1) + data = sensor.get_data() + assert data["is_valid"][:, 0].all() + assert not data["impulse_valid"].any() + assert not data["friction_valid"].any() + assert not history.force.any() + assert not history.found.any() diff --git a/tests/sim/sensors/test_contact_sensor_graph.py b/tests/sim/sensors/test_contact_sensor_graph.py index 466377c719..c198bea310 100644 --- a/tests/sim/sensors/test_contact_sensor_graph.py +++ b/tests/sim/sensors/test_contact_sensor_graph.py @@ -37,6 +37,8 @@ def __init__(self, device: torch.device) -> None: ContactActorInfo(20, "arena_1/cube", None, "arena_1", 1), ) self.buffer = ContactBuffer.allocate(4, str(device)) + if hasattr(self.buffer, "quantity_valid"): + self.buffer.quantity_valid.fill_(True) self.buffer.data.zero_() self.buffer.data[:2, 5] = 1.0 self.buffer.data[:2, 9] = 0.2 @@ -158,4 +160,32 @@ def assert_air_time(value: float) -> None: assert sensor._sample_graph is recaptured assert not history.contact.any() assert_air_time(0.3) + + if hasattr(query.buffer, "quantity_valid"): + # Replay reads changed masks without recapture and never evaluates + # unavailable poisoned force components. + query.buffer.quantity_valid.zero_() + query.buffer.data[:2, 6:10] = float("nan") + sensor.update_physics_step(0.1) + assert sensor._sample_graph is recaptured + assert not history.force.any() + assert not sensor.get_data()["impulse_valid"].any() + assert not sensor.get_data()["friction_valid"].any() + + # A topology refresh can replace the native mask storage. The + # captured pointer must be refreshed before the next sample. + query.buffer.quantity_valid = torch.ones_like(query.buffer.quantity_valid) + query.buffer.data[:2, 6:9] = 0.0 + query.buffer.data[:2, 9] = 0.4 + sensor.update_physics_step(0.1) + assert sensor._sample_graph is None + assert history.contact.all() + torch.testing.assert_close( + history.force, + torch.tensor([[[0.0, 0.0, 4.0]], [[0.0, 0.0, 4.0]]], device=device), + ) + sensor.update_physics_step(0.1) + assert sensor._sample_graph is not None + assert sensor._sample_graph is not recaptured + assert history.contact.all() caller.synchronize() diff --git a/tests/sim/test_backend_parity.py b/tests/sim/test_backend_parity.py index c5fabe1ce2..fb1fa8cf5c 100644 --- a/tests/sim/test_backend_parity.py +++ b/tests/sim/test_backend_parity.py @@ -123,7 +123,7 @@ def test_contact_sensor_guard_uses_backend_capability() -> None: [ ("mujoco_warp", "cuda:0", True), ("mujoco_warp", "cpu", False), - ("dexuni", "cuda:0", False), + ("dexuni", "cuda:0", True), ("xpbd", "cpu", True), ], ) @@ -132,6 +132,10 @@ def test_newton_contact_sensor_capability_depends_on_runtime( runtime_device: str, expected: bool, ) -> None: + from dexsim.scene import ContactBuffer + + if solver_type == "dexuni" and not hasattr(ContactBuffer, "impulse_valid"): + expected = False # Published wheels predate DexUni ContactQuery. backend = NewtonPhysicsBackend(SimpleNamespace()) backend._configured_solver_type = solver_type backend._runtime_device = runtime_device @@ -259,3 +263,55 @@ def test_matrix_covers_all_backends() -> None: if __name__ == "__main__": pytest.main([__file__, "-v"]) + + +def test_dexuni_contacts_remain_unavailable_without_quantity_validity(monkeypatch): + from dexsim.scene import ContactBuffer + + monkeypatch.delattr(ContactBuffer, "impulse_valid", raising=False) + backend = NewtonPhysicsBackend(SimpleNamespace()) + backend._configured_solver_type = "dexuni" + backend._runtime_device = "cuda:0" + assert not backend.supports_contact_sensor + + +@pytest.mark.parametrize( + "solver_type,solver_backend,two_way,expected", + [ + ("mujoco_warp", "pure_mujoco", False, False), + ("dexuni", "pure_mujoco", False, False), + ("dexuni", "coupled", True, False), + ("dexuni", "coupled", False, True), + ], +) +def test_cpu_mujoco_contact_capability_uses_resolved_solver( + monkeypatch, + solver_type, + solver_backend, + two_way, + expected, +): + from dexsim.scene import ContactBuffer + from dexsim.engine.newton_physics import backend_registry + + if solver_type == "dexuni" and not hasattr(ContactBuffer, "impulse_valid"): + pytest.skip("DexUni contacts require the per-row quantity contract.") + world = object() + native = SimpleNamespace( + solver_type=solver_type, + mujoco_solver=SimpleNamespace(use_mujoco_cpu=True), + solver=SimpleNamespace( + features=SimpleNamespace( + backend=solver_backend, + two_way_coupling_enabled=two_way, + ) + ), + ) + monkeypatch.setattr( + backend_registry, + "get_newton_backend", + lambda value: native if value is world else None, + ) + backend = NewtonPhysicsBackend(SimpleNamespace(_world=world)) + backend._runtime_device = "cuda:0" + assert backend.supports_contact_sensor is expected diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index 4b655fcd20..dae72a37e1 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -2757,7 +2757,10 @@ def test_reset_objects_state_includes_deformable_assets() -> None: sim._deformable_objects["cloth"].reset.assert_called_once_with([1]) -def test_newton_reset_clears_all_articulations_in_selected_worlds_together() -> None: +@pytest.mark.parametrize("solver_type", ["mujoco_warp", "dexuni"]) +def test_newton_reset_clears_all_articulations_in_selected_worlds_together( + solver_type, +) -> None: sim = object.__new__(SimulationManager) robot = MagicMock() articulation = MagicMock() @@ -2768,7 +2771,7 @@ 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="mujoco_warp") + sim.physics = SimpleNamespace(name="newton", solver_type=solver_type) sim.num_envs = 2 sim._spawn_scene = SimpleNamespace( builder=SimpleNamespace(is_finalized=True, result=spawn_result), @@ -2790,11 +2793,14 @@ def test_newton_reset_clears_all_articulations_in_selected_worlds_together() -> native_batch.clear_dynamics.assert_called_once_with() -def test_newton_reset_rejects_excluding_one_articulation_from_selected_worlds() -> None: +@pytest.mark.parametrize("solver_type", ["mujoco_warp", "dexuni"]) +def test_newton_reset_rejects_excluding_one_articulation_from_selected_worlds( + solver_type, +) -> None: sim = object.__new__(SimulationManager) robot = MagicMock() articulation = MagicMock() - sim.physics = SimpleNamespace(name="newton", solver_type="mujoco_warp") + sim.physics = SimpleNamespace(name="newton", solver_type=solver_type) sim._robots = {"robot": robot} sim._articulations = {"arm": articulation} sim._rigid_objects = {} From 57d02625375ad2a8a15e6b64e5d2c7f7fc5a1fe4 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 27 Oct 2026 17:53:26 +0800 Subject: [PATCH 14/27] fix(dexsim): follow current DexUni dispatch and device defaults --- .../topics/atomic-actions/atomic-actions.md | 31 ++++--------- .../topics/sensor-system/lifecycle.md | 2 +- .../topics/sensor-system/sensor-system.md | 2 +- .../topics/simulation-system/deformables.md | 2 +- .../physics-configuration.md | 9 ++-- embodichain/lab/sim/cfg/physics.py | 20 ++++----- embodichain/lab/sim/cfg/robot.py | 15 +++---- embodichain/lab/sim/physics/newton.py | 2 +- embodichain/lab/sim/sim_manager.py | 8 ++-- examples/sim/atomic_action/__main__.py | 4 +- examples/sim/atomic_action/defaults.py | 2 +- tests/examples/atomic_action/test_run.py | 36 +++++++++++++++ tests/sim/sensors/test_contact_newton_e2e.py | 44 ++++++++----------- tests/sim/test_cfg.py | 15 +++++-- tests/sim/test_sim_manager_cfg.py | 36 ++++++++------- tests/sim/test_startup_summary.py | 10 ++--- 16 files changed, 131 insertions(+), 107 deletions(-) diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index c16560bd3f..3574f6bdd6 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -113,28 +113,15 @@ Author contact dimensions and material settings before `prepare()`, never by rewriting captured solver arrays. The current values live in `examples/sim/atomic_action/support.py` and the selected physical components. -For UR5 with `dexuni`, the paper-cup `move_held_object.py` tutorial opts into -VBD contact history with `latest` collision matching, sliding friction 4 on the -gripper alone, and a 0.0115 m candidate close target. Transport orientation -is not yet repeatably aligned. The native contact alpha and the cup -and ground materials are unchanged. `create_tutorial_simulation` accepts -`newton_solver_options`, and `add_tutorial_robot` accepts -`newton_gripper_friction` and optional `arm_damping`; these are authored before -model construction. This -calibration also applies to the UR5-only `pour.py` tutorial with its 0.013 m -cube close target. The UR5 plastic-tray `coordinated_pickment.py` tutorial uses -the same contact settings with a 0.023 m close target; the dual-arm helper -`add_dual_tutorial_robot` also accepts `newton_gripper_friction`. -The current Handover fixture uses Robotiq 2F-140 and retains its fixture-owned -angular close targets and full TCP poses in `lab/scripts/_atomic_action/handover.py`. -The UR5 DexUni `place.py` profile uses a 0.013 m close target, gripper friction 4, -contact history with `latest` matching, contact alpha 0.5, and arm damping 1000. -The lower arm damping reduces tracking lag when the fingers open; finger -drives and authored release waypoints remain unchanged. Complete cached -replays preserve the grasp to release, but landing orientation still varies; -this is a scoped improvement, not a claim of full solver equivalence. -These explicit scene choices do not change world defaults -or establish a generic object-width-to-gripper-target mapping. +Scene-specific DexUni contact options are authored through +`create_tutorial_simulation(newton_solver_options=...)` before model construction. +`add_tutorial_robot` owns optional `newton_gripper_friction` and `arm_damping` +overrides; `scenario.add_dual_tutorial_robot` owns the dual-arm friction override. +Scene modules retain calibrated close targets, fixture TCPs and grippers, including +Handover's Robotiq 2F-140 fixture. Read those example owners for current values; +calibration does not establish generic physical success or solver equivalence. +The launcher preserves an omitted device through simulation configuration so the +selected backend owns its default; explicit device selections retain priority. ## Semantic integration boundary diff --git a/agent_context/topics/sensor-system/lifecycle.md b/agent_context/topics/sensor-system/lifecycle.md index 1edbb1ca84..2adbb7b887 100644 --- a/agent_context/topics/sensor-system/lifecycle.md +++ b/agent_context/topics/sensor-system/lifecycle.md @@ -42,7 +42,7 @@ Unavailable values are placeholders, not measured zeros. Older buffers without per-row flags use the query capabilities; DexUni queries require the newer validity contract. Do not infer force support from sensor construction alone. Inspect `supports_contact_sensor` in `embodichain/lab/sim/physics/` for the current -backend/solver/device matrix: Newton AutoSolver may change the answer during +backend/solver/device matrix: DexUni's scene-based dispatch may change the answer during preparation, so the manager checks again afterwards. Particle-shape contacts do not enter this rigid ContactQuery. diff --git a/agent_context/topics/sensor-system/sensor-system.md b/agent_context/topics/sensor-system/sensor-system.md index 32cb1c62da..b48e8a0d63 100644 --- a/agent_context/topics/sensor-system/sensor-system.md +++ b/agent_context/topics/sensor-system/sensor-system.md @@ -25,7 +25,7 @@ Create sensors through `SimulationManager.add_sensor()`. The explicit manager owns the World, ordered Arenas, preparation and semantic parent resolution; sensors must not rediscover a singleton manager. Cameras are render features on both physics backends. Contact availability is a backend capability checked -before preparation and again after Newton AutoSolver selection. +before preparation and again after DexUni's scene-based dispatch. Robot-mounted cameras are declared in `RobotCfg.sensors` with robot-local parent links. `SimulationManager.add_robot()` initializes them only when the diff --git a/agent_context/topics/simulation-system/deformables.md b/agent_context/topics/simulation-system/deformables.md index 910328670e..42af801ce4 100644 --- a/agent_context/topics/simulation-system/deformables.md +++ b/agent_context/topics/simulation-system/deformables.md @@ -12,7 +12,7 @@ binding. Return to the [simulation overview](simulation-system.md). uses one registry and backend dispatch for both topologies. The supported path is Newton on CUDA, without gradient mode. Declarations -accept unresolved AutoSolver or a supported particle solver (`xpbd`, +accept a supported particle solver (`xpbd`, `semi_implicit`, `vbd`, `dexuni`); they reject unsupported backends/solvers and post-finalization additions. Default soft/cloth compatibility is absent. Future backend support belongs at dispatch, not inside a monolithic config. diff --git a/agent_context/topics/simulation-system/physics-configuration.md b/agent_context/topics/simulation-system/physics-configuration.md index 0b88927fbc..2ae60a56af 100644 --- a/agent_context/topics/simulation-system/physics-configuration.md +++ b/agent_context/topics/simulation-system/physics-configuration.md @@ -21,10 +21,11 @@ device, not a separate environment tensor device. `None` retains the native default. Gym accepts the former in `physics_config` and the latter at the top level through `config_to_cfg()`. -Newton `solver_cfg=None` preserves DexSim's scene-aware `AutoSolverCfg`. -The concrete solver is resolved from the complete Spawn scene, then exposed -through `NewtonPhysicsBackend.solver_type`. Do not guess a solver-specific robot -preset while it is unresolved. Gradient mode requires an explicit +Newton `solver_cfg=None` preserves DexSim's `DexUniSolverCfg` default. +DexUni owns scene-based partitioning between MuJoCo, VBD and supported particle +paths; `NewtonPhysicsBackend.solver_type` remains `dexuni`. Legacy `auto` and +`AutoSolverCfg` mappings select DexUni. Robot presets select `newton_dexuni` +before the MuJoCo aliases and portable fallbacks. Gradient mode requires an explicit `semi_implicit` solver. Read exact solver options/defaults in `cfg/physics.py` and the installed DexSim config types; do not substitute a hardcoded fallback when an expected DexSim API is absent. diff --git a/embodichain/lab/sim/cfg/physics.py b/embodichain/lab/sim/cfg/physics.py index 5f3d45a812..0cd8af38e4 100644 --- a/embodichain/lab/sim/cfg/physics.py +++ b/embodichain/lab/sim/cfg/physics.py @@ -347,8 +347,8 @@ class NewtonPhysicsCfg(PhysicsBackendCfg): A mapping is converted to the matching DexSim Newton solver config. Include ``solver_type`` or ``class_type`` to select the solver, then add any parameters accepted by that DexSim solver config. If omitted, EmbodiChain - preserves DexSim's scene-aware ``AutoSolverCfg`` default. A DexSim build - exporting ``AutoSolverCfg`` is required; no concrete-solver fallback is used. + preserves DexSim's ``DexUniSolverCfg`` default and its scene-based dispatch. + Legacy ``auto``/``AutoSolverCfg`` mappings select DexUni as well. For ``solver_type='dexuni'``, the current DexSim contract keeps expert options under ``vbd_options``, ``mujoco_options`` and ``collision_options``; use ``joint_mode`` and ``contact_mode`` for the @@ -382,7 +382,6 @@ def to_dexsim_cfg( ) -> NewtonCfg: """Convert this config to ``dexsim.engine.newton_physics.NewtonCfg``.""" from dexsim.engine.newton_physics import ( - AutoSolverCfg, DexUniSolverCfg, FeatherstoneSolverCfg, MJWarpSolverCfg, @@ -403,7 +402,6 @@ def to_dexsim_cfg( ) solver_cfg_map: dict[str, type] = { - "auto": AutoSolverCfg, "dexuni": DexUniSolverCfg, "mujoco_warp": MJWarpSolverCfg, "xpbd": XPBDSolverCfg, @@ -421,7 +419,7 @@ def to_dexsim_cfg( ): logger.log_error( "Newton gradient mode requires an explicit " - "solver_type='semi_implicit'; AutoSolver does not select a " + "solver_type='semi_implicit'; the default does not select a " "differentiable solver." ) @@ -469,11 +467,11 @@ def _normalize_newton_solver_type(solver_type: str) -> str: """Normalize public EmbodiChain and DexSim Newton solver aliases.""" key = solver_type.replace("-", "_").lower() aliases = { - "auto": "auto", - "autosolver": "auto", - "autosolvercfg": "auto", - "auto_solver": "auto", - "auto_solver_cfg": "auto", + "auto": "dexuni", + "autosolver": "dexuni", + "autosolvercfg": "dexuni", + "auto_solver": "dexuni", + "auto_solver_cfg": "dexuni", "mjwarp": "mujoco_warp", "mjwarpsolver": "mujoco_warp", "mjwarpsolvercfg": "mujoco_warp", @@ -531,7 +529,7 @@ def _newton_solver_cfg_to_dexsim( configured_solver_type = ( solver_cfg_data.pop("solver_type", None) or solver_cfg_data.pop("class_type", None) - or "auto" + or "dexuni" ) normalized_solver_type = _normalize_newton_solver_type(str(configured_solver_type)) solver_cfg_type = solver_cfg_map[normalized_solver_type] diff --git a/embodichain/lab/sim/cfg/robot.py b/embodichain/lab/sim/cfg/robot.py index c95d0e4ffe..05fd178b25 100644 --- a/embodichain/lab/sim/cfg/robot.py +++ b/embodichain/lab/sim/cfg/robot.py @@ -387,23 +387,22 @@ def resolve( if solver_type is None: solver_cfg = physics_cfg.solver_cfg if solver_cfg is None: - solver_type = "auto" + solver_type = "dexuni" elif isinstance(solver_cfg, Mapping): solver_type = str( solver_cfg.get("solver_type") or solver_cfg.get("class_type") - or "auto" + or "dexuni" ) else: solver_type = str(getattr(solver_cfg, "solver_type")) solver_type = _normalize_newton_solver_type(solver_type) solver_candidates = [] - if solver_type != "auto": - solver_candidates.append(f"newton_{solver_type}") - if solver_type == "dexuni": - solver_candidates.extend(("newton_mujoco_warp", "newton_mjwarp")) - if solver_type == "mujoco_warp": - solver_candidates.append("newton_mjwarp") + solver_candidates.append(f"newton_{solver_type}") + if solver_type == "dexuni": + solver_candidates.extend(("newton_mujoco_warp", "newton_mjwarp")) + if solver_type == "mujoco_warp": + solver_candidates.append("newton_mjwarp") candidates = (*solver_candidates, "newton", "default") for candidate in candidates: diff --git a/embodichain/lab/sim/physics/newton.py b/embodichain/lab/sim/physics/newton.py index 1df7e74608..ac1f8544e7 100644 --- a/embodichain/lab/sim/physics/newton.py +++ b/embodichain/lab/sim/physics/newton.py @@ -202,7 +202,7 @@ def supports_rigid_object_group(self) -> bool: def supports_contact_sensor(self) -> bool: # ContactSensor consumes the backend-neutral Scene ContactQuery API, # but CPU MuJoCo cannot publish its force report. Keep this runtime- - # sensitive so ``auto`` and DexUni's scene-selected solver path are + # sensitive so DexUni's scene-selected solver path is # rechecked after Spawn finalization. solver_type = self.solver_type if solver_type is None: diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index 277249c160..8ee91a51cb 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -624,9 +624,9 @@ def _initialize_simulation( self._requested_renderer = sim_config.render_cfg.renderer solver_cfg = getattr(sim_config.physics_cfg, "solver_cfg", None) self._requested_solver = str( - solver_cfg.get("solver_type", solver_cfg.get("class_type", "auto")) + solver_cfg.get("solver_type", solver_cfg.get("class_type", "dexuni")) if isinstance(solver_cfg, Mapping) - else getattr(solver_cfg, "solver_type", "auto") + else getattr(solver_cfg, "solver_type", "dexuni") ) # Cache paths self._sim_cache_dir = SIM_CACHE_DIR @@ -4014,8 +4014,8 @@ def add_sensor(self, sensor_cfg: SensorCfg) -> BaseSensor: sensor_factory, ContactSensor ): self.prepare() - # ``solver_type='auto'`` is resolved while preparing the Spawn - # scene, so the early capability check above is not sufficient. + # DexUni resolves its contact path while preparing the Spawn scene, + # so the early capability check above is not sufficient. if not self.physics.supports_contact_sensor: raise NotImplementedError( f"ContactSensor is not supported by the {self.physics.name} " diff --git a/examples/sim/atomic_action/__main__.py b/examples/sim/atomic_action/__main__.py index b927bf2d7f..0735863d9d 100644 --- a/examples/sim/atomic_action/__main__.py +++ b/examples/sim/atomic_action/__main__.py @@ -155,8 +155,8 @@ def _resolve_sample(args: argparse.Namespace) -> AtomicSkillRunCfg: not math.isfinite(cfg.arena_space) or cfg.arena_space <= 0 ): raise ValueError("--arena-space must be finite and positive") - if not cfg.device: - raise ValueError("--device must be a nonempty device string") + if cfg.device is not None and not cfg.device: + raise ValueError("--device must be a nonempty device string when specified") if not 0 <= cfg.seed < 2**32: raise ValueError("--seed must be in [0, 2**32)") if args.output_dir is not None: diff --git a/examples/sim/atomic_action/defaults.py b/examples/sim/atomic_action/defaults.py index 8f55981f9d..53a93f0144 100644 --- a/examples/sim/atomic_action/defaults.py +++ b/examples/sim/atomic_action/defaults.py @@ -76,7 +76,7 @@ class AtomicSkillRunCfg: variants: int = 1 expansion: str = "both" seed: int = 0 - device: str = "cpu" + device: str | None = None renderer: str = "auto" headless: bool = False arena_space: float | None = None diff --git a/tests/examples/atomic_action/test_run.py b/tests/examples/atomic_action/test_run.py index 5ba062d1fa..d32c2c9aa5 100644 --- a/tests/examples/atomic_action/test_run.py +++ b/tests/examples/atomic_action/test_run.py @@ -509,6 +509,41 @@ def test_every_sample_defaults_to_one_reference_environment(skill: str) -> None: assert cfg.variants == 1 assert not cfg.affordance_enabled and not cfg.trajectory_enabled assert _scene_args(cfg).num_envs == 1 + assert cfg.device is None + assert _scene_args(cfg).device is None + + +@pytest.mark.parametrize( + "physics, expected_device", [("default", "cpu"), ("newton", "cuda:0")] +) +def test_sample_omitted_device_preserves_backend_default( + monkeypatch: pytest.MonkeyPatch, physics: str, expected_device: str +) -> None: + from examples.sim.atomic_action import support + + captured = {} + + def create_sim(cfg): + captured["cfg"] = cfg + return SimpleNamespace(add_light=lambda cfg: None) + + monkeypatch.setattr(support, "SimulationManager", create_sim) + args = launcher._create_parser().parse_args( + ["--skill", "pickup", "--physics", physics] + ) + cfg = launcher._resolve_sample(args) + support.create_tutorial_simulation(_scene_args(cfg)) + assert captured["cfg"].device == expected_device + + +@pytest.mark.parametrize("device", ["cpu", "cuda:2"]) +def test_sample_explicit_device_is_preserved(device: str) -> None: + args = launcher._create_parser().parse_args( + ["--skill", "pickup", "--physics", "newton", "--device", device] + ) + cfg = launcher._resolve_sample(args) + assert cfg.device == device + assert _scene_args(cfg).device == device @pytest.mark.parametrize("count", (2, 8)) @@ -538,6 +573,7 @@ def test_variant_count_is_the_parallel_environment_count(count: int, mode: str) (["--gpu-id", "-1"], "non-negative"), (["--seed", "-1"], "seed must"), (["--arena-space", "nan"], "finite"), + (["--device", ""], "device must"), (["--save-video", "--renderer", "no-render"], "requires a renderer"), ), ) diff --git a/tests/sim/sensors/test_contact_newton_e2e.py b/tests/sim/sensors/test_contact_newton_e2e.py index c6edfc00a5..6d611515f0 100644 --- a/tests/sim/sensors/test_contact_newton_e2e.py +++ b/tests/sim/sensors/test_contact_newton_e2e.py @@ -40,11 +40,13 @@ pytestmark = [pytest.mark.requires_sim, pytest.mark.gpu] -@pytest.mark.parametrize("solver_type", ["mujoco_warp", "dexuni"]) -def test_newton_contact_sensor_reports_each_arena(solver_type: str) -> None: +@pytest.mark.parametrize( + "solver_type", ["mujoco_warp", pytest.param(None, id="default_dexuni")] +) +def test_newton_contact_sensor_reports_each_arena(solver_type: str | None) -> None: from dexsim.scene import ContactBuffer - if solver_type == "dexuni" and not hasattr(ContactBuffer, "impulse_valid"): + if solver_type is None and not hasattr(ContactBuffer, "impulse_valid"): pytest.skip("DexUni contacts require DexSim's current dev API.") wp.init() if not wp.is_cuda_available(): @@ -61,14 +63,11 @@ def test_newton_contact_sensor_reports_each_arena(solver_type: str) -> None: use_cuda_graph=False, # Exercise a multi-point manifold so query-side reductions # cannot satisfy this regression contract accidentally. - solver_cfg={ - "solver_type": solver_type, - **( - {"enable_multiccd": True} - if solver_type == "mujoco_warp" - else {} - ), - }, + solver_cfg=( + {"solver_type": solver_type, "enable_multiccd": True} + if solver_type is not None + else None + ), ), ) ) @@ -102,27 +101,18 @@ def test_newton_contact_sensor_reports_each_arena(solver_type: str) -> None: sensor.update() assert sensor.contact_capabilities.geometry + # The default DexUni path assigns this all-rigid scene to MuJoCo. + assert sensor.contact_capabilities.impulse + assert sensor.contact_capabilities.friction counts = sensor._num_contacts_per_env.cpu().tolist() data = sensor.get_data() assert data["is_valid"].sum(dim=1).cpu().tolist() == counts if solver_type == "mujoco_warp": - assert sensor.contact_capabilities.impulse assert counts == [4, 4] - assert data["impulse_valid"][data["is_valid"]].all() - assert data["friction_valid"][data["is_valid"]].all() else: assert all(count > 0 for count in counts) - # DexUni includes geometry-only VBD rows. Its aggregate capability - # does not describe every row in a combined VBD/MuJoCo report. - assert not sensor.contact_capabilities.impulse - raw = sensor._query.fetch() - for env_id in range(sim.num_envs): - selected = raw.env_ids[: raw.count] == env_id - for name in ("impulse_valid", "friction_valid"): - assert ( - data[name][env_id].sum() - == getattr(raw, name)[: raw.count][selected].sum() - ) + assert data["impulse_valid"][data["is_valid"]].all() + assert data["friction_valid"][data["is_valid"]].all() measured = data["is_valid"] & data["impulse_valid"] assert (data["impulse"][measured] > 1.0e-7).all() actor_ids = data["user_ids"][data["is_valid"]] @@ -282,7 +272,9 @@ def test_dexuni_vbd_contact_sensor_exposes_geometry_only() -> None: device="cuda:0", num_substeps=1, use_cuda_graph=False, - solver_cfg={"solver_type": "dexuni"}, + # Explicit ownership keeps this a VBD geometry-only contract + # even though all-rigid scenes now default to pure MuJoCo. + solver_cfg={"solver_type": "dexuni", "mujoco_articulations": ()}, ), ) ) diff --git a/tests/sim/test_cfg.py b/tests/sim/test_cfg.py index ae3457ce54..712a40d426 100644 --- a/tests/sim/test_cfg.py +++ b/tests/sim/test_cfg.py @@ -596,6 +596,9 @@ def names(config_type: type) -> set[str]: "is_site", # Asset-loading adapter option, not a native ShapeConfig parameter. "use_native_mesh_loader", + "convert_to_heightfield", + "heightfield_resolution", + "preserve_mesh_transform", } assert ( newton_fields == names(NewtonCollisionDesc) - intentionally_unowned_shape_fields @@ -792,7 +795,7 @@ def test_robot_preset_accepts_newton_solver_alias() -> None: preset = _NewtonSolverAliasRobotPresetCfg() assert preset.resolve(DefaultPhysicsCfg()).uid == "fallback" - assert preset.resolve(NewtonPhysicsCfg()).uid == "fallback" + assert preset.resolve(NewtonPhysicsCfg()).uid == "mjwarp" assert ( preset.resolve(NewtonPhysicsCfg(solver_cfg={"solver_type": "mjwarp"})).uid == "mjwarp" @@ -1570,11 +1573,17 @@ class _DexUniRobotPresetCfg(RobotPresetCfg): (("newton_dexuni", "newton_mujoco_warp", "newton_mjwarp"), "portable"), ], ) -def test_dexuni_preset_priority_and_isolation(missing, expected) -> None: +@pytest.mark.parametrize( + "solver_mapping", + [None, {}, {"solver_type": "auto"}, {"solver_type": "dexuni"}], +) +def test_dexuni_preset_priority_and_isolation( + missing, expected, solver_mapping +) -> None: preset = _DexUniRobotPresetCfg() for field in missing: setattr(preset, field, None) - physics = NewtonPhysicsCfg(solver_cfg={"solver_type": "dexuni"}) + physics = NewtonPhysicsCfg(solver_cfg=solver_mapping) resolved = preset.resolve(physics) assert resolved.uid == expected resolved.control_parts["probe"] = ["joint"] diff --git a/tests/sim/test_sim_manager_cfg.py b/tests/sim/test_sim_manager_cfg.py index 993d0a791d..01d4da6cd7 100644 --- a/tests/sim/test_sim_manager_cfg.py +++ b/tests/sim/test_sim_manager_cfg.py @@ -124,15 +124,15 @@ def test_newton_physics_cfg_uses_device() -> None: @pytest.mark.no_sim -def test_newton_physics_cfg_preserves_dexsim_auto_solver_default() -> None: - from dexsim.engine.newton_physics import AutoSolverCfg +def test_newton_physics_cfg_preserves_dexsim_dexuni_default() -> None: + from dexsim.engine.newton_physics import DexUniSolverCfg cfg = NewtonPhysicsCfg() dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) - assert isinstance(dexsim_cfg.solver_cfg, AutoSolverCfg) - assert dexsim_cfg.solver_cfg.solver_type == "auto" + assert isinstance(dexsim_cfg.solver_cfg, DexUniSolverCfg) + assert dexsim_cfg.solver_cfg.solver_type == "dexuni" @pytest.mark.no_sim @@ -158,22 +158,22 @@ def test_newton_physics_cfg_forwards_render_sync_policy( @pytest.mark.no_sim -def test_newton_physics_cfg_requires_dexsim_auto_solver_api( +def test_newton_physics_cfg_requires_dexsim_dexuni_api( monkeypatch: pytest.MonkeyPatch, ) -> None: from dexsim.engine import newton_physics - monkeypatch.delattr(newton_physics, "AutoSolverCfg") + monkeypatch.delattr(newton_physics, "DexUniSolverCfg") with pytest.raises( ImportError, - match="cannot import name 'AutoSolverCfg'", + match="cannot import name 'DexUniSolverCfg'", ): NewtonPhysicsCfg().to_dexsim_cfg(gpu_id=0) @pytest.mark.no_sim -def test_newton_gradient_mode_rejects_auto_solver() -> None: +def test_newton_gradient_mode_rejects_default_solver() -> None: cfg = NewtonPhysicsCfg(solver_cfg=None, requires_grad=True) with pytest.raises(RuntimeError, match="explicit.*semi_implicit"): @@ -294,11 +294,11 @@ def test_newton_backend_exposes_resolved_solver_type() -> None: @pytest.mark.no_sim -def test_newton_backend_reports_scene_resolved_auto_solver( +def test_newton_backend_reports_runtime_solver( monkeypatch: pytest.MonkeyPatch, ) -> None: world = object() - native_backend = SimpleNamespace(solver_type="mujoco_warp") + native_backend = SimpleNamespace(solver_type="xpbd") manager = SimpleNamespace(_world=world) backend = NewtonPhysicsBackend(manager) world_config = SimpleNamespace(newton_cfg=None) @@ -310,8 +310,8 @@ def test_newton_backend_reports_scene_resolved_auto_solver( backend.configure_world(world_config, sim_config) - assert world_config.newton_cfg.solver_cfg.solver_type == "auto" - assert backend.solver_type == "mujoco_warp" + assert world_config.newton_cfg.solver_cfg.solver_type == "dexuni" + assert backend.solver_type == "xpbd" def test_newton_teardown_releases_render_views_on_the_resolved_device( @@ -452,14 +452,18 @@ def test_newton_physics_cfg_converts_mapping_solver_cfg_to_dexsim_cfg() -> None: @pytest.mark.no_sim -def test_newton_physics_cfg_accepts_explicit_auto_solver_mapping() -> None: - from dexsim.engine.newton_physics import AutoSolverCfg +@pytest.mark.parametrize( + "solver_mapping", + [{}, {"solver_type": "auto"}, {"class_type": "AutoSolverCfg"}], +) +def test_newton_physics_cfg_accepts_automatic_dexuni_mapping(solver_mapping) -> None: + from dexsim.engine.newton_physics import DexUniSolverCfg - cfg = NewtonPhysicsCfg(solver_cfg={"class_type": "AutoSolverCfg"}) + cfg = NewtonPhysicsCfg(solver_cfg=solver_mapping) dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) - assert isinstance(dexsim_cfg.solver_cfg, AutoSolverCfg) + assert isinstance(dexsim_cfg.solver_cfg, DexUniSolverCfg) @pytest.mark.no_sim diff --git a/tests/sim/test_startup_summary.py b/tests/sim/test_startup_summary.py index 3ef7d592d6..da760f00e6 100644 --- a/tests/sim/test_startup_summary.py +++ b/tests/sim/test_startup_summary.py @@ -41,13 +41,13 @@ def _sim(physics_cfg=None): "newton" if isinstance(cfg.physics_cfg, NewtonPhysicsCfg) else "default" ), solver_type=( - "mujoco_warp" if isinstance(cfg.physics_cfg, NewtonPhysicsCfg) else None + "dexuni" if isinstance(cfg.physics_cfg, NewtonPhysicsCfg) else None ), cuda_graph_status="pending", sync_render_state=lambda result: None, ), _requested_renderer="auto", - _requested_solver="auto", + _requested_solver="dexuni", is_window_opened=False, _robots={}, _articulations={}, @@ -100,18 +100,16 @@ def test_cpu_physics_does_not_imply_rendering_disabled(monkeypatch): assert "\x1b" not in text and "\x00" not in text -def test_newton_reports_pending_then_resolved_solver_and_graph(monkeypatch): +def test_newton_reports_dexuni_and_graph_readiness(monkeypatch): summary = importlib.import_module("embodichain.lab.sim._startup_summary") monkeypatch.setattr(summary.torch.cuda, "is_available", lambda: False) sim = _sim(NewtonPhysicsCfg()) - sim.physics.solver_type = "auto" before = str(summary.simulation_rows(sim)) assert "PENDING" in before sim.spawn_result = SimpleNamespace(topology_revision=1, needs_rebuild=False) sim._ready_spawn_topology_revision = 1 - sim.physics.solver_type = "mujoco_warp" after = str(summary.scene_rows(sim)) - assert "auto -> mujoco_warp" in after + assert "dexuni" in after assert "PENDING" in after and "CAPTURED" not in after sim.physics.cuda_graph_status = "captured" assert "CAPTURED" in str(summary.scene_rows(sim)) From 1eef39896e8bbcb1e092bfab553fbbb8812fc478 Mon Sep 17 00:00:00 2001 From: oem Date: Tue, 27 Oct 2026 03:00:31 -0700 Subject: [PATCH 15/27] feat(sim): expose DexSim controls and deformable initialization Expose pre-build Newton articulation and collision options, runtime-control registration, authored tetrahedral meshes and direct nodal-state writes. Support cloth rest angles and pneumatic cavities through native descriptors, and forward renderer synchronization and graph-capture settings. Keep native follower drives and mimic constraints by default; retain the Default-parity mimic stabilizer as an explicit opt-in. Include focused tests and the owning simulation context documentation. Validation: 718 CPU/context tests passed (2 GPU cases skipped in that run); 3 explicit-tetrahedra/nodal-state GPU tests and both W1 mimic modes passed. Black 26.3.1 (black .), API docs coverage (3269/3269), context checks and whitespace checks passed. Robot/visualization context ownership remains accurate; changed contracts are documented in simulation-system context. Technical-report experiments, output artifacts and the local integration report are intentionally excluded. --- .../articulation-adapters.md | 14 ++ .../topics/simulation-system/deformables.md | 20 +- .../topics/simulation-system/lifecycle.md | 1 + .../physics-configuration.md | 4 + embodichain/lab/sim/cfg/articulation.py | 93 ++++++++ embodichain/lab/sim/cfg/deformable.py | 46 +++- embodichain/lab/sim/cfg/physics.py | 45 ++++ embodichain/lab/sim/objects/articulation.py | 5 +- .../lab/sim/objects/deformable/base.py | 35 +++ embodichain/lab/sim/sim_manager.py | 65 ++++++ embodichain/lab/sim/spawn/descriptors.py | 219 ++++++++++++++++-- tests/sim/objects/test_robot.py | 65 ++++-- .../sim/objects/test_soft_object_tet_mesh.py | 115 +++++++++ tests/sim/spawn/test_descriptors.py | 157 +++++++++++++ tests/sim/test_cfg.py | 45 ++++ tests/sim/test_deformable_cfg.py | 180 ++++++++++++++ tests/sim/test_sim_manager.py | 62 +++++ 17 files changed, 1137 insertions(+), 34 deletions(-) create mode 100644 tests/sim/objects/test_soft_object_tet_mesh.py diff --git a/agent_context/topics/simulation-system/articulation-adapters.md b/agent_context/topics/simulation-system/articulation-adapters.md index 88bcdd95f6..7693599cad 100644 --- a/agent_context/topics/simulation-system/articulation-adapters.md +++ b/agent_context/topics/simulation-system/articulation-adapters.md @@ -25,6 +25,20 @@ Applying them only during facade binding is too late for captured GPU settings. both asset physics modes; omission preserves the source/default policy. It compensates articulation link weight without removing payload gravity or contact reactions. DexSim checks the resolved solver's MuJoCo support. +The other Newton creation options on `root_props` (`newton_kinematic`, +`newton_sleep_policy`, `newton_shape_collision[_links]`, +`newton_particle_collision_links`, `newton_mesh_approximation`, +`newton_force_sdf`) follow the same +rule: `None` keeps the DexSim default, explicit values require Newton. +The SDF request retains imported concave collision meshes. The compiler carries +it onto per-link entries after source configuration because those entries take +precedence over articulation-wide defaults, including attribute-only entries +inserted for native URDF geometry. Contact participation remains separate. +URDF mimic joints on Newton keep the configured follower drives and the +native MuJoCo equality by default, matching a plain DexSim scene; reset starts +followers on their mimic relation. `root_props.newton_mimic_stabilizer=True` +opts in to the Default-parity stabilizer (follower drive = 1 % of the leader, +mirrored follower targets, retuned equality `solref`). `joint_drive_props` is the sole joint-property entry point; sparse fields retain source values. `drive_type` describes the response and `target_mode` the active diff --git a/agent_context/topics/simulation-system/deformables.md b/agent_context/topics/simulation-system/deformables.md index 42af801ce4..05b0ec4db1 100644 --- a/agent_context/topics/simulation-system/deformables.md +++ b/agent_context/topics/simulation-system/deformables.md @@ -30,7 +30,12 @@ order; node selection for a runtime control is not a render vertex selection. For surfaces requiring stable authored node indices, array-backed `MeshCfg` preserves the supplied order; native file importers can reorder vertices. Volume voxelization creates tetrahedral particles whose indices do not match -the source mesh vertices. +the source mesh vertices. When stable volume node ids matter (material lines, +benchmarks), set `VolumeDeformableObjectCfg.tetrahedra` with an array-backed +`shape`: its vertices become the particles in order, meshing is skipped, and +`shape.triangles` is the optional boundary (derived from the tets if omitted). +`set_nodal_state()` writes world `[position, velocity]` nodes, the inverse of +`data.nodal_state_w`, without changing the rest shape. A surface's optional `visual_shape` can have independent indexing and UV seams; `visual_binding_mode` controls mapping from particles. Visual material/UVs @@ -46,12 +51,25 @@ ownership. Volume elasticity uses Young's modulus/Poisson's ratio, translated to Lamé coefficients at Spawn; density units differ (kg/m³ volume, kg/m² surface). Exact fields and defaults stay in the typed config source. +Surface cloth can author a constant signed rest dihedral angle through +`attrs.edge_rest_angle`; omitting it preserves the mesh-inferred rest geometry. +The surface descriptor compiler validates and forwards it to native cloth +physics. This is a rest-shape choice separate from bending stiffness and is not +a volume-surface material field. + `attrs.contact_material` optionally owns a DexUni particle object's contact stiffness, damping and dynamic friction; it is independent of elasticity and lowers to the native cloth/soft-body physics descriptor. Native validation and pair-material mixing remain DexSim-owned. Older wheels still support omitted materials and reject an explicit block if their descriptor API is unavailable. +Surface `attrs.pneumatic` optionally lowers a mapping to native +`PneumaticPhysicsDesc`. Closed-surface topology, pressure-law validation and +solver compatibility remain DexSim-owned. A closed cloth mesh alone has no +gas pressure; callers must author this block for an inflatable cavity. Omission +keeps ordinary cloth compatible with older wheels; explicit cavities fail +before declaration if the installed descriptor API cannot represent them. + ## Kinematic node controls Clear the Newton `ACTIVE` bit for all driven nodes in `particle_flags` before diff --git a/agent_context/topics/simulation-system/lifecycle.md b/agent_context/topics/simulation-system/lifecycle.md index 86cd3552d0..021c2c36c9 100644 --- a/agent_context/topics/simulation-system/lifecycle.md +++ b/agent_context/topics/simulation-system/lifecycle.md @@ -88,6 +88,7 @@ must not reach into `_spawn_scene.builder`. | `register_kinematic_nodal_trajectory()` | Declare deformable with selected nodes inactive; offsets `[env, samples, selected_nodes, 3]` are relative to finalized initial world positions | | `register_contact_material_schedule()` | Declared rigid object/robot/articulation; piecewise-constant shape friction/stiffness/damping tracks | | `register_particle_contact_material_schedule()` | Targeted DexUni deformable material via `uid`; omitted UID retains legacy scene-wide particle properties | +| `add_runtime_control()` | Any DexSim `RuntimeControl` without a dedicated method (for example `RealtimeJointTargetControl`); pass one scene-wide instance or a `factory(arena_name, env_index)`; targets use `f"{arena_name}/{uid}"`; returns the registered controls | All registrations reject finalized scenes. Joint, shape-material and targeted DexUni particle-material controls remain graph-compatible; nodal trajectories and legacy scene-wide particle-material diff --git a/agent_context/topics/simulation-system/physics-configuration.md b/agent_context/topics/simulation-system/physics-configuration.md index 2ae60a56af..62646316cc 100644 --- a/agent_context/topics/simulation-system/physics-configuration.md +++ b/agent_context/topics/simulation-system/physics-configuration.md @@ -47,6 +47,10 @@ cannot respond to contact reactions. Forward solver expert options through `NewtonPhysicsCfg.solver_cfg`; DexSim owns their validation and dispatch. Articulation gravity compensation and passive damping are separate creation settings; see [articulation adapters](articulation-adapters.md). +`async_sync`, `rigid_transform_sync_mode`, `capture_acquire_timeout` and +`capture_wait_log_interval` pass straight to DexSim `NewtonCfg`. Solver +diagnostics (graph status, effective VBD options, contact capacity) are read +from `sim.spawn_result.get_solver_diagnostics()` after `prepare()`. Capability checks remain explicit: Default owns native rigid constraints and Newton deformables are separate. Contact support is rechecked after Newton diff --git a/embodichain/lab/sim/cfg/articulation.py b/embodichain/lab/sim/cfg/articulation.py index c04838dd16..5020331f82 100644 --- a/embodichain/lab/sim/cfg/articulation.py +++ b/embodichain/lab/sim/cfg/articulation.py @@ -114,6 +114,75 @@ class ArticulationRootPropertiesCfg: Newton solver must support MuJoCo gravity compensation. """ + newton_kinematic: bool | None = None + """Whether every Newton link body is kinematic, on Newton only. + + ``True`` keeps authored mass and inertia but stops Newton from integrating + the links; prescribed motion then needs a kinematic controller such as + :meth:`SimulationManager.register_kinematic_joint_trajectory`. ``None`` + preserves the DexSim default (dynamic links). + """ + + newton_sleep_policy: Literal["auto", "never", "allowed", "init"] | None = None + """MuJoCo sleeping policy for every movable tree, on Newton only. + + ``None`` preserves the DexSim default (``"auto"``). + """ + + newton_shape_collision: bool | None = None + """Whether link collision shapes take part in rigid shape-shape contact. + + Particle contact is configured separately by + :attr:`newton_particle_collision_links`. ``None`` preserves the DexSim + default (enabled). + """ + + newton_shape_collision_links: Sequence[str] | None = None + """Exact link names allowed in rigid shape-shape contact, on Newton only. + + When set, this list takes precedence over :attr:`newton_shape_collision`. + ``None`` keeps every link eligible. + """ + + newton_particle_collision_links: Sequence[str] | None = None + """Exact link names allowed to collide with particles, on Newton only. + + Shapes on other links still render but ignore cloth and soft bodies. + ``None`` keeps every link eligible. + """ + + newton_mimic_stabilizer: bool = False + """Whether URDF mimic joints are tuned toward Default-backend behavior. + + ``False`` (default) keeps the configured follower drives and Newton's + native mimic constraint, as a plain DexSim scene does; reset starts each + follower on its mimic relation so the constraint does not kick on the + first step. On MuJoCo-Warp and + DexUni, ``True`` opts in to a Default-parity stabilizer: each mimic + follower's drive becomes one percent of its leader's gains (replacing the + follower gains in :attr:`ArticulationCfg.joint_drive_props`), follower + targets follow the mimic relation, and the MuJoCo equality ``solref`` is + retuned. + """ + + newton_mesh_approximation: Mapping[str, Any] | None = None + """Newton collision-mesh preprocessing applied before finalization. + + Keys follow DexSim ``NewtonMeshApproximationDesc``: ``method``, + ``link_names``, ``raise_on_failure``, ``keep_visual_shapes`` and + ``options``. ``None`` uses the source collision meshes unchanged. + """ + + newton_force_sdf: bool | None = None + """Keep imported collision meshes as concave Newton SDF boundaries. + + This pre-build request lowers to ``NewtonCollisionDesc.force_sdf`` on + the articulation source. It avoids converting imported collision meshes + to convex hulls. ``None`` preserves source/default behavior; primitives + retain their analytic geometry. It is independent of the rigid and + particle collision participation flags. + """ + def __post_init__(self) -> None: """Require the two values consumed by the atomic Default setter.""" if (self.min_position_iters is None) != (self.min_velocity_iters is None): @@ -121,6 +190,30 @@ def __post_init__(self) -> None: "Articulation-root min_position_iters and min_velocity_iters " "must be configured together." ) + for name in ( + "newton_shape_collision_links", + "newton_particle_collision_links", + ): + value = getattr(self, name) + if value is None: + continue + if isinstance(value, str): + raise TypeError(f"{name} must be a sequence of link names.") + setattr(self, name, tuple(str(item) for item in value)) + if self.newton_sleep_policy is not None and self.newton_sleep_policy not in ( + "auto", + "never", + "allowed", + "init", + ): + raise ValueError( + "newton_sleep_policy must be 'auto', 'never', 'allowed' or " + f"'init'; got {self.newton_sleep_policy!r}." + ) + if self.newton_mesh_approximation is not None and not isinstance( + self.newton_mesh_approximation, Mapping + ): + raise TypeError("newton_mesh_approximation must be a mapping.") @classmethod def from_dict( diff --git a/embodichain/lab/sim/cfg/deformable.py b/embodichain/lab/sim/cfg/deformable.py index a0248558c6..8d55ab898f 100644 --- a/embodichain/lab/sim/cfg/deformable.py +++ b/embodichain/lab/sim/cfg/deformable.py @@ -166,6 +166,30 @@ class SurfaceDeformablePhysicsCfg: backend defaults; explicit materials require a supporting DexSim build. """ + edge_rest_angle: float | None = None + """Constant signed rest dihedral angle [rad] for every bending edge. + + ``None`` preserves angles inferred from the authored mesh. Set ``0.0`` + for a garment that should relax toward a flat surface. This changes the + rest geometry independently of ``surface_props.edge_ke`` and requires + a DexSim build supporting the cloth ``edge_rest_angle`` parameter. + """ + + pneumatic: Mapping[str, Any] | None = None + """Optional DexUni closed-cavity pressure parameters for this surface. + + Keys follow DexSim ``PneumaticPhysicsDesc``: ``mode`` (``isothermal``, + ``adiabatic``, ``target_volume`` or ``prescribed_gauge_pressure``), + ``reference_absolute_pressure`` and ``ambient_pressure`` [Pa], + ``heat_capacity_ratio``, ``target_volume`` [m³], ``volume_stiffness``, + ``bulk_damping``, ``min_volume_ratio``, ``max_absolute_pressure`` [Pa], + and ``prescribed_gauge_pressure`` [Pa]. Native validation owns their + defaults and pressure-law constraints. The authored surface must be + closed; native model construction validates cavity topology and solver + compatibility. ``None`` preserves ordinary cloth and supports older + wheels; an explicit block requires a supporting DexSim build. + """ + def __post_init__(self) -> None: if isinstance(self.surface_props, Mapping): self.surface_props = SurfaceElementPropertiesCfg(**self.surface_props) @@ -247,7 +271,20 @@ class VolumeDeformableObjectCfg(DeformableObjectCfg): deformable_type: Literal["volume"] = "volume" meshing: VolumeDeformableMeshingCfg = VolumeDeformableMeshingCfg() - """Tetrahedral simulation-mesh voxelization attributes.""" + """Tetrahedral simulation-mesh voxelization attributes. + + Ignored when :attr:`tetrahedra` supplies an explicit simulation mesh. + """ + + tetrahedra: Sequence[Sequence[int]] | np.ndarray | None = None + """Explicit ``(M, 4)`` tetrahedra indexing ``shape.vertices``. + + When set, :attr:`shape` must be array-backed: its vertices become the + simulation particles in the same order, so particle indices equal vertex + indices, and :attr:`meshing` is not used. ``shape.triangles`` may list the + boundary surface over those vertex indices; leave it ``None`` to derive + the surface from the tetrahedra. ``None`` voxelizes :attr:`shape` instead. + """ attrs: VolumeDeformablePhysicsCfg = VolumeDeformablePhysicsCfg() """Newton volume-deformable physical attributes.""" @@ -258,6 +295,13 @@ def __post_init__(self) -> None: self.attrs = VolumeDeformablePhysicsCfg(**self.attrs) if isinstance(self.meshing, Mapping): self.meshing = VolumeDeformableMeshingCfg(**self.meshing) + if self.tetrahedra is not None: + raw = np.asarray(self.tetrahedra) + if raw.dtype.kind not in "iu": + raise TypeError("tetrahedra must contain integer indices.") + if raw.ndim != 2 or raw.shape[1:] != (4,) or len(raw) == 0: + raise ValueError("tetrahedra must have non-empty shape (M, 4).") + self.tetrahedra = raw.astype(np.int32) @configclass diff --git a/embodichain/lab/sim/cfg/physics.py b/embodichain/lab/sim/cfg/physics.py index 0cd8af38e4..f5afff584f 100644 --- a/embodichain/lab/sim/cfg/physics.py +++ b/embodichain/lab/sim/cfg/physics.py @@ -341,6 +341,34 @@ class NewtonPhysicsCfg(PhysicsBackendCfg): Genuine Newton/Warp warnings and errors are not suppressed. """ + async_sync: bool = False + """Whether Newton-to-render publication returns without waiting for rendering. + + ``True`` removes the render-thread wait from each published step, which + mostly matters while a window is open. ``False`` keeps the blocking + DexSim default. + """ + + rigid_transform_sync_mode: Literal["all", "dynamic"] = "all" + """Rigid transforms copied from Newton to the renderer after each step. + + ``"all"`` copies every mapped body. ``"dynamic"`` skips static and + kinematic bodies, which are already posed by their setters. + """ + + capture_acquire_timeout: float | None = None + """Seconds to wait for CUDA-graph capture rights shared with other users. + + ``None`` waits indefinitely. A finite value lets DexSim fall back to + uncaptured stepping when another component holds capture. + """ + + capture_wait_log_interval: float | None = 10.0 + """Seconds between progress logs while waiting for capture rights. + + ``None`` or a non-positive value disables the progress log. + """ + solver_cfg: Mapping[str, Any] | NewtonSolverCfg | None = None """Optional Newton solver configuration. @@ -375,6 +403,19 @@ def __post_init__(self) -> None: """Normalize dictionary collision settings at the config boundary.""" if isinstance(self.collision_cfg, Mapping): self.collision_cfg = NewtonCollisionPipelineCfg(**self.collision_cfg) + if self.rigid_transform_sync_mode not in ("all", "dynamic"): + raise ValueError( + "NewtonPhysicsCfg.rigid_transform_sync_mode must be 'all' or " + f"'dynamic'; got {self.rigid_transform_sync_mode!r}." + ) + if self.capture_acquire_timeout is not None and not ( + math.isfinite(self.capture_acquire_timeout) + and self.capture_acquire_timeout >= 0.0 + ): + raise ValueError( + "NewtonPhysicsCfg.capture_acquire_timeout must be None or a " + "finite non-negative number of seconds." + ) def to_dexsim_cfg( self, @@ -452,6 +493,10 @@ def to_dexsim_cfg( "suppress_warp_kernel_logs": self.suppress_warp_kernel_logs, "collision_pipeline_cfg": collision_pipeline_cfg, "sync_to_dexsim": self.sync_to_renderer, + "async_sync": self.async_sync, + "rigid_transform_sync_mode": self.rigid_transform_sync_mode, + "capture_acquire_timeout": self.capture_acquire_timeout, + "capture_wait_log_interval": self.capture_wait_log_interval, } if solver_cfg is not None: newton_cfg_args["solver_cfg"] = solver_cfg diff --git a/embodichain/lab/sim/objects/articulation.py b/embodichain/lab/sim/objects/articulation.py index 25ce5e30d7..4c0a75bc95 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -936,7 +936,10 @@ def _apply_spawn_config(self) -> None: spawn_result = getattr(self, "_spawn_result", None) self._prepare_spawn_runtime_config(spawn_result) - self._newton_mimic_compliance_configured = _configure_newton_mimic_compliance( + root_props = getattr(self.cfg, "root_props", None) + self._newton_mimic_compliance_configured = getattr( + root_props, "newton_mimic_stabilizer", False + ) and _configure_newton_mimic_compliance( result=spawn_result, entities=self._entities, state_joint_names=self._state_joint_names(), diff --git a/embodichain/lab/sim/objects/deformable/base.py b/embodichain/lab/sim/objects/deformable/base.py index 231baf3213..02a86cf07e 100644 --- a/embodichain/lab/sim/objects/deformable/base.py +++ b/embodichain/lab/sim/objects/deformable/base.py @@ -427,6 +427,41 @@ def set_local_pose( local_env_ids, ) + def set_nodal_state( + self, + nodal_state: torch.Tensor, + env_ids: Sequence[int] | None = None, + ) -> None: + """Write world-frame particle positions and velocities. + + This is the inverse of :attr:`DeformableObjectData.nodal_state_w`. + Use it to start from a deformed configuration while keeping the + authored rest shape, for example a pre-compressed block. + + Args: + nodal_state: Tensor of shape ``(len(env_ids), num_nodes, 6)`` + holding ``[position, velocity]`` per node in world frame. + env_ids: Environments to write. ``None`` selects all. + + Raises: + ValueError: If the shape does not match or values are not finite. + """ + local_env_ids = self._resolve_env_ids(env_ids) + self._require_data() + state = torch.as_tensor(nodal_state, dtype=torch.float32, device=self.device) + if state.dim() != 3 or state.shape[-1] != 6: + raise ValueError( + "nodal_state must have shape (len(env_ids), num_nodes, 6); got " + f"{tuple(state.shape)}." + ) + if not torch.isfinite(state).all(): + raise ValueError("nodal_state must contain only finite values.") + self._data._apply_nodal_state( + state[..., :3], + state[..., 3:], + local_env_ids, + ) + def _apply_local_pose( self, pose: torch.Tensor, diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index 8ee91a51cb..b61241f597 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -1741,6 +1741,71 @@ def register_kinematic_nodal_trajectory( for control in controls: scene.builder.add_runtime_control(control) + def add_runtime_control( + self, + control: Any | Callable[[str, int], Any], + ) -> tuple[Any, ...]: + """Register DexSim Newton runtime controls before :meth:`prepare`. + + Use this for controls that have no dedicated ``register_*`` method, + such as ``RealtimeJointTargetControl``, ``PinnedParticleControl`` or + ``IndexedConveyorControl`` from ``dexsim.engine.newton_physics``. + + Pass either one control instance, which is registered once for the + whole scene, or a factory called as ``factory(arena_name, env_index)`` + for every environment. Controls address entities by Spawn path, which + is ``f"{arena_name}/{uid}"`` for an object declared with ``uid``. + + Args: + control: A ``RuntimeControl`` instance, or a per-environment + factory returning one. + + Returns: + The registered controls, in environment order for a factory. + Keep them to drive live controls such as joint targets. + + Raises: + RuntimeError: If the backend is not Newton, the Spawn scene is + already finalized, or its arena count is inconsistent. + TypeError: If a control is not a ``RuntimeControl``. + """ + from dexsim.engine.newton_physics.runtime_control import RuntimeControl + + if not self.is_newton_backend: + raise RuntimeError("Runtime controls require the Newton backend.") + scene = self._spawn_scene + if scene.builder.is_finalized: + raise RuntimeError( + "Runtime controls must be registered before " + "SimulationManager.prepare()." + ) + if isinstance(control, RuntimeControl): + controls: tuple[Any, ...] = (control,) + elif callable(control): + arena_names = scene.arena_names + if len(arena_names) != self.num_envs: + raise RuntimeError( + "Spawn arena count does not match SimulationManager.num_envs: " + f"{len(arena_names)} != {self.num_envs}." + ) + controls = tuple( + control(arena_name, env_index) + for env_index, arena_name in enumerate(arena_names) + ) + else: + raise TypeError( + "control must be a RuntimeControl or a factory returning one." + ) + for item in controls: + if not isinstance(item, RuntimeControl): + raise TypeError( + "Runtime control factories must return RuntimeControl " + f"instances; got {type(item).__name__}." + ) + for item in controls: + scene.builder.add_runtime_control(item) + return controls + def prepare(self) -> None: """Materialize declarations, bind state, and restore camera parents.""" self._ready_spawn_topology_revision = -1 diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index d1c1d352f7..fd688144c9 100644 --- a/embodichain/lab/sim/spawn/descriptors.py +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -521,6 +521,74 @@ def _particle_contact_material_options( return {"contact_material": material_type(**dict(value))} +def _explicit_tet_mesh_from_cfg( + cfg: VolumeDeformableObjectCfg, uid: str +) -> dict[str, Any]: + """Validate an explicit tetrahedral mesh and build its render surface. + + Mirrors DexSim ``SceneBuilder.create_soft_mesh``: particles keep the + order of ``shape.vertices`` and the render mesh holds only surface nodes. + """ + from dexsim.spawn.soft_body_meshing import ( + SoftBodyMeshingStats, + extract_tet_surface_triangles, + ) + + shape = cfg.shape + if shape.vertices is None or ( + not _is_missing(shape.fpath) + and shape.fpath is not None + and bool(str(shape.fpath).strip()) + ): + raise ValueError( + "Volume deformable tetrahedra require an array-backed shape with " + "vertices and no fpath." + ) + positions = np.asarray(shape.vertices, dtype=np.float32) + if positions.ndim != 2 or positions.shape[1:] != (3,) or len(positions) == 0: + raise ValueError("Volume deformable vertices must have shape (N, 3).") + if not np.isfinite(positions).all(): + raise ValueError("Volume deformable vertices must be finite.") + tetrahedra = np.asarray(cfg.tetrahedra, dtype=np.int32).reshape(-1, 4) + if tetrahedra.min() < 0 or tetrahedra.max() >= len(positions): + raise ValueError("Volume deformable tetrahedra index outside the vertices.") + if shape.triangles is None: + surface_triangles = extract_tet_surface_triangles(tetrahedra) + else: + surface_triangles = np.asarray(shape.triangles) + if surface_triangles.dtype.kind not in "iu": + raise TypeError("Volume deformable triangles must be integer indices.") + surface_triangles = surface_triangles.astype(np.int32).reshape(-1, 3) + if surface_triangles.min() < 0 or surface_triangles.max() >= len(positions): + raise ValueError("Volume deformable triangles index outside the vertices.") + if shape.normals is not None or shape.uv_coords is not None: + raise ValueError( + "Volume deformable tetrahedra do not support shape normals or uv_coords." + ) + surface_ids = np.unique(surface_triangles) + render_geometry = GeometryDesc.mesh( + vertices=positions[surface_ids], + triangles=np.searchsorted(surface_ids, surface_triangles).astype(np.int32), + segment_name=uid, + ) + return { + "positions": positions, + "tetrahedra": tetrahedra, + "surface_triangles": surface_triangles, + "render_geometry": render_geometry, + "meshing_stats": SoftBodyMeshingStats( + source_kind="tet_mesh", + source_vertex_count=len(positions), + source_triangle_count=len(surface_triangles), + proxy_vertex_count=0, + proxy_triangle_count=0, + particle_count=len(positions), + tetrahedron_count=len(tetrahedra), + surface_triangle_count=len(surface_triangles), + ), + } + + def volume_deformable_desc_from_cfg( cfg: VolumeDeformableObjectCfg, *, @@ -528,7 +596,14 @@ def volume_deformable_desc_from_cfg( ) -> tuple[SoftBodyDesc, dict[str, MaterialDesc]]: """Translate a volume deformable into a Newton particle-set descriptor.""" uid = _required_uid(cfg.uid, "Volume deformable") - geometry = _mesh_geometry_from_cfg(cfg.shape, segment_name=uid) + explicit_tets = ( + None if cfg.tetrahedra is None else _explicit_tet_mesh_from_cfg(cfg, uid) + ) + geometry = ( + _mesh_geometry_from_cfg(cfg.shape, segment_name=uid) + if explicit_tets is None + else explicit_tets["render_geometry"] + ) material_ref, material_entry = _compile_visual_material( uid, cfg.shape.visual_material ) @@ -577,14 +652,28 @@ def volume_deformable_desc_from_cfg( for item in fields(surface_props) }, ), - meshing=SoftBodyMeshingDesc( - proxy_simplify_target=cfg.meshing.triangle_simplify_target, - proxy_remesh_resolution=cfg.meshing.triangle_remesh_resolution, - voxel_resolution=cfg.meshing.simulation_mesh_resolution, - voxel_num_relaxation_iters=cfg.meshing.voxel_num_relaxation_iters, - voxel_rel_min_tet_volume=cfg.meshing.voxel_rel_min_tet_volume, - voxel_surface_dist_ratio=cfg.meshing.voxel_surface_dist_ratio, - embedding_impl=cfg.meshing.embedding_impl, + meshing=( + None + if explicit_tets is not None + else SoftBodyMeshingDesc( + proxy_simplify_target=cfg.meshing.triangle_simplify_target, + proxy_remesh_resolution=cfg.meshing.triangle_remesh_resolution, + voxel_resolution=cfg.meshing.simulation_mesh_resolution, + voxel_num_relaxation_iters=cfg.meshing.voxel_num_relaxation_iters, + voxel_rel_min_tet_volume=cfg.meshing.voxel_rel_min_tet_volume, + voxel_surface_dist_ratio=cfg.meshing.voxel_surface_dist_ratio, + embedding_impl=cfg.meshing.embedding_impl, + ) + ), + **( + {} + if explicit_tets is None + else { + "positions": explicit_tets["positions"], + "tetrahedra": explicit_tets["tetrahedra"], + "surface_triangles": explicit_tets["surface_triangles"], + "meshing_stats": explicit_tets["meshing_stats"], + } ), particle_flags=_particle_flags_from_cfg(cfg.particle_flags), particle_radius=particle_radius, @@ -595,6 +684,27 @@ def volume_deformable_desc_from_cfg( return descriptor, materials +def _pneumatic_cloth_options( + values: Mapping[str, Any] | None, +) -> dict[str, Any]: + """Lower an optional cavity without importing unsupported wheel symbols.""" + if values is None: + return {} + if not isinstance(values, Mapping): + raise TypeError("Cloth pneumatic must be a mapping when configured.") + from dexsim import spawn + + native_type = getattr(spawn, "PneumaticPhysicsDesc", None) + if native_type is None or "pneumatic" not in { + item.name for item in fields(ClothPhysicsDesc) + }: + raise NotImplementedError( + "Cloth pneumatic requires a DexSim build with PneumaticPhysicsDesc " + "and ClothPhysicsDesc.pneumatic support." + ) + return {"pneumatic": native_type(**dict(values))} + + def surface_deformable_desc_from_cfg( cfg: SurfaceDeformableObjectCfg, *, @@ -623,6 +733,9 @@ def surface_deformable_desc_from_cfg( attrs = cfg.attrs surface_props = attrs.surface_props density = float(attrs.density) + rest_angle = None if attrs.edge_rest_angle is None else float(attrs.edge_rest_angle) + if rest_angle is not None and not math.isfinite(rest_angle): + raise ValueError("Cloth edge_rest_angle must be finite when set.") particle_radius = ( None if cfg.particle_radius is None else float(cfg.particle_radius) ) @@ -656,6 +769,8 @@ def surface_deformable_desc_from_cfg( visual_binding_mode=cfg.visual_binding_mode, physics=ClothPhysicsDesc( surface_density=density, + **({"edge_rest_angle": rest_angle} if rest_angle is not None else {}), + **_pneumatic_cloth_options(attrs.pneumatic), **_particle_contact_material_options(attrs.contact_material), add_springs=bool(attrs.add_springs), spring_ke=attrs.spring_ke, @@ -697,10 +812,7 @@ def articulation_desc_from_cfg( newton_solver_type=newton_solver_type, ) fixed_base, self_collision_enabled = _articulation_root_values(cfg) - compensation = _articulation_gravity_compensation(cfg, newton_solver_type) - newton_options = ( - {} if compensation is None else {"newton_gravity_compensation": compensation} - ) + newton_options = _articulation_newton_options(cfg, newton_solver_type) return ArticulationDesc( name=_articulation_uid(cfg.uid, str(path)), pose=_pose_from_cfg(cfg), @@ -741,6 +853,61 @@ def _articulation_gravity_compensation( return float(value) +_ARTICULATION_NEWTON_CREATION_FIELDS = ( + "newton_kinematic", + "newton_sleep_policy", + "newton_shape_collision", + "newton_shape_collision_links", + "newton_particle_collision_links", + "newton_mesh_approximation", + "newton_force_sdf", +) + + +def _articulation_newton_options( + cfg: ArticulationCfg, newton_solver_type: str | None +) -> dict[str, Any]: + """Collect explicit Newton articulation creation options from root props. + + Unset (``None``) fields are omitted so DexSim keeps its own defaults. + """ + options: dict[str, Any] = {} + compensation = _articulation_gravity_compensation(cfg, newton_solver_type) + if compensation is not None: + options["newton_gravity_compensation"] = compensation + root = cfg.root_props + for name in _ARTICULATION_NEWTON_CREATION_FIELDS: + value = getattr(root, name) + if value is None: + continue + if newton_solver_type is None: + raise ValueError(f"{name} requires the Newton backend.") + if name == "newton_force_sdf": + if not isinstance(value, (bool, np.bool_)): + raise TypeError("newton_force_sdf must be a boolean when configured.") + options["newton_collision"] = NewtonCollisionDesc( + **{ + item.name: bool(value) if item.name == "force_sdf" else None + for item in fields(NewtonCollisionDesc) + } + ) + continue + if name == "newton_sleep_policy": + from dexsim.spawn import NewtonSleepPolicy + + value = NewtonSleepPolicy(value) + elif name == "newton_mesh_approximation": + from dexsim.spawn import NewtonMeshApproximationDesc + + value = NewtonMeshApproximationDesc(**dict(value)) + elif name in ("newton_kinematic", "newton_shape_collision"): + value = bool(value) + else: + value = tuple(value) + options[name] = value + return options + + def _validate_articulation_rigid_physics( cfg: ArticulationCfg, *, @@ -837,9 +1004,8 @@ def configure_articulation_desc( f"Articulation source {desc.name!r} must be resolved before " "configuration." ) - compensation = _articulation_gravity_compensation(cfg, newton_solver_type) - if compensation is not None: - desc.newton_gravity_compensation = compensation + for name, value in _articulation_newton_options(cfg, newton_solver_type).items(): + setattr(desc, name, value) preserve_source_physics = cfg.resolve_asset_physics_mode() == "preserve" setattr(desc, "_embodichain_preserve_source_physics", preserve_source_physics) for link in desc.links: @@ -860,6 +1026,7 @@ def configure_articulation_desc( stacklevel=2, ) _apply_articulation_newton_gap_default(desc, newton_solver_type) + _apply_articulation_sdf_request(desc, cfg.root_props.newton_force_sdf) return desc default_physics = _resolve_rigid_physics( cfg.attrs, @@ -1010,9 +1177,29 @@ def configure_articulation_desc( "f32785317 or newer." ) _apply_articulation_newton_gap_default(desc, newton_solver_type) + _apply_articulation_sdf_request(desc, cfg.root_props.newton_force_sdf) return desc +def _apply_articulation_sdf_request( + desc: ArticulationDesc, force_sdf: bool | None +) -> None: + """Carry SDF intent onto resolved or newly inserted URDF attribute entries.""" + if force_sdf is None: + return + # Per-link entries override articulation-wide defaults in the native + # adapter. URDF resolution retains geometry in its native skeleton and + # inserts these attribute-only entries during configuration. + for link in desc.links: + for collision in link.collisions: + source = collision.newton + if source is None: + source = NewtonCollisionDesc( + **{item.name: None for item in fields(NewtonCollisionDesc)} + ) + collision.newton = replace(source, force_sdf=bool(force_sdf)) + + def _apply_newton_gap_default( collisions: Sequence[CollisionDesc], newton_solver_type: str | None, diff --git a/tests/sim/objects/test_robot.py b/tests/sim/objects/test_robot.py index 792a140359..a9d35f1c53 100644 --- a/tests/sim/objects/test_robot.py +++ b/tests/sim/objects/test_robot.py @@ -747,6 +747,8 @@ class TestRobotNewton: default/CUDA classes already cover the shared control-part/FK/IK logic. """ + MIMIC_STABILIZER = False + def setup_method(self): physics_cfg = physics_cfg_for_backend("newton") physics_cfg.solver_cfg = { @@ -760,6 +762,7 @@ def setup_method(self): self.sim = SimulationManager(config) cfg = DexforceW1Cfg.from_dict({"uid": "dexforce_w1", "version": "v021"}) cfg.init_qpos = [0.0001 * (index + 1) for index in range(W1_ACTIVE_DOF)] + cfg.root_props.newton_mimic_stabilizer = self.MIMIC_STABILIZER self.robot: Robot = self.sim.add_robot(cfg=cfg) self.sim.prepare() @@ -799,9 +802,17 @@ def test_newton_robot_spawn_and_control(self): and "_HAND_" in state_joint_names[parent_id] for mimic_id, parent_id, _, _ in mimic_relations ) + if not self.MIMIC_STABILIZER: + # Without the stabilizer, reset starts each follower on its + # native mimic relation to avoid a corrective impulse. + for mimic_id, parent_id, multiplier, offset in mimic_relations: + initial_qpos_by_name[state_joint_names[mimic_id]] = ( + initial_qpos_by_name[state_joint_names[parent_id]] * multiplier + + offset + ) initial_qpos = self.robot.body_data.qpos[0].detach().cpu().tolist() assert dict(zip(state_joint_names, initial_qpos, strict=True)) == pytest.approx( - initial_qpos_by_name + initial_qpos_by_name, abs=1e-6 ) binding = self.robot._entities[0]._physics_binding @@ -828,25 +839,37 @@ def test_newton_robot_spawn_and_control(self): selected_eq = np.isin(mapping, np.asarray(constraint_rows, dtype=np.int32)) assert int(selected_eq.sum()) == len(mimic_relations) eq_solref = np.asarray(solver.mjw_model.eq_solref.numpy()) - np.testing.assert_allclose( - eq_solref[selected_eq], - np.broadcast_to([2.0e-3, 1.0e1], (len(mimic_relations), 2)), - ) target_ke = np.asarray(model.joint_target_ke.numpy()) target_kd = np.asarray(model.joint_target_kd.numpy()) target_mode = np.asarray(model.joint_target_mode.numpy()) eq_active = np.asarray(solver.mjw_data.eq_active.numpy()) assert np.all(eq_active[selected_eq]) - for mimic_id, parent_id, _, _ in mimic_relations: - child = runtime_joints[state_joint_names[mimic_id]] - parent = runtime_joints[state_joint_names[parent_id]] - assert target_ke[child.qd_start] == pytest.approx( - target_ke[parent.qd_start] * 1.0e-2 + if self.MIMIC_STABILIZER: + np.testing.assert_allclose( + eq_solref[selected_eq], + np.broadcast_to([2.0e-3, 1.0e1], (len(mimic_relations), 2)), ) - assert target_kd[child.qd_start] == pytest.approx( - target_kd[parent.qd_start] * 1.0e-2 + for mimic_id, parent_id, _, _ in mimic_relations: + child = runtime_joints[state_joint_names[mimic_id]] + parent = runtime_joints[state_joint_names[parent_id]] + assert target_ke[child.qd_start] == pytest.approx( + target_ke[parent.qd_start] * 1.0e-2 + ) + assert target_kd[child.qd_start] == pytest.approx( + target_kd[parent.qd_start] * 1.0e-2 + ) + assert target_mode[child.qd_start] == target_mode[parent.qd_start] + else: + # Native DexSim behavior: MuJoCo's default equality solref and + # the follower drives configured by the W1 preset (none). + np.testing.assert_allclose( + eq_solref[selected_eq], + np.broadcast_to([2.0e-2, 1.0], (len(mimic_relations), 2)), ) - assert target_mode[child.qd_start] == target_mode[parent.qd_start] + for mimic_id, _, _, _ in mimic_relations: + child = runtime_joints[state_joint_names[mimic_id]] + assert target_ke[child.qd_start] == 0.0 + assert target_kd[child.qd_start] == 0.0 # This physical check covers both hands and keeps the native coupled # constraints bounded under the W1's self-contacts. Default can also @@ -897,7 +920,11 @@ def test_newton_robot_spawn_and_control(self): self.robot.set_qpos(hand_target, name="right_eef") target_qpos = self.robot.body_data.target_qpos torch.testing.assert_close(target_qpos[:, right_eef_ids], hand_target) - for mimic_id, parent_id, multiplier, offset in mimic_relations: + # Follower targets are mirrored only by the opt-in stabilizer; native + # followers keep their own (here unused, zero-gain) targets. + for mimic_id, parent_id, multiplier, offset in ( + mimic_relations if self.MIMIC_STABILIZER else [] + ): torch.testing.assert_close( target_qpos[:, mimic_id], target_qpos[:, parent_id] * multiplier + offset, @@ -910,7 +937,9 @@ def test_newton_robot_spawn_and_control(self): self.robot.set_qvel(hand_velocity_target, name="right_eef") target_qvel = self.robot.body_data.target_qvel torch.testing.assert_close(target_qvel[:, right_eef_ids], hand_velocity_target) - for mimic_id, parent_id, multiplier, _ in mimic_relations: + for mimic_id, parent_id, multiplier, _ in ( + mimic_relations if self.MIMIC_STABILIZER else [] + ): torch.testing.assert_close( target_qvel[:, mimic_id], target_qvel[:, parent_id] * multiplier, @@ -930,3 +959,9 @@ def test_newton_robot_spawn_and_control(self): test_cpu = TestRobotCUDA() test_cpu.setup_method() test_cpu.test_compute_jacobian() + + +class TestRobotNewtonMimicStabilizer(TestRobotNewton): + """The same W1 coverage with the opt-in Default-parity mimic stabilizer.""" + + MIMIC_STABILIZER = True diff --git a/tests/sim/objects/test_soft_object_tet_mesh.py b/tests/sim/objects/test_soft_object_tet_mesh.py new file mode 100644 index 0000000000..610799a75f --- /dev/null +++ b/tests/sim/objects/test_soft_object_tet_mesh.py @@ -0,0 +1,115 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Explicit tetrahedral soft bodies and direct nodal-state writes.""" + +from __future__ import annotations + +import numpy as np +import pytest +import torch + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import NewtonPhysicsCfg, VolumeDeformablePhysicsCfg +from embodichain.lab.sim.objects import ( + VolumeDeformableObject, + VolumeDeformableObjectCfg, +) + + +def _cube_tet_mesh(cells: int, size: float) -> tuple[np.ndarray, np.ndarray]: + """Structured cube with six tetrahedra per cell, vertices in x-fastest order.""" + n = cells + 1 + axis = np.linspace(-0.5 * size, 0.5 * size, n, dtype=np.float32) + z, y, x = np.meshgrid(axis, axis, axis, indexing="ij") + vertices = np.stack([x.ravel(), y.ravel(), z.ravel()], axis=1) + + def vid(i: int, j: int, k: int) -> int: + return i + n * (j + n * k) + + tets = [] + for k in range(cells): + for j in range(cells): + for i in range(cells): + c = [ + vid(i + dx, j + dy, k + dz) + for dz in (0, 1) + for dy in (0, 1) + for dx in (0, 1) + ] + for a, b in ((1, 3), (3, 2), (2, 6), (6, 4), (4, 5), (5, 1)): + tets.append([c[0], c[a], c[b], c[7]]) + return vertices, np.asarray(tets, dtype=np.int32) + + +class TestExplicitTetSoftBodyCUDA: + def setup_method(self) -> None: + self.vertices, self.tets = _cube_tet_mesh(cells=3, size=0.06) + self.init_pos = np.array([0.0, 0.0, 0.2], dtype=np.float32) + self.sim = SimulationManager( + SimulationManagerCfg( + headless=True, + physics_dt=1.0 / 60.0, + device="cuda", + num_envs=1, + physics_cfg=NewtonPhysicsCfg(solver_cfg={"solver_type": "vbd"}), + ) + ) + self.block: VolumeDeformableObject = self.sim.add_deformable_object( + cfg=VolumeDeformableObjectCfg( + uid="block", + shape={"vertices": self.vertices, "triangles": None}, + tetrahedra=self.tets, + init_pos=self.init_pos.tolist(), + attrs=VolumeDeformablePhysicsCfg( + youngs=3e4, poissons=0.35, density=250.0 + ), + ) + ) + self.sim.prepare() + + def test_particles_follow_authored_vertex_order(self) -> None: + positions = self.block.data.nodal_pos_w[0].cpu().numpy() + assert positions.shape == self.vertices.shape + np.testing.assert_allclose(positions, self.vertices + self.init_pos, atol=1e-6) + + def test_set_nodal_state_writes_positions_and_velocities(self) -> None: + rest = self.block.data.nodal_pos_w + squeezed = rest.clone() + center_z = float(self.init_pos[2]) + squeezed[..., 2] = center_z + 0.9 * (rest[..., 2] - center_z) + velocity = torch.zeros_like(rest) + velocity[..., 0] = 0.01 + + self.block.set_nodal_state(torch.cat((squeezed, velocity), dim=-1)) + + torch.testing.assert_close(self.block.data.nodal_pos_w, squeezed) + torch.testing.assert_close(self.block.data.nodal_vel_w, velocity) + for _ in range(10): + self.sim.update(step=1) + assert torch.isfinite(self.block.data.nodal_pos_w).all() + + def test_set_nodal_state_rejects_bad_shape(self) -> None: + with pytest.raises(ValueError, match="num_nodes, 6"): + self.block.set_nodal_state(self.block.data.nodal_pos_w) + + def teardown_method(self) -> None: + self.sim.destroy() + SimulationManager.flush_cleanup_queue() + self.__dict__.clear() + import gc + + gc.collect() diff --git a/tests/sim/spawn/test_descriptors.py b/tests/sim/spawn/test_descriptors.py index f52e727dd7..426a02b441 100644 --- a/tests/sim/spawn/test_descriptors.py +++ b/tests/sim/spawn/test_descriptors.py @@ -1192,6 +1192,137 @@ def test_usd_articulation_forwards_gravity_compensation(monkeypatch, mode) -> No assert descriptor.newton_gravity_compensation == 0.5 +_NEWTON_CREATION_ROOT_PROPS = { + "newton_kinematic": True, + "newton_sleep_policy": "never", + "newton_shape_collision": False, + "newton_shape_collision_links": ["base_link"], + "newton_particle_collision_links": ["base_link", "tool"], + "newton_mesh_approximation": {"method": "convex_hull", "link_names": ["tool"]}, +} + + +@pytest.mark.parametrize("mode", ["preserve", "overlay"]) +def test_articulation_newton_creation_options_are_forwarded(mode) -> None: + from dexsim.spawn import NewtonMeshApproximationDesc, NewtonSleepPolicy + + cfg = ArticulationCfg.from_dict( + { + "uid": "robot", + "fpath": "robot.urdf", + "asset_physics_mode": mode, + "root_props": _NEWTON_CREATION_ROOT_PROPS, + } + ) + descriptor = articulation_desc_from_cfg(cfg, newton_solver_type="dexuni") + resolved = _resolved_articulation_desc() + configure_articulation_desc(resolved, cfg, newton_solver_type="dexuni") + + for desc in (descriptor, resolved): + assert desc.newton_kinematic is True + assert desc.newton_sleep_policy is NewtonSleepPolicy.NEVER + assert desc.newton_shape_collision is False + assert desc.newton_shape_collision_links == ("base_link",) + assert desc.newton_particle_collision_links == ("base_link", "tool") + assert isinstance(desc.newton_mesh_approximation, NewtonMeshApproximationDesc) + assert desc.newton_mesh_approximation.link_names == ("tool",) + + +@pytest.mark.parametrize("mode", ["preserve", "overlay"]) +@pytest.mark.parametrize("native_geometry", [False, True]) +@pytest.mark.parametrize("value", [None, False, True]) +def test_articulation_sdf_request_is_authored_before_source_resolution( + value: bool | None, + mode: str, + native_geometry: bool, +) -> None: + """Preserve concave import intent without introducing contact overrides.""" + cfg = ArticulationCfg( + uid="robot", + fpath="robot.urdf", + asset_physics_mode=mode, + root_props=ArticulationRootPropertiesCfg(newton_force_sdf=value), + ) + restored = ArticulationCfg.from_dict(cfg.to_dict()) + descriptor = articulation_desc_from_cfg(restored, newton_solver_type="dexuni") + if value is None: + assert descriptor.newton_collision is None + else: + assert descriptor.newton_collision.force_sdf is value + assert descriptor.newton_collision.ke is None + assert descriptor.newton_collision.margin is None + resolved = _resolved_articulation_desc() + if native_geometry: + resolved.urdf_path = "robot.urdf" + for link in resolved.links: + link.collisions.clear() + configure_articulation_desc(resolved, restored, newton_solver_type="dexuni") + if value is not None: + for link in resolved.links: + for collision in link.collisions: + assert collision.newton.force_sdf is value + + +def test_articulation_sdf_request_rejects_default_backend() -> None: + """Do not silently apply a Newton source option under Default physics.""" + cfg = ArticulationCfg( + fpath="robot.urdf", + root_props=ArticulationRootPropertiesCfg(newton_force_sdf=True), + ) + with pytest.raises(ValueError, match="newton_force_sdf.*Newton"): + articulation_desc_from_cfg(cfg) + + +def test_articulation_sdf_request_rejects_truthy_strings() -> None: + """Require a boolean rather than treating the text 'false' as enabled.""" + cfg = ArticulationCfg( + fpath="robot.urdf", + root_props=ArticulationRootPropertiesCfg(newton_force_sdf="false"), + ) + with pytest.raises(TypeError, match="newton_force_sdf.*boolean"): + articulation_desc_from_cfg(cfg, newton_solver_type="dexuni") + + +def test_articulation_unset_newton_creation_options_keep_dexsim_defaults() -> None: + descriptor = articulation_desc_from_cfg( + ArticulationCfg(), source_path="robot.urdf", newton_solver_type="dexuni" + ) + defaults = ArticulationDesc(name="defaults", path="robot.urdf") + for name in ( + "newton_kinematic", + "newton_sleep_policy", + "newton_shape_collision", + "newton_shape_collision_links", + "newton_particle_collision_links", + "newton_mesh_approximation", + ): + assert getattr(descriptor, name) == getattr(defaults, name), name + + +@pytest.mark.parametrize("name", sorted(_NEWTON_CREATION_ROOT_PROPS)) +def test_articulation_newton_creation_options_reject_default_backend(name) -> None: + cfg = ArticulationCfg.from_dict( + {"root_props": {name: _NEWTON_CREATION_ROOT_PROPS[name]}} + ) + with pytest.raises(ValueError, match="Newton backend"): + articulation_desc_from_cfg(cfg, source_path="robot.urdf") + + +@pytest.mark.parametrize( + ("root_props", "error"), + [ + ({"newton_sleep_policy": "sometimes"}, ValueError), + ({"newton_shape_collision_links": "base_link"}, TypeError), + ({"newton_mesh_approximation": "convex_hull"}, TypeError), + ], +) +def test_articulation_newton_creation_options_reject_invalid_values( + root_props, error +) -> None: + with pytest.raises(error): + ArticulationCfg.from_dict({"root_props": root_props}) + + @pytest.mark.parametrize("value", [-0.1, 1.1, float("nan"), float("inf")]) def test_articulation_gravity_compensation_rejects_invalid_values(value) -> None: cfg = ArticulationCfg() @@ -2379,6 +2510,32 @@ def test_spawn_post_config_only_applies_render_uv() -> None: render_body.set_projective_uv.assert_called_once_with() +@pytest.mark.parametrize("enabled", [True, False]) +def test_newton_mimic_stabilizer_can_be_disabled(monkeypatch, enabled) -> None: + configure = Mock(return_value=True) + monkeypatch.setattr( + "embodichain.lab.sim.objects.articulation._configure_newton_mimic_compliance", + configure, + ) + articulation = object.__new__(Articulation) + articulation.cfg = SimpleNamespace( + compute_uv=False, + root_props=ArticulationRootPropertiesCfg(newton_mimic_stabilizer=enabled), + ) + articulation._entities = [Mock(joint_dof_layout=[])] + articulation._prepared_default_root_topology_revision = -1 + articulation._mimic_info = SimpleNamespace( + mimic_id=np.array([1], dtype=np.int32), + mimic_parent=np.array([0], dtype=np.int32), + ) + articulation._state_joint_names = Mock(return_value=["leader", "follower"]) + + articulation._apply_spawn_config() + + assert articulation._newton_mimic_compliance_configured is enabled + assert configure.called is enabled + + def test_rigid_object_spawn_applies_projective_uv() -> None: render_body = Mock() entity = Mock() diff --git a/tests/sim/test_cfg.py b/tests/sim/test_cfg.py index 712a40d426..35ea0a4909 100644 --- a/tests/sim/test_cfg.py +++ b/tests/sim/test_cfg.py @@ -1008,6 +1008,14 @@ def test_backend_joint_and_articulation_configs_round_trip() -> None: "min_position_iters": None, "min_velocity_iters": None, "newton_gravity_compensation": None, + "newton_kinematic": None, + "newton_sleep_policy": None, + "newton_shape_collision": None, + "newton_shape_collision_links": None, + "newton_particle_collision_links": None, + "newton_mimic_stabilizer": False, + "newton_mesh_approximation": None, + "newton_force_sdf": None, } assert type(restored_root) is ArticulationRootPropertiesCfg @@ -1136,6 +1144,43 @@ def test_newton_physics_can_disable_the_external_collision_pipeline() -> None: assert not hasattr(cfg, "collision_pipeline_update_interval") +def test_newton_physics_forwards_sync_and_capture_options() -> None: + defaults = NewtonPhysicsCfg().to_dexsim_cfg(gpu_id=0) + assert defaults.async_sync is False + assert defaults.rigid_transform_sync_mode == "all" + assert defaults.capture_acquire_timeout is None + assert defaults.capture_wait_log_interval == 10.0 + + dexsim_cfg = NewtonPhysicsCfg( + async_sync=True, + rigid_transform_sync_mode="dynamic", + capture_acquire_timeout=2.5, + capture_wait_log_interval=None, + ).to_dexsim_cfg(gpu_id=0) + + assert dexsim_cfg.async_sync is True + assert dexsim_cfg.rigid_transform_sync_mode == "dynamic" + assert dexsim_cfg.capture_acquire_timeout == 2.5 + assert dexsim_cfg.capture_wait_log_interval is None + + +@pytest.mark.parametrize( + ("field_name", "value", "message"), + [ + ("rigid_transform_sync_mode", "static", "rigid_transform_sync_mode"), + ("capture_acquire_timeout", -1.0, "capture_acquire_timeout"), + ("capture_acquire_timeout", float("inf"), "capture_acquire_timeout"), + ], +) +def test_newton_physics_rejects_invalid_sync_and_capture_options( + field_name: str, + value: str | float, + message: str, +) -> None: + with pytest.raises(ValueError, match=message): + NewtonPhysicsCfg(**{field_name: value}) + + @pytest.mark.parametrize( ("field_name", "value"), [("broad_phase", "sap"), ("visualizer_enabled", True)], diff --git a/tests/sim/test_deformable_cfg.py b/tests/sim/test_deformable_cfg.py index bc7d01b13b..6ec6d42beb 100644 --- a/tests/sim/test_deformable_cfg.py +++ b/tests/sim/test_deformable_cfg.py @@ -32,6 +32,7 @@ surface_deformable_desc_from_cfg, volume_deformable_desc_from_cfg, ) +from embodichain.lab.sim.shapes import MeshCfg pytestmark = pytest.mark.no_sim @@ -89,6 +90,104 @@ def test_unknown_nested_fields_are_rejected(): ) +@pytest.mark.parametrize( + "mode", ["isothermal", "adiabatic", "target_volume", "prescribed_gauge_pressure"] +) +def test_cloth_pneumatic_round_trip_and_descriptor(mode: str) -> None: + """Keep cavity law and physical parameters through config serialization.""" + gas = { + "mode": mode, + "reference_absolute_pressure": 117325.0, + "ambient_pressure": 101325.0, + "target_volume": 2.4e-4, + "volume_stiffness": 2000.0, + "bulk_damping": 3e6, + "max_absolute_pressure": 5e5, + "heat_capacity_ratio": 1.4, + "min_volume_ratio": 0.05, + "prescribed_gauge_pressure": 16000.0, + } + cfg = SurfaceDeformableObjectCfg.from_dict( + {"uid": "pouch", "shape": {"fpath": "pouch.obj"}, "attrs": {"pneumatic": gas}} + ) + restored = SurfaceDeformableObjectCfg.from_dict(cfg.to_dict()) + descriptor, _ = surface_deformable_desc_from_cfg(restored) + assert restored.attrs.pneumatic == gas + for name, value in gas.items(): + assert getattr(descriptor.physics.pneumatic, name) == value + + +def test_ordinary_cloth_does_not_require_pneumatic_support(monkeypatch) -> None: + """Omitting a cavity remains compatible with wheels lacking its descriptor.""" + import dexsim.spawn + + monkeypatch.delattr(dexsim.spawn, "PneumaticPhysicsDesc") + cfg = SurfaceDeformableObjectCfg(uid="cloth", shape=MeshCfg(fpath="cloth.obj")) + descriptor, _ = surface_deformable_desc_from_cfg(cfg) + assert descriptor.physics.pneumatic is None + + +def test_explicit_cavity_rejects_unsupported_wheel(monkeypatch) -> None: + """Never silently discard requested gas physics on an older SDK.""" + import dexsim.spawn + + monkeypatch.delattr(dexsim.spawn, "PneumaticPhysicsDesc") + cfg = SurfaceDeformableObjectCfg.from_dict( + {"uid": "pouch", "shape": {"fpath": "pouch.obj"}, "attrs": {"pneumatic": {}}} + ) + with pytest.raises(NotImplementedError, match="PneumaticPhysicsDesc"): + surface_deformable_desc_from_cfg(cfg) + + +@pytest.mark.parametrize( + "gas, error", + [ + ({"reference_absolute_pressure": float("nan")}, ValueError), + ({"bulk_damping": -1.0}, ValueError), + ({"mode": "unknown"}, ValueError), + ({"reference_pressure_typo": 1.0}, TypeError), + (True, TypeError), + ], +) +def test_cloth_cavity_rejects_invalid_parameters(gas: object, error: type) -> None: + """Validate gas parameters before committing native scene resources.""" + cfg = SurfaceDeformableObjectCfg.from_dict( + {"uid": "pouch", "shape": {"fpath": "pouch.obj"}, "attrs": {"pneumatic": gas}} + ) + with pytest.raises(error): + surface_deformable_desc_from_cfg(cfg) + + +@pytest.mark.parametrize("angle", [None, 0.0, 0.35, -0.4]) +def test_cloth_rest_angle_round_trip_and_descriptor(angle: float | None) -> None: + """Preserve inferred or authored signed rest angles through config lowering.""" + cfg = SurfaceDeformableObjectCfg.from_dict( + { + "uid": "shirt", + "shape": {"fpath": "shirt.obj"}, + "attrs": {"edge_rest_angle": angle}, + } + ) + restored = SurfaceDeformableObjectCfg.from_dict(cfg.to_dict()) + assert restored.attrs.edge_rest_angle == angle + descriptor, _ = surface_deformable_desc_from_cfg(restored) + assert descriptor.physics.edge_rest_angle == angle + + +@pytest.mark.parametrize("angle", [float("nan"), float("inf"), -float("inf")]) +def test_cloth_rest_angle_rejects_nonfinite_values(angle: float) -> None: + """Reject invalid rest geometry before creating native scene resources.""" + cfg = SurfaceDeformableObjectCfg.from_dict( + { + "uid": "shirt", + "shape": {"fpath": "shirt.obj"}, + "attrs": {"edge_rest_angle": angle}, + } + ) + with pytest.raises(ValueError, match="edge_rest_angle.*finite"): + surface_deformable_desc_from_cfg(cfg) + + @pytest.mark.parametrize("topology", ["volume", "surface"]) def test_surface_coefficients_compile_without_changing_values(topology: str) -> None: cls = ( @@ -217,3 +316,84 @@ def test_explicit_particle_material_rejects_unsupported_engine(monkeypatch) -> N ) with pytest.raises(RuntimeError, match="per-object particle materials"): surface_deformable_desc_from_cfg(cfg) + + +def _two_tet_mesh(): + import numpy as np + + vertices = np.array( + [ + [0.0, 0.0, 0.0], + [1.0, 0.0, 0.0], + [0.0, 1.0, 0.0], + [0.0, 0.0, 1.0], + [1.0, 1.0, 1.0], + [0.25, 0.25, 0.25], + ], + dtype=np.float32, + ) + tetrahedra = np.array([[0, 1, 2, 5], [1, 2, 3, 4]], dtype=np.int32) + return vertices, tetrahedra + + +def test_volume_explicit_tetrahedra_keep_particle_order() -> None: + import numpy as np + from dexsim.spawn.soft_body_meshing import extract_tet_surface_triangles + + vertices, tetrahedra = _two_tet_mesh() + cfg = VolumeDeformableObjectCfg.from_dict( + { + "uid": "block", + "shape": {"vertices": vertices, "triangles": None}, + "tetrahedra": tetrahedra.tolist(), + } + ) + desc, _ = volume_deformable_desc_from_cfg(cfg) + + np.testing.assert_array_equal(desc.positions, vertices) + np.testing.assert_array_equal(desc.tetrahedra, tetrahedra) + np.testing.assert_array_equal( + desc.surface_triangles, extract_tet_surface_triangles(tetrahedra) + ) + assert desc.meshing is None + assert desc.meshing_stats.source_kind == "tet_mesh" + assert desc.meshing_stats.particle_count == len(vertices) + surface_ids = np.unique(desc.surface_triangles) + np.testing.assert_array_equal(desc.mesh.vertices, vertices[surface_ids]) + + +def test_volume_explicit_tetrahedra_accept_authored_surface() -> None: + import numpy as np + + vertices, tetrahedra = _two_tet_mesh() + surface = np.array([[0, 2, 1], [1, 2, 3]], dtype=np.int32) + cfg = VolumeDeformableObjectCfg( + uid="block", + shape={"vertices": vertices, "triangles": surface}, + tetrahedra=tetrahedra, + ) + desc, _ = volume_deformable_desc_from_cfg(cfg) + np.testing.assert_array_equal(desc.surface_triangles, surface) + + +@pytest.mark.parametrize( + ("overrides", "error", "message"), + [ + ({"shape": {"fpath": "mesh.obj"}}, ValueError, "array-backed"), + ({"tetrahedra": [[0, 1, 2, 9]]}, ValueError, "outside the vertices"), + ({"tetrahedra": [[0.0, 1.0, 2.0, 3.0]]}, TypeError, "integer"), + ({"tetrahedra": [[0, 1, 2]]}, ValueError, r"\(M, 4\)"), + ], +) +def test_volume_explicit_tetrahedra_reject_invalid_input( + overrides, error, message +) -> None: + vertices, tetrahedra = _two_tet_mesh() + data = { + "uid": "block", + "shape": {"vertices": vertices, "triangles": None}, + "tetrahedra": tetrahedra, + **overrides, + } + with pytest.raises(error, match=message): + volume_deformable_desc_from_cfg(VolumeDeformableObjectCfg.from_dict(data)) diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index dc5ae2de98..ab6242789e 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -1846,6 +1846,68 @@ def test_register_kinematic_nodal_trajectory_rejects_finalized_scene() -> None: builder.add_runtime_control.assert_not_called() +def _runtime_control(**attrs: object): + from dexsim.engine.newton_physics.runtime_control import RuntimeControl + + control = type("RecordingControl", (RuntimeControl,), {})() + for name, value in attrs.items(): + setattr(control, name, value) + return control + + +def test_add_runtime_control_registers_one_scene_control() -> None: + sim, builder = _make_runtime_control_sim_manager() + control = _runtime_control() + + assert sim.add_runtime_control(control) == (control,) + + builder.add_runtime_control.assert_called_once_with(control) + + +def test_add_runtime_control_expands_factory_per_arena() -> None: + sim, builder = _make_runtime_control_sim_manager(num_envs=3) + + controls = sim.add_runtime_control( + lambda arena_name, env_index: _runtime_control( + target=f"{arena_name}/robot", env_index=env_index + ) + ) + + assert [control.target for control in controls] == [ + "arena_0/robot", + "arena_1/robot", + "arena_2/robot", + ] + assert [control.env_index for control in controls] == [0, 1, 2] + assert [ + call.args[0] for call in builder.add_runtime_control.call_args_list + ] == list(controls) + + +def test_add_runtime_control_rejects_non_control_factory_results() -> None: + sim, builder = _make_runtime_control_sim_manager() + + with pytest.raises(TypeError, match="RuntimeControl instances"): + sim.add_runtime_control(lambda arena_name, env_index: object()) + with pytest.raises(TypeError, match="RuntimeControl or a factory"): + sim.add_runtime_control(object()) + + builder.add_runtime_control.assert_not_called() + + +def test_add_runtime_control_rejects_finalized_scene_and_default_backend() -> None: + sim, builder = _make_runtime_control_sim_manager() + builder.is_finalized = True + with pytest.raises(RuntimeError, match=r"before SimulationManager\.prepare"): + sim.add_runtime_control(_runtime_control()) + + sim, builder = _make_runtime_control_sim_manager(backend="default") + with pytest.raises(RuntimeError, match="Newton backend"): + sim.add_runtime_control(_runtime_control()) + + builder.add_runtime_control.assert_not_called() + + def test_add_robot_resolves_backend_preset_before_declaration() -> None: @configclass class TestRobotPresetCfg(RobotPresetCfg): From 9a2d016245d914bc516ebf5e98b29d7a1e9424fc Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 27 Oct 2026 21:22:29 +0800 Subject: [PATCH 16/27] fix(sim): preserve continuous URDF joint limits for DexUni --- .../articulation-adapters.md | 5 ++ embodichain/lab/sim/spawn/source.py | 17 ++++++ tests/sim/spawn/test_source.py | 56 ++++++++++++++++++- 3 files changed, 77 insertions(+), 1 deletion(-) diff --git a/agent_context/topics/simulation-system/articulation-adapters.md b/agent_context/topics/simulation-system/articulation-adapters.md index 7693599cad..4b3c3a014c 100644 --- a/agent_context/topics/simulation-system/articulation-adapters.md +++ b/agent_context/topics/simulation-system/articulation-adapters.md @@ -13,6 +13,11 @@ resolves source metadata before its first immutable build. Do not implement initial configuration as finalize-then-rebuild or duplicate descriptor writes inside `Articulation._apply_spawn_config()`. +URDF continuous joints retain unbounded source limits during resolution. Native +Skeleton metadata may expose zero-filled limits; those must not become a locked +joint at the Newton boundary. Caller-authored limits and later config overlays +retain precedence over the source default. + `ArticulationRootPropertiesCfg` separates root intent from link physics. Fixed-base/self-collision settings enter common descriptors, independently of the source overlay policy; explicit `None` preserves USD intent and selects diff --git a/embodichain/lab/sim/spawn/source.py b/embodichain/lab/sim/spawn/source.py index e916610c1a..0e2a5163b5 100644 --- a/embodichain/lab/sim/spawn/source.py +++ b/embodichain/lab/sim/spawn/source.py @@ -31,6 +31,7 @@ DexsimPhysicsDesc, RigidBodyPhysicsDesc, ) +from dexsim.engine import JointType from embodichain.lab.sim._inertia import _principal_inertia_matrix from embodichain.utils.math import quat_wxyz_to_xyzw @@ -91,6 +92,12 @@ def resolve_articulation_source( desc.joints = [] desc.root_link_name = None + retained_joint_limits = { + joint.name + for joint in desc.joints + if joint.lower_limit is not None or joint.upper_limit is not None + } + arena = _source_arena(builder, desc) temp_name = f"__embodichain_resolve__{desc.name.replace('/', '__')}__{id(desc)}" skeleton = arena.create_skeleton("skeleton") @@ -121,6 +128,16 @@ def resolve_articulation_source( except (KeyError, RuntimeError, TypeError, AttributeError): continue _translate_urdf_articulation(skeleton, desc) + # Native Skeleton metadata exposes (0, 0) for URDF continuous joints. + # Preserve their unbounded source intent before a MuJoCo build, while + # retaining caller-authored limits and subsequent config overlays. + for joint in desc.joints: + if ( + joint.joint_type == JointType.CONTINUOUS + and joint.name not in retained_joint_limits + ): + joint.lower_limit = -np.inf + joint.upper_limit = np.inf source_states = _read_urdf_inertial_states(desc.urdf_path) _annotate_urdf_source_physics( desc, diff --git a/tests/sim/spawn/test_source.py b/tests/sim/spawn/test_source.py index 9e5d6bba4b..9756953ba9 100644 --- a/tests/sim/spawn/test_source.py +++ b/tests/sim/spawn/test_source.py @@ -22,13 +22,15 @@ import numpy as np import pytest -from dexsim.spawn import ArticulationDesc, LinkDesc +from dexsim.spawn import ArticulationDesc, JointDesc, LinkDesc +from dexsim.engine import JointType from embodichain.lab.sim.spawn.source import ( _apply_dexsim_source_overlay, _capture_dexsim_source_physics, _clear_invalid_source_com, _retain_dexsim_source_descriptor, + resolve_articulation_source, ) pytestmark = pytest.mark.no_sim @@ -38,6 +40,58 @@ def _link(name: str) -> LinkDesc: return LinkDesc(name, "", np.eye(4, dtype=np.float32)) +@pytest.mark.parametrize("authored_limits", [None, (-1.0, 1.0)]) +def test_urdf_continuous_joint_retains_unbounded_source_limits( + monkeypatch: pytest.MonkeyPatch, tmp_path, authored_limits +) -> None: + from embodichain.lab.sim.spawn import source + from dexsim.spawn.adapters import newton_articulation_adapter + + path = tmp_path / "continuous.urdf" + path.write_text('', encoding="utf-8") + continuous = JointDesc("spinner", "base", "wheel", JointType.CONTINUOUS) + if authored_limits is not None: + continuous.lower_limit, continuous.upper_limit = authored_limits + descriptor = ArticulationDesc( + name="source", + urdf_path=str(path), + joints=[] if authored_limits is None else [continuous], + ) + limited = JointDesc( + "locked", + "base", + "other", + JointType.REVOLUTE, + lower_limit=0.0, + upper_limit=0.0, + ) + skeleton = SimpleNamespace( + set_name=lambda _: None, + detach_parent=lambda: None, + load_urdf=lambda *_: 0, + get_link_names=lambda _: (), + ) + arena = SimpleNamespace( + create_skeleton=lambda _: skeleton, + remove_skeleton=lambda _: None, + ) + monkeypatch.setattr(source, "_source_arena", lambda *_: arena) + + def translate(_skeleton, desc): + if not desc.joints: + continuous.lower_limit = continuous.upper_limit = 0.0 + desc.joints = [continuous, limited] + + monkeypatch.setattr( + newton_articulation_adapter, "_translate_urdf_articulation", translate + ) + resolve_articulation_source(SimpleNamespace(), descriptor) + + expected = (-np.inf, np.inf) if authored_limits is None else authored_limits + assert (continuous.lower_limit, continuous.upper_limit) == expected + assert (limited.lower_limit, limited.upper_limit) == (0.0, 0.0) + + def _physical_attr( mass: float, inertia: tuple[float, float, float], From a48f07ca6c4732b977777bb35b313b1718ac6731 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 27 Oct 2026 22:31:07 +0800 Subject: [PATCH 17/27] fix(examples): complete DexUni workflows and physical placement checks --- .../topics/atomic-actions/atomic-actions.md | 12 +- .../atomic_action/coordinated_placement.py | 146 +++++++++++++++--- .../sim/atomic_action/pickup_rubiks_cube.py | 12 +- examples/sim/atomic_action/place.py | 4 + examples/sim/atomic_action/scenario.py | 6 + examples/sim/atomic_action/support.py | 6 +- examples/sim/demo/pick_up_cloth.py | 6 +- examples/sim/demo/scoop_ice.py | 19 ++- examples/sim/motion/solvers/fep_solver.py | 2 +- .../scenarios/_placement_geometry.py | 15 +- .../test_placement_geometry.py | 19 ++- tests/examples/atomic_action/test_support.py | 75 +++++++++ tests/sim/test_scoop_ice_demo.py | 19 ++- 13 files changed, 286 insertions(+), 55 deletions(-) diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index 3574f6bdd6..47dea7fd61 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -116,10 +116,20 @@ rewriting captured solver arrays. The current values live in Scene-specific DexUni contact options are authored through `create_tutorial_simulation(newton_solver_options=...)` before model construction. `add_tutorial_robot` owns optional `newton_gripper_friction` and `arm_damping` -overrides; `scenario.add_dual_tutorial_robot` owns the dual-arm friction override. +overrides; `scenario.add_dual_tutorial_robot` owns dual-arm friction and optional +mimic stabilization. Scene modules retain calibrated close targets, fixture TCPs and grippers, including Handover's Robotiq 2F-140 fixture. Read those example owners for current values; calibration does not establish generic physical success or solver equivalence. +The coordinated-placement host opts into mimic stabilization and two-way DexUni +feedback, checks measured +lift and release placement, and retains commanded jaw preload when reconciling +measured arm/held-object state. A contact-blocked jaw measurement is not its +holding command. Its mesh fixtures use +the authored Z-up frame; shared pan geometry selects the handle and basin along +local Y. Keep these asset-specific conventions in the scene/geometry owners. +Keep the base-to-table height invariant when raising the fixture. +Read held poses without resetting them at the replan boundary. The launcher preserves an omitted device through simulation configuration so the selected backend owns its default; explicit device selections retain priority. diff --git a/examples/sim/atomic_action/coordinated_placement.py b/examples/sim/atomic_action/coordinated_placement.py index a71b14152a..1e9ffa3e4e 100644 --- a/examples/sim/atomic_action/coordinated_placement.py +++ b/examples/sim/atomic_action/coordinated_placement.py @@ -60,6 +60,7 @@ build_pan_handle_grasp_pose, build_support_object_target_pose, build_placing_object_target_pose, + get_pan_basin_vertices, ) from examples.sim.atomic_action.scenario import ( add_dual_tutorial_robot, @@ -87,10 +88,10 @@ run_tutorial, ) -DEFAULT_MESH_FRAME_CORRECTION_EULER_DEG = (-90.0, 0.0, 0.0) -# DexSim imports this pan GLB with gym raw +Z mapped to local -Y. The +90deg X -# correction therefore makes the pan opening point upward in world Z. -PAN_MESH_FRAME_CORRECTION_EULER_DEG = (90.0, 0.0, 0.0) +# Current DexSim retains the authored Z-up GLB geometry. Preserve the gym +# table/bread orientation; the pan needs a half turn to keep its opening up. +DEFAULT_MESH_FRAME_CORRECTION_EULER_DEG = (0.0, 0.0, 0.0) +PAN_MESH_FRAME_CORRECTION_EULER_DEG = (180.0, 0.0, 0.0) PAN_WORLD_YAW_CORRECTION_DEG = 270.0 @@ -121,12 +122,19 @@ def transform_baseline_pose( PAN_MESH_PATH = f"{PLACEMENT_ASSET_ROOT}/pan.glb" BREAD_LABEL = "bread" PAN_LABEL = "pan" +# Leave opening clearance inside the PGI fingers' roughly 73 mm contact span. +BREAD_MESH_SCALE = 1.4 GRIPPER_TCP_Z = 0.121 PICK_SAMPLE_INTERVAL = 100 COORDINATED_SAMPLE_INTERVAL = 120 -ROBOT_INIT_POS = (1.85, 0.0, 0.1) +BASELINE_ROBOT_INIT_POS = (1.85, 0.0, 0.1) BASELINE_TABLE_TOP_Z = 0.3621708124799265 SCENE_Z_OFFSET = TABLE_TOP_Z - BASELINE_TABLE_TOP_Z +ROBOT_INIT_POS = ( + BASELINE_ROBOT_INIT_POS[0], + BASELINE_ROBOT_INIT_POS[1], + BASELINE_ROBOT_INIT_POS[2] + SCENE_Z_OFFSET, +) BASELINE_TABLE_INIT_POS = ( 0.00014585733079742588, 0.00023304896730074557, @@ -174,16 +182,22 @@ def transform_baseline_pose( mesh_frame_correction_euler_deg=PAN_MESH_FRAME_CORRECTION_EULER_DEG, world_yaw_correction_deg=PAN_WORLD_YAW_CORRECTION_DEG, ) -PAN_INIT_POS = (PAN_INIT_POS[0], PAN_INIT_POS[1], TABLE_TOP_Z + 0.001) -BREAD_GRASP_Z_CLEARANCE = 0.018 +# The bread is about 37 mm thick at the selected mesh scale. +# Seat both objects clear of the surface before Newton builds its contacts. +BREAD_INIT_POS = (BREAD_INIT_POS[0], BREAD_INIT_POS[1], TABLE_TOP_Z + 0.045) +PAN_INIT_POS = (PAN_INIT_POS[0], PAN_INIT_POS[1], TABLE_TOP_Z + 0.008) +BREAD_GRASP_Z_CLEARANCE = 0.015 PAN_GRASP_Z_CLEARANCE = 0.0 +# Use the straight outer handle rather than the rounded basin neck. +PAN_GRASP_HANDLE_TIP_SHIFT = 0.055 PAN_HANDLE_CLOSE_QPOS = 0.045 +DEXUNI_PAN_HANDLE_CLOSE_QPOS = 0.033 PAN_PICK_SAMPLE_INTERVAL = 130 PAN_PICK_HAND_INTERP_STEPS = 32 -BREAD_TARGET_HEIGHT_OFFSET = 0.1 +BREAD_TARGET_HEIGHT_OFFSET = 0.0 SUPPORT_TARGET_HEIGHT_OFFSET = 0.0 PICK_APPROACH_DISTANCE = 0.12 -PLACE_LIFT_HEIGHT = 0.10 +PLACE_LIFT_HEIGHT = 0.05 TRAJECTORY_SIM_STEPS = 8 @@ -215,6 +229,8 @@ def create_dual_robot( urdf_name=f"dual_{robot_type}_coordinated_placement", tcp_z=GRIPPER_TCP_Z, init_pos=ROBOT_INIT_POS, + newton_gripper_friction=4.0 if sim.is_newton_backend else None, + newton_mimic_stabilizer=True if sim.is_newton_backend else None, ) @@ -253,6 +269,10 @@ def create_bread(sim: SimulationManager) -> RigidObject: ), attrs=create_tutorial_rigid_body_physics( mass=0.01, + dynamic_friction=0.97, + static_friction=0.99, + angular_damping=2.0, + linear_damping=1.0, contact_offset=0.003, rest_offset=0.001, restitution=0.01, @@ -261,7 +281,7 @@ def create_bread(sim: SimulationManager) -> RigidObject: max_depenetration_velocity=10.0, newton_contact=sim.is_newton_backend, ), - body_scale=(1.75, 1.75, 1.75), + body_scale=(BREAD_MESH_SCALE,) * 3, init_pos=list(BREAD_INIT_POS), init_rot=list(BREAD_INIT_ROT), ) @@ -402,8 +422,11 @@ def run_coordinated_placement_demo( sim.prepare() settle_object(sim, bread, step=0) settle_object(sim, pan, step=0) + sim.update(step=10) bread_pose_batch = clone_local_pose_from_first_env(bread) pan_pose_batch = clone_local_pose_from_first_env(pan) + initial_bread_z = bread_pose_batch[:, 2, 3].clone() + initial_pan_z = pan_pose_batch[:, 2, 3].clone() bread.clear_dynamics() pan.clear_dynamics() bread_pose = bread_pose_batch[0].to(device=sim.device, dtype=torch.float32) @@ -423,7 +446,11 @@ def run_coordinated_placement_demo( right_open, right_close = get_hand_open_close_qpos( robot, hand_control_part="right_eef", - close_qpos=PAN_HANDLE_CLOSE_QPOS, + close_qpos=( + DEXUNI_PAN_HANDLE_CLOSE_QPOS + if sim.physics.solver_type == "dexuni" + else PAN_HANDLE_CLOSE_QPOS + ), ) left_open, left_close = get_hand_open_close_qpos( robot, @@ -484,6 +511,7 @@ def run_coordinated_placement_demo( sim.device, z_clearance=PAN_GRASP_Z_CLEARANCE, ) + pan_grasp_pose[:3, 3] += PAN_GRASP_HANDLE_TIP_SHIFT * pan_grasp_pose[:3, 1] pick_invocations = ( engine.make_invocation( "pick_up", @@ -518,15 +546,12 @@ def run_coordinated_placement_demo( f"Compile both pick-ups cost time: {time.time() - start_time:.2f} seconds" ) if len(pick_compiled.action_plans) != len(pick_invocations): - logger.log_warning("Failed to compile both pick-up trajectories.") - return + raise RuntimeError("Failed to compile both pick-up trajectories.") left_pick_result, right_pick_result = pick_compiled.action_plans if not left_pick_result.plan_success.all(): - logger.log_warning("Failed to plan left bread pick-up trajectory.") - return + raise RuntimeError("Failed to plan left bread pick-up trajectory.") if not pick_compiled.plan_success.all(): - logger.log_warning("Failed to plan right pan pick-up trajectory.") - return + raise RuntimeError("Failed to plan right pan pick-up trajectory.") left_pick_trajectory = left_pick_result.joint_trajectory right_pick_trajectory = right_pick_result.joint_trajectory if left_pick_trajectory is None or right_pick_trajectory is None: @@ -567,6 +592,11 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: on_trajectory_step=log_trajectory_execution, record=False, ) + bread_lift = bread.get_local_pose(to_matrix=True)[:, 2, 3] - initial_bread_z + if bool((bread_lift < 0.5 * left_pick_options.lift_height).any()): + raise RuntimeError( + f"Bread pickup did not lift the object: bread_lift={bread_lift.tolist()}." + ) bread.clear_dynamics() replay_trajectory( sim, @@ -583,16 +613,29 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: pan.clear_dynamics() # Reconcile the projected task state with measurements before compiling # placement. This keeps the second stage robust to pick-up execution error. - bread_pose_batch = clone_local_pose_from_first_env(bread).to( - device=sim.device, dtype=torch.float32 + bread_pose_batch = ( + bread.get_local_pose(to_matrix=True) + .clone() + .to(device=sim.device, dtype=torch.float32) ) - pan_pose_batch = clone_local_pose_from_first_env(pan).to( - device=sim.device, dtype=torch.float32 + pan_pose_batch = ( + pan.get_local_pose(to_matrix=True) + .clone() + .to(device=sim.device, dtype=torch.float32) ) bread.clear_dynamics() pan.clear_dynamics() bread_pose = bread_pose_batch[0] pan_pose = pan_pose_batch[0] + bread_lift = bread_pose_batch[:, 2, 3] - initial_bread_z + pan_lift = pan_pose_batch[:, 2, 3] - initial_pan_z + if bool((bread_lift < 0.5 * left_pick_options.lift_height).any()) or bool( + (pan_lift < 0.5 * right_pick_options.lift_height).any() + ): + raise RuntimeError( + "Both objects must be lifted before coordinated placement: " + f"bread_lift={bread_lift.tolist()}, pan_lift={pan_lift.tolist()}." + ) bread_held_state = compute_actual_held_state( robot, bread_semantics, @@ -610,8 +653,14 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: held_objects = dict(state.task.held_objects) held_objects["left_arm"] = bread_held_state held_objects["right_arm"] = pan_held_state + # Use measured arm kinematics, but retain the grasp commands. A blocked + # jaw's measured position is more open than its commanded preload; + # replaying that measurement would relax the grasp during placement. + placement_start_qpos = robot.get_qpos().clone() + placement_start_qpos[:, robot.get_joint_ids("left_eef")] = left_close + placement_start_qpos[:, robot.get_joint_ids("right_eef")] = right_close state = state.project( - qpos=robot.get_qpos().clone(), + qpos=placement_start_qpos, task=TaskState( batch_size=state.batch_size, device=state.robot.qpos.device, @@ -678,8 +727,7 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: f"{time.time() - start_time:.2f} seconds" ) if not coordinated_success.all(): - logger.log_warning("Failed to plan coordinated placement trajectory.") - return + raise RuntimeError("Failed to plan coordinated placement trajectory.") log_action_plan( robot, "coordinated_placement", @@ -720,6 +768,40 @@ def log_trajectory_execution(step_idx: int, total_steps: int) -> None: (0.0, 0.0, 1.0), ), ) + bread_final_pose = bread.get_local_pose(to_matrix=True) + pan_final_pose = pan.get_local_pose(to_matrix=True) + bread_world = ( + bread_vertices.unsqueeze(0) @ bread_final_pose[:, :3, :3].transpose(1, 2) + + bread_final_pose[:, None, :3, 3] + ) + basin_world = ( + get_pan_basin_vertices(pan_vertices).unsqueeze(0) + @ pan_final_pose[:, :3, :3].transpose(1, 2) + + pan_final_pose[:, None, :3, 3] + ) + bread_center_xy = 0.5 * ( + bread_world[:, :, :2].amin(dim=1) + bread_world[:, :, :2].amax(dim=1) + ) + bread_bottom_z = bread_world[:, :, 2].amin(dim=1) + inside_basin = ( + (bread_center_xy >= basin_world[:, :, :2].amin(dim=1) - 0.015) + & (bread_center_xy <= basin_world[:, :, :2].amax(dim=1) + 0.015) + ).all(dim=1) + raised_pan = pan_final_pose[:, 2, 3] > TABLE_TOP_Z + 0.03 + raised_bread = bread_bottom_z > TABLE_TOP_Z + 0.02 + if not bool((inside_basin & raised_pan & raised_bread).all()): + raise RuntimeError( + "Bread must be released into the raised pan: " + f"inside_basin={inside_basin.tolist()}, " + f"bread_bottom_z={bread_bottom_z.tolist()}, " + f"pan_z={pan_final_pose[:, 2, 3].tolist()}." + ) + logger.log_info( + "Measured placement: " + f"bread_bottom_z={bread_bottom_z.tolist()}, " + f"pan_z={pan_final_pose[:, 2, 3].tolist()}, " + f"inside_basin={inside_basin.tolist()}" + ) if wait_for_user: input("Press Enter to exit the simulation...") @@ -730,6 +812,22 @@ def main() -> None: sim = create_tutorial_simulation( args, arena_space=3.0, + newton_solver_options=( + { + "coupling": "two_way", + # DexSim's native two-way pick/place example uses soft contacts + # without rigid contact history and increases the proxy mass. + "coupling_options": {"mass_scale": 4.0, "proxy_relaxation": 0.5}, + "vbd_options": { + "iterations": 12, + "friction_epsilon": 1e-4, + "rigid_contact_hard": False, + "rigid_contact_history": False, + }, + } + if args.physics == "newton" + else None + ), ) robot = create_dual_robot(sim, args.robot) run_coordinated_placement_demo(args, sim, robot) diff --git a/examples/sim/atomic_action/pickup_rubiks_cube.py b/examples/sim/atomic_action/pickup_rubiks_cube.py index 9c9354853d..ac50c75146 100644 --- a/examples/sim/atomic_action/pickup_rubiks_cube.py +++ b/examples/sim/atomic_action/pickup_rubiks_cube.py @@ -181,7 +181,13 @@ def create_pick_object(sim, asset_path: str) -> Articulation: sim.prepare() lock_turn_joint(cube) sim.update(step=10) - cube.clear_dynamics() + if sim.is_newton_backend: + # DexUni resets coupled solver history for the complete world, including + # the robot and free-root cube. Restore the cube's runtime lock afterwards. + sim.reset_objects_state() + lock_turn_joint(cube) + else: + cube.clear_dynamics() return cube @@ -211,7 +217,9 @@ def main() -> None: cube = create_pick_object(sim, args.asset_path) sim.prepare() hand_open, hand_close = get_hand_open_close_qpos(robot) - initialize_pre_pick_robot_pose(robot, cube, hand_open) + initialize_pre_pick_robot_pose( + robot, cube, hand_open, clear_dynamics=not sim.is_newton_backend + ) motion_gen = create_curobo_motion_generator( robot, use_cuda_graph=args.physics != "newton", diff --git a/examples/sim/atomic_action/place.py b/examples/sim/atomic_action/place.py index 050a2c12c2..08b98e0d23 100644 --- a/examples/sim/atomic_action/place.py +++ b/examples/sim/atomic_action/place.py @@ -36,6 +36,7 @@ from embodichain.lab.sim.objects import RigidObject from embodichain.lab.sim.shapes import CubeCfg from embodichain.utils import logger +from embodichain.utils.math import matrix_from_quat, quat_from_matrix from examples.sim.atomic_action.support import ( DEFAULT_GRIPPER_CLOSE_QPOS, DEXUNI_CUBE_CLOSE_QPOS, @@ -148,6 +149,9 @@ def make_place_eef_poses(device: torch.device) -> torch.Tensor: dtype=torch.float32, device=device, ) + # The recorded orientation is rounded to four decimals. Reconstruct a + # proper rotation so analytical IK receives a rigid target transform. + rotation = matrix_from_quat(quat_from_matrix(rotation)) poses = [] for position in ((-0.40, 0.48, 0.20), (-0.40, 0.48, 0.10)): pose = torch.eye(4, dtype=torch.float32, device=device) diff --git a/examples/sim/atomic_action/scenario.py b/examples/sim/atomic_action/scenario.py index 8781018cde..55cdd079a8 100644 --- a/examples/sim/atomic_action/scenario.py +++ b/examples/sim/atomic_action/scenario.py @@ -268,6 +268,7 @@ def add_dual_tutorial_robot( hand_damping: float = 1e2, hand_max_effort: float = 1e4, newton_gripper_friction: float | None = None, + newton_mimic_stabilizer: bool | None = None, ) -> Robot: """Add a supported dual-arm tutorial robot to a simulation. @@ -293,6 +294,7 @@ def add_dual_tutorial_robot( hand_damping: Hand joint drive damping. hand_max_effort: Hand joint maximum effort. newton_gripper_friction: Optional sliding friction for gripper links. + newton_mimic_stabilizer: Optional Newton mimic-follower stabilization. Returns: The added dual-arm robot instance. @@ -315,6 +317,10 @@ def add_dual_tutorial_robot( hand_max_effort=hand_max_effort, ) configure_newton_gripper_contacts(sim, robot_cfg, friction=newton_gripper_friction) + if newton_mimic_stabilizer is not None: + if not sim.is_newton_backend: + raise ValueError("Newton mimic stabilization requires the Newton backend.") + robot_cfg.root_props.newton_mimic_stabilizer = newton_mimic_stabilizer return sim.add_robot(cfg=robot_cfg) diff --git a/examples/sim/atomic_action/support.py b/examples/sim/atomic_action/support.py index 7d2456fb55..542673d660 100644 --- a/examples/sim/atomic_action/support.py +++ b/examples/sim/atomic_action/support.py @@ -2056,6 +2056,7 @@ def initialize_pre_pick_robot_pose( hand_open: torch.Tensor, *, height: float = 0.36, + clear_dynamics: bool = True, ) -> None: """Set a tutorial robot at a deterministic open-gripper pose above an object. @@ -2064,6 +2065,8 @@ def initialize_pre_pick_robot_pose( obj: Object below the pre-pick TCP pose. hand_open: Open gripper joint positions. height: World-Z coordinate for the pre-pick TCP pose. + clear_dynamics: Clear robot dynamics after initialization. Disable only + after the host has cleared the complete Newton articulation world. Raises: RuntimeError: If the pre-pick pose is unreachable. @@ -2082,7 +2085,8 @@ def initialize_pre_pick_robot_pose( for target in (False, True): robot.set_qpos(arm_qpos, name="arm", target=target) robot.set_qpos(hand_qpos, name="eef", target=target) - robot.clear_dynamics() + if clear_dynamics: + robot.clear_dynamics() def prepare_tutorial_scene( diff --git a/examples/sim/demo/pick_up_cloth.py b/examples/sim/demo/pick_up_cloth.py index 0b86588f00..8699ff8bb7 100644 --- a/examples/sim/demo/pick_up_cloth.py +++ b/examples/sim/demo/pick_up_cloth.py @@ -497,11 +497,13 @@ def main(args: argparse.Namespace | None = None) -> None: if not args.headless: sim.open_window() sim.update(step=settle_steps) - input("Press Enter to start grabbing the cloth...") + if not args.headless: + input("Press Enter to start grabbing the cloth...") # The initial trajectory row is already active after settling. sim.update(step=grab_traj.shape[1] - 1) - input("Press Enter to exit the simulation...") + if not args.headless: + input("Press Enter to exit the simulation...") finally: sim.destroy() diff --git a/examples/sim/demo/scoop_ice.py b/examples/sim/demo/scoop_ice.py index 42fa4be4c0..6208c809b4 100644 --- a/examples/sim/demo/scoop_ice.py +++ b/examples/sim/demo/scoop_ice.py @@ -357,16 +357,19 @@ def create_ice_cubes(sim: SimulationManager): # Set visual material for ice cubes. # Explicit optical parameters select ice scattering in OfflineRT. - ice_mat = sim.create_visual_material( - cfg=VisualMaterialCfg( - base_color=[1.0, 1.0, 1.0, 1.0], - ior=1.31, - roughness=0.2, - parameters={"transmission": 1.0}, + ice_mat = None + if sim.has_native_renderer: + ice_mat = sim.create_visual_material( + cfg=VisualMaterialCfg( + base_color=[1.0, 1.0, 1.0, 1.0], + ior=1.31, + roughness=0.2, + parameters={"transmission": 1.0}, + ) ) - ) sim.prepare() - ice_cubes.set_visual_material(mat=ice_mat) + if ice_mat is not None: + ice_cubes.set_visual_material(mat=ice_mat) return ice_cubes diff --git a/examples/sim/motion/solvers/fep_solver.py b/examples/sim/motion/solvers/fep_solver.py index 7fc928f046..f838dad28f 100644 --- a/examples/sim/motion/solvers/fep_solver.py +++ b/examples/sim/motion/solvers/fep_solver.py @@ -128,7 +128,7 @@ def main() -> None: actual_color = (255, 140, 30) actual_rgba = np.array((*actual_color, 255), dtype=np.float32) / 255 actual_cloud = None - if not args.viser: + if not args.viser and sim.has_native_renderer: points = circle_world.reshape(-1, 3) sim.visualize_point_cloud( points, diff --git a/extensions/embodichain_benchmarks/motion_generation/scenarios/_placement_geometry.py b/extensions/embodichain_benchmarks/motion_generation/scenarios/_placement_geometry.py index 7aa20a62f4..9d7193faec 100644 --- a/extensions/embodichain_benchmarks/motion_generation/scenarios/_placement_geometry.py +++ b/extensions/embodichain_benchmarks/motion_generation/scenarios/_placement_geometry.py @@ -21,6 +21,7 @@ import torch from embodichain.utils import logger +from embodichain.utils.math import matrix_from_quat, quat_from_matrix from .._atomic_fixtures import ( compute_world_bounds, normalize_vector, @@ -41,10 +42,11 @@ TABLE_TOP_Z = 0.65 PAN_TARGET_CENTER_XY = (-0.06, 0.0) PAN_TARGET_Z_LIFT = 0.06 -BREAD_PLACE_TARGET_OFFSET_XY = (-0.06, -0.16) +BREAD_PLACE_TARGET_OFFSET_XY = (0.0, 0.0) BREAD_ON_PAN_CLEARANCE = 0.006 -PAN_HANDLE_LOCAL_Z_MIN = 0.04 -PAN_BASIN_LOCAL_Z_MAX = 0.04 +# The authored Z-up pan has its basin at negative Y and handle at positive Y. +PAN_HANDLE_LOCAL_Y_MIN = 0.06 +PAN_BASIN_LOCAL_Y_MAX = 0.04 PAN_HANDLE_ROOT_OFFSET = 0.035 BREAD_TARGET_WORLD_YAW_DEG = 0.0 @@ -56,9 +58,9 @@ def get_pan_basin_vertices(pan_vertices: torch.Tensor) -> torch.Tensor: pan_vertices: Local pan mesh vertices with shape ``(N, 3)``. Returns: - Vertices whose local z is within the configured basin height. + Vertices on the basin side of the handle attachment. """ - basin_vertices = pan_vertices[pan_vertices[:, 2] <= PAN_BASIN_LOCAL_Z_MAX] + basin_vertices = pan_vertices[pan_vertices[:, 1] <= PAN_BASIN_LOCAL_Y_MAX] if basin_vertices.numel() == 0: logger.log_warning("Pan basin vertex mask is empty; falling back to full mesh.") return pan_vertices @@ -87,6 +89,7 @@ def build_top_down_tcp_pose( dtype=torch.float32, device=device, ) + pose[:3, :3] = matrix_from_quat(quat_from_matrix(pose[:3, :3])) pose[:3, 3] = position return pose @@ -143,7 +146,7 @@ def build_pan_handle_grasp_pose( Returns: A world TCP transform with shape ``(4, 4)``. """ - handle_vertices = pan_vertices[pan_vertices[:, 2] > PAN_HANDLE_LOCAL_Z_MIN] + handle_vertices = pan_vertices[pan_vertices[:, 1] >= PAN_HANDLE_LOCAL_Y_MIN] if handle_vertices.numel() == 0: logger.log_warning( "Pan handle vertex mask is empty; falling back to full mesh." diff --git a/tests/benchmark/motion_generation/test_placement_geometry.py b/tests/benchmark/motion_generation/test_placement_geometry.py index ba4d9986dd..676994bb55 100644 --- a/tests/benchmark/motion_generation/test_placement_geometry.py +++ b/tests/benchmark/motion_generation/test_placement_geometry.py @@ -48,31 +48,36 @@ def test_flat_grasp_uses_transformed_center_and_world_top() -> None: ) torch.testing.assert_close(grasp[:3, 3], torch.tensor([1.0, 2.0, 3.038])) torch.testing.assert_close(pose[:3, 3], torch.tensor([1.0, 2.0, 3.0])) + torch.testing.assert_close( + grasp[:3, :3].T @ grasp[:3, :3], torch.eye(3), rtol=0.0, atol=1e-6 + ) def test_handle_grasp_points_down_and_moves_toward_handle_root() -> None: pan = torch.tensor( - [[-0.1, -0.1, 0.0], [0.1, 0.1, 0.02], [0.0, 0.3, 0.06], [0.0, 0.5, 0.08]] + [[-0.1, -0.1, 0.0], [0.1, 0.03, 0.048], [-0.01, 0.1, 0.04], [0.01, 0.2, 0.05]] ) grasp = build_pan_handle_grasp_pose(torch.eye(4), pan, _CPU) - torch.testing.assert_close(grasp[:3, 3], torch.tensor([0.0, 0.365, 0.086])) + torch.testing.assert_close(grasp[:3, 3], torch.tensor([0.0, 0.115, 0.056])) torch.testing.assert_close(grasp[:3, 2], torch.tensor([0.0, 0.0, -1.0])) torch.testing.assert_close(grasp[:3, :3].T @ grasp[:3, :3], torch.eye(3)) assert torch.linalg.det(grasp[:3, :3]).item() == pytest.approx(1.0) -def test_placing_target_clears_basin_and_ignores_tall_handle() -> None: +def test_placing_target_clears_basin_and_ignores_handle() -> None: bread = torch.tensor([[-0.1, -0.05, 0.0], [0.1, 0.05, 0.02]]) - pan = torch.tensor([[-0.1, -0.1, 0.0], [0.1, 0.1, 0.02], [0.0, 0.5, 0.08]]) + pan = torch.tensor( + [[-0.1, -0.1, 0.0], [0.1, 0.03, 0.048], [0.0, 0.5, 0.08], [0.0, 0.55, 0.01]] + ) initial = torch.eye(4) support = build_support_object_target_pose(initial, _CPU) placing = build_placing_object_target_pose(initial, bread, pan, support, _CPU) - # The basin top is 0.731 m; the bread bottom retains 6 mm clearance. + # The basin top is 0.759 m; the bread bottom retains 6 mm clearance. torch.testing.assert_close(support[:3, 3], torch.tensor([-0.06, 0.0, 0.711])) - torch.testing.assert_close(placing[:3, 3], torch.tensor([-0.12, -0.16, 0.737])) + torch.testing.assert_close(placing[:3, 3], torch.tensor([-0.06, -0.035, 0.765])) torch.testing.assert_close(initial, torch.eye(4)) def test_empty_basin_selection_preserves_full_mesh() -> None: - pan = torch.tensor([[0.0, 0.0, 0.1], [0.0, 0.2, 0.2]]) + pan = torch.tensor([[0.0, 0.1, 0.1], [0.0, 0.2, 0.2]]) torch.testing.assert_close(get_pan_basin_vertices(pan), pan) diff --git a/tests/examples/atomic_action/test_support.py b/tests/examples/atomic_action/test_support.py index 423947e0d1..f11cb8776c 100644 --- a/tests/examples/atomic_action/test_support.py +++ b/tests/examples/atomic_action/test_support.py @@ -52,6 +52,7 @@ compute_left_to_right_arm_direction, ) from examples.sim.atomic_action.scenario import ( + add_dual_tutorial_robot, create_dual_tutorial_robot_cfg, ) from examples.sim.atomic_action.support import ( @@ -535,6 +536,67 @@ def test_franka_tutorial_config_uses_ur5_gripper_component() -> None: assert "fr3_finger_joint[1-2]" not in franka_values +def test_place_waypoints_are_rigid_transforms() -> None: + from examples.sim.atomic_action.place import make_place_eef_poses + + poses = make_place_eef_poses(torch.device("cpu")) + rotation = poses[:, :3, :3] + torch.testing.assert_close( + rotation.transpose(-1, -2) @ rotation, + torch.eye(3).expand(len(poses), 3, 3), + rtol=0.0, + atol=1e-6, + ) + torch.testing.assert_close( + torch.linalg.det(rotation), torch.ones(len(poses)), rtol=0.0, atol=1e-6 + ) + + +def test_coordinated_placement_preserves_baseline_scene_frames() -> None: + from examples.sim.atomic_action import coordinated_placement as module + from embodichain.utils.math import matrix_from_euler + + for angles in (module.TABLE_INIT_ROT, module.PAN_INIT_ROT): + rotation = matrix_from_euler(torch.tensor(angles).deg2rad(), "XYZ") + assert float(rotation[2, 2]) > 0.999 + assert module.TABLE_TOP_Z - module.ROBOT_INIT_POS[2] == pytest.approx( + module.BASELINE_TABLE_TOP_Z - module.BASELINE_ROBOT_INIT_POS[2] + ) + + +@pytest.mark.parametrize("is_newton_backend", [False, True]) +def test_rubiks_cube_initialization_clears_supported_scope_and_keeps_lock( + monkeypatch: pytest.MonkeyPatch, is_newton_backend: bool +) -> None: + from examples.sim.atomic_action import pickup_rubiks_cube as module + + cleared = [] + cube = SimpleNamespace(locked=False, clear_dynamics=lambda: cleared.append("cube")) + + def reset_world(): + cleared.append("world") + cube.locked = False + + def update(*, step): + assert cube.locked + + sim = SimpleNamespace( + is_newton_backend=is_newton_backend, + add_articulation=lambda **kwargs: cube, + prepare=lambda: None, + update=update, + reset_objects_state=reset_world, + ) + monkeypatch.setattr(module.Path, "is_file", lambda _: True) + monkeypatch.setattr( + module, "lock_turn_joint", lambda obj: setattr(obj, "locked", True) + ) + + assert module.create_pick_object(sim, "/tmp/cube.usdc") is cube + assert cube.locked + assert cleared == (["world"] if is_newton_backend else ["cube"]) + + def test_ur5_and_franka_configs_share_place_binding_contract() -> None: configs = ( create_ur5_gripper_robot_cfg(), @@ -668,6 +730,19 @@ def test_dual_tutorial_configs_share_hand_binding_contract( assert dual_cfg.init_qpos[-12:] == [0.0] * 12 +@pytest.mark.parametrize("stabilizer", [None, True]) +def test_dual_scene_mimic_stabilization_is_explicit(stabilizer: bool | None) -> None: + sim = SimpleNamespace(is_newton_backend=True, add_robot=lambda *, cfg: cfg) + cfg = add_dual_tutorial_robot( + sim, + robot_type="ur5", + uid="test_dual_mimic", + urdf_name="test_dual_mimic", + newton_mimic_stabilizer=stabilizer, + ) + assert cfg.root_props.newton_mimic_stabilizer is (stabilizer is True) + + @pytest.mark.parametrize("robot_type", ("ur5", "ur10")) def test_dual_pytorch_ur_tutorial_solvers_preserve_runtime_joint_limits( robot_type: str, diff --git a/tests/sim/test_scoop_ice_demo.py b/tests/sim/test_scoop_ice_demo.py index 12495f7702..7186ea27b2 100644 --- a/tests/sim/test_scoop_ice_demo.py +++ b/tests/sim/test_scoop_ice_demo.py @@ -154,15 +154,26 @@ def test_default_scoops_and_retains_ice(seed: int) -> None: demo.SimulationManager.flush_cleanup_queue() -def test_ice_spawn_configs_are_separated_before_prepare(tmp_path, monkeypatch) -> None: +@pytest.mark.parametrize("has_native_renderer", [False, True]) +def test_ice_spawn_configs_are_separated_before_prepare( + tmp_path, monkeypatch, has_native_renderer: bool +) -> None: """Newton evaluates initial contacts before runtime randomization can run.""" (tmp_path / "ice.obj").touch() monkeypatch.setattr(demo, "get_data_path", lambda name: str(tmp_path)) declared = [] + material_calls = [] def add_group(*, cfg): declared.extend(cfg.rigid_objects.values()) - return SimpleNamespace(set_visual_material=lambda **kwargs: None) + return SimpleNamespace( + set_visual_material=lambda **kwargs: material_calls.append("assign") + ) + + def create_visual_material(**kwargs): + assert has_native_renderer + material_calls.append("create") + return object() def prepare(): positions = np.array( @@ -180,11 +191,13 @@ def prepare(): assert separations.min() >= 0.030 sim = SimpleNamespace( + has_native_renderer=has_native_renderer, add_rigid_object_group=add_group, - create_visual_material=lambda **kwargs: object(), + create_visual_material=create_visual_material, prepare=prepare, ) demo.create_ice_cubes(sim) + assert material_calls == (["create", "assign"] if has_native_renderer else []) def test_newton_uses_cg_for_the_large_free_body_pile(monkeypatch) -> None: From 4d72509c9e90c6e47d5c9a615cc6470e251defdf Mon Sep 17 00:00:00 2001 From: yuecideng Date: Tue, 27 Oct 2026 23:15:44 +0800 Subject: [PATCH 18/27] fix(examples): preserve placement asset frames across renderers --- .../topics/atomic-actions/atomic-actions.md | 2 + .../atomic_action/coordinated_placement.py | 40 ++++++++++++++++--- tests/examples/atomic_action/test_support.py | 35 ++++++++++++++++ 3 files changed, 72 insertions(+), 5 deletions(-) diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index dfa6872e78..bfa4fec0c1 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -137,6 +137,8 @@ measured arm/held-object state. A contact-blocked jaw measurement is not its holding command. Its mesh fixtures use the authored Z-up frame; shared pan geometry selects the handle and basin along local Y. Keep these asset-specific conventions in the scene/geometry owners. +The host caches textured OBJ geometry so native renderers and NoRender load the +same baked asset frame. Keep the base-to-table height invariant when raising the fixture. Read held poses without resetting them at the replan boundary. The launcher preserves an omitted device through simulation configuration so the diff --git a/examples/sim/atomic_action/coordinated_placement.py b/examples/sim/atomic_action/coordinated_placement.py index 1e9ffa3e4e..be4e628e31 100644 --- a/examples/sim/atomic_action/coordinated_placement.py +++ b/examples/sim/atomic_action/coordinated_placement.py @@ -24,7 +24,9 @@ from __future__ import annotations import argparse +import hashlib import sys +import tempfile import time from pathlib import Path @@ -34,8 +36,10 @@ import numpy as np import torch +import trimesh from scipy.spatial.transform import Rotation as SciRotation +from embodichain.data.constants import EMBODICHAIN_DEFAULT_DATA_ROOT from embodichain.lab.sim import SimulationManager from embodichain.lab.sim.atomic_actions import ( ControlPartCommandProfile, @@ -88,8 +92,8 @@ run_tutorial, ) -# Current DexSim retains the authored Z-up GLB geometry. Preserve the gym -# table/bread orientation; the pan needs a half turn to keep its opening up. +# Bake the GLB geometry into the authored Z-up frame before native loading. +# Preserve the gym table/bread orientation; turn the pan's opening upward. DEFAULT_MESH_FRAME_CORRECTION_EULER_DEG = (0.0, 0.0, 0.0) PAN_MESH_FRAME_CORRECTION_EULER_DEG = (180.0, 0.0, 0.0) PAN_WORLD_YAW_CORRECTION_DEG = 270.0 @@ -234,12 +238,38 @@ def create_dual_robot( ) +def _placement_mesh_path(data_path: str) -> str: + """Cache a textured OBJ with the same authored Z-up frame in every renderer.""" + source = Path(resolve_cached_data_path(data_path)) + digest = hashlib.sha256(source.read_bytes()).hexdigest()[:20] + cache_dir = ( + Path(EMBODICHAIN_DEFAULT_DATA_ROOT) + / "derived" + / "coordinated_placement_z_up_v1" + / digest + ) + target = cache_dir / "mesh.obj" + if not target.is_file(): + cache_dir.parent.mkdir(parents=True, exist_ok=True) + # Publish the OBJ, MTL and textures together so concurrent examples + # cannot observe a partially exported asset. + with tempfile.TemporaryDirectory(dir=cache_dir.parent) as staging: + mesh = trimesh.load(str(source), force="mesh", process=False) + mesh.export(Path(staging) / target.name) + try: + Path(staging).rename(cache_dir) + except OSError: + if not target.is_file(): + raise + return str(target) + + def create_table(sim: SimulationManager) -> RigidObject: """Create the table mesh from the bread-pan gym project.""" return sim.add_rigid_object( cfg=RigidObjectCfg( uid="table", - shape=MeshCfg(fpath=resolve_cached_data_path(TABLE_MESH_PATH)), + shape=MeshCfg(fpath=_placement_mesh_path(TABLE_MESH_PATH)), attrs=create_tutorial_rigid_body_physics( mass=10.0, dynamic_friction=0.9, @@ -260,7 +290,7 @@ def create_bread(sim: SimulationManager) -> RigidObject: cfg=RigidObjectCfg( uid="bread", shape=MeshCfg( - fpath=resolve_cached_data_path(BREAD_MESH_PATH), + fpath=_placement_mesh_path(BREAD_MESH_PATH), compute_uv=False, collision=MeshCollisionCfg( approximation="convex_decomposition", @@ -294,7 +324,7 @@ def create_pan(sim: SimulationManager) -> RigidObject: cfg=RigidObjectCfg( uid="pan", shape=MeshCfg( - fpath=resolve_cached_data_path(PAN_MESH_PATH), + fpath=_placement_mesh_path(PAN_MESH_PATH), compute_uv=False, collision=MeshCollisionCfg( approximation="convex_decomposition", diff --git a/tests/examples/atomic_action/test_support.py b/tests/examples/atomic_action/test_support.py index f11cb8776c..f715959c62 100644 --- a/tests/examples/atomic_action/test_support.py +++ b/tests/examples/atomic_action/test_support.py @@ -24,6 +24,7 @@ import re import xml.etree.ElementTree as ET from argparse import Namespace +from pathlib import Path from types import SimpleNamespace from unittest.mock import MagicMock, call, patch @@ -564,6 +565,40 @@ def test_coordinated_placement_preserves_baseline_scene_frames() -> None: ) +def test_placement_mesh_cache_preserves_authored_axes_and_texture( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + import trimesh + from PIL import Image + from examples.sim.atomic_action import coordinated_placement as module + + mesh = trimesh.creation.box(extents=(0.1, 0.2, 0.3)) + mesh.visual = trimesh.visual.TextureVisuals( + uv=torch.zeros((len(mesh.vertices), 2)).numpy(), + image=Image.new("RGB", (2, 2), (210, 70, 15)), + ) + source = tmp_path / "asset.glb" + mesh.export(source) + monkeypatch.setattr(module, "resolve_cached_data_path", lambda _: str(source)) + monkeypatch.setattr( + module, "EMBODICHAIN_DEFAULT_DATA_ROOT", str(tmp_path / "cache") + ) + + path = Path(module._placement_mesh_path("asset.glb")) + loaded = trimesh.load(path, force="mesh", process=False) + torch.testing.assert_close( + torch.as_tensor(loaded.bounds.copy()), + torch.tensor([[-0.05, -0.1, -0.15], [0.05, 0.1, 0.15]], dtype=torch.float64), + atol=1e-7, + rtol=0, + ) + assert loaded.visual.material.image.getpixel((0, 0))[:3] == (210, 70, 15) + loader = MagicMock(side_effect=AssertionError("Cached mesh must not be reloaded")) + monkeypatch.setattr(module.trimesh, "load", loader) + assert module._placement_mesh_path("asset.glb") == str(path) + loader.assert_not_called() + + @pytest.mark.parametrize("is_newton_backend", [False, True]) def test_rubiks_cube_initialization_clears_supported_scope_and_keeps_lock( monkeypatch: pytest.MonkeyPatch, is_newton_backend: bool From 731427ff63b97c142b9c021fddb9c956db8595a5 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 7 Nov 2026 14:23:25 +0800 Subject: [PATCH 19/27] refactor(sim): narrow Newton articulation root options Keep gravity compensation and opt-in mimic stabilization as the supported Newton extensions. Remove seven unused root fields, generic SDK option forwarding and articulation-wide SDF rewriting. Leave native creation/geometry choices at source or SDK defaults and update the owning context. Reuse existing configuration, source overlay and mimic coverage; reject withdrawn fields at both public decode boundaries and roundtrip non-default values for retained fields. Remove obsolete capability-forwarding cases. Validation on conda open / DexSim 0.6: 724 CPU tests passed, 7 deselected; 2 native two-way coupling tests passed with and without CUDA Graphs. Black 26.10.1: 2215 files unchanged; API docs 4286/4286; context checks/tests and final wheel/source metadata validation passed. --- .../articulation-adapters.md | 9 -- embodichain/lab/sim/cfg/articulation.py | 79 ----------- embodichain/lab/sim/spawn/descriptors.py | 88 ++---------- tests/sim/spawn/test_descriptors.py | 131 ------------------ tests/sim/test_cfg.py | 41 ++++-- 5 files changed, 34 insertions(+), 314 deletions(-) diff --git a/agent_context/topics/simulation-system/articulation-adapters.md b/agent_context/topics/simulation-system/articulation-adapters.md index 4b3c3a014c..19357565c4 100644 --- a/agent_context/topics/simulation-system/articulation-adapters.md +++ b/agent_context/topics/simulation-system/articulation-adapters.md @@ -30,15 +30,6 @@ Applying them only during facade binding is too late for captured GPU settings. both asset physics modes; omission preserves the source/default policy. It compensates articulation link weight without removing payload gravity or contact reactions. DexSim checks the resolved solver's MuJoCo support. -The other Newton creation options on `root_props` (`newton_kinematic`, -`newton_sleep_policy`, `newton_shape_collision[_links]`, -`newton_particle_collision_links`, `newton_mesh_approximation`, -`newton_force_sdf`) follow the same -rule: `None` keeps the DexSim default, explicit values require Newton. -The SDF request retains imported concave collision meshes. The compiler carries -it onto per-link entries after source configuration because those entries take -precedence over articulation-wide defaults, including attribute-only entries -inserted for native URDF geometry. Contact participation remains separate. URDF mimic joints on Newton keep the configured follower drives and the native MuJoCo equality by default, matching a plain DexSim scene; reset starts followers on their mimic relation. `root_props.newton_mimic_stabilizer=True` diff --git a/embodichain/lab/sim/cfg/articulation.py b/embodichain/lab/sim/cfg/articulation.py index 5020331f82..19cba5b716 100644 --- a/embodichain/lab/sim/cfg/articulation.py +++ b/embodichain/lab/sim/cfg/articulation.py @@ -114,43 +114,6 @@ class ArticulationRootPropertiesCfg: Newton solver must support MuJoCo gravity compensation. """ - newton_kinematic: bool | None = None - """Whether every Newton link body is kinematic, on Newton only. - - ``True`` keeps authored mass and inertia but stops Newton from integrating - the links; prescribed motion then needs a kinematic controller such as - :meth:`SimulationManager.register_kinematic_joint_trajectory`. ``None`` - preserves the DexSim default (dynamic links). - """ - - newton_sleep_policy: Literal["auto", "never", "allowed", "init"] | None = None - """MuJoCo sleeping policy for every movable tree, on Newton only. - - ``None`` preserves the DexSim default (``"auto"``). - """ - - newton_shape_collision: bool | None = None - """Whether link collision shapes take part in rigid shape-shape contact. - - Particle contact is configured separately by - :attr:`newton_particle_collision_links`. ``None`` preserves the DexSim - default (enabled). - """ - - newton_shape_collision_links: Sequence[str] | None = None - """Exact link names allowed in rigid shape-shape contact, on Newton only. - - When set, this list takes precedence over :attr:`newton_shape_collision`. - ``None`` keeps every link eligible. - """ - - newton_particle_collision_links: Sequence[str] | None = None - """Exact link names allowed to collide with particles, on Newton only. - - Shapes on other links still render but ignore cloth and soft bodies. - ``None`` keeps every link eligible. - """ - newton_mimic_stabilizer: bool = False """Whether URDF mimic joints are tuned toward Default-backend behavior. @@ -165,24 +128,6 @@ class ArticulationRootPropertiesCfg: retuned. """ - newton_mesh_approximation: Mapping[str, Any] | None = None - """Newton collision-mesh preprocessing applied before finalization. - - Keys follow DexSim ``NewtonMeshApproximationDesc``: ``method``, - ``link_names``, ``raise_on_failure``, ``keep_visual_shapes`` and - ``options``. ``None`` uses the source collision meshes unchanged. - """ - - newton_force_sdf: bool | None = None - """Keep imported collision meshes as concave Newton SDF boundaries. - - This pre-build request lowers to ``NewtonCollisionDesc.force_sdf`` on - the articulation source. It avoids converting imported collision meshes - to convex hulls. ``None`` preserves source/default behavior; primitives - retain their analytic geometry. It is independent of the rigid and - particle collision participation flags. - """ - def __post_init__(self) -> None: """Require the two values consumed by the atomic Default setter.""" if (self.min_position_iters is None) != (self.min_velocity_iters is None): @@ -190,30 +135,6 @@ def __post_init__(self) -> None: "Articulation-root min_position_iters and min_velocity_iters " "must be configured together." ) - for name in ( - "newton_shape_collision_links", - "newton_particle_collision_links", - ): - value = getattr(self, name) - if value is None: - continue - if isinstance(value, str): - raise TypeError(f"{name} must be a sequence of link names.") - setattr(self, name, tuple(str(item) for item in value)) - if self.newton_sleep_policy is not None and self.newton_sleep_policy not in ( - "auto", - "never", - "allowed", - "init", - ): - raise ValueError( - "newton_sleep_policy must be 'auto', 'never', 'allowed' or " - f"'init'; got {self.newton_sleep_policy!r}." - ) - if self.newton_mesh_approximation is not None and not isinstance( - self.newton_mesh_approximation, Mapping - ): - raise TypeError("newton_mesh_approximation must be a mapping.") @classmethod def from_dict( diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index fd688144c9..7a79643e65 100644 --- a/embodichain/lab/sim/spawn/descriptors.py +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -812,7 +812,10 @@ def articulation_desc_from_cfg( newton_solver_type=newton_solver_type, ) fixed_base, self_collision_enabled = _articulation_root_values(cfg) - newton_options = _articulation_newton_options(cfg, newton_solver_type) + compensation = _articulation_gravity_compensation(cfg, newton_solver_type) + gravity_options = ( + {} if compensation is None else {"newton_gravity_compensation": compensation} + ) return ArticulationDesc( name=_articulation_uid(cfg.uid, str(path)), pose=_pose_from_cfg(cfg), @@ -827,7 +830,7 @@ def articulation_desc_from_cfg( urdf_read_inertia=True, per_env=per_env, body_scale=_vector3(cfg.body_scale, field_name="body_scale"), - **newton_options, + **gravity_options, ) @@ -853,61 +856,6 @@ def _articulation_gravity_compensation( return float(value) -_ARTICULATION_NEWTON_CREATION_FIELDS = ( - "newton_kinematic", - "newton_sleep_policy", - "newton_shape_collision", - "newton_shape_collision_links", - "newton_particle_collision_links", - "newton_mesh_approximation", - "newton_force_sdf", -) - - -def _articulation_newton_options( - cfg: ArticulationCfg, newton_solver_type: str | None -) -> dict[str, Any]: - """Collect explicit Newton articulation creation options from root props. - - Unset (``None``) fields are omitted so DexSim keeps its own defaults. - """ - options: dict[str, Any] = {} - compensation = _articulation_gravity_compensation(cfg, newton_solver_type) - if compensation is not None: - options["newton_gravity_compensation"] = compensation - root = cfg.root_props - for name in _ARTICULATION_NEWTON_CREATION_FIELDS: - value = getattr(root, name) - if value is None: - continue - if newton_solver_type is None: - raise ValueError(f"{name} requires the Newton backend.") - if name == "newton_force_sdf": - if not isinstance(value, (bool, np.bool_)): - raise TypeError("newton_force_sdf must be a boolean when configured.") - options["newton_collision"] = NewtonCollisionDesc( - **{ - item.name: bool(value) if item.name == "force_sdf" else None - for item in fields(NewtonCollisionDesc) - } - ) - continue - if name == "newton_sleep_policy": - from dexsim.spawn import NewtonSleepPolicy - - value = NewtonSleepPolicy(value) - elif name == "newton_mesh_approximation": - from dexsim.spawn import NewtonMeshApproximationDesc - - value = NewtonMeshApproximationDesc(**dict(value)) - elif name in ("newton_kinematic", "newton_shape_collision"): - value = bool(value) - else: - value = tuple(value) - options[name] = value - return options - - def _validate_articulation_rigid_physics( cfg: ArticulationCfg, *, @@ -1004,8 +952,9 @@ def configure_articulation_desc( f"Articulation source {desc.name!r} must be resolved before " "configuration." ) - for name, value in _articulation_newton_options(cfg, newton_solver_type).items(): - setattr(desc, name, value) + compensation = _articulation_gravity_compensation(cfg, newton_solver_type) + if compensation is not None: + desc.newton_gravity_compensation = compensation preserve_source_physics = cfg.resolve_asset_physics_mode() == "preserve" setattr(desc, "_embodichain_preserve_source_physics", preserve_source_physics) for link in desc.links: @@ -1026,7 +975,6 @@ def configure_articulation_desc( stacklevel=2, ) _apply_articulation_newton_gap_default(desc, newton_solver_type) - _apply_articulation_sdf_request(desc, cfg.root_props.newton_force_sdf) return desc default_physics = _resolve_rigid_physics( cfg.attrs, @@ -1177,29 +1125,9 @@ def configure_articulation_desc( "f32785317 or newer." ) _apply_articulation_newton_gap_default(desc, newton_solver_type) - _apply_articulation_sdf_request(desc, cfg.root_props.newton_force_sdf) return desc -def _apply_articulation_sdf_request( - desc: ArticulationDesc, force_sdf: bool | None -) -> None: - """Carry SDF intent onto resolved or newly inserted URDF attribute entries.""" - if force_sdf is None: - return - # Per-link entries override articulation-wide defaults in the native - # adapter. URDF resolution retains geometry in its native skeleton and - # inserts these attribute-only entries during configuration. - for link in desc.links: - for collision in link.collisions: - source = collision.newton - if source is None: - source = NewtonCollisionDesc( - **{item.name: None for item in fields(NewtonCollisionDesc)} - ) - collision.newton = replace(source, force_sdf=bool(force_sdf)) - - def _apply_newton_gap_default( collisions: Sequence[CollisionDesc], newton_solver_type: str | None, diff --git a/tests/sim/spawn/test_descriptors.py b/tests/sim/spawn/test_descriptors.py index 4bdf5d73d5..920dd96fd4 100644 --- a/tests/sim/spawn/test_descriptors.py +++ b/tests/sim/spawn/test_descriptors.py @@ -1225,137 +1225,6 @@ def test_usd_articulation_forwards_gravity_compensation(monkeypatch, mode) -> No assert descriptor.newton_gravity_compensation == 0.5 -_NEWTON_CREATION_ROOT_PROPS = { - "newton_kinematic": True, - "newton_sleep_policy": "never", - "newton_shape_collision": False, - "newton_shape_collision_links": ["base_link"], - "newton_particle_collision_links": ["base_link", "tool"], - "newton_mesh_approximation": {"method": "convex_hull", "link_names": ["tool"]}, -} - - -@pytest.mark.parametrize("mode", ["preserve", "overlay"]) -def test_articulation_newton_creation_options_are_forwarded(mode) -> None: - from dexsim.spawn import NewtonMeshApproximationDesc, NewtonSleepPolicy - - cfg = ArticulationCfg.from_dict( - { - "uid": "robot", - "fpath": "robot.urdf", - "asset_physics_mode": mode, - "root_props": _NEWTON_CREATION_ROOT_PROPS, - } - ) - descriptor = articulation_desc_from_cfg(cfg, newton_solver_type="dexuni") - resolved = _resolved_articulation_desc() - configure_articulation_desc(resolved, cfg, newton_solver_type="dexuni") - - for desc in (descriptor, resolved): - assert desc.newton_kinematic is True - assert desc.newton_sleep_policy is NewtonSleepPolicy.NEVER - assert desc.newton_shape_collision is False - assert desc.newton_shape_collision_links == ("base_link",) - assert desc.newton_particle_collision_links == ("base_link", "tool") - assert isinstance(desc.newton_mesh_approximation, NewtonMeshApproximationDesc) - assert desc.newton_mesh_approximation.link_names == ("tool",) - - -@pytest.mark.parametrize("mode", ["preserve", "overlay"]) -@pytest.mark.parametrize("native_geometry", [False, True]) -@pytest.mark.parametrize("value", [None, False, True]) -def test_articulation_sdf_request_is_authored_before_source_resolution( - value: bool | None, - mode: str, - native_geometry: bool, -) -> None: - """Preserve concave import intent without introducing contact overrides.""" - cfg = ArticulationCfg( - uid="robot", - fpath="robot.urdf", - asset_physics_mode=mode, - root_props=ArticulationRootPropertiesCfg(newton_force_sdf=value), - ) - restored = ArticulationCfg.from_dict(cfg.to_dict()) - descriptor = articulation_desc_from_cfg(restored, newton_solver_type="dexuni") - if value is None: - assert descriptor.newton_collision is None - else: - assert descriptor.newton_collision.force_sdf is value - assert descriptor.newton_collision.ke is None - assert descriptor.newton_collision.margin is None - resolved = _resolved_articulation_desc() - if native_geometry: - resolved.urdf_path = "robot.urdf" - for link in resolved.links: - link.collisions.clear() - configure_articulation_desc(resolved, restored, newton_solver_type="dexuni") - if value is not None: - for link in resolved.links: - for collision in link.collisions: - assert collision.newton.force_sdf is value - - -def test_articulation_sdf_request_rejects_default_backend() -> None: - """Do not silently apply a Newton source option under Default physics.""" - cfg = ArticulationCfg( - fpath="robot.urdf", - root_props=ArticulationRootPropertiesCfg(newton_force_sdf=True), - ) - with pytest.raises(ValueError, match="newton_force_sdf.*Newton"): - articulation_desc_from_cfg(cfg) - - -def test_articulation_sdf_request_rejects_truthy_strings() -> None: - """Require a boolean rather than treating the text 'false' as enabled.""" - cfg = ArticulationCfg( - fpath="robot.urdf", - root_props=ArticulationRootPropertiesCfg(newton_force_sdf="false"), - ) - with pytest.raises(TypeError, match="newton_force_sdf.*boolean"): - articulation_desc_from_cfg(cfg, newton_solver_type="dexuni") - - -def test_articulation_unset_newton_creation_options_keep_dexsim_defaults() -> None: - descriptor = articulation_desc_from_cfg( - ArticulationCfg(), source_path="robot.urdf", newton_solver_type="dexuni" - ) - defaults = ArticulationDesc(name="defaults", path="robot.urdf") - for name in ( - "newton_kinematic", - "newton_sleep_policy", - "newton_shape_collision", - "newton_shape_collision_links", - "newton_particle_collision_links", - "newton_mesh_approximation", - ): - assert getattr(descriptor, name) == getattr(defaults, name), name - - -@pytest.mark.parametrize("name", sorted(_NEWTON_CREATION_ROOT_PROPS)) -def test_articulation_newton_creation_options_reject_default_backend(name) -> None: - cfg = ArticulationCfg.from_dict( - {"root_props": {name: _NEWTON_CREATION_ROOT_PROPS[name]}} - ) - with pytest.raises(ValueError, match="Newton backend"): - articulation_desc_from_cfg(cfg, source_path="robot.urdf") - - -@pytest.mark.parametrize( - ("root_props", "error"), - [ - ({"newton_sleep_policy": "sometimes"}, ValueError), - ({"newton_shape_collision_links": "base_link"}, TypeError), - ({"newton_mesh_approximation": "convex_hull"}, TypeError), - ], -) -def test_articulation_newton_creation_options_reject_invalid_values( - root_props, error -) -> None: - with pytest.raises(error): - ArticulationCfg.from_dict({"root_props": root_props}) - - @pytest.mark.parametrize("value", [-0.1, 1.1, float("nan"), float("inf")]) def test_articulation_gravity_compensation_rejects_invalid_values(value) -> None: cfg = ArticulationCfg() diff --git a/tests/sim/test_cfg.py b/tests/sim/test_cfg.py index 877e996b5f..8741ac69f1 100644 --- a/tests/sim/test_cfg.py +++ b/tests/sim/test_cfg.py @@ -996,7 +996,11 @@ def test_legacy_mesh_collision_physics_rejects_non_mesh_shape() -> None: def test_backend_joint_and_articulation_configs_round_trip() -> None: drive = NewtonJointDrivePropertiesCfg(target_mode=None) - root = ArticulationRootPropertiesCfg(fixed_base=False) + root = ArticulationRootPropertiesCfg( + fixed_base=False, + newton_gravity_compensation=0.5, + newton_mimic_stabilizer=True, + ) restored_drive = JointDrivePropertiesCfg.from_dict(drive.to_dict()) restored_root = ArticulationRootPropertiesCfg.from_dict(root.to_dict()) @@ -1008,24 +1012,31 @@ def test_backend_joint_and_articulation_configs_round_trip() -> None: "sleep_threshold": None, "min_position_iters": None, "min_velocity_iters": None, - "newton_gravity_compensation": None, - "newton_kinematic": None, - "newton_sleep_policy": None, - "newton_shape_collision": None, - "newton_shape_collision_links": None, - "newton_particle_collision_links": None, - "newton_mimic_stabilizer": False, - "newton_mesh_approximation": None, - "newton_force_sdf": None, + "newton_gravity_compensation": 0.5, + "newton_mimic_stabilizer": True, } assert type(restored_root) is ArticulationRootPropertiesCfg + assert restored_root == root -def test_articulation_root_config_rejects_backend_discriminator() -> None: - with pytest.raises(TypeError, match="backend"): - ArticulationRootPropertiesCfg.from_dict( - {"backend": "newton", "fixed_base": False} - ) +@pytest.mark.parametrize( + ("name", "value"), + [ + ("backend", "newton"), + ("newton_kinematic", True), + ("newton_sleep_policy", "never"), + ("newton_shape_collision", False), + ("newton_shape_collision_links", ["tool"]), + ("newton_particle_collision_links", ["tool"]), + ("newton_mesh_approximation", {"method": "convex_hull"}), + ("newton_force_sdf", True), + ], +) +def test_articulation_root_config_rejects_unsupported_fields(name, value) -> None: + with pytest.raises(TypeError, match=name): + ArticulationRootPropertiesCfg.from_dict({name: value}) + with pytest.raises(TypeError, match=name): + ArticulationCfg.from_dict({"root_props": {name: value}}) def test_articulation_root_config_round_trip() -> None: From 834f88d4aff1cb6c27a34a45ecbe43ef5f90362d Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 7 Nov 2026 14:31:34 +0800 Subject: [PATCH 20/27] refactor(sim): remove legacy AutoSolver compatibility Use the current DexUni configuration contract directly. Remove AutoSolver aliases, legacy flat-option translation and attribute injection, SDK-version probing, and stale solver guards. Preserve native default routing, grouped expert options and contact_mode=auto. Update configuration regression coverage and project context. Validation: 728 focused CPU tests passed, 20 deselected; black 26.10.1 left 2215 files unchanged; API docs cover 4286/4286 exports; context validation and routing checks pass. The rebuilt wheel matches the changed source and accepts the installed DexSim 0.6.0+c3e92584 build. Native GPU selections were not rerun for this configuration cleanup. --- agent_context/MAP.yaml | 2 +- .../physics-configuration.md | 7 ++- embodichain/lab/sim/cfg/physics.py | 60 ++----------------- embodichain/lab/sim/sim_manager.py | 5 +- embodichain/lab/sim/spawn/descriptors.py | 1 - tests/sim/spawn/test_descriptors.py | 14 ++--- tests/sim/test_cfg.py | 2 +- tests/sim/test_sim_manager_cfg.py | 31 +++++++++- 8 files changed, 49 insertions(+), 73 deletions(-) diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index b26d742e0e..3245122fb7 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -11,7 +11,7 @@ topics: aliases: [simulation system, simulation manager, Newton physics backend, motion module, sim motion, camera attachment, 运动能力模块, 仿真系统, 仿真管理器] keywords: [SimulationManager, SimulationManagerCfg, RenderCfg, DenoisingCfg, DenoisingMode, DLSSCfg, NRDCfg, DLSS, NRD, frame_time_delta_ms, explicit physics stepping, simulation_time, arena, DexSim, PhysicsBackendCfg, - DefaultPhysicsCfg, NewtonPhysicsCfg, VisualObject, VisualObjectCfg, VisualMaterial, VisualMaterialCfg, VisualMaterialInst, preview-material, 材质预览, UsdSourceCfg, source_prim_path, add_visual_object, 纯渲染对象, AutoSolverCfg, DexUniSolverCfg, DexUni, runtime control, deformable, particle set, + DefaultPhysicsCfg, NewtonPhysicsCfg, VisualObject, VisualObjectCfg, VisualMaterial, VisualMaterialCfg, VisualMaterialInst, preview-material, 材质预览, UsdSourceCfg, source_prim_path, add_visual_object, 纯渲染对象, DexUniSolverCfg, DexUni, runtime control, deformable, particle set, UsdSourceSnapshot, read_usd_source, LinkMesh, PhysicalCollisionGeometry, capture_physical_collision_geometry, ArticulationJointKinematics, get_parent_joint_chain, asset_physics_mode, joint_drive_props, enable_gravity, diff --git a/agent_context/topics/simulation-system/physics-configuration.md b/agent_context/topics/simulation-system/physics-configuration.md index accd9e23ad..3ad2942683 100644 --- a/agent_context/topics/simulation-system/physics-configuration.md +++ b/agent_context/topics/simulation-system/physics-configuration.md @@ -23,8 +23,11 @@ and the latter at the top level through `config_to_cfg()`. Newton `solver_cfg=None` preserves DexSim's `DexUniSolverCfg` default. DexUni owns scene-based partitioning between MuJoCo, VBD and supported particle -paths; `NewtonPhysicsBackend.solver_type` remains `dexuni`. Legacy `auto` and -`AutoSolverCfg` mappings select DexUni. Robot presets select `newton_dexuni` +paths; `NewtonPhysicsBackend.solver_type` remains `dexuni`. Explicit solver +selection uses `dexuni`; `contact_mode="auto"` remains DexUni's native +contact-path policy. Expert settings use the native option groups +(`vbd_options`, `mujoco_options`, `collision_options`); unknown solver names +and flat DexUni options are rejected. Robot presets select `newton_dexuni` before the MuJoCo aliases and portable fallbacks. Gradient mode requires an explicit `semi_implicit` solver. Read exact solver options/defaults in `cfg/physics.py` and the installed DexSim config types; do not substitute a diff --git a/embodichain/lab/sim/cfg/physics.py b/embodichain/lab/sim/cfg/physics.py index f5afff584f..a8ea910173 100644 --- a/embodichain/lab/sim/cfg/physics.py +++ b/embodichain/lab/sim/cfg/physics.py @@ -18,7 +18,6 @@ from __future__ import annotations -import inspect import math from collections.abc import Mapping @@ -376,7 +375,6 @@ class NewtonPhysicsCfg(PhysicsBackendCfg): ``solver_type`` or ``class_type`` to select the solver, then add any parameters accepted by that DexSim solver config. If omitted, EmbodiChain preserves DexSim's ``DexUniSolverCfg`` default and its scene-based dispatch. - Legacy ``auto``/``AutoSolverCfg`` mappings select DexUni as well. For ``solver_type='dexuni'``, the current DexSim contract keeps expert options under ``vbd_options``, ``mujoco_options`` and ``collision_options``; use ``joint_mode`` and ``contact_mode`` for the @@ -471,14 +469,9 @@ def to_dexsim_cfg( for item in fields(self.collision_cfg) } collision_values["requires_grad"] = self.requires_grad - # Newton 1.6 renamed this field; published DexSim builds also use - # Newton 1.4, whose descriptor retains soft_contact_margin. - if "soft_contact_gap" in { - item.name for item in fields(DexsimNewtonCollisionPipelineCfg) - }: - collision_values["soft_contact_gap"] = collision_values.pop( - "soft_contact_margin" - ) + collision_values["soft_contact_gap"] = collision_values.pop( + "soft_contact_margin" + ) collision_pipeline_cfg = DexsimNewtonCollisionPipelineCfg( **collision_values ) @@ -512,11 +505,6 @@ def _normalize_newton_solver_type(solver_type: str) -> str: """Normalize public EmbodiChain and DexSim Newton solver aliases.""" key = solver_type.replace("-", "_").lower() aliases = { - "auto": "dexuni", - "autosolver": "dexuni", - "autosolvercfg": "dexuni", - "auto_solver": "dexuni", - "auto_solver_cfg": "dexuni", "mjwarp": "mujoco_warp", "mjwarpsolver": "mujoco_warp", "mjwarpsolvercfg": "mujoco_warp", @@ -548,7 +536,7 @@ def _normalize_newton_solver_type(solver_type: str) -> str: if key not in aliases: logger.log_error( f"Unsupported Newton solver type '{solver_type}'. " - "Expected one of 'auto', 'dexuni', 'mjwarp', 'xpbd', 'semi_implicit', " + "Expected one of 'dexuni', 'mjwarp', 'xpbd', 'semi_implicit', " "'featherstone', or 'vbd'." ) return aliases[key] @@ -578,46 +566,6 @@ def _newton_solver_cfg_to_dexsim( ) normalized_solver_type = _normalize_newton_solver_type(str(configured_solver_type)) solver_cfg_type = solver_cfg_map[normalized_solver_type] - if normalized_solver_type == "dexuni": - try: - uses_grouped_options = ( - "vbd_options" in inspect.signature(solver_cfg_type).parameters - ) - except (TypeError, ValueError): - uses_grouped_options = True - if not uses_grouped_options: - # DexSim 0.5.0 still owns the legacy flat DexUni dataclass. - return solver_cfg_type(**solver_cfg_data) - - # DexSim 0.5.0 exposed these options directly on DexUniSolverCfg. The - # 0.5.1rc1 facade groups them by owned solver path, so keep existing - # EmbodiChain mappings source-compatible while forwarding the values to - # the native option contract. - legacy_values: dict[str, Any] = {} - vbd_options = dict(solver_cfg_data.get("vbd_options", {})) - if "iterations" in solver_cfg_data: - legacy_values["iterations"] = solver_cfg_data.pop("iterations") - vbd_options.setdefault("iterations", legacy_values["iterations"]) - if "step_rigid_bodies" in solver_cfg_data: - legacy_values["step_rigid_bodies"] = solver_cfg_data.pop( - "step_rigid_bodies" - ) - # The new option describes the inverse operation: when true, VBD - # integrates rigid state supplied by an external rigid solver. - vbd_options.setdefault( - "integrate_with_external_rigid_solver", - not legacy_values["step_rigid_bodies"], - ) - if vbd_options: - solver_cfg_data["vbd_options"] = vbd_options - - native_cfg = solver_cfg_type(**solver_cfg_data) - # Preserve read-only compatibility for callers that still inspect the - # legacy attributes after conversion. Native DexSim dataclasses are - # not slotted, so these aliases do not affect its option validation. - for name, value in legacy_values.items(): - setattr(native_cfg, name, value) - return native_cfg return solver_cfg_type(**solver_cfg_data) diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index 69a7c4674c..6cf61904d1 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -2987,11 +2987,11 @@ def add_deformable_object(self, cfg: DeformableObjectCfg) -> DeformableObject: "Newton deformable particle sets currently require a CUDA device." ) solver_type = self._active_newton_solver_type - supported_solvers = {"auto", "dexuni", "xpbd", "semi_implicit", "vbd"} + supported_solvers = {"dexuni", "xpbd", "semi_implicit", "vbd"} if solver_type not in supported_solvers: raise NotImplementedError( f"Newton solver {solver_type!r} does not support deformable " - "particle sets; select one of 'auto', 'dexuni', 'xpbd', " + "particle sets; select one of 'dexuni', 'xpbd', " "'semi_implicit', or 'vbd'." ) physics_cfg = self.sim_config.physics_cfg @@ -5045,7 +5045,6 @@ def reset_objects_state( solver_type = getattr(self.physics, "solver_type", None) requires_complete_world_clear = solver_type in { None, - "auto", "mujoco_warp", "dexuni", } diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index 7a79643e65..2435590223 100644 --- a/embodichain/lab/sim/spawn/descriptors.py +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -1422,7 +1422,6 @@ def _compile_joint_properties( ) if normalized_solver not in { None, - "auto", "mujoco_warp", "mjwarp", "dexuni", diff --git a/tests/sim/spawn/test_descriptors.py b/tests/sim/spawn/test_descriptors.py index 920dd96fd4..48e2da9d29 100644 --- a/tests/sim/spawn/test_descriptors.py +++ b/tests/sim/spawn/test_descriptors.py @@ -1314,7 +1314,7 @@ def discard_damping(*args, **kwargs): configure_articulation_desc(descriptor, cfg, newton_solver_type="dexuni") -@pytest.mark.parametrize("solver_type", [None, "auto", "mujoco_warp"]) +@pytest.mark.parametrize("solver_type", [None, "dexuni", "mujoco_warp"]) @pytest.mark.parametrize("mode", ["preserve", "overlay"]) @pytest.mark.parametrize("source_gap", [None, 0.0, 0.025]) def test_imported_articulation_gap_default_is_owned_by_embodichain( @@ -1332,7 +1332,7 @@ def test_imported_articulation_gap_default_is_owned_by_embodichain( assert collision.newton.margin == 0.012 -@pytest.mark.parametrize("solver_type", [None, "auto", "mujoco_warp"]) +@pytest.mark.parametrize("solver_type", [None, "dexuni", "mujoco_warp"]) @pytest.mark.parametrize("has_geometry", [True, False, None]) def test_urdf_native_shapes_receive_gap_without_synthetic_geometry( solver_type: str | None, has_geometry: bool | None @@ -1357,7 +1357,7 @@ def test_urdf_native_shapes_receive_gap_without_synthetic_geometry( assert link.collisions[0].newton.ke is None -@pytest.mark.parametrize("solver_type", [None, "auto", "mujoco_warp"]) +@pytest.mark.parametrize("solver_type", [None, "dexuni", "mujoco_warp"]) @pytest.mark.parametrize("mode", ["preserve", "overlay"]) @pytest.mark.parametrize("source_gap", [None, 0.0, 0.025]) def test_imported_rigid_gap_default_is_owned_by_embodichain( @@ -1636,7 +1636,7 @@ def test_non_mode_aware_newton_position_fallback_is_explicit() -> None: configure_articulation_desc(descriptor, cfg, newton_solver_type="xpbd") -def test_auto_solver_defers_position_mode_compatibility_warning() -> None: +def test_dexuni_solver_preserves_position_mode_without_compatibility_warning() -> None: cfg = ArticulationCfg( uid="robot", fpath="robot.urdf", @@ -1651,7 +1651,7 @@ def test_auto_solver_defers_position_mode_compatibility_warning() -> None: with warnings.catch_warnings(record=True) as caught: warnings.simplefilter("always") - configure_articulation_desc(descriptor, cfg, newton_solver_type="auto") + configure_articulation_desc(descriptor, cfg, newton_solver_type="dexuni") assert not caught @@ -1810,7 +1810,7 @@ def test_articulation_array_qpos_limits_compile_before_backend_build() -> None: assert joint.upper_limit == pytest.approx(0.75) -@pytest.mark.parametrize("solver_type", [None, "auto"]) +@pytest.mark.parametrize("solver_type", [None, "dexuni"]) @pytest.mark.parametrize("authored_velocity", [0.05, None]) def test_robot_default_drive_preserves_authored_velocity_limit( solver_type: str | None, @@ -1828,7 +1828,7 @@ def test_robot_default_drive_preserves_authored_velocity_limit( assert joint.dexsim.max_velocity == authored_velocity -@pytest.mark.parametrize("solver_type", [None, "auto"]) +@pytest.mark.parametrize("solver_type", [None, "dexuni"]) def test_explicit_zero_speed_override_is_preserved(solver_type: str | None) -> None: descriptor = _resolved_articulation_desc() joint = descriptor.get_joint_desc("arm_joint") diff --git a/tests/sim/test_cfg.py b/tests/sim/test_cfg.py index 8741ac69f1..55c0736185 100644 --- a/tests/sim/test_cfg.py +++ b/tests/sim/test_cfg.py @@ -1632,7 +1632,7 @@ class _DexUniRobotPresetCfg(RobotPresetCfg): ) @pytest.mark.parametrize( "solver_mapping", - [None, {}, {"solver_type": "auto"}, {"solver_type": "dexuni"}], + [None, {}, {"solver_type": "dexuni"}], ) def test_dexuni_preset_priority_and_isolation( missing, expected, solver_mapping diff --git a/tests/sim/test_sim_manager_cfg.py b/tests/sim/test_sim_manager_cfg.py index 01d4da6cd7..fcb613287e 100644 --- a/tests/sim/test_sim_manager_cfg.py +++ b/tests/sim/test_sim_manager_cfg.py @@ -454,9 +454,9 @@ def test_newton_physics_cfg_converts_mapping_solver_cfg_to_dexsim_cfg() -> None: @pytest.mark.no_sim @pytest.mark.parametrize( "solver_mapping", - [{}, {"solver_type": "auto"}, {"class_type": "AutoSolverCfg"}], + [None, {}], ) -def test_newton_physics_cfg_accepts_automatic_dexuni_mapping(solver_mapping) -> None: +def test_newton_physics_cfg_preserves_automatic_dexuni_defaults(solver_mapping) -> None: from dexsim.engine.newton_physics import DexUniSolverCfg cfg = NewtonPhysicsCfg(solver_cfg=solver_mapping) @@ -464,6 +464,33 @@ def test_newton_physics_cfg_accepts_automatic_dexuni_mapping(solver_mapping) -> dexsim_cfg = cfg.to_dexsim_cfg(gpu_id=0) assert isinstance(dexsim_cfg.solver_cfg, DexUniSolverCfg) + assert dexsim_cfg.solver_cfg.contact_mode == "auto" + assert dexsim_cfg.solver_cfg.mujoco_articulations is None + + +@pytest.mark.no_sim +@pytest.mark.parametrize( + "solver_mapping", + [ + {"solver_type": "auto"}, + {"class_type": "AutoSolverCfg"}, + {"solver_type": "auto_solver"}, + ], +) +def test_newton_physics_cfg_rejects_removed_auto_solver_names(solver_mapping) -> None: + with pytest.raises(RuntimeError, match="Unsupported Newton solver type"): + NewtonPhysicsCfg(solver_cfg=solver_mapping).to_dexsim_cfg(gpu_id=0) + + +@pytest.mark.no_sim +@pytest.mark.parametrize( + "field,value", [("iterations", 8), ("step_rigid_bodies", False)] +) +def test_dexuni_rejects_legacy_flat_options(field, value) -> None: + with pytest.raises(TypeError, match=field): + NewtonPhysicsCfg( + solver_cfg={"solver_type": "dexuni", field: value} + ).to_dexsim_cfg(gpu_id=0) @pytest.mark.no_sim From a6c3693f97d7e8a87949fc72e2963db23f4dcdd8 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 7 Nov 2026 14:46:41 +0800 Subject: [PATCH 21/27] docs(sim): standardize native DexUni automatic dispatch Replace retired solver terminology throughout the tracked documentation and tests. Document the default DexUni family separately from scene ownership and contact_mode=auto, migrate the example's iteration options into vbd_options, and correct path-specific contact-report guidance. Cover the explicit automatic contact policy and native ownership defaults using existing configuration cases. Validation: 738 focused CPU/docs regressions passed, 20 deselected; nine Python snippets parse, and updated Python/YAML configurations convert to native DexUni automatic defaults. Black 26.10.1, API documentation coverage (4286/4286), context validation and whitespace checks pass. A tracked-file scan finds no retired solver names. The Sphinx build is retried after this commit because the architecture generator requires committed source. --- .../physics-configuration.md | 3 +- design/newton-backend-design.md | 7 +-- docs/source/guides/configuration.md | 8 ++-- .../sim/sim_manager/physics/newton.md | 47 ++++++++++--------- .../sim/sim_manager/physics/newton/dexuni.md | 30 +++++++----- .../physics/newton/runtime_qualification.md | 12 +++-- embodichain/lab/sim/cfg/physics.py | 6 ++- tests/sim/test_sim_manager_cfg.py | 10 ++-- 8 files changed, 71 insertions(+), 52 deletions(-) diff --git a/agent_context/topics/simulation-system/physics-configuration.md b/agent_context/topics/simulation-system/physics-configuration.md index 3ad2942683..0e87bf929f 100644 --- a/agent_context/topics/simulation-system/physics-configuration.md +++ b/agent_context/topics/simulation-system/physics-configuration.md @@ -21,7 +21,8 @@ device, not a separate environment tensor device. `None` retains the native default. Gym accepts the former in `physics_config` and the latter at the top level through `config_to_cfg()`. -Newton `solver_cfg=None` preserves DexSim's `DexUniSolverCfg` default. +Newton `solver_cfg=None` or an empty mapping preserves DexSim's `DexUniSolverCfg` +defaults. Unset ownership selectors retain native scene-based selection. DexUni owns scene-based partitioning between MuJoCo, VBD and supported particle paths; `NewtonPhysicsBackend.solver_type` remains `dexuni`. Explicit solver selection uses `dexuni`; `contact_mode="auto"` remains DexUni's native diff --git a/design/newton-backend-design.md b/design/newton-backend-design.md index 4fe4c84412..86a59c16b6 100644 --- a/design/newton-backend-design.md +++ b/design/newton-backend-design.md @@ -282,8 +282,9 @@ mixed deformable tasks, destroy/recreate, and steady-state control throughput. ### Object Groups and Deformables - Maintain Newton rigid-object-group parity through the Scene rigid-body batch. -- Expand mixed rigid/articulation/deformable qualification across AutoSolver - outcomes, especially DexUni's rigid-contact limitation. +- Expand mixed rigid/articulation/deformable qualification across DexUni's + automatic MuJoCo, VBD, and coupled paths, including per-row contact-force + availability. ### Articulation / Robot @@ -364,7 +365,7 @@ Remaining: 7. Add rigid-only Newton gym smoke tests. 10. Add mixed-scene deformable qualification for the supported volume/surface - object APIs and their AutoSolver outcomes. + object APIs and DexUni's scene-based dispatch. 11. Newton-native per-link contact params for articulations (after dexsim exposes a per-link shape-material setter). 12. Full migration off legacy `PhysicalAttr` to dexsim's spawn descriptors diff --git a/docs/source/guides/configuration.md b/docs/source/guides/configuration.md index dbfe9efae1..fd060cfca2 100644 --- a/docs/source/guides/configuration.md +++ b/docs/source/guides/configuration.md @@ -295,12 +295,14 @@ physics_config: gravity: [0.0, 0.0, -9.81] num_substeps: 10 solver_cfg: - solver_type: mujoco_warp + solver_type: dexuni + contact_mode: auto use_cuda_graph: true ``` -Omit `solver_cfg` to let Newton's AutoSolver select from the complete scene at -`prepare()`. For componentized tasks, place these backend declarations in +Omit `solver_cfg` to retain the same DexUni defaults: scene-based ownership +selection at `prepare()` and `contact_mode="auto"`. For componentized tasks, +place these backend declarations in `envs/default.yaml` and `envs/newton.yaml`, then point the deployment's `environment.default` and `environment.newton` entries at those files. Do not repeat `physics` or `physics_config` in the deployment. Keep the diff --git a/docs/source/overview/sim/sim_manager/physics/newton.md b/docs/source/overview/sim/sim_manager/physics/newton.md index 663c182fc1..03caf44a91 100644 --- a/docs/source/overview/sim/sim_manager/physics/newton.md +++ b/docs/source/overview/sim/sim_manager/physics/newton.md @@ -16,10 +16,10 @@ mixed scene class and is the main Newton path documented by EmbodiChain. ## DexSim Unified Coupled Solver For Embodied AI -DexUni supports scenes that combine articulated robots with cloth or volume -soft bodies. It is selected automatically when a finalized scene contains both -an articulation and a supported deformable, or explicitly with -`solver_type="dexuni"`. +DexUni is the default Newton solver configuration. It supports rigid scenes +and scenes that combine articulated robots with cloth or volume soft bodies. +Leave `solver_cfg` unset to retain its defaults, or select +`solver_type="dexuni"` explicitly. ```python from embodichain.lab.sim import SimulationManagerCfg @@ -32,7 +32,8 @@ sim_config = SimulationManagerCfg( num_substeps=10, solver_cfg={ "solver_type": "dexuni", - "iterations": 8, + "contact_mode": "auto", + "vbd_options": {"iterations": 8}, }, # DexUni owns particle-shape collision generation. collision_cfg=None, @@ -44,24 +45,26 @@ DexUni is a DexSim integration configuration, not another native solver class in Newton's public solver list. Its capabilities and current EmbodiChain limitations are documented in {doc}`newton/dexuni`. -## Automatic solver selection +## DexUni automatic dispatch -Leaving `solver_cfg=None` preserves DexSim's scene-aware `AutoSolverCfg`. -DexSim resolves it when {meth}`SimulationManager.prepare` finalizes the complete -Spawn scene: +Leaving `solver_cfg=None` preserves DexSim's `DexUniSolverCfg`. DexUni resolves +ownership and its internal execution path when {meth}`SimulationManager.prepare` +finalizes the complete Spawn scene. Unset ownership selectors retain scene-based +partitioning, while `contact_mode="auto"` selects the native contact path. -| Finalized EmbodiChain scene | Resolved configuration | Solver type | -| :--- | :--- | :--- | -| Independent rigid bodies only | `XPBDSolverCfg` | `xpbd` | -| One or more articulations, without deformables | `MJWarpSolverCfg` | `mujoco_warp` | -| Cloth or volume soft bodies, without articulations | `VBDSolverCfg` | `vbd` | -| Articulations together with cloth or volume soft bodies | `DexUniSolverCfg` | `dexuni` | +| Finalized scene under default ownership | Internal execution | +| :--- | :--- | +| Independent rigid bodies only | MuJoCo branch | +| Articulations with no VBD-owned dynamic bodies or deformables | MuJoCo branch for dynamic joints | +| Deformables with no MuJoCo-owned joints | VBD branch | +| Articulations together with VBD-owned rigid bodies or deformables | Mixed execution selected from ownership, joint mode and contact policy | Add every initial robot and object before `prepare()` so selection sees the -complete scene. An explicit `{"solver_type": "auto"}` or -`{"class_type": "AutoSolverCfg"}` mapping is equivalent to leaving -`solver_cfg` unset. After preparation, inspect `sim.physics.solver_type`; do -not treat `physics: newton` alone as a solver qualification. +complete scene. An explicit +`{"solver_type": "dexuni", "contact_mode": "auto"}` mapping preserves this +automatic policy. `sim.physics.solver_type` remains `"dexuni"`; inspect +`sim.spawn_result.get_solver_diagnostics()` after preparation for the resolved +path. Explicit ownership selectors and kinematic control can change dispatch. For native solver feature comparisons, joint support, contact-material support, and algorithm details, use the @@ -72,11 +75,11 @@ DexSim's scene routing as exposed by this EmbodiChain integration. ## Explicit solver selection Choose a concrete solver when an algorithm or solver-specific parameter must be -fixed. EmbodiChain mapping configs recognize `auto`, `dexuni`, `mujoco_warp` +fixed. EmbodiChain mapping configs recognize `dexuni`, `mujoco_warp` (or `mjwarp`), `xpbd`, `semi_implicit`, `featherstone`, and `vbd`. A DexSim `NewtonSolverCfg` object can also be assigned directly. -AutoSolver does not select a differentiable solver. `requires_grad=True` +DexUni's default dispatch does not select a differentiable solver. `requires_grad=True` therefore requires an explicit `semi_implicit` configuration. See {doc}`newton/runtime_qualification` before treating a successfully decoded configuration as a supported runtime path. @@ -93,7 +96,7 @@ such as `gpu_memory`, `bounce_threshold`, and `enable_ccd` do not belong here. | `device` | `"cuda:0"` | Compute device; explicit CPU execution remains solver/asset dependent. | | `gravity` | `[0.0, 0.0, -9.81]` | World-frame acceleration in m/s². | | `num_substeps` | `10` | Solver substeps per physics step; interval is `physics_dt / num_substeps`. | -| `solver_cfg` | `None` | Preserve DexSim AutoSolver; a mapping or native config fixes a solver. | +| `solver_cfg` | `None` | Preserve native DexUni defaults; mappings or native configs select a solver family and its options. | | `collision_cfg` | `NewtonCollisionPipelineCfg()` | External Newton collision pipeline. Set `None` when the selected path owns collision generation, including DexUni. | | `requires_grad` | `False` | Enable the current differentiable integration; requires explicit `semi_implicit`. | | `use_cuda_graph` | `True` | Request graph capture when supported; unavailable on CPU and disabled in gradient mode. | diff --git a/docs/source/overview/sim/sim_manager/physics/newton/dexuni.md b/docs/source/overview/sim/sim_manager/physics/newton/dexuni.md index 3607c3bff6..dc53b1b797 100644 --- a/docs/source/overview/sim/sim_manager/physics/newton/dexuni.md +++ b/docs/source/overview/sim/sim_manager/physics/newton/dexuni.md @@ -26,30 +26,30 @@ Newton-native solver details, use the | Cable | Unsupported in the current version | Use DexUni for tasks in which Articulated Objects manipulate cloth, garments, -cushions, or other supported deformable assets. If a scene has only deformables -or only rigid bodies, let AutoSolver select the simpler matching solver unless -the task needs an explicit choice. +cushions, or other supported deformable assets. The same default configuration +also handles rigid-only and deformable-only scenes through its internal +scene-based MuJoCo/VBD dispatch. The current version does not support fluid or cable simulation. ## Minimal configuration -AutoSolver selects DexUni when the complete scene contains both an articulation -and a supported deformable. To keep the choice explicit: +`NewtonPhysicsCfg()` retains the native DexUni defaults: automatic ownership +selection and `contact_mode="auto"`. To keep the policy explicit: ```python from embodichain.lab.sim.cfg import NewtonPhysicsCfg physics_cfg = NewtonPhysicsCfg( device="cuda:0", - solver_cfg={"solver_type": "dexuni"}, + solver_cfg={"solver_type": "dexuni", "contact_mode": "auto"}, collision_cfg=None, ) ``` Add all Articulated Objects and deformables before -{meth}`SimulationManager.prepare`. DexUni requires GPU execution and -`requires_grad=False`. Set `collision_cfg=None` because this solver owns the +{meth}`SimulationManager.prepare`. Deformable scenes require CUDA execution and +`requires_grad=False`. Set `collision_cfg=None` because DexUni owns the robot/deformable contact path it uses. For prescribed motion, register the complete joint trajectory before @@ -68,16 +68,20 @@ public joint order. ## Current limitations ```{warning} -The current DexUni path does not solve or publish rigid-rigid or rigid-ground -contacts. `ContactSensor` is therefore unsupported on this path. +Rigid-contact quantities depend on DexUni's selected path. Pure MuJoCo can +publish measured impulses on CUDA; VBD-owned rows can provide geometry only, +and mixed reports can contain both. Honor `is_valid`, `impulse_valid` and +`friction_valid` per row. CPU MuJoCo force-reporting paths do not support +`ContactSensor`. ``` -Full capability coverage, including fluid, cable, and the rigid-contact -limitations above, will be completed in the next release. +Fluid and cable asset APIs are not exposed by this EmbodiChain integration. Qualify any task that mixes deformables with loose rigid objects against its required contact pairs. See {doc}`runtime_qualification` for the complete -solver/device matrix and validation workflow. +solver/device matrix and validation workflow. The outer solver type remains +`"dexuni"`; use `sim.spawn_result.get_solver_diagnostics()` after preparation +to inspect native dispatch. ## Examples and further reading diff --git a/docs/source/overview/sim/sim_manager/physics/newton/runtime_qualification.md b/docs/source/overview/sim/sim_manager/physics/newton/runtime_qualification.md index 8e86277e8b..825d88548d 100644 --- a/docs/source/overview/sim/sim_manager/physics/newton/runtime_qualification.md +++ b/docs/source/overview/sim/sim_manager/physics/newton/runtime_qualification.md @@ -19,7 +19,9 @@ feature does not imply that every asset or operation exposes it here. | MuJoCo-Warp, CPU | Supported | No | Unsupported | CPU contact buffers are not exposed to the sensor. | | XPBD, CUDA | Supported | Scene-dependent | Geometry-only where available | Validate the exact joints, assets, and task. | | VBD, CUDA | Limited by scene | Volume/surface | Geometry-only where available | Deformable-focused path. | -| DexUni, CUDA | Coupled articulation/deformable stepping | Volume/surface | Unsupported | No rigid-rigid or rigid-ground solve/publication on the current path. | +| DexUni, CUDA, pure MuJoCo path | Supported | No deformables in this branch | Geometry + impulse | Rigid scene routed internally to MuJoCo. | +| DexUni, CUDA, mixed path | Coupled articulation/deformable stepping | Volume/surface | Per-row geometry/impulse availability | Honor native quantity-validity masks; qualify required contact pairs. | +| DexUni, CUDA, VBD-owned contacts | Scene-dependent | Volume/surface | Geometry-only | Geometry does not establish measured force. | | Semi-implicit, CUDA, gradients | Not stepped by `DifferentiableEnv` | Unsupported | Unsupported | Current integration is a kinematics bridge only. | The matrix is an admission guide, not a substitute for a task-level rollout. @@ -28,12 +30,14 @@ controls can narrow support further. ## Resolve before qualifying -AutoSolver is unresolved until the complete Spawn scene is finalized. Query the -concrete value only after {meth}`SimulationManager.prepare`: +DexUni resolves scene ownership and its internal execution path after the +complete Spawn scene is finalized. The outer solver type remains `"dexuni"`; +query native diagnostics after {meth}`SimulationManager.prepare`: ```python sim.prepare() print(sim.physics.solver_type) +print(sim.spawn_result.get_solver_diagnostics()) print(sim.device) ``` @@ -61,7 +65,7 @@ Volume soft bodies and surface cloth use `VolumeDeformableObjectCfg` and `SurfaceDeformableObjectCfg`. The current integration requires: - CUDA execution; -- `auto`, `dexuni`, `xpbd`, `semi_implicit`, or `vbd` as the configured solver; +- `dexuni`, `xpbd`, `semi_implicit`, or `vbd` as the configured solver; - `requires_grad=False`; - every deformable declaration before the first `prepare()`/scene finalization. diff --git a/embodichain/lab/sim/cfg/physics.py b/embodichain/lab/sim/cfg/physics.py index a8ea910173..f7f01b1885 100644 --- a/embodichain/lab/sim/cfg/physics.py +++ b/embodichain/lab/sim/cfg/physics.py @@ -375,10 +375,12 @@ class NewtonPhysicsCfg(PhysicsBackendCfg): ``solver_type`` or ``class_type`` to select the solver, then add any parameters accepted by that DexSim solver config. If omitted, EmbodiChain preserves DexSim's ``DexUniSolverCfg`` default and its scene-based dispatch. + Unset ownership selectors retain automatic scene partitioning; + ``contact_mode='auto'`` independently selects the native contact path. For ``solver_type='dexuni'``, the current DexSim contract keeps expert options under ``vbd_options``, ``mujoco_options`` and - ``collision_options``; use ``joint_mode`` and ``contact_mode`` for the - coupled dispatch policy. ``coupling='two_way'`` enables contact reactions + ``collision_options``. ``joint_mode`` controls joint integration. + ``coupling='two_way'`` enables contact reactions on dynamically integrated articulations. Optional ``coupling_options`` accepts ``mass_scale``, ``proxy_relaxation`` and ``iterations``. Two-way coupling requires ``joint_mode='dynamic'``. Current DexSim supports diff --git a/tests/sim/test_sim_manager_cfg.py b/tests/sim/test_sim_manager_cfg.py index fcb613287e..bbaa2c811b 100644 --- a/tests/sim/test_sim_manager_cfg.py +++ b/tests/sim/test_sim_manager_cfg.py @@ -472,12 +472,11 @@ def test_newton_physics_cfg_preserves_automatic_dexuni_defaults(solver_mapping) @pytest.mark.parametrize( "solver_mapping", [ - {"solver_type": "auto"}, - {"class_type": "AutoSolverCfg"}, - {"solver_type": "auto_solver"}, + {"solver_type": "unknown_solver"}, + {"class_type": "UnknownSolverCfg"}, ], ) -def test_newton_physics_cfg_rejects_removed_auto_solver_names(solver_mapping) -> None: +def test_newton_physics_cfg_rejects_unknown_solver_names(solver_mapping) -> None: with pytest.raises(RuntimeError, match="Unsupported Newton solver type"): NewtonPhysicsCfg(solver_cfg=solver_mapping).to_dexsim_cfg(gpu_id=0) @@ -508,6 +507,7 @@ def test_newton_physics_cfg_accepts_dexuni_solver_mapping( solver_cfg={ config_key: solver_name, "joint_mode": "kinematic", + "contact_mode": "auto", "vbd_options": {"iterations": 12}, } ) @@ -517,6 +517,8 @@ def test_newton_physics_cfg_accepts_dexuni_solver_mapping( assert isinstance(dexsim_cfg.solver_cfg, DexUniSolverCfg) assert dexsim_cfg.solver_cfg.solver_type == "dexuni" assert dexsim_cfg.solver_cfg.joint_mode == "kinematic" + assert dexsim_cfg.solver_cfg.contact_mode == "auto" + assert dexsim_cfg.solver_cfg.mujoco_articulations is None assert dexsim_cfg.solver_cfg.vbd_options["iterations"] == 12 From f249fbe69fcc645509a8fd879686b422e09934b2 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 7 Nov 2026 15:06:31 +0800 Subject: [PATCH 22/27] update dexsim version --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 86785aface..b86eaa0f5f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -26,7 +26,7 @@ dynamic = ["version"] # Core install dependencies (kept from requirements.txt). dependencies = [ # Solver contact reports use the DexSim 0.6 API. - "dexsim_engine>=0.6.0,<0.7.0", + "dexsim_engine==0.6.0rc1", # DexSim and LeRobot require different OpenCV distributions sharing cv2. # Keep their bindings on the same release to avoid mixed Python/native APIs. "opencv-python==4.12.0.88; python_version < '3.12'", From 28f8b6a901de7bc72d0724c0185c5f0599a60aa4 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Sat, 7 Nov 2026 21:08:42 +0800 Subject: [PATCH 23/27] fix(sensors): honor native contact sampling boundaries Keep reset-time contact observations readable until the next solved report, require eager native acquisition outside caller-owned CUDA capture, and normalize Newton impulses with the internal solver duration while retaining World-step history clocks. Cover real articulation resets, native graph consumers, multi-substep support forces, variable World intervals and history clocks; update sensor guidance and align timeline reset test buffers with quantity validity. --- .../topics/sensor-system/contact-history.md | 34 ++-- .../topics/sensor-system/lifecycle.md | 6 + .../embodichain.lab.sim.sensors.rst | 15 +- .../overview/sim/sensors/contact_sensor.md | 45 ++++- .../lab/sim/sensors/contact/_warp/history.py | 8 +- .../lab/sim/sensors/contact/history.py | 25 ++- embodichain/lab/sim/sensors/contact/sensor.py | 97 ++++++---- .../task_program/test_contact_evidence.py | 2 + tests/sim/sensors/test_contact_history.py | 30 +++ tests/sim/sensors/test_contact_newton_e2e.py | 183 +++++++++++++++++- .../sim/sensors/test_contact_query_sensor.py | 12 ++ .../sim/sensors/test_contact_sensor_graph.py | 112 ----------- 12 files changed, 393 insertions(+), 176 deletions(-) diff --git a/agent_context/topics/sensor-system/contact-history.md b/agent_context/topics/sensor-system/contact-history.md index f4c0216b9c..652d19f37d 100644 --- a/agent_context/topics/sensor-system/contact-history.md +++ b/agent_context/topics/sensor-system/contact-history.md @@ -21,20 +21,22 @@ boundary. Observation reads reuse the sampled sensor buffer. `SimulationManager.physics_step_index` and the query's current actor binding. Reading a raw sample before its history callback does not consume the history update. Query metadata is checked even on cache hits so topology rebuilds cannot -retain old contacts. Cached CUDA reads wait for the producing stream. Caller-owned -CUDA graph capture bypasses raw/history deduplication and records the query read -and reductions on the capture stream. Capture invalidates eager cache keys rather -than marking unexecuted work as a completed sample; replay does not execute Python -cache bookkeeping. Callers own replay scheduling and establish stream dependencies before capture -and replay. Capture skips eager producer waits and never records its stream as -a completed producer, including control-interval initialization and reset. +retain old contacts. Cached CUDA reads wait for the producing stream. +`update()` and `update_physics_step()` require eager calls outside caller-owned +CUDA graph capture. Native snapshot reads can wait on uncaptured producer events; +the sensor rejects capture before calling the query or sampling observers. +Callers sample eagerly before capture/replay and establish stream dependencies, +then consume the stable `get_data()` buffers inside their graphs. Replays alone +do not refresh contacts or advance history. The sensor's own reduction graph +remains supported because native acquisition runs before its capture boundary. Sensor reset clears only selected rows and invalidates this cache; it does not reset the physics solver. An explicit `update()` may therefore refresh a valid settled snapshot without advancing history, as scene preparation requires. -It cannot recompute solved contacts after physical state writes; the backend's -contact-availability rules still apply. Empty selections do not invalidate the -cache. `BaseEnv` retains its ordinary acquisition and substep scheduling. +If DexSim reports that physics reset invalidated its solved snapshot, observation +reads return invalid contact rows until a new solve. Other query failures still +propagate. Empty selections do not invalidate the cache. `BaseEnv` retains its +ordinary acquisition and substep scheduling. CUDA queries use `fetch_solver_report_async()` and device-resident counts. Sparse Warp kernels process valid contact rows, using precomputed sorted environment/actor indices; @@ -46,8 +48,12 @@ components marked available by the sample's normal/friction validity flags. Geometry-only rows can count as contact when no force threshold is requested; they do not create measured force or satisfy a positive force threshold. Native friction is the tangential impulse on actor 0, while the normal points -toward actor 1: actor-0 force is `(-normal * impulse + friction) / dt` and -actor 1 receives the opposite force. +toward actor 1: actor-0 force is `(-normal * impulse + friction) / impulse_dt` +and actor 1 receives the opposite force. Newton reports the final internal +solver substep's impulse, so the sensor uses the actual World-step duration +divided by the active backend's `num_substeps` for `impulse_dt`. History clocks +still advance by the full World-step duration. Standalone `ContactHistory.update` +accepts `impulse_dt` explicitly and defaults it to the elapsed `dt`. `dropped_contacts` reports query loss plus separately counted scatter overflow across the control interval. Query counts stay on the device until this diagnostic @@ -70,8 +76,8 @@ and `tests/gym/envs/test_substep_sensors.py`. observer to the same lifecycle. It receives begin, before-sample, after-sample and selected-row reset notifications. With no observers, the update path does not enumerate subscriptions, construct token generators or enter callback loops. -Hooks run outside the sensor's internal CUDA capture; external capture with -observers fails before callbacks run. Observer tokens and after-sample callbacks +Hooks run outside the sensor's internal CUDA capture; all sampling in external +capture fails before callbacks run. Observer tokens and after-sample callbacks run once per physics step alongside history reduction. A raw sample fetched earlier in the same step is reused; repeated reads do not advance history or repeat callbacks. Removal can supply `expected=observer` to retain a replacement diff --git a/agent_context/topics/sensor-system/lifecycle.md b/agent_context/topics/sensor-system/lifecycle.md index 3549723a7c..b1029747c2 100644 --- a/agent_context/topics/sensor-system/lifecycle.md +++ b/agent_context/topics/sensor-system/lifecycle.md @@ -29,6 +29,12 @@ Sampling reads completed solver contacts through DexSim's `fetch()` operate on a separate geometry-only observation and cannot replace solver reports when interpreting measured impulses or force histories. +After a physics reset invalidates the native solved report, sensor observation +reads return invalid rows until the next solve. A sensor-buffer-only reset may +still reread the same valid native report. Contact sampling must run outside +caller-owned CUDA graph capture; only the stable observation buffers are consumed +inside that graph. See [history and substeps](contact-history.md) for ordering. + Preserve these query/buffer contracts: - `filter_need_both_actor` chooses query `match="all"` or `"any"`. diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.sensors.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.sensors.rst index 537959c7ed..5514ee2bcc 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.sensors.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.sensors.rst @@ -120,6 +120,17 @@ sparse Warp kernels. The ``dropped_contacts`` diagnostic sums query loss and scatter overflow across the current control interval; standalone updates without histories or sampling observers report the most recent sample. +Newton force conversion uses the final internal solver substep's duration, +while history clocks advance by the full World-step duration. Standalone +``ContactHistory.update`` accepts an explicit ``impulse_dt`` for this conversion. +Physics-reset-invalidated reports yield invalid observation rows until a new +solve; a sensor-only reset can still reread a valid current report. + +Contact sampling through ``update`` or ``update_physics_step`` must run eagerly +outside caller-owned CUDA Graph capture. Sample before each replay and establish +stream dependencies before consuming the stable ``get_data()`` buffers in the +graph. The sensor's internal reduction graph remains supported. + ``latest_sample_dropped_contacts`` reports the latest fetch's query and scatter loss even when observers retain interval counters. Evidence sampling rejects either diagnostic and takes their maximum to avoid double counting. Full reset @@ -139,8 +150,8 @@ adds no observer snapshots or callbacks. Begin and selective-reset notifications follow the sensor's own history/counter updates; before/after tokens surround one physics-step sample. A prior raw fetch in that same step is reused; repeated substep calls do not repeat callbacks. Observation callbacks run outside the -internal CUDA capture, and external capture with observers is rejected before -callbacks begin. The observer owns retention limits and diagnostics and must not +internal CUDA capture, and all sampling in external capture is rejected before +native queries or callbacks begin. The observer owns retention limits and diagnostics and must not step physics or issue commands. .. currentmodule:: embodichain.lab.sim.sensors.contact.sensor diff --git a/docs/source/overview/sim/sensors/contact_sensor.md b/docs/source/overview/sim/sensors/contact_sensor.md index bfdef8c72b..194ecc82b8 100644 --- a/docs/source/overview/sim/sensors/contact_sensor.md +++ b/docs/source/overview/sim/sensors/contact_sensor.md @@ -139,7 +139,10 @@ may be left over from an earlier update. - **`filter_by_user_ids(item_user_ids, env_ids=None)`**: Filter contact report by contact actor IDs. The method name is retained for compatibility. Optionally filter by specific environment IDs. - **`set_contact_point_visibility(visible, rgba, point_size, env_ids=None)`**: Enable/disable visualization of contact points with customizable color and size. Optionally visualize only specific environments. -Newton MuJoCo-Warp exposes contact forces, so both impulse fields are available. Other supported Newton rigid solvers currently expose contact geometry with zero-valued impulse fields. MuJoCo CPU mode does not expose device contact buffers, and DexUni does not currently publish rigid contacts through `ContactQuery`; those modes are therefore unsupported by this sensor. +Newton MuJoCo-Warp and DexUni's MuJoCo-owned rows expose normal and friction +impulses. DexUni can mix these with VBD geometry-only rows; inspect +`impulse_valid` and `friction_valid` before interpreting forces. MuJoCo CPU mode +does not expose device contact buffers. Default Direct GPU reports static counterparts with actor ID `-1` because its raw contact buffer does not expose their object identity. To monitor a dynamic body or articulation link against arbitrary static geometry, select the dynamic/link object and set `filter_need_both_actor=False`. Default CPU and Newton can identify registered static shapes. @@ -159,6 +162,46 @@ owns its snapshot and query graphs; the sensor consumes the current query buffer and its device-resident count. Results are ordered onto the caller's Torch stream. CPU sampling uses the same reductions without capture. +Newton reports impulses from the final internal solver substep. History force +conversion uses the actual World-step duration divided by the active backend's +`num_substeps`; contact and air-time clocks use the full World-step duration. +Standalone `ContactHistory.update(data, dt, impulse_dt=...)` accepts both durations. + +### Caller-owned CUDA Graphs + +Call `contact_sensor.update()` or `update_physics_step(dt)` outside caller-owned +CUDA Graph capture. These methods reject capture before querying DexSim, whose +native snapshots may wait on events from uncaptured work. The sensor's internal +reduction graph above remains supported. + +Capture computations that consume the preallocated `get_data()` tensors. Before +each replay, advance physics, sample eagerly and order the graph's stream after +the sampling stream. Replay alone neither refreshes contacts nor advances history. + +```python +import torch + +contact_sensor.update() # Eager warmup after a completed physics step. +stream = torch.cuda.Stream(device=contact_sensor.device) +load = torch.empty_like(contact_sensor.get_data()["impulse"]) +stream.wait_stream(torch.cuda.current_stream(contact_sensor.device)) +graph = torch.cuda.CUDAGraph() +with torch.cuda.graph(graph, stream=stream): + data = contact_sensor.get_data() + load.copy_(torch.where(data["is_valid"], data["impulse"], 0.0)) + +sim.update(step=1) +contact_sensor.update() # Refresh the same buffers before every replay. +stream.wait_stream(torch.cuda.current_stream(contact_sensor.device)) +with torch.cuda.stream(stream): + graph.replay() +``` + +After a physics reset invalidates the solved report, observation reads return +invalid rows until a new solve. Resetting only the sensor buffers still permits +a fresh read of an existing valid report. Values outside valid rows remain +unspecified; a reset-time empty observation is not measured absence of contact. + The physics loop, simulation clock and rendering callbacks remain outside the sensor graph. Window, offscreen and browser visualization retain their existing update order. diff --git a/embodichain/lab/sim/sensors/contact/_warp/history.py b/embodichain/lab/sim/sensors/contact/_warp/history.py index 9617b79add..2eab89abad 100644 --- a/embodichain/lab/sim/sensors/contact/_warp/history.py +++ b/embodichain/lab/sim/sensors/contact/_warp/history.py @@ -94,7 +94,7 @@ def reduce_contact_rows( accept_all: bool, accept_unknown: bool, threshold: float, - dt: float, + impulse_dt: float, forces: wp.array(dtype=wp.float32, ndim=3), hits: wp.array(dtype=wp.int32, ndim=2), env_hits: wp.array(dtype=wp.int32), @@ -117,7 +117,7 @@ def reduce_contact_rows( force = wp.vec3(data[i, 3], data[i, 4], data[i, 5]) * data[i, 9] if tangent_valid: force = force - wp.vec3(data[i, 6], data[i, 7], data[i, 8]) - force = force / dt + force = force / impulse_dt reduce_pair( env, actors[i, 0], @@ -151,7 +151,7 @@ def reduce_contact_batch( accept_all: bool, accept_unknown: bool, threshold: float, - dt: float, + impulse_dt: float, forces: wp.array(dtype=wp.float32, ndim=3), hits: wp.array(dtype=wp.int32, ndim=2), env_hits: wp.array(dtype=wp.int32), @@ -169,7 +169,7 @@ def reduce_contact_batch( force = force - wp.vec3( friction[env, i, 0], friction[env, i, 1], friction[env, i, 2] ) - force = force / dt + force = force / impulse_dt reduce_pair( env, actors[env, i, 0], diff --git a/embodichain/lab/sim/sensors/contact/history.py b/embodichain/lab/sim/sensors/contact/history.py index 09baa7bb35..037fabd413 100644 --- a/embodichain/lab/sim/sensors/contact/history.py +++ b/embodichain/lab/sim/sensors/contact/history.py @@ -108,9 +108,12 @@ def _index_actors(ids: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: keys, rows = keys.flatten().sort() return keys, rows.to(torch.int32) - def _start_sample(self, dt: float) -> list: + def _start_sample(self, dt: float, impulse_dt: float | None) -> list: if dt <= 0: raise ValueError("Contact sampling dt must be positive.") + impulse_dt = dt if impulse_dt is None else impulse_dt + if impulse_dt <= 0: + raise ValueError("Contact impulse duration must be positive.") self.force.zero_() self._hits.zero_() self._env_hits.zero_() @@ -121,7 +124,7 @@ def _start_sample(self, dt: float) -> list: self.counterpart_ids is None, self.include_unknown_counterpart, self.force_threshold, - dt, + impulse_dt, wp.from_torch(self.force), wp.from_torch(self._hits), wp.from_torch(self._env_hits), @@ -165,11 +168,12 @@ def _update_from_query( buffer: ContactBuffer, dt: float, *, + impulse_dt: float | None = None, impulse_available: bool = True, friction_available: bool = True, ) -> None: """Reduce compact query rows using its device-resident valid-row count.""" - args = self._start_sample(dt) + args = self._start_sample(dt, impulse_dt) quantity_valid = getattr(buffer, "quantity_valid", None) self._launch( reduce_contact_rows, @@ -197,14 +201,23 @@ def begin_control_step(self) -> None: self.peak_force.zero_() self.contact_count.zero_() - def update(self, data: Mapping[str, torch.Tensor], dt: float) -> None: + def update( + self, + data: Mapping[str, torch.Tensor], + dt: float, + *, + impulse_dt: float | None = None, + ) -> None: """Reduce a new physics sample and advance timing exactly once. Args: data: ContactSensor data in its documented actor/impulse convention. - dt: Duration of the physics sample in seconds. + dt: Elapsed World-step duration for history clocks, in seconds. + impulse_dt: Duration represented by the reported impulses, in + seconds. Defaults to dt. Newton solver reports represent the + final internal substep, so pass World dt / num_substeps. """ - args = self._start_sample(dt) + args = self._start_sample(dt, impulse_dt) self._launch( reduce_contact_batch, data["is_valid"].shape, diff --git a/embodichain/lab/sim/sensors/contact/sensor.py b/embodichain/lab/sim/sensors/contact/sensor.py index 89dcc62622..7c8b15eda1 100644 --- a/embodichain/lab/sim/sensors/contact/sensor.py +++ b/embodichain/lab/sim/sensors/contact/sensor.py @@ -194,6 +194,7 @@ def __init__( self.device = device self.cfg = config self._query: ContactQuery | None = None + self._empty_contact_buffer: ContactBuffer | None = None self._metadata_actor_table: tuple[ContactActorInfo, ...] | None = None self._metadata_selected_ids: tuple[int, ...] | None = None @@ -391,30 +392,34 @@ def update(self, **kwargs) -> None: Repeated calls reuse the scattered data and establish readiness on the caller's CUDA stream. Resetting sensor buffers permits a fresh read of the current backend snapshot without advancing contact history. This - does not recompute contacts after physical state writes: backend rules - still determine whether a solved contact snapshot is available. + does not recompute contacts after physical state writes. A report + invalidated by physics reset yields invalid rows until the next solve. + Call outside caller-owned CUDA capture; graphs may consume get_data(). Args: **kwargs: Additional keyword arguments for sensor update. """ + self._require_eager_sampling() self._wait_for_sample() - capturing = ( - self.device.type == "cuda" and torch.cuda.is_current_stream_capturing() - ) - key = None if capturing else self._contact_sample_key() - if not capturing and key == self._last_update_key: + key = self._contact_sample_key() + if key == self._last_update_key: return buffer = self._fetch_contacts() if not self.requires_substep_update: self._query_dropped_count.copy_(buffer.dropped_count_device) self._scatter_dropped_count.copy_(self._sample_scatter_dropped_count) self._last_update_key = key - self._last_contact_buffer = None if capturing else buffer - if capturing: - self._last_history_key = None - else: - self._record_sample_stream() + self._last_contact_buffer = buffer + self._record_sample_stream() + + def _require_eager_sampling(self) -> None: + if self.device.type == "cuda" and torch.cuda.is_current_stream_capturing(): + raise RuntimeError( + "Contact sampling must run outside external CUDA capture. " + "Sample eagerly before capture/replay, establish stream " + "dependencies, and consume get_data() buffers inside the graph." + ) def _contact_sample_key(self) -> tuple[int, int, int]: """Consult public query metadata so topology refreshes cannot be skipped.""" @@ -446,10 +451,25 @@ def _fetch_contacts(self) -> ContactBuffer: def _read_contacts(self) -> ContactBuffer: assert self._query is not None - if self.device.type == "cuda": - buffer = self._query.fetch_solver_report_async() - else: - buffer = self._query.fetch_solver_report() + try: + if self.device.type == "cuda": + buffer = self._query.fetch_solver_report_async() + else: + buffer = self._query.fetch_solver_report() + except RuntimeError as error: + # DexSim 0.6 has no public report-availability flag or dedicated + # exception. Only this reset-time state is an empty observation; + # unrelated query and device failures must still reach the caller. + if str(error) != ( + "Contact state is unavailable after reset until the next World.update" + ): + raise + if self._empty_contact_buffer is None: + from dexsim.scene import ContactBuffer + + self._empty_contact_buffer = ContactBuffer.allocate(1, self.device) + buffer = self._empty_contact_buffer + if self.device.type == "cpu": buffer.device_counts[0] = buffer.count buffer.device_counts[1] = buffer.dropped_count self._sync_filter_actor_metadata() @@ -763,18 +783,11 @@ def update_physics_step(self, dt: float) -> None: """ if dt <= 0: raise ValueError("Contact sampling dt must be positive.") + self._require_eager_sampling() self._wait_for_sample() - # Capturing records future work; Python cache keys cannot track replay. - capturing = ( - self.device.type == "cuda" and torch.cuda.is_current_stream_capturing() - ) observers = self._sampling_observers - if observers and capturing: - raise RuntimeError( - "Contact sampling observers require calls outside external CUDA capture." - ) - key = None if capturing else self._contact_sample_key() - if not capturing and key == self._last_history_key: + key = self._contact_sample_key() + if key == self._last_history_key: return # Keep the default history-only path free of observer snapshots and # generator allocation. Capture a stable subscription set only when used. @@ -786,24 +799,34 @@ def update_physics_step(self, dt: float) -> None: ) buffer = ( self._last_contact_buffer - if not capturing and key == self._last_update_key + if key == self._last_update_key else self._read_contacts() ) assert buffer is not None self._sample_physics_contacts(buffer, dt) self._last_update_key = key self._last_history_key = key - self._last_contact_buffer = None if capturing else buffer - if not capturing: - self._record_sample_stream() + self._last_contact_buffer = buffer + self._record_sample_stream() if tokens is not None: for observer, token in tokens: observer.after_sample(dt, token) def _sample_physics_contacts(self, buffer: ContactBuffer, dt: float) -> None: """Run the reduction on the existing caller/capture stream boundary.""" - if self.device.type != "cuda" or torch.cuda.is_current_stream_capturing(): - self._sample_contacts(buffer, dt) + impulse_dt = dt + if ( + self._histories + and getattr(getattr(self._sim, "physics", None), "name", None) == "newton" + ): + from dexsim.engine.newton_physics.backend_registry import get_newton_backend + + backend = get_newton_backend(self._sim._world) + # Newton reports the last internal solver substep's impulse, not + # the impulse integrated over the whole World.update interval. + impulse_dt = dt / max(int(backend.cfg.num_substeps), 1) + if self.device.type != "cuda": + self._sample_contacts(buffer, dt, impulse_dt) return key = ( id(buffer), @@ -811,6 +834,7 @@ def _sample_physics_contacts(self, buffer: ContactBuffer, dt: float) -> None: self.contact_capabilities.impulse, self.contact_capabilities.friction, dt, + impulse_dt, self.cfg.max_contacts_per_env, tuple( ( @@ -838,17 +862,19 @@ def _sample_physics_contacts(self, buffer: ContactBuffer, dt: float) -> None: self._sample_graph_key = key # This real sample also compiles/warms every kernel. Do not # execute history updates twice while preparing capture. - self._sample_contacts(buffer, dt) + self._sample_contacts(buffer, dt, impulse_dt) else: if self._sample_graph is None: with wp.ScopedCapture(stream=self._sample_stream) as capture: - self._sample_contacts(buffer, dt) + self._sample_contacts(buffer, dt, impulse_dt) self._sample_graph = capture.graph wp.capture_launch(self._sample_graph, stream=self._sample_stream) if not same_stream: caller.wait_stream(self._sample_torch_stream) - def _sample_contacts(self, buffer: ContactBuffer, dt: float) -> None: + def _sample_contacts( + self, buffer: ContactBuffer, dt: float, impulse_dt: float + ) -> None: self._scatter_contacts(buffer) self._query_dropped_count.add_(buffer.dropped_count_device) self._scatter_dropped_count.add_(self._sample_scatter_dropped_count) @@ -856,6 +882,7 @@ def _sample_contacts(self, buffer: ContactBuffer, dt: float) -> None: history._update_from_query( buffer, dt, + impulse_dt=impulse_dt, impulse_available=self.contact_capabilities.impulse, friction_available=self.contact_capabilities.friction, ) diff --git a/tests/gym/envs/task_program/test_contact_evidence.py b/tests/gym/envs/task_program/test_contact_evidence.py index 0313fad2b4..5635a6b889 100644 --- a/tests/gym/envs/task_program/test_contact_evidence.py +++ b/tests/gym/envs/task_program/test_contact_evidence.py @@ -901,6 +901,8 @@ def test_timeline_reset_matches_sensor_indices_without_double_generation( indices, generations ): source = ContactSource(count=3) + for name in ("impulse_valid", "friction_valid"): + source.data[name] = source.data["is_valid"].clone() timeline, _ = make_timeline(source) capture_step(timeline, source) before = timeline.snapshot() diff --git a/tests/sim/sensors/test_contact_history.py b/tests/sim/sensors/test_contact_history.py index e3948c7bcd..00a133c444 100644 --- a/tests/sim/sensors/test_contact_history.py +++ b/tests/sim/sensors/test_contact_history.py @@ -83,6 +83,36 @@ def test_landing_retains_completed_air_time_until_next_landing(): assert not history.first_contact.item() +@pytest.mark.parametrize("compact", [False, True]) +def test_impulse_duration_does_not_shorten_history_clocks(compact): + from dexsim.scene import ContactBuffer + + history = ContactHistory(torch.tensor([[10]]), force_threshold=1.0) + data = sample([[[0, 10]]]) + data["impulse"].fill_(0.00981) + buffer = ContactBuffer.allocate(1, "cpu") + buffer.data.zero_() + buffer.data[:, 5] = 1.0 + buffer.data[:, 9] = 0.00981 + buffer.actor_ids[:] = torch.tensor([[0, 10]]) + buffer.env_ids.zero_() + buffer.quantity_valid.fill_(True) + + for contact in (False, False, True): + data["is_valid"].fill_(contact) + buffer.device_counts[0] = int(contact) + if compact: + history._update_from_query(buffer, 0.01, impulse_dt=0.001) + else: + history.update(data, 0.01, impulse_dt=0.001) + if not contact: + assert not history.found.any() + assert history.found.item() + assert history.force[0, 0, 2].item() == pytest.approx(9.81) + assert history.current_air_time.item() == 0.0 + assert history.last_air_time.item() == pytest.approx(0.03) + + def test_selective_reset_preserves_every_untouched_history_buffer(): history = ContactHistory(torch.tensor([[10], [20]])) history.update(sample([[[0, 10]], [[0, 20]]], valid=False), 0.1) diff --git a/tests/sim/sensors/test_contact_newton_e2e.py b/tests/sim/sensors/test_contact_newton_e2e.py index 5775b5ee37..203295ebed 100644 --- a/tests/sim/sensors/test_contact_newton_e2e.py +++ b/tests/sim/sensors/test_contact_newton_e2e.py @@ -18,17 +18,21 @@ from __future__ import annotations import math +from contextlib import nullcontext +from types import SimpleNamespace import pytest import torch import warp as wp from embodichain.lab.sim.cfg import ( + ArticulationCfg, MassPropertiesCfg, NewtonPhysicsCfg, RigidBodyPhysicsCfg, RigidObjectCfg, ) +from embodichain.lab.gym.envs.base_env import BaseEnv from embodichain.lab.sim.sensors import ( ArticulationContactFilterCfg, ContactSensorCfg, @@ -43,7 +47,10 @@ @pytest.mark.parametrize( "solver_type", ["mujoco_warp", pytest.param(None, id="default_dexuni")] ) -def test_newton_contact_sensor_reports_each_arena(solver_type: str | None) -> None: +@pytest.mark.parametrize("num_substeps", [1, 10]) +def test_newton_contact_sensor_reports_each_arena( + solver_type: str | None, num_substeps: int +) -> None: from dexsim.scene import ContactBuffer if solver_type is None and not hasattr(ContactBuffer, "impulse_valid"): @@ -59,7 +66,7 @@ def test_newton_contact_sensor_reports_each_arena(solver_type: str | None) -> No arena_space=2.0, physics_cfg=NewtonPhysicsCfg( device="cuda:0", - num_substeps=1, + num_substeps=num_substeps, use_cuda_graph=False, # Exercise a multi-point manifold so query-side reductions # cannot satisfy this regression contract accidentally. @@ -120,6 +127,24 @@ def test_newton_contact_sensor_reports_each_arena(solver_type: str | None) -> No sensor.get_actor_info(actor_id).path.endswith(("/ground", "/cube")) for actor_id in set(actor_ids.flatten().cpu().tolist()) ) + history = sensor.create_history( + "support", + sensor.get_actor_ids("cube"), + counterpart_ids=sensor.get_actor_ids("ground"), + force_threshold=1.0, + ) + # Keep the physical load invariant while varying both the solver's + # internal substeps and the actual World.update interval. + for dt in (0.01, 0.02): + sensor.begin_control_step() + sim.update(dt, step=3, after_substep=sensor.update_physics_step) + torch.testing.assert_close( + history.force[..., 2], + torch.full_like(history.contact, 9.81, dtype=torch.float32), + rtol=0.01, + atol=0.05, + ) + assert history.found.all() finally: sim.destroy(exit_process=False) SimulationManager.flush_cleanup_queue() @@ -128,6 +153,160 @@ def test_newton_contact_sensor_reports_each_arena(solver_type: str | None) -> No teardown_newton_physics() +@pytest.fixture +def dexuni_contact_scene(tmp_path): + """A native contact report with articulation dynamics invalidated by reset.""" + urdf = tmp_path / "slider.urdf" + urdf.write_text(""" + + + + + + + + + + + """) + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + device="cuda:0", + physics_cfg=NewtonPhysicsCfg( + device="cuda:0", + physics_dt=0.01, + num_substeps=10, + use_cuda_graph=False, + ), + ) + ) + try: + sim.add_articulation( + ArticulationCfg(uid="slider", fpath=str(urdf), build_pk_chain=False) + ) + sim.add_rigid_object( + RigidObjectCfg( + uid="ground", + shape=CubeCfg(size=[1.0, 1.0, 0.1]), + attrs=RigidBodyPhysicsCfg(), + body_type="static", + init_pos=(0.0, 0.0, 0.1), + ) + ) + sim.add_rigid_object( + RigidObjectCfg( + uid="cube", + shape=CubeCfg(size=[0.2, 0.2, 0.2]), + attrs=RigidBodyPhysicsCfg(mass_props=MassPropertiesCfg(mass=1.0)), + init_pos=(0.0, 0.0, 0.4), + ) + ) + sensor = sim.add_sensor( + ContactSensorCfg( + uid="contacts", + rigid_uid_list=["ground", "cube"], + max_contacts_per_env=32, + ) + ) + sim.update(step=150) + sensor.update() + assert sensor.get_data()["is_valid"].any() + yield sim, sensor + finally: + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + from dexsim.engine.newton_physics import teardown_newton_physics + + teardown_newton_physics() + + +def test_dexuni_physics_reset_keeps_initial_sensor_observations_readable( + dexuni_contact_scene, +): + sim, sensor = dexuni_contact_scene + env = SimpleNamespace( + sim=sim, + sensors={"contacts": sensor}, + device=sim.device, + num_envs=sim.num_envs, + _camera_group_ids=[], + _profiler=SimpleNamespace(section=lambda *_: nullcontext()), + ) + assert not sensor.requires_substep_update + assert BaseEnv._get_sensor_obs(env)["contacts"]["is_valid"].any() + # Sensor-only reset must retain the valid, same-step native report. + sensor.reset([0]) + assert BaseEnv._get_sensor_obs(env)["contacts"]["is_valid"].any() + # Reproduce Gym's physical reset ordering, including articulation clear. + sim.reset_objects_state(env_ids=[0], excluded_sensor_uids=[sensor.uid]) + sensor.reset([0]) + for _ in range(2): + data = BaseEnv._get_sensor_obs(env)["contacts"] + assert not data["is_valid"].any() + assert not data["impulse_valid"].any() + assert not data["friction_valid"].any() + assert sensor.total_current_contacts == 0 + sim.update(step=150) + assert BaseEnv._get_sensor_obs(env)["contacts"]["is_valid"].any() + + +@pytest.mark.parametrize("history_sample", [False, True]) +def test_dexuni_external_graph_consumes_eager_native_samples( + dexuni_contact_scene, history_sample +): + sim, sensor = dexuni_contact_scene + if history_sample: + sensor.create_history( + "support", sensor.get_actor_ids("cube"), force_threshold=1.0 + ) + sim.update(step=3, after_substep=sensor.update_physics_step) + stream = torch.cuda.Stream(device=sim.device) + stream.wait_stream(torch.cuda.current_stream(sim.device)) + with torch.cuda.stream(stream): + sensor.update() + stream.synchronize() + # This is the review's native same-step warmup/capture sequence. The + # boundary error must occur before DexSim can wait on an uncaptured event. + with pytest.raises(RuntimeError, match="outside external CUDA capture"): + with torch.cuda.graph(torch.cuda.CUDAGraph(), stream=stream): + if history_sample: + sensor.update_physics_step(0.01) + else: + sensor.update() + + # Mixed DexUni reports may provide geometry-only rows with zero impulse. + # Consume their validity so replay freshness does not assume force support. + output = torch.empty_like(sensor.get_data()["is_valid"]) + stream.wait_stream(torch.cuda.current_stream(sim.device)) + graph = torch.cuda.CUDAGraph() + with torch.cuda.graph(graph, stream=stream): + data = sensor.get_data() + output.copy_(data["is_valid"]) + + def replay_latest(): + stream.wait_stream(torch.cuda.current_stream(sim.device)) + with torch.cuda.stream(stream): + graph.replay() + stream.synchronize() + data = sensor.get_data() + torch.testing.assert_close(output, data["is_valid"]) + + replay_latest() + assert output.any() + sim.reset_objects_state(env_ids=[0], excluded_sensor_uids=[sensor.uid]) + sensor.reset([0]) + sensor.update() + replay_latest() + assert output.count_nonzero() == 0 + sim.update( + step=150, after_substep=sensor.update_physics_step if history_sample else None + ) + sensor.update() + replay_latest() + assert output.any() + + @pytest.mark.parametrize("solver_path", ["pure_mujoco", "coupled"]) @pytest.mark.usefixtures("requires_dexuni_contact_query") def test_dexuni_pure_mujoco_contact_sensor_reports_impulse(solver_path: str) -> None: diff --git a/tests/sim/sensors/test_contact_query_sensor.py b/tests/sim/sensors/test_contact_query_sensor.py index 7074269b36..66c87b8b16 100644 --- a/tests/sim/sensors/test_contact_query_sensor.py +++ b/tests/sim/sensors/test_contact_query_sensor.py @@ -165,6 +165,18 @@ def _sensor_for_query(query, capacity=2): ) +def test_contact_report_errors_other_than_reset_are_not_hidden(): + query = _FakeQuery() + sensor = _sensor_for_query(query) + + def fail(): + raise RuntimeError("contact device failure") + + query.fetch_solver_report = fail + with pytest.raises(RuntimeError, match="contact device failure"): + sensor.update() + + def test_default_history_enables_substep_sampling_and_preserves_interval_drops(): query = _FakeQuery() sensor = _sensor_for_query(query) diff --git a/tests/sim/sensors/test_contact_sensor_graph.py b/tests/sim/sensors/test_contact_sensor_graph.py index 74e1118038..2241692cbb 100644 --- a/tests/sim/sensors/test_contact_sensor_graph.py +++ b/tests/sim/sensors/test_contact_sensor_graph.py @@ -266,115 +266,3 @@ def sample() -> None: torch.tensor([[[0.0, 0.0, 2.0]], [[0.0, 0.0, 2.0]]], device=device), ) producer.synchronize() - - -@pytest.mark.parametrize("history_sample", [False, True]) -@pytest.mark.parametrize("capture_after_advance", [False, True]) -@pytest.mark.parametrize("capture_mode", ["same_stream", "other_stream", "lifecycle"]) -def test_outer_capture_preserves_sampling_and_eager_refresh( - history_sample: bool, capture_after_advance: bool, capture_mode: str -) -> None: - """Warmup must not suppress captured fetch_solver_report/scatter/history or eager refresh.""" - wp.init() - device = torch.device("cuda:0") - query = _CudaQuery(device) - source = query.buffer.data.clone() - - def fetch_solver_report_async() -> ContactBuffer: - # Model a backend whose query must copy fresh device data on each replay. - query.buffer.data.copy_(source) - return query.buffer - - query.fetch_solver_report_async = fetch_solver_report_async - owner = SimpleNamespace( - num_envs=2, - physics_step_index=0, - arena_offsets=torch.zeros((2, 3), device=device), - _spawn_scene=SimpleNamespace( - handles=lambda uid: tuple( - SimpleNamespace(path=actor.path) for actor in query.actors - ) - ), - spawn_result=SimpleNamespace(create_contact_query=lambda *a, **kw: query), - ) - sensor = ContactSensor( - ContactSensorCfg( - uid="contacts", rigid_uid_list=["cube"], max_contacts_per_env=2 - ), - device, - owner=owner, - ) - history = ( - sensor.create_history("feet", torch.tensor([[10], [20]], device=device)) - if history_sample - else None - ) - - def sample() -> None: - if history_sample: - sensor.update_physics_step(0.1) - else: - sensor.update() - - def assert_sample(impulse: float) -> None: - assert sensor.get_data()["is_valid"].tolist() == [[True, False], [True, False]] - torch.testing.assert_close( - sensor.get_data()["impulse"][:, 0], - torch.full_like(sensor.get_data()["impulse"][:, 0], impulse), - ) - if history is not None: - torch.testing.assert_close( - history.force, - torch.tensor( - [[[0.0, 0.0, impulse / 0.1]], [[0.0, 0.0, impulse / 0.1]]], - device=device, - ), - ) - - stream = torch.cuda.Stream(device=device) - stream.wait_stream(torch.cuda.current_stream(device)) - producer = ( - stream if capture_mode == "same_stream" else torch.cuda.Stream(device=device) - ) - producer.wait_stream(torch.cuda.current_stream(device)) - with torch.cuda.stream(producer): - sample() - # Even a completed producer must not introduce an uncaptured event into capture. - producer.synchronize() - with torch.cuda.stream(stream): - if capture_after_advance: - owner.physics_step_index += 1 - graph = torch.cuda.CUDAGraph() - with torch.cuda.graph(graph, stream=stream): - if capture_mode == "lifecycle": - sensor.reset() - sensor.begin_control_step() - sample() - # Capture has not executed a new sample. Same-step eager calls must work. - query.buffer.device_counts[0] = 2 - source[:2, 9] = 0.3 - sample() - assert_sample(0.3) - sensor.reset() - for impulse in (0.2, 0.5): - owner.physics_step_index += 1 - source[:2, 9] = impulse - graph.replay() - assert_sample(impulse) - if history is not None: - torch.testing.assert_close( - history.contact_count, - torch.full_like( - history.contact_count, 1 if capture_mode == "lifecycle" else 2 - ), - ) - # Caller-owned replay must restore latest loss after full reset too. - query.buffer.device_counts[0] = 0 - for loss in (3, 0): - sensor.reset() - assert sensor.evidence_dropped_contacts == 0 - query.buffer.device_counts[1] = loss - graph.replay() - assert sensor.latest_sample_dropped_contacts == loss - assert sensor.evidence_dropped_contacts == loss - stream.synchronize() From 0e304ee8e1628a404f3c9541b288729720a699b3 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 9 Nov 2026 10:16:35 +0800 Subject: [PATCH 24/27] feat(cable): adapt foundation and episode defaults to DexSim 0.6 --- agent_context/MAP.yaml | 4 +- .../topics/simulation-system/deformables.md | 57 ++ .../topics/simulation-system/lifecycle.md | 13 +- .../simulation-system/simulation-system.md | 4 + .../embodichain/embodichain.lab.sim.cfg.rst | 31 ++ .../embodichain.lab.sim.objects.rst | 58 ++ docs/source/api_reference/public_api.rst | 1 + embodichain/lab/sim/_runtime_controls.py | 129 +++++ embodichain/lab/sim/cfg/__init__.py | 4 + embodichain/lab/sim/cfg/cable.py | 161 ++++++ embodichain/lab/sim/objects/__init__.py | 1 + embodichain/lab/sim/objects/articulation.py | 24 +- .../objects/backends/articulation_state.py | 21 + .../lab/sim/objects/backends/newton.py | 27 +- embodichain/lab/sim/objects/cable/__init__.py | 24 + embodichain/lab/sim/objects/cable/cable.py | 320 +++++++++++ embodichain/lab/sim/objects/cable/data.py | 138 +++++ embodichain/lab/sim/physics/base.py | 24 + embodichain/lab/sim/physics/newton.py | 98 ++++ embodichain/lab/sim/sim_manager.py | 96 +++- embodichain/lab/sim/spawn/descriptors.py | 123 +++- embodichain/lab/sim/spawn/scene.py | 21 +- examples/README.md | 21 + pyproject.toml | 5 +- tests/sim/objects/test_cable_object.py | 525 ++++++++++++++++++ tests/sim/objects/test_cable_object_cuda.py | 363 ++++++++++++ tests/sim/objects/test_cable_two_way_cuda.py | 239 ++++++++ tests/sim/test_cable_cfg.py | 188 +++++++ tests/sim/test_runtime_controls.py | 296 ++++++++++ tests/sim/test_sim_manager.py | 37 +- tests/test_release_metadata.py | 29 + 31 files changed, 3040 insertions(+), 42 deletions(-) create mode 100644 embodichain/lab/sim/cfg/cable.py create mode 100644 embodichain/lab/sim/objects/cable/__init__.py create mode 100644 embodichain/lab/sim/objects/cable/cable.py create mode 100644 embodichain/lab/sim/objects/cable/data.py create mode 100644 tests/sim/objects/test_cable_object.py create mode 100644 tests/sim/objects/test_cable_object_cuda.py create mode 100644 tests/sim/objects/test_cable_two_way_cuda.py create mode 100644 tests/sim/test_cable_cfg.py diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index edd30064d4..7d003ee101 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -15,7 +15,8 @@ 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, quaternion, xyzw] + fixed-cadence playback, quaternion, xyzw, cable, CableObject, CableObjectCfg, + CableObjectData, 线缆对象] paths: [topics/simulation-system/simulation-system.md] source_of_truth: - embodichain/lab/sim/motion/__init__.py @@ -36,6 +37,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 05b0ec4db1..e4c44dc4d1 100644 --- a/agent_context/topics/simulation-system/deformables.md +++ b/agent_context/topics/simulation-system/deformables.md @@ -95,3 +95,60 @@ 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. + +`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. +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..aef6fba311 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 @@ -62,6 +63,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 +107,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 0617f1eba7..47c5fe2438 100644 --- a/agent_context/topics/simulation-system/simulation-system.md +++ b/agent_context/topics/simulation-system/simulation-system.md @@ -146,6 +146,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. @@ -157,6 +160,7 @@ tests need non-symmetric rotations to expose component-order mistakes. | Backend/device selection, source overlays, collision/mass policy | [Physics configuration](physics-configuration.md); `cfg/`, `spawn/descriptors.py` | | Joint state order, drive lowering, mimic coupling, native adaptation | [Articulation and batch adapters](articulation-adapters.md); `objects/backends/`, `objects/articulation.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 18a97f4b05..29a3cef60d 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.cfg.rst @@ -73,6 +73,9 @@ UV seams and render detail. LightCfg VisualObjectCfg RigidObjectCfg + CableObjectCfg + CablePhysicsCfg + CableAttachmentCfg RigidObjectGroupCfg RigidConstraintCfg URDFCfg @@ -138,6 +141,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 0887ae748c..5870957338 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.objects.rst @@ -558,3 +558,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 67410f4d88..a5ac8787a5 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -3538,6 +3538,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/embodichain/lab/sim/_runtime_controls.py b/embodichain/lab/sim/_runtime_controls.py index 6c0b23954b..0d3416da9e 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,130 @@ def close(self) -> None: """Release runtime particle references retained after initialization.""" self._particle_set = None self._initial_positions = None + + +class _InitialJointState(RuntimeControl): + """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. + """ + + graph_safe = True + + @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 __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: + if self._initializer is not None: + self._initializer.close() diff --git a/embodichain/lab/sim/cfg/__init__.py b/embodichain/lab/sim/cfg/__init__.py index 51e2b2f0ff..40efdb506f 100644 --- a/embodichain/lab/sim/cfg/__init__.py +++ b/embodichain/lab/sim/cfg/__init__.py @@ -40,6 +40,7 @@ ) from .asset import AssetPhysicsMode, ObjectBaseCfg, _resolve_asset_physics_mode from .source import UsdSourceCfg +from .cable import CableAttachmentCfg, CableObjectCfg, CablePhysicsCfg from .deformable import ( SurfaceElementPropertiesCfg, VolumeDeformableMeshingCfg, @@ -123,6 +124,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 b948e3591d..ced4d1f833 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..2e31ec24ff 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, @@ -1101,24 +1102,13 @@ def _state_mimic_info(self) -> _MimicInfo: 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..edb7e42b7b 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,22 +212,13 @@ 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 backend = get_newton_backend(result.world) - if ( - backend is None - or backend.solver_type not in {"mujoco_warp", "dexuni"} - or backend.cfg.requires_grad - or (backend.model is not None and backend.model.requires_grad) - ): - return False solver = backend.mujoco_solver - if solver is None: - return False relation_names = [ (state_joint_names[child_id], state_joint_names[parent_id]) 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 ac1f8544e7..8e7fb57994 100644 --- a/embodichain/lab/sim/physics/newton.py +++ b/embodichain/lab/sim/physics/newton.py @@ -18,10 +18,12 @@ from __future__ import annotations import importlib +from collections.abc import Sequence from typing import TYPE_CHECKING import weakref import warp as wp +import numpy as np from .base import PhysicsBackend @@ -58,6 +60,7 @@ def __init__(self, manager) -> None: self._differentiable_runtime = None self._runtime_device: str | None = None self._configured_solver_type: str | None = None + self._initial_joint_state_targets: set[str] = set() @property def solver_type(self) -> str | None: @@ -100,6 +103,96 @@ 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 freeze episode joint defaults once.""" + manager = self._manager + cables = getattr(manager, "_cable_objects", {}) + if not cables: + return + from embodichain.lab.sim._runtime_controls import _InitialJointState + + 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 + registered = self._initial_joint_state_targets + for uid, values in positions.items(): + if values is not None and not values.size: + continue + for arena_name in scene.arena_names: + target = f"{arena_name}/{uid}" + if target not in registered: + scene.builder.add_runtime_control( + _InitialJointState( + target, + values, + mimic_stabilizer=getattr( + articulations[uid].cfg.root_props, + "newton_mimic_stabilizer", + False, + ), + has_permanent_attachment=uid in attached_articulations, + ) + ) + registered.add(target) + + 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: @@ -179,6 +272,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 6cf61904d1..dd636299ad 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -73,6 +73,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, @@ -95,6 +96,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, @@ -121,6 +123,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, @@ -718,6 +721,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() @@ -1002,6 +1006,7 @@ def asset_uids(self) -> List[str]: uid_list.extend(list(self._rigid_objects.keys())) uid_list.extend(list(self._rigid_object_groups.keys())) uid_list.extend(list(self._deformable_objects.keys())) + uid_list.extend(getattr(self, "_cable_objects", {})) uid_list.extend(list(self._articulations.keys())) return uid_list @@ -1070,6 +1075,7 @@ def start_visualization(self) -> VisualizationRuntime | None: getattr(self, "_rigid_objects", {}), getattr(self, "_rigid_object_groups", {}), getattr(self, "_deformable_objects", {}), + getattr(self, "_cable_objects", {}), getattr(self, "_articulations", {}), getattr(self, "_robots", {}), getattr(self, "_sensors", {}), @@ -1839,6 +1845,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] @@ -2445,13 +2452,14 @@ def get_asset( | VisualObject | RigidObjectGroup | DeformableObject + | 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, + cable, or articulation. Args: uid (str): The UID of the asset. @@ -2473,6 +2481,8 @@ def get_asset( return self._rigid_object_groups[uid] if uid in self._deformable_objects: return self._deformable_objects[uid] + if uid in getattr(self, "_cable_objects", {}): + return self._cable_objects[uid] if uid in self._articulations: return self._articulations[uid] @@ -2950,6 +2960,65 @@ def _declare_rigid_objects( self.prepare() return [item[3] for item in declarations] + 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) + cable = CableObject(cfg, device=self.device) + self._spawn_scene.builder.materials.update( + self._spawn_scene._resolve_materials(materials) + ) + self._spawn_scene.declare( + "cable_object", descriptor.name, descriptor, facade=cable + ) + 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. @@ -4264,6 +4333,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() @@ -5026,6 +5096,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. @@ -5034,6 +5109,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 @@ -5270,6 +5360,7 @@ def _deferred_destroy(self) -> None: "_rigid_objects", "_rigid_object_groups", "_deformable_objects", + "_cable_objects", "_articulations", "_robots", ): @@ -5332,6 +5423,7 @@ def _sever_wrapper_refs(obj_registry): _sever_wrapper_refs("_constraints") _sever_wrapper_refs("_rigid_object_groups") _sever_wrapper_refs("_deformable_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 2435590223..ca010c304a 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", ] @@ -589,6 +598,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 ed4bea6ad6..88dcc598b2 100644 --- a/embodichain/lab/sim/spawn/scene.py +++ b/embodichain/lab/sim/spawn/scene.py @@ -37,6 +37,7 @@ "articulation", "soft_object", "cloth_object", + "cable_object", ] @@ -128,6 +129,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( @@ -158,6 +164,7 @@ def declare( """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}.") + self._require_mutable_topology() self._bind_resource_materials(descriptor) self._initialize_facade_declaration(facade) declaration = _AssetDeclaration( @@ -196,6 +203,7 @@ def declare( "articulation": "add_articulation", "soft_object": "add_soft_object", "cloth_object": "add_cloth_object", + "cable_object": "add_cable", }[kind] declaration.descriptor = getattr(self.builder, add_name)(descriptor) self._assets[uid] = declaration @@ -261,11 +269,12 @@ def remove(self, uid: str) -> None: if getattr(declaration, "declaration_only", False): del self._assets[uid] return - if declaration.kind in {"soft_object", "cloth_object"}: + if declaration.kind in {"soft_object", "cloth_object", "cable_object"}: raise NotImplementedError( "DexSim Spawn does not yet expose pending removal for " f"{declaration.kind.replace('_', ' ')}." ) + self._require_mutable_topology() if declaration.kind == "rigid_object_group": for member in declaration.descriptor: self.builder.remove_object(member.name) @@ -285,6 +294,16 @@ def remove(self, uid: str) -> None: key: materials for key, materials in pending.items() if key[0] != uid } + def _require_mutable_topology(self) -> None: + """Reject unsupported cable-scene rebuild before Builder mutation.""" + if self.builder.is_finalized and any( + item.kind == "cable_object" for item in self._assets.values() + ): + raise NotImplementedError( + "DexSim does not support physical topology changes in a " + "finalized cable scene; 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/examples/README.md b/examples/README.md index a40c1fe3d8..cc2cb26f0c 100644 --- a/examples/README.md +++ b/examples/README.md @@ -83,3 +83,24 @@ 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 demo + +The cable examples require CUDA and the qualified DexSim wheel +`0.6.0.dev11491` (DexVerse dev `505ee1b5600ca0b3d0a2c8235930e7684283d049`). +The build is available from internal GitLab pipeline 77665: job 317267 for +Python 3.12 or job 317266 for Python 3.11. Download the job archive and install +its matching wheel with its declared dependencies; that pipeline did not publish +to the package index. A source checkout alone does not update an installed wheel. + +```bash +python -m pip install /path/to/dexsim_engine-0.6.0.dev11491---manylinux_2_31_x86_64.whl +python -c "import dexsim; from dexsim.scene import SpawnedCable; print(dexsim.__version__, dexsim.__commit_id__)" +python -m embodichain run-example sim/demo/cable_viser --num-envs 2 --port 8080 --duration 12 +``` + +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/pyproject.toml b/pyproject.toml index b86eaa0f5f..64d0fa8f43 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -26,7 +26,8 @@ dynamic = ["version"] # Core install dependencies (kept from requirements.txt). dependencies = [ # Solver contact reports use the DexSim 0.6 API. - "dexsim_engine==0.6.0rc1", + # Cable qualification: DexVerse dev pipeline 77665, commit 505ee1b5. + "dexsim_engine==0.6.0.dev11491", # DexSim and LeRobot require different OpenCV distributions sharing cv2. # Keep their bindings on the same release to avoid mixed Python/native APIs. "opencv-python==4.12.0.88; python_version < '3.12'", @@ -53,7 +54,7 @@ dependencies = [ "ortools", "prettytable", "psutil>=5.9", - "black==26.10.1", + "black==26.3.1", "fvcore", "h5py", "tensordict>=0.13,<0.15; python_version < '3.12'", diff --git a/tests/sim/objects/test_cable_object.py b/tests/sim/objects/test_cable_object.py new file mode 100644 index 0000000000..2beed25fcd --- /dev/null +++ b/tests/sim/objects/test_cable_object.py @@ -0,0 +1,525 @@ +# ---------------------------------------------------------------------------- +# 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 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", + "_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"] + with pytest.raises(ValueError, match="already declared"): + sim.add_cable_object(_cfg()) + + +@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_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() + controls = [call.args[0] for call in builder.add_runtime_control.call_args_list] + 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() + assert builder.add_runtime_control.call_count == 2 + np.testing.assert_allclose( + builder.add_runtime_control.call_args.args[0].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..0ec10e5acf --- /dev/null +++ b/tests/sim/objects/test_cable_object_cuda.py @@ -0,0 +1,363 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +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, + JointDrivePropertiesCfg, + NewtonPhysicsCfg, + RenderCfg, +) + +pytestmark = [ + pytest.mark.subprocess_sim, + pytest.mark.gpu, + pytest.mark.skipif( + not torch.cuda.is_available(), reason="CableObject requires CUDA" + ), +] + + +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), + ) + ) + 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 + 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=[0]) + reset = cable.data.segment_state_w + torch.testing.assert_close(reset[0], initial[0], rtol=0, atol=0) + torch.testing.assert_close(reset[1], after[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], after[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_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(""" + + + +""") + + robot = self.sim.add_articulation( + ArticulationCfg( + 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.add_cable_object( + CableObjectCfg( + uid="wire", + points=[[0, 0, 0], [0.1, 0, 0]], + init_pos=(1, 0, 1), + radius=0.005, + ) + ) + self.sim.prepare() + expected = torch.full_like(robot.get_qpos(), 0.025) + 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, tmp_path: Path) -> None: + _assert_cable_case_in_subprocess( + "test_replicated_state_force_and_cleanup", tmp_path + ) + + @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/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_runtime_controls.py b/tests/sim/test_runtime_controls.py index 531fba0800..029a6d172c 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,298 @@ 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 + + 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 = _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(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 cbba0834ea..5120c7558b 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -2739,7 +2739,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 @@ -3063,7 +3065,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 = {} @@ -3083,6 +3085,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, @@ -3097,7 +3118,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), @@ -3126,7 +3149,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 = {} @@ -3149,7 +3174,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), diff --git a/tests/test_release_metadata.py b/tests/test_release_metadata.py index 399fdf5cb6..3166432a30 100644 --- a/tests/test_release_metadata.py +++ b/tests/test_release_metadata.py @@ -147,6 +147,35 @@ def test_project_metadata_has_no_direct_dependencies() -> None: assert direct_dependencies == [] +@pytest.mark.parametrize( + ("version", "supported"), + [ + ("0.5.1.dev10146", False), + ("0.5.1.dev10194", False), + ("0.5.1.dev10195", False), + ("0.5.1rc1", False), + ("0.5.1rc2", False), + ("0.5.1", False), + ("0.6.0.dev11491", True), + ("0.6.0rc1", False), + ], +) +def test_dexsim_dependency_selects_cable_development_builds( + 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, From 6ca6566312d64ed774290bca9a480057289cb10b Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 9 Nov 2026 10:17:32 +0800 Subject: [PATCH 25/27] feat(cable): integrate unified observations replay and visualization --- agent_context/MAP.yaml | 3 +- .../topics/env-framework/env-framework.md | 1 + .../topics/env-framework/execution.md | 13 +- .../topics/manager-functor/manager-functor.md | 8 +- .../topics/sim-visualization/configuration.md | 13 +- .../topics/sim-visualization/runtime.md | 14 +- .../sim-visualization/sim-visualization.md | 3 +- embodichain/lab/gym/envs/embodied_env.py | 7 + .../lab/gym/envs/managers/observations.py | 38 ++- embodichain/lab/gym/envs/wrapper/replay.py | 78 ++++- .../lab/gym/utils/_component_composition.py | 1 + embodichain/lab/gym/utils/gym_utils.py | 35 ++- embodichain/lab/gym/utils/trajectory_state.py | 30 +- embodichain/lab/visualization/runtime.py | 16 +- .../lab/visualization/scene_exporter.py | 126 +++++++- examples/sim/demo/cable_viser.py | 115 +++++++ tests/gym/envs/test_cable_recording_cuda.py | 223 +++++++++++++ tests/gym/envs/test_embodied_env_cables.py | 292 ++++++++++++++++++ tests/gym/envs/test_replay_wrapper.py | 97 ++++++ tests/gym/utils/test_gym_utils.py | 36 +++ tests/gym/utils/test_trajectory_state.py | 46 +++ tests/sim/test_sim_manager.py | 10 +- tests/visualization/test_runtime.py | 75 +++++ tests/visualization/test_scene_exporter.py | 219 +++++++++++++ 24 files changed, 1454 insertions(+), 45 deletions(-) create mode 100644 examples/sim/demo/cable_viser.py create mode 100644 tests/gym/envs/test_cable_recording_cuda.py create mode 100644 tests/gym/envs/test_embodied_env_cables.py diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 7d003ee101..5896d44ed7 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -180,7 +180,8 @@ topics: title: Simulation Visualization aliases: [browser visualization, viser, sim visualization, 仿真可视化] keywords: [VisualizationCfg, ViserServerCfg, SceneManifest, visualization, EntityGizmoManipulator, robot_ik_gizmo, - GizmoCfg, backpressure, capture_visualization, Newton deformable, particle set, render topology, quaternion, xyzw] + GizmoCfg, backpressure, capture_visualization, Newton deformable, particle set, render topology, quaternion, xyzw, + cable visualization, cable capsule] paths: [topics/sim-visualization/sim-visualization.md] source_of_truth: - embodichain/lab/visualization/protocol.py diff --git a/agent_context/topics/env-framework/env-framework.md b/agent_context/topics/env-framework/env-framework.md index b2fe81f739..9abefe1145 100644 --- a/agent_context/topics/env-framework/env-framework.md +++ b/agent_context/topics/env-framework/env-framework.md @@ -24,6 +24,7 @@ Lightweight RL environments have a separate owner in | Raw schema, optional history and exact historical queries | `managers/observation_manager.py`; Env dispatches capture/reset/close boundaries | | Optional measured physical objectives | `embodichain/lab/gym/envs/objectives/`; Gym owns observation/reset hooks independently of expert acceptance | | Controller-ready commands | `embodichain/lab/gym/envs/types.py`: `ControllerAction` | +| Cable scene declarations and raw recording | `gym_utils.py` → `EmbodiedEnv._setup_interactive_objects()`; `trajectory_state.py` → `observations.get_entity_state` | | Demo segment execution / outcomes | `embodichain/lab/gym/envs/demo.py` | | Configured Task Program registration | `embodichain/lab/gym/envs/task_program/registration.py` | | Timing instrumentation | `embodichain/lab/gym/utils/profiler.py` | diff --git a/agent_context/topics/env-framework/execution.md b/agent_context/topics/env-framework/execution.md index 831d980574..6a67704a63 100644 --- a/agent_context/topics/env-framework/execution.md +++ b/agent_context/topics/env-framework/execution.md @@ -285,7 +285,8 @@ not orchestrate multiple experiments or qualify kinematic playback. cursors keep their accepted-versus-diagnostic semantics; there is no independent history instance or write lifecycle. Standard state fields use observation functors; the host does not perform a separate physics read in its write hook. Full robot state, - articulations and selected rigid objects retain their existing action schema. + articulations, selected rigid objects and cable segments share that acquisition + and retain their existing action schema. `cfg.trajectory_uids` restricts non-robot replay fields. Raw channels, T+1 timing and lifetime queries are owned by the [recording contract](env-framework.md#expert-trajectory-contract). @@ -311,6 +312,16 @@ not orchestrate multiple experiments or qualify kinematic playback. keeps its own cursor and layout. Internally allocated expert policy observations include the terminal T+1 slot in the backing store; the public T-transition view and existing dataset action/annotation alignment stay compatible. +- **Cable observations**: `states.cables[uid].segment_state_w` and the matching + raw observation field share the T+1 rollout storage. `get_entity_state` describes + world segment position, XYZW orientation, COM linear velocity and angular + velocity, preserving each cable's segment topology and row validity. The field + records observations, not solver history. `ReplayWrapper` accepts dynamic + replay only when every recorded initial state matches the full reset; it + rejects scrubbing, partial reset and broadcasting world-space cable states. + `trajectory_state.restore_trajectory_state()` rejects cable entries before + any write. Use action replay to validate a new episode, not a visible-state + restore as a history-preserving checkpoint. - **Fixed action layout**: an ActionManager-backed trajectory uses the batched policy action space; a controller/expert trajectory uses the private `ExpertActionSpec` layout. One trajectory cannot mix these schemas. diff --git a/agent_context/topics/manager-functor/manager-functor.md b/agent_context/topics/manager-functor/manager-functor.md index ff298eb981..b68b2bfdbe 100644 --- a/agent_context/topics/manager-functor/manager-functor.md +++ b/agent_context/topics/manager-functor/manager-functor.md @@ -103,8 +103,12 @@ matching their source functions/parameters, including callable strings and omitted default arguments. Env only supplies the declarations. Recording-only fields never enlarge policy observations or policy rollout allocation. -`observations.get_entity_state` derives native UID, quantity, arena/joint frame, -representation, shape, dtype and joint order from its source. Immutable +`observations.get_entity_state` derives native UID, quantity, reference frame, +representation, shape, dtype and joint order from its source. Cable +`segment_state_w` uses world coordinates and a segment axis; its descriptor +includes COM linear velocity and angular velocity alongside position and XYZW +orientation. Recording declares these fields through `trajectory_observation_cfgs`, +so cable observations share the ordinary acquisition and history lifecycle. Immutable schema descriptors are reused by semantic value; no simulation handles enter the bounded cache. Pose acquisition uses the resolved UID without constructing another `SceneEntityCfg` during each step. Explicit boundaries diff --git a/agent_context/topics/sim-visualization/configuration.md b/agent_context/topics/sim-visualization/configuration.md index 5d535d012b..7907ce5aee 100644 --- a/agent_context/topics/sim-visualization/configuration.md +++ b/agent_context/topics/sim-visualization/configuration.md @@ -19,7 +19,7 @@ are nested under `visualization.viser_server`; there is no top-level | `point_cloud_max_points` | `100000` | Deterministic per-cloud downsampling limit | | `sensor_image_fps` | `2.0` | Maximum RGB image capture rate; `None` captures once per eligible simulation step | | `soft_body_fps` | `5.0` | Maximum soft-body and cloth vertex capture rate | -| `allow_commands` | `False` | Allow trusted Viser clients to drag configured Gizmos; common `--viser` launchers set this to `True` | +| `allow_commands` | `False` | Allow trusted Viser clients to drag configured Gizmos; common `--viser` launchers enable commands when the deployment does not specify a policy | | `viser_server` | `ViserServerCfg()` | Server bind configuration | ### `ViserServerCfg` @@ -76,7 +76,10 @@ manifest when manager APIs marked the topology dirty. `merge_args_with_gym_config()` makes `--viser` set `headless=True`, changes the backend to `"viser"`, and applies the CLI sampling/server overrides. A -file-based config may also contain a `visualization` section. The old nested +file-based config may also contain a `visualization` section. An explicit +`visualization.allow_commands=False` remains read-only when `--viser` enables +the viewer, so automated deployments can reserve robot drive targets for their +expert. The old nested key `visualization.server` is accepted and normalized to `visualization.viser_server`. @@ -87,9 +90,9 @@ limiting. `visualization_cfg_from_args()` only builds visualization config. `SimulationManagerCfg` makes the simulation headless whenever the resulting -backend is `"viser"`. The common `--viser` option also enables Gizmo commands; -there is no separate `--viser-gizmo` option. Programmatic configurations may -retain `allow_commands=False` for read-only deployments. +backend is `"viser"`. The common `--viser` option defaults to enabling Gizmo +commands when no file policy is present; there is no separate `--viser-gizmo` +option. Programmatic configurations retain their configured command policy. ### Asset preview diff --git a/agent_context/topics/sim-visualization/runtime.md b/agent_context/topics/sim-visualization/runtime.md index 1f16ad3350..d0f2e6c20a 100644 --- a/agent_context/topics/sim-visualization/runtime.md +++ b/agent_context/topics/sim-visualization/runtime.md @@ -6,7 +6,7 @@ Read this when the request needs these details. [Topic overview](sim-visualizati | API | Behavior | |---|---| -| `start_visualization()` | No-op for backend `"none"`; otherwise starts or returns the active runtime and forces an initial capture | +| `start_visualization()` | No-op for backend `"none"`; prepares declared assets (including cable-only scenes), otherwise leaves an empty Spawn builder open; starts or returns the active runtime and forces an initial capture | | `notify_visualization_topology_changed()` | Increments the manager-local topology revision | | `refresh_visualization()` | Rebuilds and asynchronously publishes a manifest immediately | | `capture_visualization(force=False)` | Refreshes dirty topology, then captures a due frame | @@ -22,7 +22,7 @@ Drawing markers and capturing visualization do not advance physics; interactive loops call `SimulationManager.update(step=1)` to process Gizmos and step the world. Manager add methods mark topology dirty for rigid objects, rigid-object groups, -volume/surface deformables, robots, articulations, and `Camera` sensors. Supported +volume/surface deformables, cables, robots, articulations, and `Camera` sensors. Supported `remove_asset()` branches do the same. The next simulation update refreshes the browser automatically. Use `refresh_visualization()` when the refresh must happen before another physics step. Code that changes mesh topology outside @@ -48,6 +48,8 @@ fields already named `wxyz` remain protocol-native. Arena offsets are added to rigid, robot, articulation, and camera poses. Deformable vertices are stored relative to the corresponding arena node. +Cable `segment_pose_w` already contains world positions, including Arena +offsets; export must not add those offsets again. ### Exported content @@ -55,6 +57,7 @@ Deformable vertices are stored relative to the corresponding arena node. |---|---| | `RigidObject` | Complete combined render mesh; multi-segment faces receive matching vertex offsets | | `RigidObjectGroup` | One node and pose per constituent object | +| `CableObject` | One capsule node per segment, shared rest geometry, and live world poses | | `Robot` | One mesh node per non-empty link | | `Articulation` | One mesh node per non-empty link | | `VolumeDeformableObject` | Live Newton render-surface vertices and triangles | @@ -68,6 +71,13 @@ are not exported as scene nodes by the current `SceneExporter`. Visual materials and textures are not mirrored; meshes use category colors from `SceneExporter._COLORS`. +Cables use capsule observer geometry generated from configured endpoint +distances and radius, centered along each segment's local +Z axis. Export does +not depend on native rendering resources and does not reproduce tube skins or +custom visual meshes. Prepared cable data is sampled once per cable per frame; +segment poses update at `scene_fps` through the static-mesh batch path, without +rebuilding geometry or using the deformable-vertex channel. + ## Cameras Only sensors whose `sensor_type` is exactly `"Camera"` are added to the diff --git a/agent_context/topics/sim-visualization/sim-visualization.md b/agent_context/topics/sim-visualization/sim-visualization.md index 181d834a91..5b0516ab78 100644 --- a/agent_context/topics/sim-visualization/sim-visualization.md +++ b/agent_context/topics/sim-visualization/sim-visualization.md @@ -98,7 +98,8 @@ this invariant. | Symptom | Cause / action | |---|---| | `Visualization env_ids ... outside simulation range` | Selected IDs do not exist in the configured arenas. Validate them against `SimulationManager.num_envs`. | -| Startup timeout or address-in-use error | The Viser worker did not become ready or the configured port is occupied. Select another port and inspect `visualization_health.worker_error`. | +| Startup timeout, possibly just after Viser starts listening | The worker startup budget includes Viser frontend dependency installation/build and initial manifest publication. `VisualizationRuntime.start()` waits up to 120 seconds by default and returns as soon as ready; an unusually slow build may need a longer explicit `start(timeout=...)`. Inspect startup logs before retrying. | +| Address-in-use error | The configured port is occupied. Select another port and inspect `visualization_health.worker_error`. | | Asset added after startup is missing | Step once, call `refresh_visualization()`, or mark topology dirty if the change bypassed manager APIs. | | Newton Viser shows only the grid after declaring a robot | Check that Viser startup did not finalize an empty Spawn scene before asset declaration and that link poses remain finite after the first update. | | A Newton articulation link is missing geometry or emits `mesh_id 0 out of range` | Export all render-body mesh segments and treat a zero-mesh link as empty geometry. Do not use the single-mesh articulation helper for Newton render bodies. | diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index 4ea89db809..90c4390744 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -54,6 +54,7 @@ VisualObjectCfg, RigidObjectGroupCfg, ArticulationCfg, + CableObjectCfg, LightCfg, ) from embodichain.lab.sim.objects import Robot @@ -241,6 +242,9 @@ class EnvLightCfg: articulation: List[ArticulationCfg] = [] + cable_object: list[CableObjectCfg] = [] + """Fixed-topology cables declared before simulation preparation.""" + events: Union[object, None] = None """Event settings. Defaults to None, in which case no events are applied through the event manager. @@ -3141,6 +3145,9 @@ def _setup_interactive_objects(self) -> None: for cfg in self.cfg.rigid_object_group: self.sim.add_rigid_object_group(cfg=cfg) + for cfg in self.cfg.cable_object: + self.sim.add_cable_object(cfg=cfg) + def preview_sensor_data( self, name: str, diff --git a/embodichain/lab/gym/envs/managers/observations.py b/embodichain/lab/gym/envs/managers/observations.py index 4f30bb24ff..9bb652e345 100644 --- a/embodichain/lab/gym/envs/managers/observations.py +++ b/embodichain/lab/gym/envs/managers/observations.py @@ -81,6 +81,16 @@ def _state_descriptor( names: tuple[str, ...], ) -> ObservationDescriptor: # Only immutable schema values are cached; no environment or asset handle. + if quantity == "segment_state_w": + return ObservationDescriptor( + uid, + quantity, + "world", + "xyz_xyzw_com_lin_vel_ang_vel", + ("m", "unitless", "m/s", "rad/s"), + shape, + dtype, + ) joint = quantity in ("qpos", "qvel") return ObservationDescriptor( uid, @@ -128,7 +138,9 @@ def get_entity_state( env: EmbodiedEnv, obs: EnvObs, entity_cfg: SceneEntityCfg, - quantity: Literal["pose", "qpos", "qvel", "lin_vel", "ang_vel"] = "pose", + quantity: Literal[ + "pose", "qpos", "qvel", "lin_vel", "ang_vel", "segment_state_w" + ] = "pose", ) -> ObservationSample: """Acquire a described original state channel for any observation consumer. @@ -136,7 +148,9 @@ def get_entity_state( functors. Missing entities remain invalid, even though policy views retain the historical zero-fill behavior. Joint channels contain the complete asset-defined order, including mimic joints; selectors are rejected rather - than silently describing a subset as the complete state. + than silently describing a subset as the complete state. Cable + ``segment_state_w`` contains each segment's world position, XYZW quaternion, + COM linear velocity and angular velocity; it does not include solver history. Args: env: Environment owning the native scene. @@ -147,7 +161,14 @@ def get_entity_state( Returns: Raw values and explicit row validity, before policy transformations. """ - if quantity not in ("pose", "qpos", "qvel", "lin_vel", "ang_vel"): + if quantity not in ( + "pose", + "qpos", + "qvel", + "lin_vel", + "ang_vel", + "segment_state_w", + ): raise ValueError(f"Unsupported raw state quantity: {quantity!r}.") if entity_cfg.joint_names is not None or entity_cfg.joint_ids != slice(None): raise ValueError("Raw entity state requires the complete joint order.") @@ -165,11 +186,14 @@ def produce() -> ObservationSample: joint = quantity in ("qpos", "qvel") if obj is None: width = 0 if joint else 7 if quantity == "pose" else 3 - values = torch.zeros((env.num_envs, width), device=env.device) + shape = (0, 13) if quantity == "segment_state_w" else (width,) + values = torch.zeros((env.num_envs, *shape), device=env.device) valid = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) errors = (f"Entity {uid!r} is unavailable.",) * env.num_envs else: - if quantity == "pose": + if quantity == "segment_state_w": + values = obj.data.segment_state_w + elif quantity == "pose": values = obj.get_local_pose(to_matrix=False) elif joint: values = obj.get_qpos() if quantity == "qpos" else obj.get_qvel() @@ -180,9 +204,9 @@ def produce() -> ObservationSample: else: values = getattr(obj.body_data, quantity) valid = torch.isfinite(values).reshape(env.num_envs, -1).all(dim=-1) - if quantity == "pose": + if quantity in ("pose", "segment_state_w"): valid &= ( - (torch.linalg.vector_norm(values[..., 3:], dim=-1) > 0) + (torch.linalg.vector_norm(values[..., 3:7], dim=-1) > 0) .reshape(env.num_envs, -1) .all(dim=-1) ) diff --git a/embodichain/lab/gym/envs/wrapper/replay.py b/embodichain/lab/gym/envs/wrapper/replay.py index f9766e26b4..5e72e9ca9c 100644 --- a/embodichain/lab/gym/envs/wrapper/replay.py +++ b/embodichain/lab/gym/envs/wrapper/replay.py @@ -29,7 +29,10 @@ ) from embodichain.lab.gym.envs.types import ControllerAction from embodichain.lab.gym.utils.gym_utils import load_trajectory -from embodichain.lab.gym.utils.trajectory_state import restore_trajectory_state +from embodichain.lab.gym.utils.trajectory_state import ( + capture_trajectory_state, + restore_trajectory_state, +) from embodichain.utils import logger if TYPE_CHECKING: @@ -60,6 +63,10 @@ class ReplayWrapper(gym.Wrapper): full resets only. Stepping past exhaustion raises instead of extending objective dwell time by repeating the final command. + Cable recordings support dynamic replay from matching full-reset states + only. Their visible state omits contact/solver history, so state scrubbing, + partial resets and broadcasting world-space cable recordings are rejected. + Args: env: The environment to wrap (constructed without ``record_trajectory``). trajectory: A ``.pt`` path or loaded dict from @@ -82,6 +89,23 @@ def __init__( self._mode = mode self._trajectory = load_trajectory(trajectory) meta = self._trajectory["meta"] + self._cable_replay = "cables" in self._trajectory["states"].keys() + if ( + getattr(getattr(env, "sim", None), "_cable_objects", {}) + and not self._cable_replay + ): + raise ValueError( + "Cable replay requires recorded initial states for every scene cable; include them in trajectory_uids." + ) + if self._cable_replay: + if mode != "dynamic": + raise NotImplementedError( + "Cable recordings require dynamic replay from reset." + ) + if int(meta["num_envs"]) != env.num_envs: + raise ValueError( + "World-space cable recordings require matching environment counts." + ) self._controller_spec = None self._task_outcome_replay = mode == "dynamic" and ( getattr(env, "termination_manager", None) is not None @@ -201,13 +225,22 @@ def _expand_to_env_count(self) -> None: def reset( self, *, seed: int | None = None, options: dict | None = None ) -> tuple[EnvObs, dict]: - if self._objective_replay and options is not None and "reset_ids" in options: - raise ValueError("Physical-objective replay does not support reset_ids.") + if ( + (self._objective_replay or self._cable_replay) + and options is not None + and "reset_ids" in options + ): + raise ValueError( + "Physical-objective/cable replay does not support reset_ids." + ) obs, info = self.env.reset(seed=seed, options=options) # Disable physics during restore so set_local_pose's internal update # does not integrate dynamics. - self.env.sim.enable_physics(False) - restore_trajectory_state(self.env, self._trajectory["states"][:, 0]) + if self._cable_replay: + self._validate_cable_reset_state() + else: + self.env.sim.enable_physics(False) + restore_trajectory_state(self.env, self._trajectory["states"][:, 0]) if self._mode == "dynamic": self.env.sim.enable_physics(True) self.env._replay_no_auto_reset = True @@ -234,6 +267,37 @@ def reset( self.env._record_initial_observation(frame, env_ids) return obs, info + def _validate_cable_reset_state(self) -> None: + """Validate initial observations without approximating a checkpoint.""" + initial = self._trajectory["states"][:, 0] + for category, assets in ( + ("cables", getattr(self.env.sim, "_cable_objects", {})), + ("articulations", self.env.sim._articulations), + ("rigid_objects", self.env.sim._rigid_objects), + ): + recorded = ( + set(initial[category].keys()) if category in initial.keys() else set() + ) + if not recorded.issubset(assets) or ( + category == "cables" and recorded != set(assets) + ): + raise ValueError( + f"Cable replay scene does not match recorded {category}." + ) + current = initial.clone() + capture_trajectory_state( + self.env, current, torch.arange(self.env.num_envs, device=self.env.device) + ) + for key, expected in initial.items(include_nested=True, leaves_only=True): + observed = current[key].to(expected) + if observed.shape != expected.shape or not torch.allclose( + observed, expected, rtol=1e-5, atol=1e-6 + ): + raise ValueError( + f"Cable replay initial state {key!r} differs from the full reset; " + "a recorded observation cannot restore solver history." + ) + def step( self, action: Any ) -> tuple[EnvObs, torch.Tensor, torch.Tensor, torch.Tensor, dict]: @@ -282,6 +346,10 @@ def go_to_step(self, step: int) -> EnvObs: Returns: The observation at the target step. """ + if self._cable_replay: + raise NotImplementedError( + "Cable recordings do not support state scrubbing." + ) env = self.env max_step = self.control_max_step step = max(0, min(int(step), max_step)) diff --git a/embodichain/lab/gym/utils/_component_composition.py b/embodichain/lab/gym/utils/_component_composition.py index e4150a0986..d37d3a152d 100644 --- a/embodichain/lab/gym/utils/_component_composition.py +++ b/embodichain/lab/gym/utils/_component_composition.py @@ -62,6 +62,7 @@ "rigid_object", "rigid_object_group", "articulation", + "cable_object", } ) diff --git a/embodichain/lab/gym/utils/gym_utils.py b/embodichain/lab/gym/utils/gym_utils.py index 7c79e884c0..f18515e338 100644 --- a/embodichain/lab/gym/utils/gym_utils.py +++ b/embodichain/lab/gym/utils/gym_utils.py @@ -651,6 +651,7 @@ def config_to_cfg( MeshCfg, RigidObjectGroupCfg, ArticulationCfg, + CableObjectCfg, LightCfg, DLSSCfg, NRDCfg, @@ -965,6 +966,13 @@ class ComponentCfg: resolve_asset_paths(cfg) env_cfg.articulation.append(cfg) + env_cfg.cable_object = [] + for value in config.get("cable_object", []): + cable = CableObjectCfg.from_dict(value) + if isinstance(cable.visual_shape, MeshCfg): + cable.visual_shape.fpath = get_data_path(cable.visual_shape.fpath) + env_cfg.cable_object.append(cable) + env_cfg.control_parts = config["env"].get("control_parts", None) env_cfg.sim_steps_per_control = config["env"].get("sim_steps_per_control", 4) ignore_terminations = config["env"].get( @@ -1555,7 +1563,9 @@ def merge_args_with_gym_config(args: argparse.Namespace, gym_config: dict) -> di if cli_visualization.env_ids is None else list(cli_visualization.env_ids) ) - visualization["allow_commands"] = cli_visualization.allow_commands + # Enabling the viewer must preserve a deployment's explicit read-only + # policy, especially while an expert owns the robot drive targets. + visualization.setdefault("allow_commands", cli_visualization.allow_commands) legacy_server = visualization.pop("server", {}) viser_server = deepcopy(visualization.get("viser_server", legacy_server)) viser_server["host"] = cli_visualization.viser_server.host @@ -2308,6 +2318,29 @@ def _zeros(*shape: int) -> torch.Tensor: device=device, ) + cables = { + uid: obj + for uid, obj in getattr(env.sim, "_cable_objects", {}).items() + if uids is None or uid in uids + } + if cables: + states["cables"] = TensorDict( + { + uid: TensorDict( + { + "segment_state_w": _zeros( + num_envs, max_steps, obj.data.num_segments, 13 + ) + }, + batch_size=[num_envs, max_steps], + device=device, + ) + for uid, obj in cables.items() + }, + batch_size=[num_envs, max_steps], + device=device, + ) + td: dict = { "states": TensorDict(states, batch_size=[num_envs, max_steps], device=device) } diff --git a/embodichain/lab/gym/utils/trajectory_state.py b/embodichain/lab/gym/utils/trajectory_state.py index 1646703586..75a8c389a0 100644 --- a/embodichain/lab/gym/utils/trajectory_state.py +++ b/embodichain/lab/gym/utils/trajectory_state.py @@ -72,7 +72,10 @@ def capture_trajectory_state( env_ids: torch.Tensor, step_ids: torch.Tensor | None = None, ) -> None: - """Capture restorable simulation state into a trajectory state view. + """Capture robot/rigid states and cable observations into a trajectory view. + + Cable segment states contain world XYZ, XYZW, COM linear velocity and + angular velocity. They omit solver history and do not define a checkpoint. Args: env: Environment containing the robot and simulation scene entities. @@ -111,6 +114,16 @@ def capture_trajectory_state( step_ids, ) + if "cables" in states.keys(): + for uid, cable in env.sim._cable_objects.items(): + if uid in states["cables"].keys(): + _assign_state( + states["cables"][uid]["segment_state_w"], + cable.data.segment_state_w, + env_ids, + step_ids, + ) + if "rigid_objects" in states.keys(): rigid_states = states["rigid_objects"] for uid, rigid_object in env.sim._rigid_objects.items(): @@ -145,10 +158,22 @@ def restore_trajectory_state( ) -> None: """Restore one trajectory timestep into an environment scene. + Cable recordings require action replay from an episode reset. A cable + entry is rejected before any robot or object write, because restoring its + visible pose and velocity cannot restore solver/contact history. + Args: env: Environment containing the robot and simulation scene entities. states: One timestep of recorded robot and scene-object states. + + Raises: + NotImplementedError: The recording contains cable observations. """ + if "cables" in states.keys(): + raise NotImplementedError( + "Recorded cable state is an observation, not a solver-history " + "checkpoint. Reset the episode and replay its commands instead." + ) robot_states = states["robot"] env.robot.set_local_pose(robot_states["root_pose"]) # Restore complete qpos, including mimic joints. Kinematic replay disables @@ -241,6 +266,9 @@ def add(prefix: str, uid: str, quantities: tuple[str, ...]) -> None: for uid in env.sim._rigid_objects: if uids is None or uid in uids: add("rigid_objects", uid, ("pose", "lin_vel", "ang_vel")) + for uid in getattr(env.sim, "_cable_objects", {}): + if uids is None or uid in uids: + add("cables", uid, ("segment_state_w",)) return result diff --git a/embodichain/lab/visualization/runtime.py b/embodichain/lab/visualization/runtime.py index 8bf02824a0..84c4d9d295 100644 --- a/embodichain/lab/visualization/runtime.py +++ b/embodichain/lab/visualization/runtime.py @@ -443,8 +443,14 @@ def _raise_worker_error(self) -> None: "Visualization update thread failed." ) from self._worker_error - def start(self, timeout: float = 10.0) -> None: - """Build the initial manifest and start the backend update thread.""" + def start(self, timeout: float = 120.0) -> None: + """Build the initial manifest and start the backend update thread. + + Args: + timeout: Maximum seconds to wait for backend readiness, including + any frontend dependency installation/build and initial manifest + publication. Returns as soon as the backend is ready. + """ if self.is_running: return manifest = self.exporter.build_manifest() @@ -461,7 +467,11 @@ def start(self, timeout: float = 10.0) -> None: self._thread.start() if not self._ready_event.wait(timeout=timeout): self.stop(timeout=timeout) - raise TimeoutError("Timed out while starting the visualization backend.") + raise TimeoutError( + f"Timed out after {timeout:g}s while starting the visualization backend. " + "Check backend startup logs; frontend installation/build may " + "require a longer start(timeout=...) budget." + ) self._raise_worker_error() self._next_capture_time = perf_counter() self._next_image_capture_time = perf_counter() diff --git a/embodichain/lab/visualization/scene_exporter.py b/embodichain/lab/visualization/scene_exporter.py index f3e08f2e8e..b8079e6655 100644 --- a/embodichain/lab/visualization/scene_exporter.py +++ b/embodichain/lab/visualization/scene_exporter.py @@ -84,6 +84,44 @@ def mesh_geometry_id( return f"sha256:{digest.hexdigest()}" +def _capsule_mesh(radius: float, length: float) -> tuple[np.ndarray, np.ndarray]: + """Build a centered +Z capsule; length is the distance between cap centers.""" + radial_segments = 12 + cap_segments = 4 + angles = np.arange(radial_segments) * (2.0 * np.pi / radial_segments) + latitudes = np.linspace(0.0, np.pi / 2.0, cap_segments + 1)[:-1] + ring_radii = radius * np.cos(np.concatenate((-latitudes[::-1], latitudes))) + ring_heights = np.concatenate( + ( + -length / 2.0 - radius * np.sin(latitudes[::-1]), + length / 2.0 + radius * np.sin(latitudes), + ) + ) + rings = np.empty((len(ring_heights), radial_segments, 3), dtype=np.float32) + rings[..., 0] = ring_radii[:, None] * np.cos(angles) + rings[..., 1] = ring_radii[:, None] * np.sin(angles) + rings[..., 2] = ring_heights[:, None] + vertices = np.concatenate( + ( + [[0.0, 0.0, -length / 2.0 - radius]], + rings.reshape(-1, 3), + [[0.0, 0.0, length / 2.0 + radius]], + ) + ).astype(np.float32) + faces: list[tuple[int, int, int]] = [] + for side in range(radial_segments): + next_side = (side + 1) % radial_segments + faces.append((0, 1 + next_side, 1 + side)) + for ring in range(len(ring_heights) - 1): + lower = 1 + ring * radial_segments + upper = lower + radial_segments + faces.append((lower + side, lower + next_side, upper + side)) + faces.append((lower + next_side, upper + next_side, upper + side)) + top_ring = 1 + (len(ring_heights) - 1) * radial_segments + faces.append((top_ring + side, top_ring + next_side, len(vertices) - 1)) + return vertices, np.asarray(faces, dtype=np.uint32) + + @dataclass(frozen=True) class CaptureResult: """A captured frame and the time spent copying scene data.""" @@ -105,8 +143,7 @@ class _NodeSource: node: SceneNode asset_key: tuple[str, str] asset: object - link_index: int | None = None - object_index: int | None = None + component_index: int | None = None @dataclass(frozen=True) @@ -153,6 +190,7 @@ class SceneExporter: "articulation_link": (255, 175, 70), "soft_object": (230, 120, 255), "cloth_object": (255, 105, 145), + "cable_segment": (255, 210, 85), } def __init__( @@ -239,12 +277,7 @@ def _build_pose_batches(sources: tuple[_NodeSource, ...]) -> tuple[_PoseBatch, . batches: list[_PoseBatch] = [] for asset_key, indexed_sources in grouped.items(): component_indices = [ - ( - source.link_index - if source.link_index is not None - else source.object_index - ) - for _, source in indexed_sources + source.component_index for _, source in indexed_sources ] has_components = component_indices[0] is not None if any( @@ -363,6 +396,7 @@ def build_manifest(self) -> SceneManifest: ) self._append_rigid_object_groups(sources, geometries) + self._append_cable_objects(sources, geometries) self._append_articulations( sources=sources, geometries=geometries, @@ -562,7 +596,65 @@ def _append_rigid_object_groups( node=node, asset_key=("rigid_group", uid), asset=asset, - object_index=object_index, + component_index=object_index, + ) + ) + + def _append_cable_objects( + self, + sources: list[_NodeSource], + geometries: dict[str, MeshGeometry], + ) -> None: + """Export segment capsules independently of native tube/custom visuals.""" + geometry_ids: dict[tuple[float, float], str] = {} + for uid in self._sim.get_cable_object_uid_list(): + asset = self._sim.get_cable_object(uid) + if asset is None: + continue + data = asset.data + if data is None: + raise RuntimeError( + f"Call SimulationManager.prepare() before exporting cable {uid!r}." + ) + points = _to_numpy(asset.cfg.points, np.float32, copy=False) + lengths = np.linalg.norm(np.diff(points, axis=0), axis=1) + if data.num_segments != len(lengths): + raise ValueError( + f"Cable {uid!r} has {data.num_segments} segments but " + f"its configured points describe {len(lengths)}." + ) + radius = float(asset.cfg.radius) + uid_component = safe_path_component(uid) + for segment_index, length in enumerate(lengths): + dimensions = (radius, float(length)) + if dimensions not in geometry_ids: + vertices, faces = _capsule_mesh(*dimensions) + geometry_id = self._add_geometry( + geometries, vertices, faces, self._COLORS["cable_segment"] + ) + assert geometry_id is not None + geometry_ids[dimensions] = geometry_id + for env_id in self._env_ids: + node = SceneNode( + node_id=( + f"env:{env_id}/cable:{uid_component}/" + f"segment:{segment_index}" + ), + path=( + f"/envs/{env_id}/cable_objects/{uid_component}/" + f"segments/{segment_index}" + ), + parent_id=f"env:{env_id}/cable:{uid_component}", + env_id=env_id, + kind="cable_segment", + geometry_id=geometry_ids[dimensions], + ) + sources.append( + _NodeSource( + node=node, + asset_key=("cable", uid), + asset=asset, + component_index=segment_index, ) ) @@ -722,7 +814,7 @@ def _append_articulations( node=node, asset_key=(asset_prefix, uid), asset=asset, - link_index=link_index, + component_index=link_index, ) ) @@ -849,6 +941,13 @@ def capture( ) if batch.asset_kind in {"rigid", "rigid_group", "visual"}: pose = batch.asset.get_local_pose(to_matrix=False) + elif batch.asset_kind == "cable": + data = batch.asset.data + if data is None: + raise RuntimeError( + "Call SimulationManager.prepare() before capturing cable state." + ) + pose = data.segment_pose_w else: pose = batch.asset.body_data.body_link_pose all_positions, all_wxyz = pose_to_position_wxyz(pose) @@ -864,9 +963,10 @@ def capture( batch.env_ids, batch.component_indices, ] - positions[batch.destination_indices] = ( - batch_positions + self._env_offsets[batch.env_ids] - ) + # Cable snapshots already contain world positions, including Arena origins. + if batch.asset_kind != "cable": + batch_positions = batch_positions + self._env_offsets[batch.env_ids] + positions[batch.destination_indices] = batch_positions wxyz[batch.destination_indices] = batch_wxyz dynamic_meshes: list[DynamicMeshUpdate] = [] diff --git a/examples/sim/demo/cable_viser.py b/examples/sim/demo/cable_viser.py new file mode 100644 index 0000000000..13f6e32806 --- /dev/null +++ b/examples/sim/demo/cable_viser.py @@ -0,0 +1,115 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Preview replicated, anchored cables in Viser until Ctrl+C. + +Requires CUDA and a DexSim build with cable support. Viser displays segment +capsules without a native renderer. Gravity bends each cable; the scene resets +every six simulated seconds so newly connected browsers can see the motion. +""" + +from __future__ import annotations + +import argparse +import time + +import numpy as np + +from embodichain.lab.sim import SimulationManager, SimulationManagerCfg +from embodichain.lab.sim.cfg import ( + CableObjectCfg, + CablePhysicsCfg, + NewtonPhysicsCfg, + RenderCfg, +) +from embodichain.lab.visualization import VisualizationCfg, ViserServerCfg + + +def main() -> None: + """Run a paced cable-only scene and release its resources on exit.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--port", type=int, default=8080) + parser.add_argument("--num-envs", type=int, default=2) + parser.add_argument( + "--duration", + type=float, + default=0.0, + help="Simulated seconds; 0 runs until Ctrl+C.", + ) + args = parser.parse_args() + if args.num_envs < 1 or args.duration < 0: + parser.error("--num-envs must be positive and --duration must be nonnegative") + + fps = 60 + dt = 1.0 / fps + cfg = SimulationManagerCfg( + headless=True, + device="cuda:0", + num_envs=args.num_envs, + arena_space=2.0, + physics_dt=dt, + enable_entity_gizmo=False, + render_cfg=RenderCfg(renderer="no-render"), + physics_cfg=NewtonPhysicsCfg( + solver_cfg={"solver_type": "dexuni"}, + num_substeps=8, + sync_to_renderer=False, + ), + visualization=VisualizationCfg( + backend="viser", + env_ids=None, + scene_fps=30.0, + viser_server=ViserServerCfg(host="127.0.0.1", port=args.port), + ), + ) + sim = SimulationManager(cfg) + try: + points = np.zeros((25, 3), dtype=np.float32) + points[:, 0] = np.linspace(0.0, 0.8, len(points)) + sim.add_cable_object( + CableObjectCfg( + uid="preview_cable", + points=points, + radius=0.01, + init_pos=(-0.4, 0.0, 1.2), + fixed_segments=(0,), + render_mode="capsules", + attrs=CablePhysicsCfg(bend_stiffness=0.01, twist_stiffness=0.01), + ) + ) + runtime = sim.start_visualization() + sim.capture_visualization(force=True) + print( + f"Open {runtime.endpoint} — {args.num_envs} cables; Ctrl+C to stop.", + flush=True, + ) + frame = 0 + while args.duration == 0.0 or frame * dt < args.duration: + started = time.perf_counter() + if frame and frame % (6 * fps) == 0: + sim.reset_objects_state() + sim.capture_visualization(force=True) + sim.update(step=1) + frame += 1 + time.sleep(max(0.0, dt - (time.perf_counter() - started))) + except KeyboardInterrupt: + pass + finally: + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + + +if __name__ == "__main__": + main() diff --git a/tests/gym/envs/test_cable_recording_cuda.py b/tests/gym/envs/test_cable_recording_cuda.py new file mode 100644 index 0000000000..7187fa5dd4 --- /dev/null +++ b/tests/gym/envs/test_cable_recording_cuda.py @@ -0,0 +1,223 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Real cable observations across Gym steps and selective episode resets.""" + +from __future__ import annotations + +import faulthandler +import gc +import os +from pathlib import Path +import subprocess +import sys + +import pytest + +pytestmark = [pytest.mark.subprocess_sim, pytest.mark.gpu] + + +def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( + tmp_path: Path, +) -> None: + # A fresh process preserves NoRender without the shared test engine's + # renderer initialization. No downloaded robot or scene assets are needed. + urdf = tmp_path / "slider.urdf" + urdf.write_text( + """ + + + +""", + encoding="utf-8", + ) + result = subprocess.run( + [sys.executable, str(Path(__file__).resolve()), str(urdf)], + env=os.environ.copy(), + capture_output=True, + text=True, + timeout=180, + ) + assert result.returncode == 0, result.stdout + result.stderr + + +def _run_recording(urdf: str) -> None: + import torch + + from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg + from embodichain.lab.gym.envs.managers import ObservationCfg, SceneEntityCfg + from embodichain.lab.gym.envs.managers.observations import get_entity_state + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + from embodichain.lab.sim.cfg import ( + CableObjectCfg, + NewtonPhysicsCfg, + RenderCfg, + RobotCfg, + ) + + cfg = EmbodiedEnvCfg( + num_envs=2, + max_episode_steps=4, + sim_steps_per_control=1, + ignore_terminations=True, + record_trajectory=True, + trajectory_auto_save=False, + trajectory_uids=["wire", "long_wire"], + sim_cfg=SimulationManagerCfg( + headless=True, + device="cuda:0", + arena_space=3.0, + physics_dt=1 / 120, + enable_entity_gizmo=False, + startup_summary="off", + render_cfg=RenderCfg(renderer="no-render"), + physics_cfg=NewtonPhysicsCfg( + device="cuda:0", + num_substeps=2, + solver_cfg={"solver_type": "dexuni"}, + ), + ), + robot=RobotCfg( + uid="slider", + fpath=urdf, + init_pos=(-1.0, 0.0, 1.0), + asset_physics_mode="overlay", + ), + cable_object=[ + CableObjectCfg( + uid=uid, + points=[[0.1 * index, 0, 0] for index in range(segments + 1)], + radius=0.005, + init_pos=(0.0, y, 1.0), + ) + for uid, segments, y in (("wire", 2, 0.0), ("long_wire", 3, 0.5)) + ], + observations={ + "wire": ObservationCfg( + func=get_entity_state, + name="wire_alias", + mode="add", + consumers=("policy", "recording"), + params={ + "entity_cfg": SceneEntityCfg(uid="wire"), + "quantity": "segment_state_w", + }, + ) + }, + ) + env = None + try: + env = EmbodiedEnv(cfg) + 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} + fields = {uid: f"cables/{uid}/segment_state_w" for uid in cables} + episodes = env.episode_context.episode_ids + expected = { + uid: [cable.data.segment_state_w.clone()] for uid, cable in cables.items() + } + torch.testing.assert_close(obs["wire_alias"], expected["wire"][0]) + assert manager.field_ids["wire_alias"] == manager.field_ids[fields["wire"]] + + action = torch.zeros((2, 1), device=env.device) + for _ in range(2): + env.step(action) + for uid, cable in cables.items(): + expected[uid].append(cable.data.segment_state_w.clone()) + assert env._traj_steps.tolist() == [2, 2] + window = manager.read_history_window( + [0, 0], + [2, 2], + fields=list(fields.values()), + episode_ids=episodes, + device=env.device, + ) + for uid, field in fields.items(): + states = torch.stack(expected[uid], dim=1) + assert states.shape == (2, 3, cables[uid].data.num_segments, 13) + assert bool((states[:, -1, :, 2] < states[:, 0, :, 2]).all()) + observed = torch.stack(window["fields"][field]["values"]) + torch.testing.assert_close(observed, states, rtol=0, atol=0) + assert bool(torch.stack(window["fields"][field]["valid"]).all()) + descriptor = manager.schema[field] + assert descriptor.coordinate_frame == "world" + assert descriptor.shape == (cables[uid].data.num_segments, 13) + assert descriptor.representation == "xyz_xyzw_com_lin_vel_ang_vel" + recorded = env._traj_buffer["states", "cables", uid, "segment_state_w"] + assert recorded.shape[1] == cfg.max_episode_steps + 1 + torch.testing.assert_close(recorded[:, :3], states, rtol=0, atol=0) + + # Reset row 0 through the public lifecycle. Row 1 must keep both its + # physical state and all three historical boundaries, including S2. + env.reset(options={"reset_ids": [0]}) + new_episodes = env.episode_context.episode_ids + assert new_episodes == (episodes[0] + 1, episodes[1]) + assert env._traj_steps.tolist() == [0, 2] + reset_state = manager.read_history( + [0, 2], fields=list(fields.values()), episode_ids=new_episodes + ) + for uid, field in fields.items(): + native = cables[uid].data.segment_state_w + torch.testing.assert_close(native[0], expected[uid][0][0], rtol=0, atol=0) + torch.testing.assert_close(native[1], expected[uid][-1][1], rtol=0, atol=0) + torch.testing.assert_close( + reset_state.values[field], native.cpu(), rtol=0, atol=0 + ) + with pytest.raises(ValueError, match="episode"): + manager.read_history( + [0], fields=[fields["wire"]], episode_ids=[episodes[0]], env_ids=[0] + ) + + env.step(action) + assert env._traj_steps.tolist() == [1, 3] + latest = manager.read_history( + [1, 3], fields=list(fields.values()), episode_ids=new_episodes + ) + retained = manager.read_history_window( + [0], + [2], + fields=list(fields.values()), + episode_ids=[episodes[1]], + env_ids=[1], + device=env.device, + ) + for uid, field in fields.items(): + torch.testing.assert_close( + latest.values[field], + cables[uid].data.segment_state_w.cpu(), + rtol=0, + atol=0, + ) + torch.testing.assert_close( + retained["fields"][field]["values"][0], + torch.stack(expected[uid], dim=1)[1], + rtol=0, + atol=0, + ) + finally: + if env is not None: + env.close(exit_process=False) + env = None + gc.collect() + SimulationManager.flush_cleanup_queue() + + +if __name__ == "__main__": + faulthandler.enable() + faulthandler.dump_traceback_later(120) + _run_recording(sys.argv[1]) diff --git a/tests/gym/envs/test_embodied_env_cables.py b/tests/gym/envs/test_embodied_env_cables.py new file mode 100644 index 0000000000..872e4dfd2f --- /dev/null +++ b/tests/gym/envs/test_embodied_env_cables.py @@ -0,0 +1,292 @@ +# ---------------------------------------------------------------------------- +# 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 scene composition through the public Gym configuration loader.""" + +from __future__ import annotations + +from pathlib import Path +from unittest.mock import MagicMock + +import pytest +import yaml + +from embodichain.lab.gym.envs import EmbodiedEnv +from embodichain.lab.gym.utils.gym_utils import config_to_cfg +from embodichain.lab.sim.cfg import CableObjectCfg, CablePhysicsCfg + +pytestmark = pytest.mark.no_sim + + +@pytest.mark.parametrize("component", [False, True], ids=["inline", "component"]) +def test_cable_config_is_declared_before_environment_preparation( + tmp_path: Path, component: bool +) -> None: + cable = { + "uid": "wire", + "points": [[0, 0, 1], [0.2, 0, 1]], + "radius": 0.01, + "fixed_segments": [0], + "attrs": {"bend_stiffness": 0.1}, + } + config = { + "id": "EmbodiedEnv-v1", + "physics": "newton", + "env": {}, + "robot": {"uid": "robot"}, + "cable_object": [cable], + } + if component: + (tmp_path / "env.yaml").write_text( + yaml.safe_dump( + { + "environment_id": "EmbodiedEnv-v1", + "physics": "newton", + "env": {}, + "simulation": {"cable_object": [cable]}, + } + ) + ) + config = { + "id": "EmbodiedEnv-v1", + "environment": {"component": "env.yaml"}, + "robot": {"uid": "robot"}, + } + cfg = config_to_cfg(config, source_path=tmp_path / "task.yaml") + assert len(cfg.cable_object) == 1 + assert isinstance(cfg.cable_object[0], CableObjectCfg) + assert isinstance(cfg.cable_object[0].attrs, CablePhysicsCfg) + assert cfg.cable_object[0].attrs.bend_stiffness == 0.1 + + env = object.__new__(EmbodiedEnv) + env.cfg = cfg + env.sim = MagicMock() + env._setup_interactive_objects() + env.sim.add_cable_object.assert_called_once_with(cfg=cfg.cable_object[0]) + env.sim.prepare.assert_not_called() + + +@pytest.mark.parametrize("resource", ["registry", "relative"]) +@pytest.mark.parametrize("component", [False, True]) +def test_cable_visual_mesh_resolves_from_dataset_or_declaring_component( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch, resource: str, component: bool +) -> None: + from embodichain import data + + owner = tmp_path / "environment" if component else tmp_path + owner.mkdir(exist_ok=True) + mesh = owner / "wire.obj" + mesh.write_text("v 0 0 0\n") + reference = "CableAssets/wire.obj" if resource == "registry" else "./wire.obj" + resolved = [] + + def get_data_path(path: str) -> str: + resolved.append(path) + assert path == (reference if resource == "registry" else str(mesh)) + return str(mesh) + + monkeypatch.setattr(data, "get_data_path", get_data_path) + cable = { + "uid": "wire", + "points": [[0, 0, 1], [0.2, 0, 1]], + "radius": 0.01, + "visual_shape": {"fpath": reference}, + } + config = { + "id": "EmbodiedEnv-v1", + "physics": "newton", + "env": {}, + "robot": {"uid": "robot"}, + "cable_object": [cable], + } + if component: + (owner / "env.yaml").write_text( + yaml.safe_dump( + { + "environment_id": "EmbodiedEnv-v1", + "physics": "newton", + "env": {}, + "simulation": {"cable_object": [cable]}, + } + ) + ) + config = { + "id": "EmbodiedEnv-v1", + "environment": {"component": "environment/env.yaml"}, + "robot": {"uid": "robot"}, + } + cfg = config_to_cfg(config, source_path=tmp_path / "task.yaml") + assert cfg.cable_object[0].visual_shape.fpath == str(mesh) + assert len(resolved) == 1 + assert cable["visual_shape"]["fpath"] == reference + + +@pytest.mark.parametrize("uids", [None, ["wire"]]) +def test_cable_recording_uses_shared_observation_history_and_t_plus_one_slots( + uids, +) -> None: + from types import SimpleNamespace + + import gymnasium.spaces + import torch + from tensordict import TensorDict + + from embodichain.lab.gym.envs.episode_context import EpisodeContext + from embodichain.lab.gym.envs.managers import ( + ObservationCfg, + ObservationManager, + SceneEntityCfg, + ) + from embodichain.lab.gym.envs.managers.observations import get_entity_state + from embodichain.lab.gym.utils.gym_utils import ( + _init_raw_rollout, + build_trajectory_buffer, + ) + from embodichain.lab.gym.utils.trajectory_state import ( + export_observation_history, + trajectory_observation_cfgs, + ) + + class CableData: + def __init__(self, segments: int): + self.num_segments = segments + self.state = torch.zeros(2, segments, 13) + self.state[..., 6] = 1 + self.reads = 0 + + @property + def segment_state_w(self): + self.reads += 1 + return self.state + + wires = { + uid: SimpleNamespace(data=CableData(segments)) + for uid, segments in (("wire", 2), ("other", 4)) + } + pose = torch.zeros(2, 7) + pose[:, 6] = 1 + robot = SimpleNamespace( + uid="robot", + dof=1, + joint_names=["joint"], + get_local_pose=lambda to_matrix=False: pose, + get_qpos=lambda: torch.zeros(2, 1), + get_qvel=lambda: torch.zeros(2, 1), + ) + assets = {"robot": robot, **wires} + context = EpisodeContext(2) + context._begin_episode([0, 1]) + context._finish_initialization([0, 1]) + env = SimpleNamespace( + num_envs=2, + device=torch.device("cpu"), + robot=robot, + episode_context=context, + sim=SimpleNamespace( + _articulations={}, + _rigid_objects={}, + _cable_objects=wires, + asset_uids=list(assets), + get_asset=assets.get, + ), + ) + alias = ObservationCfg( + func=get_entity_state, + mode="add", + name="wire_alias", + params={ + "entity_cfg": SceneEntityCfg(uid="wire"), + "quantity": "segment_state_w", + }, + consumers=("policy", "recording"), + ) + manager = ObservationManager( + {"alias": alias}, env, recording=trajectory_observation_cfgs(env, uids) + ) + env.observation_manager = manager + trajectory = build_trajectory_buffer( + env, + max_steps=3, + num_envs=2, + device="cpu", + uids=uids, + action_space=gymnasium.spaces.Box(-1, 1, shape=(2, 1)), + ) + storage = TensorDict({"trajectory": trajectory}, batch_size=[2]) + env._rollout_storage = storage + fields = { + "/".join(path): value + for path, value in trajectory["states"].items( + include_nested=True, leaves_only=True + ) + } + _init_raw_rollout(storage, context, fields, trajectory["actions"][:, :2]) + manager.bind_rollout_storage(storage) + + def capture(slot): + return manager.capture( + lambda: manager.compute(TensorDict({}, batch_size=[2])), + episode_context=context, + timestamp=slot * 0.04, + ) + + for slot in range(3): + for wire in wires.values(): + wire.data.state[..., 0] = torch.tensor([slot, 100 + slot])[:, None] + wires["wire"].data.state[1, :, 6] = 0 if slot == 1 else 1 + frame = capture(slot) + # A policy consumer may mutate its view without corrupting recorded state. + frame.policy["wire_alias"].add_(1000) + if slot == 0: + EmbodiedEnv._record_initial_observation(env, frame, torch.arange(2)) + else: + env._traj_raw_action = torch.full((2, 1), float(slot)) + EmbodiedEnv._write_trajectory_step(env, frame) + + selected = set(wires) if uids is None else set(uids) + assert set(trajectory["states", "cables"].keys()) == selected + for uid, wire in wires.items(): + assert wire.data.reads == (3 if uid in selected else 0) + if uid in selected: + state = trajectory["states", "cables", uid, "segment_state_w"] + assert state.shape == (2, 3, wire.data.num_segments, 13) + torch.testing.assert_close( + state[:, :, 0, 0], + torch.tensor([[0.0, 1.0, 2.0], [100.0, 101.0, 102.0]]), + ) + field = "cables/wire/segment_state_w" + assert manager.field_ids["wire_alias"] == manager.field_ids[field] + descriptor = manager.schema[field] + assert descriptor.coordinate_frame == "world" + assert descriptor.representation == "xyz_xyzw_com_lin_vel_ang_vel" + assert descriptor.units == ("m", "unitless", "m/s", "rad/s") + window = manager.read_history_window( + [0], [2], fields=[field], episode_ids=[0], env_ids=[1] + ) + assert window["fields"][field]["valid"][0].tolist() == [True, False, True] + exported = export_observation_history(manager) + assert exported["fields"][field]["descriptor"]["shape"] == (2, 13) + assert storage["raw_steps"].tolist() == [2, 2] + + context._begin_episode([0]) + context._finish_initialization([0]) + wires["wire"].data.state[0, :, 0] = 99 + EmbodiedEnv._record_initial_observation(env, capture(3), torch.tensor([0])) + assert storage["raw_steps"].tolist() == [0, 2] + retained = manager.read_history([2], fields=[field], episode_ids=[0], env_ids=[1]) + assert retained.values[field][0, 0, 0] == 102 + with pytest.raises(ValueError, match="episode"): + manager.read_history([0], fields=[field], episode_ids=[0], env_ids=[0]) diff --git a/tests/gym/envs/test_replay_wrapper.py b/tests/gym/envs/test_replay_wrapper.py index 38f3858821..d9c3d20c44 100644 --- a/tests/gym/envs/test_replay_wrapper.py +++ b/tests/gym/envs/test_replay_wrapper.py @@ -204,3 +204,100 @@ def test_dynamic_replay_decodes_position_velocity_expert_action(fake_restore): def test_replay_rejects_unknown_mode(): with pytest.raises(ValueError, match="Invalid replay mode"): ReplayWrapper(_FakeEnv(), _trajectory(), mode="unsupported") + + +def _cable_replay_case(): + env = _FakeEnv() + trajectory = _trajectory() + cable_state = torch.zeros(1, 3, 13) + cable_state[..., 6] = 1 + trajectory["states"]["cables"] = TensorDict( + { + "wire": { + "segment_state_w": cable_state[:, None].expand(-1, 2, -1, -1).clone() + } + }, + batch_size=[1, 2], + ) + env.sim._cable_objects = { + "wire": SimpleNamespace(data=SimpleNamespace(segment_state_w=cable_state)) + } + env.sim._articulations = {} + env.sim._rigid_objects = {} + env.robot.get_qpos = lambda: torch.tensor([[0.0, 1.0]]) + env.robot.get_local_pose = lambda: torch.zeros(1, 7) + return env, trajectory + + +def test_cable_dynamic_replay_uses_reset_without_restoring_observations(fake_restore): + env, trajectory = _cable_replay_case() + wrapper = ReplayWrapper(env, trajectory, mode="dynamic") + wrapper.reset() + assert env.last_restored is None + assert env.sim.physics_enabled == [True] + wrapper.step(None) + torch.testing.assert_close(env.received_actions[0], trajectory["actions"][:, 0]) + with pytest.raises(NotImplementedError, match="scrubbing"): + wrapper.go_to_step(1) + with pytest.raises(ValueError, match="reset_ids"): + wrapper.reset(options={"reset_ids": [0]}) + + +def test_cable_dynamic_replay_rejects_a_mid_episode_initial_observation(fake_restore): + env, trajectory = _cable_replay_case() + trajectory["states"]["cables", "wire", "segment_state_w"][0, 0, 0, 0] = 0.1 + wrapper = ReplayWrapper(env, trajectory, mode="dynamic") + with pytest.raises(ValueError, match="differs from the full reset"): + wrapper.reset() + assert env.last_restored is None and not env.received_actions + + +@pytest.mark.parametrize("mode", ["kinematic", "control"]) +def test_cable_replay_rejects_history_free_state_modes_before_mutation(mode): + env, trajectory = _cable_replay_case() + with pytest.raises(NotImplementedError, match="dynamic"): + ReplayWrapper(env, trajectory, mode=mode) + assert env.sim.physics_enabled == [] + + +def test_cable_replay_rejects_broadcast_world_states(): + env, trajectory = _cable_replay_case() + with pytest.raises(ValueError, match="environment counts"): + ReplayWrapper(_FakeEnv(num_envs=2), trajectory, mode="dynamic") + + +def test_cable_scene_replay_rejects_recording_without_cable_initial_observations(): + env, trajectory = _cable_replay_case() + del trajectory["states"]["cables"] + with pytest.raises(ValueError, match="every scene cable"): + ReplayWrapper(env, trajectory, mode="dynamic") + assert env.sim.physics_enabled == [] + + +def test_cable_replay_preserves_task_context_and_initial_observation_history( + fake_restore, +): + from unittest.mock import Mock + + env, trajectory = _cable_replay_case() + initial = {"schema_version": 1, "targets": {"goal": [0.0, 0.0, 1.0]}} + trajectory["meta"]["demo_episodes"] = [{"task_initial_state": initial}] + env.termination_manager = object() + env.restore_task_initial_state = Mock() + env.get_info = lambda: {"task_context": "restored"} + env.observation_manager = SimpleNamespace(has_history=True) + frame = SimpleNamespace(policy={"recorded_observation": torch.ones(1, 1)}) + env._capture_observation = lambda: frame + env._record_initial_observation = Mock() + env._seed_recording_state = Mock() + + wrapper = ReplayWrapper(env, trajectory, mode="dynamic") + obs, info = wrapper.reset() + + assert env.last_restored is None + env.restore_task_initial_state.assert_called_once_with(initial) + assert obs is frame.policy + assert info == {"task_context": "restored"} + assert env._record_initial_observation.call_args.args[0] is frame + assert env._record_initial_observation.call_args.args[1].tolist() == [0] + assert env._seed_recording_state.call_args.args[0] is frame.policy diff --git a/tests/gym/utils/test_gym_utils.py b/tests/gym/utils/test_gym_utils.py index 1dc9bcb274..6fe6e83726 100644 --- a/tests/gym/utils/test_gym_utils.py +++ b/tests/gym/utils/test_gym_utils.py @@ -872,6 +872,24 @@ def test_viser_launcher_flag_implies_headless_viser_commands(): assert merged_config["visualization"]["allow_commands"] is True +@pytest.mark.parametrize("allow_commands", [False, True]) +def test_viser_launcher_preserves_explicit_command_policy(allow_commands): + parser = argparse.ArgumentParser() + add_env_launcher_args_to_parser(parser) + config = { + "id": "Dummy-v0", + "physics": "default", + "visualization": {"allow_commands": allow_commands}, + } + original = deepcopy(config) + + merged = merge_args_with_gym_config(parser.parse_args(["--viser"]), config) + + assert merged["visualization"]["backend"] == "viser" + assert merged["visualization"]["allow_commands"] is allow_commands + assert config == original + + def test_sensor_and_extra_obs_together(): """Test that both sensors and extra observations work together.""" config = { @@ -2586,6 +2604,24 @@ def test_build_trajectory_buffer_uids_filter(): assert "ball" not in buf["states"]["rigid_objects"].keys() +@pytest.mark.parametrize("uids", [None, ["wire"], ["other"]]) +def test_cable_trajectory_buffer_preserves_topology_and_uid_selection(uids): + env = _stub_env() + env.sim._cable_objects = { + "wire": SimpleNamespace(data=SimpleNamespace(num_segments=3)), + "other": SimpleNamespace(data=SimpleNamespace(num_segments=5)), + } + buffer = build_trajectory_buffer( + env, max_steps=4, num_envs=2, device="cpu", uids=uids + ) + cables = buffer["states", "cables"] + assert set(cables.keys()) == set(uids or env.sim._cable_objects) + for uid in cables.keys(): + state = cables[uid, "segment_state_w"] + assert state.shape == (2, 4, env.sim._cable_objects[uid].data.num_segments, 13) + assert state.dtype == torch.float32 + + def test_load_trajectory_validates_and_returns_dict(tmp_path): data = { "states": TensorDict({"a": torch.zeros(1, 4)}, batch_size=[1, 4]), diff --git a/tests/gym/utils/test_trajectory_state.py b/tests/gym/utils/test_trajectory_state.py index 16ed488476..58b498b472 100644 --- a/tests/gym/utils/test_trajectory_state.py +++ b/tests/gym/utils/test_trajectory_state.py @@ -138,3 +138,49 @@ def test_restore_trajectory_state_writes_complete_robot_state() -> None: robot.set_qpos.assert_called_once_with(recorded_qpos, target=False) robot.set_qvel.assert_called_once_with(recorded_qvel, target=False) + + +def test_cable_capture_is_world_observation_and_selected_rows_only() -> None: + from embodichain.lab.gym.utils.gym_utils import build_trajectory_buffer + + cable_state = torch.arange(2 * 3 * 13, dtype=torch.float32).reshape(2, 3, 13) + robot = SimpleNamespace( + dof=2, + get_local_pose=lambda: torch.zeros(2, 7), + get_qpos=lambda: torch.zeros(2, 2), + get_qvel=lambda: torch.zeros(2, 2), + ) + cable = SimpleNamespace( + data=SimpleNamespace(num_segments=3, segment_state_w=cable_state) + ) + env = SimpleNamespace( + robot=robot, + sim=SimpleNamespace( + _articulations={}, _rigid_objects={}, _cable_objects={"wire": cable} + ), + ) + buffer = build_trajectory_buffer(env, max_steps=3, num_envs=2, device="cpu") + capture_trajectory_state( + env, buffer["states"], torch.tensor([1]), torch.tensor([2]) + ) + torch.testing.assert_close( + buffer["states", "cables", "wire", "segment_state_w"][1, 2], cable_state[1] + ) + assert not buffer["states", "cables", "wire", "segment_state_w"][0].any() + filtered = build_trajectory_buffer( + env, max_steps=3, num_envs=2, device="cpu", uids=["other"] + ) + assert "cables" not in filtered["states"].keys() + + +def test_cable_restore_fails_before_any_robot_or_scene_write() -> None: + import pytest + + robot = MagicMock() + states = { + "robot": {"qpos": torch.ones(2, 2), "root_pose": torch.ones(2, 7)}, + "cables": {"wire": {"segment_state_w": torch.zeros(2, 3, 13)}}, + } + with pytest.raises(NotImplementedError, match="solver-history"): + restore_trajectory_state(SimpleNamespace(robot=robot, sim=MagicMock()), states) + assert robot.mock_calls == [] diff --git a/tests/sim/test_sim_manager.py b/tests/sim/test_sim_manager.py index 5120c7558b..c50de2a698 100644 --- a/tests/sim/test_sim_manager.py +++ b/tests/sim/test_sim_manager.py @@ -1274,10 +1274,12 @@ def test_start_visualization_rejects_open_native_window() -> None: sim.start_visualization() -def test_start_visualization_does_not_prepare_an_empty_declaration_phase( +@pytest.mark.parametrize("has_cable", [False, True], ids=["empty", "cable-only"]) +def test_start_visualization_prepares_only_when_assets_are_declared( monkeypatch: pytest.MonkeyPatch, + has_cable: bool, ) -> None: - """Starting Viser must not finalize Newton before assets are declared.""" + """Viser prepares cable-only scenes but leaves an empty builder open.""" class _Exporter: def __init__(self, sim: SimulationManager, cfg: object) -> None: @@ -1293,6 +1295,7 @@ def __init__(self, exporter: _Exporter, cfg: object) -> None: self.captures = 0 def start(self) -> None: + assert self.exporter.sim.prepare.call_count == int(has_cable) self.is_running = True def capture(self, **_kwargs: object) -> None: @@ -1315,6 +1318,7 @@ def capture(self, **_kwargs: object) -> None: sim._rigid_objects = {} sim._rigid_object_groups = {} sim._deformable_objects = {} + sim._cable_objects = {"cable": object()} if has_cable else {} sim._articulations = {} sim._robots = {} sim._sensors = {} @@ -1331,7 +1335,7 @@ def capture(self, **_kwargs: object) -> None: runtime = sim.start_visualization() - sim.prepare.assert_not_called() + assert sim.prepare.call_count == int(has_cable) sim.sync_render_state.assert_not_called() assert runtime is not None assert runtime.captures == 1 diff --git a/tests/visualization/test_runtime.py b/tests/visualization/test_runtime.py index 8d009a64d9..eacade19df 100644 --- a/tests/visualization/test_runtime.py +++ b/tests/visualization/test_runtime.py @@ -326,6 +326,81 @@ def test_runtime_keeps_backend_lifecycle_on_update_thread() -> None: assert runtime.stats.published_image_frames == 1 +@pytest.mark.parametrize("timeout", [None, 5.0], ids=["default", "explicit-timeout"]) +def test_runtime_allows_client_build_but_honors_explicit_startup_deadline( + monkeypatch: pytest.MonkeyPatch, timeout: float | None +) -> None: + """Model a 20-second frontend build without slowing down the test suite.""" + build_seconds = 20.0 + finish_build = threading.Event() + + class _BuildingBackend(_Backend): + def start(self) -> None: + super().start() + assert finish_build.wait(timeout=2.0) + + published = threading.Event() + backend = _BuildingBackend(published, threading.Event()) + runtime = VisualizationRuntime( + _Exporter(published), VisualizationCfg(backend="viser"), backend=backend + ) + real_wait = runtime._ready_event.wait + + def wait_for_build(timeout: float) -> bool: + # Model elapsed build time at the readiness boundary; keep the actual + # worker, manifest publication, and cleanup running on their real thread. + finish_build.set() + if timeout < build_seconds: + return False + return real_wait(timeout=2.0) + + monkeypatch.setattr(runtime._ready_event, "wait", wait_for_build) + try: + if timeout is None: + runtime.start() + assert runtime.health.status == "running" + assert runtime.health.published_scene_revision == 1 + assert not backend.stopped + else: + with pytest.raises( + TimeoutError, match="starting the visualization backend" + ): + runtime.start(timeout=timeout) + assert not runtime.is_running + assert backend.stopped + finally: + finish_build.set() + runtime.stop() + assert len(set(backend.thread_ids)) == 1 + + +def test_runtime_reports_backend_start_failure_without_waiting_for_timeout() -> None: + failure = ValueError("Viser client build failed") + + class _FailingBackend(_Backend): + def start(self) -> None: + super().start() + raise failure + + published = threading.Event() + backend = _FailingBackend(published, threading.Event()) + runtime = VisualizationRuntime( + _Exporter(published), VisualizationCfg(backend="viser"), backend=backend + ) + try: + with pytest.raises( + RuntimeError, match="Visualization update thread failed" + ) as error: + runtime.start() + assert error.value.__cause__ is failure + assert runtime.health.status == "failed" + finally: + with pytest.raises(RuntimeError, match="Visualization update thread failed"): + runtime.stop() + assert backend.stopped + assert not runtime.is_running + + @pytest.mark.parametrize("due", ["none", "pose", "deformable", "image", "forced"]) def test_capture_publishes_source_once_and_only_when_a_consumer_is_due(due): published = threading.Event() diff --git a/tests/visualization/test_scene_exporter.py b/tests/visualization/test_scene_exporter.py index 9e3d09fb75..8ad3084cce 100644 --- a/tests/visualization/test_scene_exporter.py +++ b/tests/visualization/test_scene_exporter.py @@ -19,6 +19,7 @@ from types import SimpleNamespace import numpy as np +import pytest from embodichain.lab.visualization import ( FrameOverlay, @@ -225,6 +226,12 @@ def get_deformable_object_uid_list(self) -> list[str]: def get_deformable_object(self, uid: str) -> None: raise AssertionError(f"Unexpected deformable-object lookup: {uid}") + def get_cable_object_uid_list(self) -> list[str]: + return [] + + def get_cable_object(self, uid: str) -> None: + raise AssertionError(f"Unexpected cable-object lookup: {uid}") + def get_sensor_uid_list(self) -> list[str]: return [] @@ -260,6 +267,12 @@ def get_deformable_object_uid_list(self) -> list[str]: def get_deformable_object(self, uid: str) -> None: raise AssertionError(f"Unexpected deformable-object lookup: {uid}") + def get_cable_object_uid_list(self) -> list[str]: + return [] + + def get_cable_object(self, uid: str) -> None: + raise AssertionError(f"Unexpected cable-object lookup: {uid}") + def get_sensor_uid_list(self) -> list[str]: return [] @@ -267,6 +280,61 @@ def get_sensor(self, uid: str) -> None: raise AssertionError(f"Unexpected sensor lookup: {uid}") +class _CableData: + def __init__(self, arena_offsets: np.ndarray) -> None: + self.num_instances = len(arena_offsets) + self.num_segments = 3 + self.poses = np.zeros( + (self.num_instances, self.num_segments, 7), dtype=np.float32 + ) + self.poses[..., :3] = arena_offsets[:, None, :] + np.array( + [[0.1, 0.2, 0.3], [0.4, 0.5, 0.6], [0.7, 0.8, 0.9]] + ) + self.poses[..., 3:] = np.array([1.0, 2.0, 3.0, 4.0]) / np.sqrt(30.0) + self.fetch_count = 0 + self.released = False + + @property + def segment_pose_w(self) -> np.ndarray: + if self.released: + raise RuntimeError("Cable batch has been released") + self.fetch_count += 1 + return self.poses + + +class _CableObject: + def __init__(self, arena_offsets: np.ndarray) -> None: + self.cfg = SimpleNamespace( + points=np.array( + [ + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.25], + [0.25, 0.0, 0.25], + [0.25, 0.0, 0.75], + ] + ), + radius=0.02, + render_mode="tube", + ) + self.data = _CableData(arena_offsets) + + +class _CableSimulation(_EmptySimulation): + num_envs = 3 + arena_offsets = np.array( + [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [-7.0, 8.0, 9.0]], dtype=np.float32 + ) + + def __init__(self) -> None: + self.cables = {"cable/main": _CableObject(self.arena_offsets)} + + def get_cable_object_uid_list(self) -> list[str]: + return list(self.cables) + + def get_cable_object(self, uid: str) -> _CableObject: + return self.cables[uid] + + class _JointControlProvider: def __init__(self) -> None: self.specs = tuple( @@ -535,6 +603,157 @@ def test_empty_scene_can_publish_a_current_frame() -> None: assert result.frame.positions.shape == (0, 3) +@pytest.mark.parametrize("render_mode", ["tube", "capsules"]) +def test_cable_manifest_shares_closed_capsules_without_native_render_meshes( + render_mode: str, +) -> None: + sim = _CableSimulation() + sim.cables["second"] = _CableObject(sim.arena_offsets) + for cable in sim.cables.values(): + cable.cfg.render_mode = render_mode + exporter = SceneExporter(sim, VisualizationCfg(backend="viser", env_ids=[2, 0])) + + manifest = exporter.build_manifest() + + assert len(manifest.nodes) == 12 + assert len(manifest.geometries) == 2 # The two distinct rest segment lengths. + assert not exporter.has_deformables + assert all(cable.data.fetch_count == 0 for cable in sim.cables.values()) + geometries = {geometry.geometry_id: geometry for geometry in manifest.geometries} + for node in manifest.nodes: + assert node.kind == "cable_segment" + assert not node.dynamic_geometry + segment_index = int(node.node_id.rsplit(":", 1)[1]) + length = (0.25, 0.25, 0.5)[segment_index] + mesh = geometries[node.geometry_id] + np.testing.assert_allclose( + mesh.vertices.min(axis=0), [-0.02, -0.02, -length / 2 - 0.02] + ) + np.testing.assert_allclose( + mesh.vertices.max(axis=0), [0.02, 0.02, length / 2 + 0.02] + ) + triangles = mesh.vertices[mesh.faces] + normals = np.cross( + triangles[:, 1] - triangles[:, 0], triangles[:, 2] - triangles[:, 0] + ) + centroids = triangles.mean(axis=1) + axis_points = np.zeros_like(centroids) + axis_points[:, 2] = np.clip(centroids[:, 2], -length / 2, length / 2) + assert np.all(np.sum(normals * (centroids - axis_points), axis=1) > 0) + edges = np.concatenate( + [mesh.faces[:, [0, 1]], mesh.faces[:, [1, 2]], mesh.faces[:, [2, 0]]] + ) + _, counts = np.unique(np.sort(edges, axis=1), axis=0, return_counts=True) + assert np.all(counts == 2) + cable_node = next( + node + for node in manifest.nodes + if node.node_id == "env:2/cable:cable%2Fmain/segment:0" + ) + assert cable_node.path == "/envs/2/cable_objects/cable%2Fmain/segments/0" + assert exporter.resolve_node_target(cable_node.node_id) == ("cable/main", "cable") + + +def test_cable_capture_batches_world_poses_for_selected_environments() -> None: + sim = _CableSimulation() + sim.cables["second"] = _CableObject(sim.arena_offsets) + sim.cables["second"].data.poses[..., 0] += 0.75 + exporter = SceneExporter(sim, VisualizationCfg(backend="viser", env_ids=[2, 0])) + manifest = exporter.build_manifest() + + frame = exporter.capture( + sim_step=1, sim_time=0.01, capture_dynamic_geometry=False + ).frame + + assert {node.env_id for node in manifest.nodes} == {2, 0} + assert frame.dynamic_meshes == () + assert all(cable.data.fetch_count == 1 for cable in sim.cables.values()) + for index, node in enumerate(manifest.nodes): + uid, _ = exporter.resolve_node_target(node.node_id) + segment_index = int(node.node_id.rsplit(":", 1)[1]) + pose = sim.cables[uid].data.poses[node.env_id, segment_index] + np.testing.assert_allclose(frame.positions[index], pose[:3]) + np.testing.assert_allclose(frame.wxyz[index], pose[[6, 3, 4, 5]], atol=1e-7) + + +def test_cable_pose_changes_keep_geometry_and_previous_frames_independent() -> None: + sim = _CableSimulation() + exporter = SceneExporter(sim, VisualizationCfg(backend="viser", env_ids=None)) + manifest = exporter.build_manifest() + first_frame = exporter.capture(sim_step=0, sim_time=0.0).frame + saved_positions = first_frame.positions.copy() + saved_wxyz = first_frame.wxyz.copy() + data = sim.cables["cable/main"].data + data.poses[..., :3] += [0.25, -0.5, 0.75] + data.poses[..., 3:] = [0.0, 0.0, 0.0, 1.0] + + next_frame = exporter.capture(sim_step=1, sim_time=0.01).frame + rebuilt = exporter.build_manifest() + + assert data.fetch_count == 2 + assert next_frame.node_ids == first_frame.node_ids + assert [node.geometry_id for node in rebuilt.nodes] == [ + node.geometry_id for node in manifest.nodes + ] + assert next_frame.scene_revision == first_frame.scene_revision + np.testing.assert_array_equal(first_frame.positions, saved_positions) + np.testing.assert_array_equal(first_frame.wxyz, saved_wxyz) + np.testing.assert_allclose( + next_frame.positions, saved_positions + [0.25, -0.5, 0.75] + ) + np.testing.assert_allclose(next_frame.wxyz, np.tile([1, 0, 0, 0], (9, 1))) + + +def test_cable_can_coexist_with_rigid_and_robot_assets() -> None: + class _MixedSimulation(_Simulation): + def __init__(self) -> None: + super().__init__() + self.cable = _CableObject(self.arena_offsets) + + def get_cable_object_uid_list(self) -> list[str]: + return ["cable"] + + def get_cable_object(self, uid: str) -> _CableObject: + return self.cable + + sim = _MixedSimulation() + exporter = SceneExporter(sim, VisualizationCfg(backend="viser", env_ids=None)) + manifest = exporter.build_manifest() + frame = exporter.capture(sim_step=0, sim_time=0).frame + + assert len(manifest.nodes) == 12 + assert {node.kind for node in manifest.nodes} == { + "rigid_object", + "robot_link", + "cable_segment", + } + np.testing.assert_allclose( + frame.positions[frame.node_ids.index("env:1/rigid:cube%2Fname")], + [2.2, 0.0, 0.3], + ) + np.testing.assert_allclose( + frame.positions[frame.node_ids.index("env:1/cable:cable/segment:0")], + [2.1, 0.2, 0.3], + ) + + +def test_cable_export_requires_prepared_data_and_propagates_released_state() -> None: + sim = _CableSimulation() + cable = sim.cables["cable/main"] + data = cable.data + cable.data = None + exporter = SceneExporter(sim, VisualizationCfg(backend="viser")) + + with pytest.raises(RuntimeError, match="prepare"): + exporter.build_manifest() + + cable.data = data + exporter.build_manifest() + data.released = True + with pytest.raises(RuntimeError, match="released"): + exporter.capture(sim_step=0, sim_time=0) + + def test_joint_control_provider_is_filtered_and_captured() -> None: exporter = SceneExporter( _EmptySimulation(), From edf83b9852830cab6c22458cee2ece52bbffbe20 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Wed, 11 Nov 2026 15:21:09 +0800 Subject: [PATCH 26/27] fix(gym): preserve inline cable mesh geometry in config loading --- embodichain/lab/gym/utils/gym_utils.py | 5 ++++- tests/gym/envs/test_embodied_env_cables.py | 24 ++++++++++++++++++++++ 2 files changed, 28 insertions(+), 1 deletion(-) diff --git a/embodichain/lab/gym/utils/gym_utils.py b/embodichain/lab/gym/utils/gym_utils.py index f18515e338..7db91d0c8c 100644 --- a/embodichain/lab/gym/utils/gym_utils.py +++ b/embodichain/lab/gym/utils/gym_utils.py @@ -969,7 +969,10 @@ class ComponentCfg: env_cfg.cable_object = [] for value in config.get("cable_object", []): cable = CableObjectCfg.from_dict(value) - if isinstance(cable.visual_shape, MeshCfg): + if ( + isinstance(cable.visual_shape, MeshCfg) + and cable.visual_shape.fpath is not None + ): cable.visual_shape.fpath = get_data_path(cable.visual_shape.fpath) env_cfg.cable_object.append(cable) diff --git a/tests/gym/envs/test_embodied_env_cables.py b/tests/gym/envs/test_embodied_env_cables.py index 872e4dfd2f..20bdeef1e5 100644 --- a/tests/gym/envs/test_embodied_env_cables.py +++ b/tests/gym/envs/test_embodied_env_cables.py @@ -134,6 +134,30 @@ def get_data_path(path: str) -> str: assert cable["visual_shape"]["fpath"] == reference +def test_cable_visual_mesh_loads_inline_geometry() -> None: + vertices = [[0.0, 0.0, 0.0], [0.2, 0.0, 0.0], [0.0, 0.01, 0.0]] + triangles = [[0, 1, 2]] + cable = { + "uid": "wire", + "points": [[0, 0, 1], [0.2, 0, 1]], + "radius": 0.01, + "visual_shape": {"vertices": vertices, "triangles": triangles}, + } + cfg = config_to_cfg( + { + "id": "EmbodiedEnv-v1", + "physics": "newton", + "env": {}, + "robot": {"uid": "robot"}, + "cable_object": [cable], + } + ) + mesh = cfg.cable_object[0].visual_shape + assert mesh.fpath is None + assert mesh.vertices == vertices + assert mesh.triangles == triangles + + @pytest.mark.parametrize("uids", [None, ["wire"]]) def test_cable_recording_uses_shared_observation_history_and_t_plus_one_slots( uids, From 6a7a29d5d23d63d3e7099ced710b384251a612e0 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Thu, 12 Nov 2026 18:26:14 +0800 Subject: [PATCH 27/27] fix(cable): preserve replay origins and select visible poses before export --- .../topics/env-framework/execution.md | 5 ++ .../topics/sim-visualization/runtime.md | 3 + embodichain/lab/gym/envs/embodied_env.py | 6 ++ embodichain/lab/gym/envs/wrapper/replay.py | 21 +++++ .../lab/visualization/scene_exporter.py | 16 ++-- tests/gym/envs/test_cable_recording_cuda.py | 38 ++++++++ tests/gym/envs/test_replay_wrapper.py | 90 +++++++++++++++++++ tests/visualization/test_scene_exporter.py | 25 +++++- 8 files changed, 192 insertions(+), 12 deletions(-) diff --git a/agent_context/topics/env-framework/execution.md b/agent_context/topics/env-framework/execution.md index 85223b6c14..bd44791a66 100644 --- a/agent_context/topics/env-framework/execution.md +++ b/agent_context/topics/env-framework/execution.md @@ -346,6 +346,11 @@ not orchestrate multiple experiments or qualify kinematic playback. records observations, not solver history. `ReplayWrapper` accepts dynamic replay only when every recorded initial state matches the full reset; it rejects scrubbing, partial reset and broadcasting world-space cable states. + Cable artifacts store `meta.arena_origins_w` in exported row order. Replay + translates captured reset positions into the source world frame for comparison, + allowing a per-environment export to replay at a different arena origin. + Raw observations, orientations and velocities retain their original meaning. + Older files without origin metadata require matching world placement. `trajectory_state.restore_trajectory_state()` rejects cable entries before any write. Use action replay to validate a new episode, not a visible-state restore as a history-preserving checkpoint. diff --git a/agent_context/topics/sim-visualization/runtime.md b/agent_context/topics/sim-visualization/runtime.md index 0ec30b307a..b862655b80 100644 --- a/agent_context/topics/sim-visualization/runtime.md +++ b/agent_context/topics/sim-visualization/runtime.md @@ -82,6 +82,9 @@ Arena offsets are added to rigid, robot, articulation, and camera poses. Deformable vertices are stored relative to the corresponding arena node. Cable `segment_pose_w` already contains world positions, including Arena offsets; export must not add those offsets again. +Pose export gathers visible environment/component rows on the source device +before CPU conversion and quaternion normalization. Native state fetching remains +asset-owned; host conversion scales with the exported node count. ### Exported content diff --git a/embodichain/lab/gym/envs/embodied_env.py b/embodichain/lab/gym/envs/embodied_env.py index f314c72451..65ead96a96 100644 --- a/embodichain/lab/gym/envs/embodied_env.py +++ b/embodichain/lab/gym/envs/embodied_env.py @@ -4262,6 +4262,12 @@ def save_trajectory( self.get_demo_episode_metadata(int(env_id)) for env_id in env_ids ], } + if "cables" in states.keys(): + # Cable observations retain world coordinates. Keep each exported + # row's origin so action replay can validate another arena placement. + meta["arena_origins_w"] = ( + self.sim.arena_offsets[env_ids_t].detach().cpu().tolist() + ) meta.update(self._trajectory_action_metadata()) final_observations = _export_final_observations( getattr(self, "_final_acceptance_rules", ()), diff --git a/embodichain/lab/gym/envs/wrapper/replay.py b/embodichain/lab/gym/envs/wrapper/replay.py index df919ec639..5d387482a4 100644 --- a/embodichain/lab/gym/envs/wrapper/replay.py +++ b/embodichain/lab/gym/envs/wrapper/replay.py @@ -70,6 +70,8 @@ class ReplayWrapper(gym.Wrapper): Cable recordings support dynamic replay from matching full-reset states only. Their visible state omits contact/solver history, so state scrubbing, partial resets and broadcasting world-space cable recordings are rejected. + Source arena origins, when recorded, translate the captured reset positions + into the saved frame for validation. Older files require matching placement. For a deployment with final acceptance rules, ``final_acceptance`` reports the offline evaluation of the saved terminal capture, independently of @@ -98,6 +100,7 @@ def __init__( self._trajectory = load_trajectory(trajectory) meta = self._trajectory["meta"] self._cable_replay = "cables" in self._trajectory["states"].keys() + self._cable_source_origins: torch.Tensor | None = None if ( getattr(getattr(env, "sim", None), "_cable_objects", {}) and not self._cable_replay @@ -114,6 +117,15 @@ def __init__( raise ValueError( "World-space cable recordings require matching environment counts." ) + if "arena_origins_w" in meta: + origins = torch.as_tensor( + meta["arena_origins_w"], dtype=torch.float32, device=env.device + ) + if origins.shape != (env.num_envs, 3) or not bool( + torch.isfinite(origins).all() + ): + raise ValueError("arena_origins_w must be finite (num_envs, 3).") + self._cable_source_origins = origins self._controller_spec = None self._task_outcome_replay = mode == "dynamic" and ( getattr(env, "termination_manager", None) is not None @@ -317,6 +329,15 @@ def _validate_cable_reset_state(self) -> None: capture_trajectory_state( self.env, current, torch.arange(self.env.num_envs, device=self.env.device) ) + if self._cable_source_origins is not None: + translation = self._cable_source_origins - self.env.sim.arena_offsets.to( + self._cable_source_origins + ) + # Compare the captured reset in the saved world's frame. The live + # scene and the recording keep their original poses and history. + for cable in current["cables"].values(): + state = cable["segment_state_w"] + state[..., :3] += translation.to(state)[:, None, :] for key, expected in initial.items(include_nested=True, leaves_only=True): observed = current[key].to(expected) if observed.shape != expected.shape or not torch.allclose( diff --git a/embodichain/lab/visualization/scene_exporter.py b/embodichain/lab/visualization/scene_exporter.py index 20e9f6929e..614d5d516b 100644 --- a/embodichain/lab/visualization/scene_exporter.py +++ b/embodichain/lab/visualization/scene_exporter.py @@ -1005,19 +1005,13 @@ def capture( pose = data.segment_pose_w else: pose = batch.asset.body_data.body_link_pose - all_positions, all_wxyz = pose_to_position_wxyz(pose) if batch.component_indices is None: - batch_positions = all_positions[batch.env_ids] - batch_wxyz = all_wxyz[batch.env_ids] + pose = pose[batch.env_ids] else: - batch_positions = all_positions[ - batch.env_ids, - batch.component_indices, - ] - batch_wxyz = all_wxyz[ - batch.env_ids, - batch.component_indices, - ] + pose = pose[batch.env_ids, batch.component_indices] + # Gather on the source device before CPU conversion and quaternion + # normalization, so export scales with the visible node count. + batch_positions, batch_wxyz = pose_to_position_wxyz(pose) # Cable snapshots already contain world positions, including Arena origins. if batch.asset_kind != "cable": batch_positions = batch_positions + self._env_offsets[batch.env_ids] diff --git a/tests/gym/envs/test_cable_recording_cuda.py b/tests/gym/envs/test_cable_recording_cuda.py index 7187fa5dd4..81c39b5eae 100644 --- a/tests/gym/envs/test_cable_recording_cuda.py +++ b/tests/gym/envs/test_cable_recording_cuda.py @@ -209,6 +209,44 @@ def _run_recording(urdf: str) -> None: rtol=0, atol=0, ) + # Commit the nonzero arena through public reset with auto-save enabled, + # then replay that actual artifact in a fresh single-environment scene. + from embodichain.lab.gym.envs.wrapper import ReplayWrapper + from embodichain.lab.gym.utils.gym_utils import load_trajectory + + env.cfg.trajectory_auto_save = True + env.cfg.trajectory_save_dir = str(Path(urdf).parent) + source_origin = env.sim.arena_offsets[1].cpu().clone() + env.reset(options={"reset_ids": [1]}) + paths = list(Path(urdf).parent.glob("traj_env1_*.pt")) + assert len(paths) == 1 + saved = load_trajectory(str(paths[0])) + assert saved["meta"]["env_ids"] == [1] + 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 + + env.close(exit_process=False) + env = None + manager = None + cables.clear() + gc.collect() + SimulationManager.flush_cleanup_queue() + + cfg.num_envs = 1 + cfg.sim_cfg.num_envs = 1 + cfg.record_trajectory = False + env = EmbodiedEnv(cfg) + replay = ReplayWrapper(env, saved) + replay.reset() + for _ in range(saved["meta"]["lengths"][0]): + replay.step(None) + for uid, field in fields.items(): + expected_final = latest.values[field][[1]].clone() + expected_final[..., :3] -= source_origin + observed = env.sim.get_cable_object(uid).data.segment_state_w.cpu() + observed[..., :3] -= env.sim.arena_offsets.cpu()[:, None, :] + torch.testing.assert_close(observed, expected_final, atol=2e-6, rtol=1e-5) finally: if env is not None: env.close(exit_process=False) diff --git a/tests/gym/envs/test_replay_wrapper.py b/tests/gym/envs/test_replay_wrapper.py index 3217413eb2..98e0510ae1 100644 --- a/tests/gym/envs/test_replay_wrapper.py +++ b/tests/gym/envs/test_replay_wrapper.py @@ -361,3 +361,93 @@ def test_cable_replay_preserves_task_context_and_initial_observation_history( assert env._record_initial_observation.call_args.args[0] is frame assert env._record_initial_observation.call_args.args[1].tolist() == [0] assert env._seed_recording_state.call_args.args[0] is frame.policy + + +@pytest.mark.parametrize("source_env_id", [0, 1]) +@pytest.mark.parametrize("target_origin", [(0.0, 0.0, 0.0), (10.0, -2.0, 3.0)]) +def test_auto_saved_cable_row_replays_at_target_arena_origin( + tmp_path, source_env_id, target_origin +): + from copy import deepcopy + from types import MethodType + + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + from embodichain.lab.gym.envs.episode_context import EpisodeContext + from embodichain.lab.gym.utils.gym_utils import load_trajectory + + target, _ = _cable_replay_case() + local_state = target.sim._cable_objects["wire"].data.segment_state_w.clone() + local_state[0, :, 0] = torch.tensor([0.1, 0.2, 0.3]) + local_state[..., 7:] = 0.01 + target.sim.arena_offsets = torch.tensor([target_origin]) + target_state = local_state.clone() + target_state[..., :3] += target.sim.arena_offsets[:, None, :] + target.sim._cable_objects["wire"].data.segment_state_w = target_state + + source = _FakeEnv(num_envs=2) + source.robot.uid = "robot" + source.physics_dt = 0.01 + source.control_frequency = 25 + source.sim._articulations = {} + source.sim._rigid_objects = {} + source.sim.arena_offsets = torch.tensor([[0.0, 0.0, 0.0], [5.0, 2.0, 1.0]]) + trajectory = _trajectory(num_envs=2) + trajectory["states"]["robot", "qpos"][:] = torch.tensor([0.0, 1.0]) + states = local_state[:, None].repeat(2, 2, 1, 1) + states[..., :3] += source.sim.arena_offsets[:, None, None, :] + trajectory["states"]["cables"] = TensorDict( + {"wire": {"segment_state_w": states}}, batch_size=[2, 2] + ) + source._traj_buffer = TensorDict( + {"states": trajectory["states"], "actions": trajectory["actions"]}, + batch_size=[2, 2], + ) + source._traj_steps = torch.tensor([2, 2]) + source.cfg.trajectory_auto_save = True + source.cfg.trajectory_save_dir = str(tmp_path) + source._traj_save_count = 0 + source._demo_episode_metadata = [ + {"env_id": row, "length": 2, "episode_uuid": f"episode-{row}"} + for row in range(2) + ] + source.get_demo_episode_metadata = lambda row: deepcopy( + source._demo_episode_metadata[row] + ) + source._trajectory_action_metadata = lambda: {"action_kind": "policy"} + source.episode_context = EpisodeContext(2) + source.episode_context._begin_episode([0, 1]) + source.episode_context._finish_initialization([0, 1]) + source.save_trajectory = MethodType(EmbodiedEnv.save_trajectory, source) + + path = EmbodiedEnv._save_trajectory_for_env(source, source_env_id) + saved = load_trajectory(path) + assert saved["meta"]["num_envs"] == 1 + assert saved["meta"]["env_ids"] == [source_env_id] + assert saved["meta"]["arena_origins_w"] == [ + source.sim.arena_offsets[source_env_id].tolist() + ] + recorded = saved["states"]["cables", "wire", "segment_state_w"].clone() + wrapper = ReplayWrapper(target, saved) + wrapper.reset() + wrapper.step(None) + torch.testing.assert_close( + target.received_actions[0], trajectory["actions"][[source_env_id], 0] + ) + torch.testing.assert_close( + saved["states"]["cables", "wire", "segment_state_w"], recorded + ) + torch.testing.assert_close( + target.sim._cable_objects["wire"].data.segment_state_w, target_state + ) + target_state[..., 0] += 0.1 + with pytest.raises(ValueError, match="differs from the full reset"): + wrapper.reset() + + +@pytest.mark.parametrize("origins", [[[0.0, 0.0]], [[float("nan"), 0.0, 0.0]]]) +def test_cable_replay_rejects_invalid_source_origins_before_reset(origins): + env, trajectory = _cable_replay_case() + trajectory["meta"]["arena_origins_w"] = origins + with pytest.raises(ValueError, match="arena_origins_w"): + ReplayWrapper(env, trajectory) + assert env.sim.physics_enabled == [] diff --git a/tests/visualization/test_scene_exporter.py b/tests/visualization/test_scene_exporter.py index 03c1fd53e4..201d894d26 100644 --- a/tests/visualization/test_scene_exporter.py +++ b/tests/visualization/test_scene_exporter.py @@ -681,10 +681,31 @@ def test_cable_manifest_shares_closed_capsules_without_native_render_meshes( assert exporter.resolve_node_target(cable_node.node_id) == ("cable/main", "cable") -def test_cable_capture_batches_world_poses_for_selected_environments() -> None: +@pytest.mark.parametrize( + "device", ["numpy", "cpu", pytest.param("cuda", marks=pytest.mark.gpu)] +) +def test_cable_capture_batches_world_poses_for_selected_environments( + monkeypatch: pytest.MonkeyPatch, device: str +) -> None: + import torch + from embodichain.lab.visualization import scene_exporter + sim = _CableSimulation() sim.cables["second"] = _CableObject(sim.arena_offsets) sim.cables["second"].data.poses[..., 0] += 0.75 + if device != "numpy": + for cable in sim.cables.values(): + cable.data.poses = torch.as_tensor(cable.data.poses, device=device) + convert_pose = scene_exporter.pose_to_position_wxyz + + def convert_selected_pose(pose): + # Only visible rows may reach the host conversion boundary. + assert tuple(pose.shape) == (6, 7) + if isinstance(pose, torch.Tensor): + assert pose.device.type == device + return convert_pose(pose) + + monkeypatch.setattr(scene_exporter, "pose_to_position_wxyz", convert_selected_pose) exporter = SceneExporter(sim, VisualizationCfg(backend="viser", env_ids=[2, 0])) manifest = exporter.build_manifest() @@ -699,6 +720,8 @@ def test_cable_capture_batches_world_poses_for_selected_environments() -> None: uid, _ = exporter.resolve_node_target(node.node_id) segment_index = int(node.node_id.rsplit(":", 1)[1]) pose = sim.cables[uid].data.poses[node.env_id, segment_index] + if isinstance(pose, torch.Tensor): + pose = pose.cpu().numpy() np.testing.assert_allclose(frame.positions[index], pose[:3]) np.testing.assert_allclose(frame.wxyz[index], pose[[6, 3, 4, 5]], atol=1e-7)