From 75d3c9a3561862c56e58fd676ab7e8c137b88a39 Mon Sep 17 00:00:00 2001 From: xiemenghong Date: Thu, 17 Sep 2026 09:48:58 +0800 Subject: [PATCH 01/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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/43] 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 da1dabd6852dc8ced57c155d3e517b6edb795b60 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 9 Nov 2026 10:18:18 +0800 Subject: [PATCH 26/43] feat(cable): adapt grasping and YAM routing to DexSim 0.6 --- NOTICE | 17 + agent_context/MAP.yaml | 7 +- .../topics/atomic-actions/atomic-actions.md | 8 + .../topics/data-assets/data-assets.md | 19 + .../topics/robot-system/robot-system.md | 1 + .../embodichain.lab.sim.shapes.rst | 10 +- docs/source/api_reference/public_api.rst | 77 ++ docs/source/api_reference/robot_catalogs.rst | 14 + docs/source/resources/robot/yam_i2rt.md | 52 + embodichain/compute/geometry/cable.py | 105 ++ .../lab/sim/atomic_actions/__init__.py | 6 + .../sim/atomic_actions/primitives/__init__.py | 5 + .../atomic_actions/primitives/grasp_cable.py | 241 +++++ embodichain/lab/sim/shapes.py | 10 + embodichain/lab/sim/spawn/descriptors.py | 17 +- examples/sim/demo/cable_two_posts.py | 416 ++++++++ examples/sim/demo/cable_wall_franka.py | 966 ++++++++++++++++++ extensions/embodichain_robots/__init__.py | 2 + .../embodichain_robots/_asset_catalog.py | 4 + extensions/embodichain_robots/_catalog.py | 6 + .../embodichain_robots/i2rt/__init__.py | 21 + .../embodichain_robots/i2rt/yam/README.md | 13 + .../embodichain_robots/i2rt/yam/__init__.py | 36 + .../embodichain_robots/i2rt/yam/assets.py | 66 ++ extensions/embodichain_robots/i2rt/yam/cfg.py | 178 ++++ .../components/embodiments/yam_i2rt.yaml | 36 + .../cable/route_the_cable/README.md | 223 ++++ .../cable/route_the_cable/catalog.yaml | 8 + .../cable/route_the_cable/env.yaml | 428 ++++++++ .../cable/route_the_cable/task.yam.yaml | 14 + .../manipulation/cable/__init__.py | 19 + .../manipulation/cable/route_the_cable.py | 885 ++++++++++++++++ licenses/YamI2rt-MIT.txt | 9 + pyproject.toml | 1 + scripts/tools/prepare_yam_i2rt_assets.py | 461 +++++++++ tests/compute/geometry/test_cable.py | 101 ++ tests/data/test_download_prefix.py | 8 +- tests/data/test_yam_i2rt_assets.py | 94 ++ tests/gym/envs/tasks/test_route_the_cable.py | 614 +++++++++++ .../envs/tasks/test_route_the_cable_cuda.py | 252 +++++ .../tools/test_prepare_yam_i2rt_assets.py | 236 +++++ tests/sim/atomic_actions/test_actions.py | 82 ++ tests/sim/robots/test_yam_i2rt.py | 132 +++ tests/sim/spawn/test_descriptors.py | 28 +- tests/test_robot_package.py | 2 + tests/test_robot_registry.py | 29 + 46 files changed, 5951 insertions(+), 8 deletions(-) create mode 100644 NOTICE create mode 100644 docs/source/resources/robot/yam_i2rt.md create mode 100644 embodichain/compute/geometry/cable.py create mode 100644 embodichain/lab/sim/atomic_actions/primitives/grasp_cable.py create mode 100644 examples/sim/demo/cable_two_posts.py create mode 100644 examples/sim/demo/cable_wall_franka.py create mode 100644 extensions/embodichain_robots/i2rt/__init__.py create mode 100644 extensions/embodichain_robots/i2rt/yam/README.md create mode 100644 extensions/embodichain_robots/i2rt/yam/__init__.py create mode 100644 extensions/embodichain_robots/i2rt/yam/assets.py create mode 100644 extensions/embodichain_robots/i2rt/yam/cfg.py create mode 100644 extensions/embodichain_tasks/configs/components/embodiments/yam_i2rt.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/catalog.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/__init__.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/route_the_cable.py create mode 100644 licenses/YamI2rt-MIT.txt create mode 100644 scripts/tools/prepare_yam_i2rt_assets.py create mode 100644 tests/compute/geometry/test_cable.py create mode 100644 tests/data/test_yam_i2rt_assets.py create mode 100644 tests/gym/envs/tasks/test_route_the_cable.py create mode 100644 tests/gym/envs/tasks/test_route_the_cable_cuda.py create mode 100644 tests/scripts/tools/test_prepare_yam_i2rt_assets.py create mode 100644 tests/sim/robots/test_yam_i2rt.py diff --git a/NOTICE b/NOTICE new file mode 100644 index 0000000000..acbb29d38e --- /dev/null +++ b/NOTICE @@ -0,0 +1,17 @@ +EmbodiChain +Copyright (c) 2021-2026 DexForce Technology Co., Ltd. + +The cable routing application includes geometry and routines adapted from +RoboSimStudio, Copyright 2026 Kaiyuan Chen, licensed under Apache-2.0. +Source: https://github.com/robosimstudio/robosimstudio +Revision: 73739abe212f963e8a9f5862b27bca370df091a7 + +The external YAM i2rt model contains the YAM arm and linear_4310 gripper, +Copyright (c) I2RT Robotics, under the MIT license. The source model directory +and RoboSimStudio NOTICE identify this attribution and license. +Upstream: https://github.com/i2rt-robotics/i2rt +Source directory: assets/embodiments/yam_i2rt/ in the RoboSimStudio revision above. + +Original robot meshes and generated models are distributed separately from +this source repository. Their archive retains LICENSE, NOTICE and +PROVENANCE.json. The MIT license is also reproduced in licenses/YamI2rt-MIT.txt. diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 5896d44ed7..189c89e936 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -49,7 +49,7 @@ topics: title: Environment Framework aliases: [env framework, environment framework, task registration, list task, expert trajectory, 环境框架, 任务环境, 专家轨迹] - keywords: [BaseEnv, EmbodiedEnv, EpisodeContext, EnvCfg, ControlTiming, resolve_control_timing, register_env, list-task, run-task, run-example, target_control_frequency, + keywords: [BaseEnv, EmbodiedEnv, RouteTheCableEnv, EpisodeContext, EnvCfg, ControlTiming, resolve_control_timing, register_env, list-task, run-task, run-example, target_control_frequency, sim_steps_per_control, step_dt, environment.component, embodiment.component, file-owned physics backend, physics_config, ControllerAction, EnvProfiler, startup_summary, dexsim_startup_info, ExpertTrajectoryCfg, joint_command_mode, position_velocity, qpos qvel, expert action schema, ObservationHistorySnapshot, ObservationFrame, @@ -133,7 +133,7 @@ topics: title: Robot System aliases: [robot system, robot config, robot catalog, list robot, robot preview, 机器人配置, 机器人系统, 机型解析, 列出机器人, 机器人预览] keywords: [UnitreeGo1Cfg, UnitreeGo2Cfg, UnitreeG1Cfg, UnitreeH12Cfg, ANYmalCCfg, MicroDuckCfg, load_locomotion_robot_profile, RobotCfg, RobotPresetCfg, control_parts, get_qvel_limits, build_pk_serial_chain, DexforceW1Cfg, CobotMagicCfg, ARX5Cfg, ARX5, FlexivRizon4SCfg, FlexivRizon4S, - FrankaCfg, FrankaPandaCfg, URRobotCfg, BrainCoRevo1Cfg, DualArmRobotCfg, merge_robot_cfg, JointDrivePropertiesCfg, + FrankaCfg, FrankaPandaCfg, YamI2rtCfg, URRobotCfg, BrainCoRevo1Cfg, DualArmRobotCfg, merge_robot_cfg, JointDrivePropertiesCfg, asset_physics_mode, target_mode, mimic joint, quaternion, xyzw, embodichain_robots, resolve_robot_cfg, list-robot, preview-robot, RobotHandGeometryCfg, RobotHandGeometryProvider, RobotCoordinatedGraspProvider, 机器人手部几何采样] paths: [topics/robot-system/robot-system.md] @@ -445,7 +445,7 @@ topics: - id: atomic-actions title: Atomic Skills aliases: [atomic action, atomic skill, 原子动作, 原子技能] - keywords: [PlanningBudget, PlanningRecovery, ExecutionPlanLifecycle, AtomicActionEngine, run-atomic-action, PlanningContext, ExecutionSession, ExecutionRunner, ActionInvocation, + keywords: [PlanningBudget, PlanningRecovery, ExecutionPlanLifecycle, GraspCable, CableGraspGoal, GraspCableOptions, AtomicActionEngine, run-atomic-action, PlanningContext, ExecutionSession, ExecutionRunner, ActionInvocation, hand_geometry_cfgs, GraspOpeningTarget, prepare_grasp_query, scoped grasp preparation, RecoveryPolicy, HeldObjectGuardRequest, PhaseEffectGateRequest, articulation_geometry, control-grid retiming, AffordanceSamplingContext, AffordancePoseCandidates, AffordanceSample, affordance sampling] @@ -586,6 +586,7 @@ topics: - embodichain/data/assets/demo_assets.py - embodichain/data/default_materials.py - embodichain/data/default_materials.json + - scripts/tools/prepare_yam_i2rt_assets.py - embodichain/_material_catalog.py watch_paths: [embodichain/data/, extensions/embodichain_robots/, embodichain/_material_catalog.py, embodichain/cli/main.py, tests/data/] related_topics: [gen-sim, robot-system, data-pipeline] diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index f7b5bc8a4c..5d3d469448 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -95,6 +95,14 @@ gate, or held-object guard. Evidence acquisition stays outside planning and does not mutate symbolic task state; verification results are the only path that commits or invalidates observed effects. +`GraspCable` directly extends `AtomicAction`: its caller +supplies an observed material-coordinate pinch pose, and its plan declares no +rigid attachment. An optional stationary closed-command hold belongs to the +`close` phase on the control grid; its end precedes lifting and is the routing +host's grasp-reference sampling boundary. The routing expert validates actual +lift and tool-frame material slip; numerical cable observations live in +`compute/geometry/cable.py`. + `MoveHeldObject` accepts ordered object-pose waypoints without rebasing the verified grasp. Its goal optionally frees only the final world yaw; default goals remain exact. An explicit `HeldObjectPoseGoal.object_id` requires that exact diff --git a/agent_context/topics/data-assets/data-assets.md b/agent_context/topics/data-assets/data-assets.md index 844184a5e7..a9b2ce6035 100644 --- a/agent_context/topics/data-assets/data-assets.md +++ b/agent_context/topics/data-assets/data-assets.md @@ -19,6 +19,8 @@ Online sampling and demonstration persistence belong to | Default material catalog / provenance | `embodichain/_material_catalog.py` → `embodichain/data/default_materials.py`; `embodichain/data/default_materials.json` | | Default material archive | `embodichain/data/assets/materials.py`: `DefaultMaterials` | | Dataset class visibility | `embodichain/data/__init__.py`, `embodichain/data/assets/__init__.py` | +| YAM versioned asset archive | `embodichain_robots.i2rt.yam.assets`: `YamI2rt` | +| YAM source conversion and archive packaging | `scripts/tools/prepare_yam_i2rt_assets.py` | | Pretrained policy bundles | `embodichain/data/assets/policy_assets.py`: `download_pretrained_policy()` | | Unified CLI registration | `embodichain/cli/main.py`: `embodichain data` | @@ -44,6 +46,23 @@ entry points for missing IDs. Explicit loaded modules remain available through ## Download and registry boundaries +YAM is a robot dataset resolved through `get_data_path("YamI2rt/yam_i2rt.urdf")`. +`YamI2rt` downloads `robot_assets/YamI2rtV1.zip` through the configured source, +with the official Hugging Face dataset as fallback, and verifies its MD5 through +the shared download/extraction protocol. `YamI2rtCfg` uses the same source URDF +for its simulation component and local FK/IK chains. An explicit `fpath` +bypasses automatic asset resolution. + +The preparation script remains the source-regeneration/offline path: it downloads +a pinned upstream revision, converts and verifies the qualified model, and can +package a versioned ZIP with stable metadata using `--archive`. Its default +directory is `/YamI2rt`, which `get_data_path()` reuses without a +download. The archive retains MIT `LICENSE`, `NOTICE` and `PROVENANCE.json`; +model files and release archives are excluded from Git and the wheel. +The versioned archive must be published on the selected data host before relying +on a network-only installation. Task usage and publication requirements are in +`extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md`. + `EmbodiChainDataset` locks `/download/.locks/.lock` around both ZIP integrity repair and the Open3D constructor's download/extraction. Keep the lock outside the directory repair may delete. POSIX uses `flock`; diff --git a/agent_context/topics/robot-system/robot-system.md b/agent_context/topics/robot-system/robot-system.md index ae86ac224e..f457335d44 100644 --- a/agent_context/topics/robot-system/robot-system.md +++ b/agent_context/topics/robot-system/robot-system.md @@ -15,6 +15,7 @@ | Bare Franka arm selection and explicit tool assembly | `extensions/embodichain_robots/franka/cfg.py`, `panda.py` → [catalog contracts](catalogs.md) | | W1 variants and serialization | `extensions/embodichain_robots/dexforce/w1/` → `cfg.py`, `specs.py`, `hand_specs.py` | | Independent Revo1 hand or shared hand asset names | `extensions/embodichain_robots/brainco/revo1/` → `cfg.py`, `_specs.py` | +| YAM dual arm / asset provenance | `extensions/embodichain_robots/i2rt/yam/`: `cfg.py`, `assets.py`; source conversion in `scripts/tools/prepare_yam_i2rt_assets.py` | | Composed robot frames and joint-name mapping | `embodichain/lab/sim/robots/dual_arm.py` | | Shared articulation state and drive properties | `embodichain/lab/sim/objects/articulation.py`, `embodichain/lab/sim/cfg/articulation.py` | diff --git a/docs/source/api_reference/embodichain/embodichain.lab.sim.shapes.rst b/docs/source/api_reference/embodichain/embodichain.lab.sim.shapes.rst index f2cf39198e..88d54ad1b4 100644 --- a/docs/source/api_reference/embodichain/embodichain.lab.sim.shapes.rst +++ b/docs/source/api_reference/embodichain/embodichain.lab.sim.shapes.rst @@ -8,8 +8,9 @@ Overview Geometry configuration objects used to build collision, visual, and deformable simulation shapes. :class:`ShapeCfg` is the common base; :class:`MeshCfg`, -:class:`CubeCfg`, and :class:`SphereCfg` describe triangle-mesh, box, and -sphere primitives respectively. :class:`MeshCollisionCfg` explicitly selects +:class:`CubeCfg`, :class:`SphereCfg`, and :class:`CylinderCfg` describe meshes, +boxes, spheres, and cylinders respectively. Cylinders use full height along +local Z and native cylinder collision geometry. :class:`MeshCollisionCfg` explicitly selects the collision representation and its cooking settings, while :class:`LoadOption` controls mesh loading. @@ -29,6 +30,7 @@ tetrahedral simulation mesh during voxelization. .. autosummary:: CubeCfg + CylinderCfg LoadOption MeshCollisionCfg MeshCfg @@ -67,6 +69,10 @@ tetrahedral simulation mesh during voxelization. :show-inheritance: :exclude-members: __init__, copy, replace, to_dict, validate +.. autoclass:: CylinderCfg + :members: + :exclude-members: __init__, copy, replace, to_dict, validate + .. autoclass:: LoadOption :members: :undoc-members: diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index a5ac8787a5..541a2aa0bc 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -2834,6 +2834,20 @@ embodichain.lab.sim.atomic_actions.requirements SkillEndpointRequirement SkillResourceSlot +embodichain.lab.sim.atomic_actions.primitives.grasp_cable +--------------------------------------------------------- + +Plan a contact-only pinch, close and lift. The caller measures actual lift and +slip after execution; the plan creates no rigid attachment. + +.. currentmodule:: embodichain.lab.sim.atomic_actions.primitives.grasp_cable + +.. autosummary:: + + CableGraspGoal + GraspCableOptions + GraspCable + embodichain.lab.sim.atomic_actions.runner ----------------------------------------- @@ -4276,6 +4290,29 @@ Differentiable lightweight point-mass task and learning-environment registration PointMassEnv +embodichain_robots +----------------- + +Official robot configurations are loaded lazily from the bundled catalog. +The YAM preset uses a prepared external asset cache for simulation and FK. + +.. currentmodule:: embodichain_robots + +.. autosummary:: + + YamI2rtCfg + +embodichain_robots.i2rt.yam.cfg +------------------------- + +YAM dual-arm defaults, including independent finger drives and local FK chains. + +.. currentmodule:: embodichain_robots.i2rt.yam.cfg + +.. autosummary:: + + YamI2rtCfg + embodichain_tasks.configs ------------------------- @@ -4285,6 +4322,19 @@ embodichain_tasks.configs get_config_path +embodichain_tasks.manipulation.cable.route_the_cable +---------------------------------------------------- + +Two-spool routing task with lazy expert stages and measured cable geometry. +Intermediate grasp validators reject excessive slip before continuing. + +.. currentmodule:: embodichain_tasks.manipulation.cable.route_the_cable + +.. autosummary:: + + RouteTheCableEnv + measure_route + embodichain_tasks.manipulation.push_cube ---------------------------------------- @@ -4834,3 +4884,30 @@ Offline held-object adapters validate exact execution and acquisition identities .. autofunction:: held_observation_request .. autofunction:: resolve_hold_observation_condition + +Cable material geometry +----------------------- + +Pure tensor helpers observe ordered segment endpoints and stable cable material +coordinates. Slip compares the same material point in the moving tool frame. + +.. currentmodule:: embodichain.compute.geometry.cable + +.. autosummary:: + + sample_cable_material + cable_endpoints + cable_grasp_slip + +.. automodule:: embodichain.compute.geometry.cable + :members: sample_cable_material, cable_endpoints, cable_grasp_slip + +I2RT YAM model and assets +------------------------- + +The model package loads configuration lazily; asset discovery can import the +versioned dataset independently of robot construction. + +.. autoclass:: embodichain_robots.i2rt.yam.YamI2rtCfg +.. autoclass:: embodichain_robots.i2rt.yam.cfg.YamI2rtCfg +.. autoclass:: embodichain_robots.i2rt.yam.assets.YamI2rt diff --git a/docs/source/api_reference/robot_catalogs.rst b/docs/source/api_reference/robot_catalogs.rst index 9c2231d3b9..e2be0b0c6c 100644 --- a/docs/source/api_reference/robot_catalogs.rst +++ b/docs/source/api_reference/robot_catalogs.rst @@ -28,6 +28,20 @@ below identify canonical imports without re-expanding inherited configuration fields or generated constructors. +embodichain.data.assets +----------------------- + +.. py:class:: embodichain.data.assets.YamI2rt(data_root=None) + + Download the versioned YAM dual-arm URDF, visual meshes, convex fingertip + collisions, MIT license and provenance through the shared dataset cache. + ``data_root`` selects the cache root; when omitted, the configured + ``EMBODICHAIN_DATA_ROOT`` or its default is used. The archive is checked + against its registered MD5. Prefer ``get_data_path("YamI2rt/yam_i2rt.urdf")`` + when resolving the model for a robot. Publication status and offline + preparation are documented in :doc:`/resources/robot/yam_i2rt`. + + embodichain.robot_registry -------------------------- diff --git a/docs/source/resources/robot/yam_i2rt.md b/docs/source/resources/robot/yam_i2rt.md new file mode 100644 index 0000000000..dc65b70715 --- /dev/null +++ b/docs/source/resources/robot/yam_i2rt.md @@ -0,0 +1,52 @@ +# YAM i2rt + +`embodichain_robots.YamI2rtCfg` configures two six-joint arms and two +independently driven fingers per hand. The shared robot catalog ID is `YamI2rt`; +control groups are `right_arm`, `left_arm`, `right_eef`, and `left_eef`. + +Like Franka, the default configuration resolves a registered external asset +through `get_data_path("YamI2rt/yam_i2rt.urdf")`. It downloads the prepared +`robot_assets/YamI2rtV1.zip` bundle when the cache is empty. Predownload with: + +```bash +python -m embodichain data download --name YamI2rt +``` + +The registry uses `EMBODICHAIN_DOWNLOAD_PREFIX` and falls back to the official +`DexForceAI/embodichain_data` Hugging Face dataset. The prepared download needs +no CoACD installation. The bundle was +[published on 2026-11-05](https://huggingface.co/datasets/DexForceAI/embodichain_data/commit/8c0b7dcc8a80ab5a1f177c7384ad9d6950281a68) +and is publicly downloadable without a Hugging Face token. + +For optional reconstruction from the pinned source in a checkout: + +```bash +python -m pip install coacd==1.0.14 trimesh==4.12.2 scipy==1.18.1 +python scripts/tools/prepare_yam_i2rt_assets.py +``` + +The script downloads pinned robosimstudio sources, generates the URDF and +convex fingertip collision meshes, verifies every SHA-256, and preserves the +upstream MIT license and provenance. Its default destination is +`/YamI2rt/`; registry downloads extract to +`/extract/YamI2rt/`. Both locations are recognized. The default data +root is `~/.cache/embodichain_data/`. Set `EMBODICHAIN_DATA_ROOT` before imports +to change the cache root, or pass an explicit `fpath` to a prepared URDF. +Assets are excluded from the repository and wheel. `--source-dir` supports +preparation from an offline copy of the pinned source tree. + +For maintainers, `--archive /path/to/YamI2rtV1.zip` packages the verified files +with fixed ZIP metadata, their MIT license, NOTICE and provenance. The registered +archive MD5 is `23601ea820ff3e6260336c4962ef01a5`. Repository attribution is +retained in `NOTICE` and `licenses/YamI2rt-MIT.txt`; meshes remain external. + +The complete source-named dual-arm URDF is one `arm` component with an identity +transform; the existing component loader returns that source directly. +Simulation and `build_pk_serial_chain()` share the same URDF. The numerical +solver builds local six-joint FK/IK chains from each arm base to its TCP. +The preset requests Newton gravity compensation to match its source MJCF. + +The handwritten routing deployment is documented in +`extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md`. +It requires a DexSim build with cable-state support. Intermediate grasp tests +do not establish routing success across seeds or repeated resets. diff --git a/embodichain/compute/geometry/cable.py b/embodichain/compute/geometry/cable.py new file mode 100644 index 0000000000..f26085fe6f --- /dev/null +++ b/embodichain/compute/geometry/cable.py @@ -0,0 +1,105 @@ +# ---------------------------------------------------------------------------- +# 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 material coordinates and geometric observations, independent of simulation.""" + +from __future__ import annotations + +import torch + +__all__ = ["sample_cable_material", "cable_endpoints", "cable_grasp_slip"] + + +def cable_endpoints( + state: torch.Tensor, rest_lengths: torch.Tensor +) -> tuple[torch.Tensor, torch.Tensor]: + """Return proximal/distal capsule ends from xyz+xyzw segment states. + + Args: + state: World segment states with shape ``(..., S, 13)``. + rest_lengths: Positive capsule axis lengths with shape ``(S,)``. + + Returns: + Two tensors of shape ``(..., S, 3)``, in material order. + """ + if ( + state.ndim < 2 + or state.shape[-1] != 13 + or state.shape[-2] != rest_lengths.numel() + ): + raise ValueError("Cable state and rest lengths must agree on segment count.") + if ( + rest_lengths.ndim != 1 + or rest_lengths.numel() == 0 + or not bool((torch.isfinite(rest_lengths) & (rest_lengths > 0)).all()) + ): + raise ValueError("Cable rest lengths must be a positive vector.") + q = state[..., 3:7] + # The capsule's local +Z axis in world coordinates; quaternions are XYZW. + x, y, z, w = q.unbind(-1) + axis = torch.stack( + (2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y)), -1 + ) + half_axis = 0.5 * rest_lengths.to(state)[..., None] * axis + return state[..., :3] - half_axis, state[..., :3] + half_axis + + +def sample_cable_material( + state: torch.Tensor, rest_lengths: torch.Tensor, distance: float +) -> tuple[torch.Tensor, torch.Tensor, int]: + """Observe a fixed material coordinate, rather than a nearest world point. + + Args: + state: World states with shape ``(B,S,13)``. + rest_lengths: Positive capsule axis lengths ``(S,)``. + distance: Material arc length from the proximal cable end, in metres. + + Returns: + World point, unit material tangent, and stable segment index. + """ + if state.ndim != 3: + raise ValueError("Material sampling requires batched cable state (B,S,13).") + starts, ends = cable_endpoints(state, rest_lengths) + cumulative = rest_lengths.to(state).cumsum(0) + if not 0 <= distance <= float(cumulative[-1]): + raise ValueError("Material distance is outside this cable.") + index = min( + int(torch.searchsorted(cumulative, cumulative.new_tensor(distance))), + state.shape[-2] - 1, + ) + previous = cumulative[index - 1] if index else cumulative.new_tensor(0) + fraction = (distance - previous) / rest_lengths.to(state)[index] + direction = ends[:, index] - starts[:, index] + return ( + starts[:, index] + fraction * direction, + torch.nn.functional.normalize(direction, dim=-1), + index, + ) + + +def cable_grasp_slip( + point_in_tool: torch.Tensor, initial_point_in_tool: torch.Tensor +) -> torch.Tensor: + """Measure material-point slip in the tool frame, invariant to wrist motion. + + Args: + point_in_tool: Current material point in the TCP frame, ``(B,3)``. + initial_point_in_tool: Independent snapshot when the fingers closed. + + Returns: + Slip distance in metres for each environment. + """ + return torch.linalg.vector_norm(point_in_tool - initial_point_in_tool, dim=-1) diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index e226147d56..7080a7dc8b 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -188,6 +188,9 @@ WholeBodyPoseTrackingState, ) from .primitives import ( + CableGraspGoal, + GraspCable, + GraspCableOptions, AssembleGoal, AxisAlign, AxisAlignGoal, @@ -285,6 +288,9 @@ ) __all__ = [ + "CableGraspGoal", + "GraspCable", + "GraspCableOptions", "ActionBinding", "ActionControlOverrides", "ActionInvocation", diff --git a/embodichain/lab/sim/atomic_actions/primitives/__init__.py b/embodichain/lab/sim/atomic_actions/primitives/__init__.py index bc7cdf2c2b..4ae39dec9e 100644 --- a/embodichain/lab/sim/atomic_actions/primitives/__init__.py +++ b/embodichain/lab/sim/atomic_actions/primitives/__init__.py @@ -31,6 +31,7 @@ CoordinatedPlacementOptions, ) from .hand_over import HandOver, HandOverGoal, HandOverOptions +from .grasp_cable import CableGraspGoal, GraspCable, GraspCableOptions from .move_end_effector import ( EndEffectorPoseGoal, MoveEndEffector, @@ -71,6 +72,7 @@ MoveEndEffector, MoveJoints, PickUp, + GraspCable, AxisAlign, MoveHeldObject, Place, @@ -92,6 +94,9 @@ "AxisAlignGoal", "AxisAlignOptions", "BUILTIN_ACTION_TYPES", + "CableGraspGoal", + "GraspCable", + "GraspCableOptions", "CoordinatedPickGoal", "CoordinatedPickment", "CoordinatedPickmentOptions", diff --git a/embodichain/lab/sim/atomic_actions/primitives/grasp_cable.py b/embodichain/lab/sim/atomic_actions/primitives/grasp_cable.py new file mode 100644 index 0000000000..cd31b47ec7 --- /dev/null +++ b/embodichain/lab/sim/atomic_actions/primitives/grasp_cable.py @@ -0,0 +1,241 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Plan a frictional cable pinch without a rigid-object attachment effect.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +import math +from typing import ClassVar +import torch +from embodichain.lab.sim.atomic_actions.core import AtomicAction +from embodichain.lab.sim.atomic_actions.invocation import ( + ActionOptions, + ResolvedActionRequest, +) +from embodichain.lab.sim.atomic_actions.plans import ActionPlan +from embodichain.lab.sim.atomic_actions.state import PlanningContext +from embodichain.lab.sim.atomic_actions.bindings import JointPositionTarget +from embodichain.lab.sim.atomic_actions.requirements import ( + SkillBindingContract, + CARTESIAN_POSE_CAPABILITY, + FORWARD_KINEMATICS_CAPABILITY, +) +from embodichain.lab.sim.atomic_actions.control import ( + OPEN_COMMAND, + GRASP_COMMAND, + JointPositionCommand, +) +from embodichain.lab.sim.atomic_actions.primitives._binding_contracts import ( + make_manipulation_slot, +) +from embodichain.lab.sim.atomic_actions.trajectory_ops import ( + build_pose_plan_states, + resolve_pose_target, + to_full_robot_trajectory, +) +from embodichain.lab.sim.motion.planners.utils import interpolate_xpos_batched +from embodichain.lab.sim.atomic_actions.plans import TimedTrajectory + +__all__ = ["CableGraspGoal", "GraspCableOptions", "GraspCable"] + + +@dataclass(frozen=True, slots=True, eq=False) +class CableGraspGoal: + """Resolved pinch pose plus a stable material-coordinate identity. + + Args: + cable_uid: Cable identity retained for the caller's physical validator. + material_distance: Resolved arc length from the proximal end, in metres. + grasp_pose: ``(4,4)`` or ``(B,4,4)`` TCP pose in Robot FK's Arena-local + frame. The caller converts world cable observations before planning. + """ + + goal_kind: ClassVar[str] = "cable_grasp" + cable_uid: str + material_distance: float + grasp_pose: torch.Tensor + + def __post_init__(self) -> None: + from embodichain.lab.sim.atomic_actions.goals import validate_pose_goal + + if ( + not isinstance(self.cable_uid, str) + or not self.cable_uid + or not math.isfinite(self.material_distance) + or self.material_distance < 0 + ): + raise ValueError( + "Cable identity and finite nonnegative material distance are required." + ) + validate_pose_goal(self.grasp_pose, "grasp_pose", allow_waypoints=False) + + +@dataclass(frozen=True, slots=True, eq=False) +class GraspCableOptions(ActionOptions): + """Pinch dwell and world-Z lift behavior; timing grid is engine-owned.""" + + close_steps: int = 48 + close_hold_duration: float = 0.0 + """Seconds to maintain the closed command before lifting. + + Rounded up to the control grid and included in the ``close`` phase. + """ + lift_height: float = 0.0185 + lift_speed: float = 0.05 + """World-Z lift speed in metres per second, sampled on the control grid.""" + + def __post_init__(self) -> None: + if ( + isinstance(self.close_steps, bool) + or not isinstance(self.close_steps, int) + or self.close_steps < 2 + or isinstance(self.close_hold_duration, bool) + or not math.isfinite(self.close_hold_duration) + or self.close_hold_duration < 0 + or not math.isfinite(self.lift_height) + or self.lift_height <= 0 + or not math.isfinite(self.lift_speed) + or self.lift_speed <= 0 + ): + raise ValueError( + "Cable grasp requires finite positive lift height/speed, " + "nonnegative close_hold_duration and integer close_steps >= 2." + ) + + +class GraspCable(AtomicAction[CableGraspGoal, GraspCableOptions]): + """Descend, close and lift using contact alone. + + The caller supplies a fresh cable-derived pose and verifies material-point + lift/slip after execution. Planning does not step physics or assert a grasp. + """ + + skill_id: ClassVar[str] = "grasp_cable" + GoalType: ClassVar[type] = CableGraspGoal + OptionsType: ClassVar[type] = GraspCableOptions + binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( + slots=( + make_manipulation_slot( + "primary", + motion_capabilities=frozenset( + {CARTESIAN_POSE_CAPABILITY, FORWARD_KINEMATICS_CAPABILITY} + ), + grasp_commands={ + OPEN_COMMAND: JointPositionCommand, + GRASP_COMMAND: JointPositionCommand, + }, + ), + ) + ) + + def _plan( + self, + request: ResolvedActionRequest[CableGraspGoal, GraspCableOptions], + context: PlanningContext, + ) -> ActionPlan: + goal = self.require_goal(request) + motion = request.binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + hand = request.binding.endpoint("primary", "grasp") + fingers = list(hand.require_target(JointPositionTarget).joint_ids) + ids = list(motion.joint_ids) + open_q = hand.joint_positions( + OPEN_COMMAND, + num_envs=context.batch_size, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + closed_q = hand.joint_positions( + GRASP_COMMAND, + num_envs=context.batch_size, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + target = resolve_pose_target( + goal.grasp_pose, num_envs=context.batch_size, device=self.device + ).to(context.robot.qpos) + + def move(start_full: torch.Tensor, end: torch.Tensor, *, sample_count: int): + start = self.robot.compute_fk( + start_full[:, ids], name=motion.control_part, to_matrix=True + ) + poses = interpolate_xpos_batched(start, end, sample_count)[:, 1:] + result = self.motion_generator.generate( + build_pose_plan_states(poses), + options=replace( + request.motion_policy, sample_count=sample_count + ).to_motion_gen_options( + start_qpos=start_full[:, ids], + control_part=motion.control_part, + interpolation_dt=context.require_control_dt(), + cartesian_linear=True, + ), + ) + success, trajectory = to_full_robot_trajectory( + result, + base_qpos=start_full, + joint_ids=ids, + env_ids=context.env_ids, + control_dt=context.require_control_dt(), + ) + return success, trajectory.positions.clone() + + ok1, down = move( + context.robot.qpos, target, sample_count=request.motion_policy.sample_count + ) + down[:, :, fingers] = open_q[:, None, :] + close = down[:, -1:].expand(-1, request.skill_options.close_steps, -1).clone() + weights = torch.linspace( + 0, 1, request.skill_options.close_steps, device=self.device + )[None, :, None] + close[:, :, fingers] = ( + open_q[:, None, :] * (1 - weights) + closed_q[:, None, :] * weights + ) + hold_steps = math.ceil( + request.skill_options.close_hold_duration / context.require_control_dt() + ) + if hold_steps: + close = torch.cat((close, close[:, -1:].expand(-1, hold_steps, -1)), dim=1) + lifted = target.clone() + lifted[:, 2, 3] += request.skill_options.lift_height + lift_samples = max( + 2, + math.ceil( + request.skill_options.lift_height + / (request.skill_options.lift_speed * context.require_control_dt()) + ) + + 1, + ) + ok2, up = move(close[:, -1], lifted, sample_count=lift_samples) + up[:, :, fingers] = closed_q[:, None, :] + return self.build_plan( + request, + context, + success=ok1 & ok2, + trajectory=TimedTrajectory.from_uniform_step( + torch.cat((down, close, up), 1), + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), + segment_lengths={ + "descend": down.shape[1], + "close": close.shape[1], + "lift": up.shape[1], + }, + ) diff --git a/embodichain/lab/sim/shapes.py b/embodichain/lab/sim/shapes.py index 2c2e4612b4..5fd9524ebc 100755 --- a/embodichain/lab/sim/shapes.py +++ b/embodichain/lab/sim/shapes.py @@ -37,6 +37,7 @@ "MeshCfg", "CubeCfg", "SphereCfg", + "CylinderCfg", ] @@ -473,3 +474,12 @@ class SphereCfg(ShapeCfg): resolution: int = 20 """Resolution of the sphere mesh. Defaults to 20.""" + + +@configclass +class CylinderCfg(ShapeCfg): + """Native cylinder geometry, with full height measured along local Z.""" + + shape_type: str = "Cylinder" + radius: float = 0.01 + height: float = 0.01 diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index ca010c304a..70225f1707 100644 --- a/embodichain/lab/sim/spawn/descriptors.py +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -86,6 +86,7 @@ ) from embodichain.lab.sim.shapes import ( CubeCfg, + CylinderCfg, MeshCfg, MeshCollisionCfg, SphereCfg, @@ -2007,6 +2008,20 @@ def _compile_geometry( raise ValueError("CubeCfg.size must contain three positive values.") return GeometryDesc.cube(size), CollisionApproximation.NONE, 1, "coacd" + if isinstance(shape, CylinderCfg): + if ( + not math.isfinite(shape.radius) + or not math.isfinite(shape.height) + or min(shape.radius, shape.height) <= 0 + ): + raise ValueError("Cylinder radius and height must be finite and positive.") + return ( + GeometryDesc.cylinder(shape.radius, shape.height / 2), + CollisionApproximation.NONE, + 1, + "coacd", + ) + if isinstance(shape, SphereCfg): if shape.radius <= 0: raise ValueError("SphereCfg.radius must be positive.") @@ -2019,7 +2034,7 @@ def _compile_geometry( raise NotImplementedError( f"RigidObjectCfg shape {type(shape).__name__!r} is not supported by " - "the Spawn converter; supported shapes are MeshCfg, CubeCfg, and SphereCfg." + "the Spawn converter; supported shapes are MeshCfg, CubeCfg, CylinderCfg, and SphereCfg." ) diff --git a/examples/sim/demo/cable_two_posts.py b/examples/sim/demo/cable_two_posts.py new file mode 100644 index 0000000000..c10843a667 --- /dev/null +++ b/examples/sim/demo/cable_two_posts.py @@ -0,0 +1,416 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Contact-driven two-post cable preview through EmbodiChain's manager. + +The Cartesian gripper is a prescribed articulation. Only its fingers are +controlled: the cable has one fixed root and no moving attachments. +""" + +from __future__ import annotations + +import argparse +import hashlib +import json +from pathlib import Path +import subprocess +import time + +import cv2 +import numpy as np +from scipy.interpolate import make_interp_spline + +FPS = 60 +DURATION = 14.0 +RADIUS = 0.008 +POST_RADIUS = 0.085 +POSTS = ((0.0, 0.0), (0.0, 0.4)) +# t, x, y, z, finger centre distance, phase +WAYPOINTS = ( + (0.0, 0.0, -0.28, 0.020, 0.045, "APPROACH"), + (0.5, 0.0, -0.28, 0.020, 0.045, "APPROACH"), + (1.2, 0.0, -0.28, 0.020, 0.0125, "GRIP"), + (2.0, 0.0, -0.25, 0.035, 0.0125, "LIFT"), + (3.3, 0.17, -0.12, 0.035, 0.0125, "WRAP POST 1"), + (4.7, 0.17, 0.10, 0.035, 0.0125, "WRAP POST 1"), + (5.6, 0.09, 0.20, 0.035, 0.0125, "BETWEEN POSTS"), + (6.8, -0.17, 0.24, 0.035, 0.0125, "WRAP POST 2"), + (8.2, -0.17, 0.49, 0.035, 0.0125, "WRAP POST 2"), + (9.2, 0.0, 0.73, 0.035, 0.0125, "WRAP POST 2"), + (10.2, 0.0, 0.73, 0.035, 0.0125, "HOLD"), + (10.7, 0.0, 0.73, 0.035, 0.045, "RELEASE"), + (11.5, 0.0, 0.73, 0.22, 0.045, "RETREAT"), + (14.0, 0.0, 0.73, 0.22, 0.045, "OBSERVE"), +) + + +def control_at(t: float) -> tuple[np.ndarray, str]: + """Sample a smooth deterministic Cartesian gripper command.""" + for a, b in zip(WAYPOINTS[:-1], WAYPOINTS[1:]): + if t <= b[0]: + u = np.clip((t - a[0]) / (b[0] - a[0]), 0, 1) + u = u * u * (3 - 2 * u) + values = (1 - u) * np.array(a[1:5]) + u * np.array(b[1:5]) + return np.array([*values[:3], values[3], values[3]], np.float32), b[5] + return np.array([0, 0.73, 0.22, 0.045, 0.045], np.float32), "OBSERVE" + + +def initial_points(segments: int) -> np.ndarray: + """Sample one continuous slack shape uniformly by arc length.""" + waypoints = np.array( + [ + [0, -0.52], + [-0.36, -0.65], + [-0.57, -0.50], + [-0.40, -0.34], + [-0.12, -0.43], + [0, -0.39], + [0, -0.28], + ] + ) + spline = make_interp_spline(np.arange(len(waypoints)), waypoints, k=2) + dense = spline(np.linspace(0, len(waypoints) - 1, 3000)) + arc = np.r_[0, np.cumsum(np.linalg.norm(np.diff(dense, axis=0), axis=1))] + sample = np.linspace(0, arc[-1], segments + 1) + return np.column_stack( + [ + np.interp(sample, arc, dense[:, 0]), + np.interp(sample, arc, dense[:, 1]), + np.full_like(sample, 0.011), + ] + ).astype(np.float32) + + +def write_stage(path: Path) -> None: + """Author fixed posts and a five-axis parallel-jaw gripper in one URDF.""" + parts = [''] + + def link( + name: str, geometry: str = "", xyz: str = "0 0 0", color: str = ".3 .4 .5 1" + ) -> None: + parts.append( + f'' + ) + if geometry: + parts.append( + f'{geometry}' + ) + parts.append( + f'{geometry}' + ) + parts.append("") + + def joint( + name: str, + parent: str, + child: str, + kind: str = "fixed", + xyz: str = "0 0 0", + axis: str = "1 0 0", + ) -> None: + parts.append( + f'' + ) + + link("base", '', "-.15 .10 -.02", ".15 .20 .25 1") + for i, (x, y) in enumerate(POSTS): + link( + f"post_{i+1}", + f'', + "0 0 .11", + ".15 .60 .73 1", + ) + joint(f"post_{i+1}_fixed", "base", f"post_{i+1}", xyz=f"{x} {y} 0") + link("anchor", '', "0 0 0", ".95 .57 .12 1") + joint("anchor_fixed", "base", "anchor", xyz="0 -.52 .013") + for name, parent, axis in ( + ("x", "base", "1 0 0"), + ("y", "slide_x", "0 1 0"), + ("z", "slide_y", "0 0 1"), + ): + link(f"slide_{name}") + joint(f"slide_{name}_joint", parent, f"slide_{name}", "prismatic", axis=axis) + link("palm", '', "0 0 .085", ".7 .75 .8 1") + joint("palm_fixed", "slide_z", "palm") + for name, axis in (("left", "1 0 0"), ("right", "-1 0 0")): + link(f"finger_{name}", '', "0 0 .025", ".87 .9 .94 1") + joint(f"finger_{name}_joint", "palm", f"finger_{name}", "prismatic", axis=axis) + parts.append("") + path.write_text("\n".join(parts)) + + +def run(args: argparse.Namespace) -> None: + """Build with EmbodiChain, step cable physics, and record camera observations.""" + import dexsim + import newton + import warp as wp + from dexsim.engine.newton_physics import DexUniSolverCfg + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + from embodichain.lab.sim.cfg import ( + ArticulationCfg, + CableObjectCfg, + CablePhysicsCfg, + NewtonCollisionPropertiesCfg, + NewtonPhysicsCfg, + NewtonRigidBodyMaterialCfg, + RenderCfg, + DLSSCfg, + RigidBodyPhysicsCfg, + ) + from embodichain.lab.sim.sensors import CameraCfg + from embodichain.lab.sim.material import VisualMaterialCfg + from embodichain.utils.math import xyz_quat_to_4x4_matrix + + out = args.output.resolve() + out.mkdir(parents=True, exist_ok=True) + urdf = out / "two_post_stage.urdf" + write_stage(urdf) + points = initial_points(args.segments) + frames = round(min(args.duration, DURATION) * FPS) + times = np.arange(frames + 1) / FPS + controls = np.array([control_at(t)[0] for t in times]) + np.savez(out / "inputs.npz", points=points, controls=controls, times=times) + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + startup_summary="compact", + robot_ik_gizmo=None, + render_cfg=RenderCfg( + renderer="hybrid", spp=4, dlss=DLSSCfg(dlss_enabled=False) + ), + physics_cfg=NewtonPhysicsCfg( + physics_dt=1 / FPS, + num_substeps=args.substeps, + solver_cfg=DexUniSolverCfg( + joint_mode="kinematic", + vbd_options={ + "iterations": args.iterations, + "rigid_contact_history": True, + "friction_epsilon": 0.001, + }, + collision_options={"contact_matching": "latest"}, + ), + collision_cfg=None, + sync_to_renderer=False, + use_cuda_graph=True, + ), + ) + ) + video = camera = cable = None + poses, speeds, slips = [], [], [] + start = time.perf_counter() + try: + sim.add_articulation( + ArticulationCfg( + uid="stage", + fpath=str(urdf), + init_qpos=controls[0], + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg.from_dict( + { + "material_props": { + "static_friction": 1.2, + "dynamic_friction": 1.0, + "restitution": 0.0, + }, + "collision_props": { + "contact_offset": 0.002, + "rest_offset": 0.0, + }, + } + ), + ) + ) + sim.register_kinematic_joint_trajectory("stage", controls[None], fps=FPS) + sim.add_cable_object( + CableObjectCfg( + uid="cable", + points=points, + radius=RADIUS, + fixed_segments=(0,), + collision_filter_span=1, + attrs=CablePhysicsCfg( + density=800, + collision_props=NewtonCollisionPropertiesCfg(margin=0, gap=0.002), + material_props=NewtonRigidBodyMaterialCfg(dynamic_friction=1.2), + stretch_stiffness=1e5, + shear_stiffness=1e5, + bend_stiffness=0.012, + twist_stiffness=0.012, + stretch_damping=1, + shear_damping=1, + bend_damping=0.002, + twist_damping=0.002, + ), + visual_material=VisualMaterialCfg( + uid="orange_cable", + base_color=(1.0, 0.25, 0.035, 1.0), + roughness=0.6, + ), + ) + ) + if not args.no_video: + camera = sim.add_sensor( + CameraCfg( + uid="overview", + width=1280, + height=960, + intrinsics=(1050.0, 1050.0, 640.0, 480.0), + enable_color=True, + extrinsics=CameraCfg.ExtrinsicsCfg( + eye=(-0.12, -0.60, 2.30), target=(-0.16, 0.05, 0.025) + ), + ) + ) + sim.prepare() + cable = sim.get_cable_object("cable") + assert cable.data is not None + print("READY", sim.physics.solver_type, cable.data.num_segments, flush=True) + if camera is not None: + video = cv2.VideoWriter( + str(out / "preview_raw.mp4"), + cv2.VideoWriter_fourcc(*"mp4v"), + 30, + (1280, 960), + ) + if not video.isOpened(): + raise RuntimeError("Cannot open video writer") + phase = None + initial_tip = cable.data.segment_pos_w[0, -1].detach().cpu().numpy() + tip_offset = initial_tip - controls[0, :3] + for frame, t in enumerate(times): + if frame: + sim.update(step=1, render_final_step=False) + state = cable.data.segment_state_w[0].detach().cpu() + p = xyz_quat_to_4x4_matrix(state[:, :7]).numpy() + v = state[:, 7:].numpy() + if not np.isfinite(p).all() or not np.isfinite(v).all(): + raise RuntimeError(f"Non-finite cable state at {t}") + poses.append(p) + speeds.append(np.sqrt(np.mean(np.sum(v[:, :3] ** 2, axis=1)))) + slips.append( + float(np.linalg.norm(p[-1, :3, 3] - controls[frame, :3] - tip_offset)) + ) + current = control_at(t)[1] + if current != phase: + print( + f"{t:.2f}s {current}, tip={p[-1,:3,3]}, slip={slips[-1]:.4f}", + flush=True, + ) + phase = current + if camera is not None and frame % 2 == 0: + with sim.render_frame(capture_camera_images=False): + sim.sync_render_state() + camera.update() + rgb = camera.get_data()["color"][0, ..., :3].cpu().numpy() + bgr = cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR) + cv2.rectangle(bgr, (0, 0), (1280, 88), (25, 29, 34), -1) + cv2.putText( + bgr, + "P-11 | CABLE AROUND TWO POSTS", + (28, 35), + cv2.FONT_HERSHEY_SIMPLEX, + 0.85, + (245, 245, 245), + 2, + cv2.LINE_AA, + ) + cv2.putText( + bgr, + f"{t:05.2f} s {current} | EmbodiChain + cable DexSim", + (28, 68), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (170, 200, 215), + 1, + cv2.LINE_AA, + ) + video.write(bgr) + if frame % 60 == 0: + cv2.imwrite(str(out / f"frame_{frame:04d}.jpg"), bgr) + wp.synchronize() + np.savez_compressed( + out / "states.npz", + times=times, + poses=poses, + speed_rms=speeds, + grip_tip_error=slips, + ) + metrics = { + "demo_only": True, + "grasp": "finger frictional contact; no endpoint attachment", + "fixed_root_segment": 0, + "dexsim_commit": dexsim.__commit_id__, + "dexsim_path": dexsim.__file__, + "newton": newton.__version__, + "warp": wp.__version__, + "solver": sim.physics.solver_type, + "segments": args.segments, + "initial_length_m": float( + np.linalg.norm(np.diff(points, axis=0), axis=1).sum() + ), + "dt": 1 / FPS, + "substeps": args.substeps, + "iterations": args.iterations, + "duration_s": float(times[-1]), + "wall_s": time.perf_counter() - start, + "max_grip_tip_error_before_release_m": ( + float(np.max(np.array(slips)[(times >= 1.3) & (times <= 10.2)])) + if frames >= 78 + else None + ), + "final_speed_rms_m_s": float(speeds[-1]), + "urdf_sha256": hashlib.sha256(urdf.read_bytes()).hexdigest(), + } + (out / "manifest.json").write_text(json.dumps(metrics, indent=2) + "\n") + print(json.dumps(metrics, indent=2), flush=True) + finally: + if video is not None: + video.release() + cable = camera = None + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + if not args.no_video: + subprocess.run( + [ + "ffmpeg", + "-y", + "-loglevel", + "error", + "-i", + str(out / "preview_raw.mp4"), + "-c:v", + "libx264", + "-crf", + "19", + "-pix_fmt", + "yuv420p", + "-movflags", + "+faststart", + str(out / "cable_two_posts.mp4"), + ], + check=True, + ) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--segments", type=int, default=64) + parser.add_argument("--duration", type=float, default=DURATION) + parser.add_argument("--substeps", type=int, default=16) + parser.add_argument("--iterations", type=int, default=20) + parser.add_argument("--no-video", action="store_true") + run(parser.parse_args()) diff --git a/examples/sim/demo/cable_wall_franka.py b/examples/sim/demo/cable_wall_franka.py new file mode 100644 index 0000000000..ed5dee6d9c --- /dev/null +++ b/examples/sim/demo/cable_wall_franka.py @@ -0,0 +1,966 @@ +# ---------------------------------------------------------------------------- +# 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 a Franka top-feed winding trajectory with gravity and cable contact. + +The commanded path makes three turns per post. Frictional grasp can still slip; +this preview does not certify successful wrapping or P-11 acceptance. +""" + +from __future__ import annotations + +import argparse +import hashlib +import json +from pathlib import Path +import subprocess +import time +import xml.etree.ElementTree as ET + +import cv2 +import numpy as np +from scipy.spatial.transform import Rotation + +FPS = 60 +CONTROL_FPS = 30 +TIME_SCALE = 3.5 +DURATION = 41.0 * TIME_SCALE +BASE = np.array([0.25, 0.0, 2.4]) +POSTS = np.array([[0.75, -0.20, 2.75], [0.75, 0.20, 2.75]]) +POST_RADIUS = 0.05 +POST_HALF_LENGTH = 0.05 +RADIUS = 0.004 +ORBIT_RADIUS = 0.16 +CABLE_LENGTH = 3.10 +CLOSED_FINGER = 0.0155 +FEED_FINGER = 0.0155 +BEND_STIFFNESS = 0.0003 +ANCHOR = np.array([0.76, -0.40, 2.92]) +JOINT_NAMES = tuple(f"fr3_joint{i}" for i in range(1, 8)) + ( + "fr3_finger_joint1", + "fr3_finger_joint2", +) +HAND_QUAT = Rotation.from_euler("x", np.pi).as_quat() +TOOL_POINT = np.array([0.13, 0.0, 0.0]) + + +def smooth(t: float) -> float: + u = float(np.clip(t, 0, 1)) + return u * u * (3 - 2 * u) + + +def orbit(post: int, u: float) -> np.ndarray: + theta = np.pi / 4 - 6 * np.pi * u + return np.array( + [ + 0.76 - 0.042 * u, + POSTS[post, 1] + ORBIT_RADIUS * np.cos(theta), + POSTS[post, 2] + ORBIT_RADIUS * np.sin(theta), + ] + ) + + +def target_at(t: float) -> tuple[np.ndarray, float, str, tuple[float, float]]: + """Three complete end-effector revolutions at each post, then release.""" + t = t / TIME_SCALE + if t <= 1: + return orbit(0, 0), FEED_FINGER, "THREAD / SETTLE", (0.0, 0.0) + if t <= 16: + u = smooth((t - 1) / 15) + return orbit(0, u), FEED_FINGER, "WIND + FEED POST 1", (3 * u, 0.0) + if t <= 19: + u = smooth((t - 16) / 3) + p = (1 - u) * orbit(0, 1) + u * orbit(1, 0) + p[2] += 0.10 * np.sin(np.pi * u) + return p, FEED_FINGER, "TRANSFER + FEED", (3.0, 0.0) + if t <= 34: + u = smooth((t - 19) / 15) + return orbit(1, u), FEED_FINGER, "WIND + FEED POST 2", (3.0, 3 * u) + if t <= 36: + return orbit(1, 1), CLOSED_FINGER, "HOLD", (3.0, 3.0) + if t <= 36.6: + return ( + orbit(1, 1), + CLOSED_FINGER + (0.04 - CLOSED_FINGER) * smooth((t - 36) / 0.6), + "RELEASE", + (3.0, 3.0), + ) + p = orbit(1, 1) + np.array([-0.07 * smooth((t - 36.6) / 1.4), 0, 0]) + return p, 0.04, "OBSERVE GRAVITY", (3.0, 3.0) + + +def initial_points(segments: int) -> np.ndarray: + """Short working lead passes through the hand; the remaining tail is free.""" + grip = orbit(0, 0) + # Enter the guide from the wall side and feed out toward the observer. + # The hanging tail then stays in front of both post tips. + u = np.linspace(0, 1, 2000)[:, None] + a, b = ANCHOR, ANCHOR + np.array([0.02, 0.16, 0.06]) + c, d = grip + np.array([0.035, 0, 0]), grip + lead = ( + (1 - u) ** 3 * a + 3 * (1 - u) ** 2 * u * b + 3 * (1 - u) * u**2 * c + u**3 * d + ) + lead_length = np.linalg.norm(np.diff(lead, axis=0), axis=1).sum() + outlet = grip - np.array([0.19, 0, 0]) + straight = np.linspace(grip, outlet, 500) + theta = np.linspace(0, np.pi / 2, 500) + bend = outlet + np.column_stack( + (-0.035 * np.sin(theta), np.zeros(500), -0.035 * (1 - np.cos(theta))) + ) + tail_length = CABLE_LENGTH - lead_length - 0.19 - 0.035 * np.pi / 2 + tail = bend[-1] + np.column_stack( + (np.zeros(2000), np.zeros(2000), -np.linspace(0, tail_length, 2000)) + ) + dense = np.vstack((lead[:-1], straight[:-1], bend[:-1], tail)) + arc = np.r_[0, np.cumsum(np.linalg.norm(np.diff(dense, axis=0), axis=1))] + return np.column_stack( + [ + np.interp(np.linspace(0, arc[-1], segments + 1), arc, dense[:, axis]) + for axis in range(3) + ] + ).astype(np.float32) + + +def write_feed_gripper(source: Path, path: Path) -> None: + """Add two visible grooved finger pads, open along the cable feed axis. + + The rectangular channel closes transversely around the rope and routes the + hanging tail in front of the post tips. + Nothing constrains a cable node or assigns its material position. + """ + root = ET.parse(source).getroot() + mesh_root = path.parent / "meshes" + source_mesh_root = (source.parent / "meshes").resolve() + if not mesh_root.exists(): + mesh_root.symlink_to(source_mesh_root, target_is_directory=True) + if mesh_root.resolve() != source_mesh_root: + raise ValueError("Output meshes path conflicts with the original Franka assets") + for mesh in root.findall(".//mesh"): + if not (path.parent / mesh.get("filename")).is_file(): + raise FileNotFoundError(mesh.get("filename")) + for name in ("fr3_leftfinger", "fr3_rightfinger"): + # Right finger is authored with a half turn around local Z. + center = 0.07 if name == "fr3_leftfinger" else -0.07 + boxes = [ + ((center, 0.002, 0.045), (0.180, 0.004, 0.035)), + ((center, -0.00575, 0.0275), (0.180, 0.0195, 0.004)), + ((center, -0.00575, 0.0625), (0.180, 0.0195, 0.004)), + ] + for index, (xyz, size) in enumerate(boxes): + pad_name = f"{name}_feed_pad_{index}" + link = ET.SubElement(root, "link", name=pad_name) + inertial = ET.SubElement(link, "inertial") + ET.SubElement(inertial, "mass", value=".0005") + ET.SubElement( + inertial, + "inertia", + ixx="1e-7", + iyy="1e-7", + izz="1e-7", + ixy="0", + ixz="0", + iyz="0", + ) + joint = ET.SubElement(root, "joint", name=pad_name + "_fixed", type="fixed") + ET.SubElement(joint, "parent", link=name) + ET.SubElement(joint, "child", link=pad_name) + ET.SubElement(joint, "origin", xyz=" ".join(map(str, xyz)), rpy="0 0 0") + for purpose in ("collision", "visual"): + item = ET.SubElement(link, purpose, name=f"feed_pad_{index}") + geometry = ET.SubElement(item, "geometry") + ET.SubElement(geometry, "box", size=" ".join(map(str, size))) + if purpose == "visual": + mat = ET.SubElement(item, "material", name="feed_pad") + ET.SubElement(mat, "color", rgba=".85 .58 .12 1") + ET.indent(root) + ET.ElementTree(root).write(path, encoding="unicode", xml_declaration=True) + + +def material_at_gripper( + poses: np.ndarray, rest_lengths: np.ndarray, grip: np.ndarray +) -> tuple[float, float, int]: + """Closest point on complete capsule axes, expressed in material arc length.""" + axis = poses[:, :3, 2] + starts = poses[:, :3, 3] - 0.5 * rest_lengths[:, None] * axis + local = np.clip(np.sum((grip - starts) * axis, axis=1), 0, rest_lengths) + distances = np.linalg.norm(starts + local[:, None] * axis - grip, axis=1) + index = int(np.argmin(distances)) + return ( + float(rest_lengths[:index].sum() + local[index]), + float(distances[index]), + index, + ) + + +def summarize_feeding( + times: np.ndarray, + poses: np.ndarray, + feed_points: np.ndarray, + material_s: np.ndarray, + distances: np.ndarray, + segment_ids: np.ndarray, + windings: np.ndarray, + post_penetrations: np.ndarray, +) -> dict[str, object]: + """Preview diagnostics; retain failure and incomplete runs as such.""" + pre = times < 36 * TIME_SCALE + lost = (distances > 0.02) & pre + result = { + "scope": "demo diagnostics, not P-11 physical acceptance", + "grip_loss_episodes": int(np.sum(lost & ~np.r_[False, lost[:-1]])), + "feed_after_settle_m": float( + material_s[np.flatnonzero(pre)[-1]] + - material_s[np.argmin(abs(times - TIME_SCALE))] + ), + "max_material_jump_per_frame_m": ( + float(np.max(abs(np.diff(material_s[pre])))) if pre.sum() > 1 else 0.0 + ), + "complete": bool(times[-1] >= DURATION - 1 / FPS), + "maximum_sampled_post_penetration_m": float(post_penetrations.max()), + "wraps_before_release": None, + "near_post_turns_before_release": None, + } + if times[-1] >= 35 * TIME_SCALE: + k = int(np.argmin(abs(times - 35 * TIME_SCALE))) + line = np.vstack((poses[k, : segment_ids[k] + 1, :3, 3], feed_points[k])) + wraps = [] + contact_turns = [] + for j, post in enumerate(POSTS): + angles = np.arctan2(line[:, 2] - post[2], line[:, 1] - post[1]) + endpoint_delta = np.angle(np.exp(1j * (angles[-1] - angles[0]))) + wraps.append(int(round(windings[k, j] - endpoint_delta / (2 * np.pi)))) + radial = np.linalg.norm(line[:, 1:] - post[1:], axis=1) + near = ( + (abs(line[:, 0] - post[0]) < POST_HALF_LENGTH) + & (radial < POST_RADIUS + RADIUS + 0.018) + & (radial > POST_RADIUS - 0.006) + ) + increments = np.angle(np.exp(1j * np.diff(angles))) + contact_turns.append( + float(abs(np.sum(increments[near[:-1] & near[1:]])) / (2 * np.pi)) + ) + result["wraps_before_release"] = wraps + result["near_post_turns_before_release"] = contact_turns + result["preview_checks_passed"] = bool( + result["complete"] + and not lost.any() + and all(x == -3 for x in wraps) + and min(contact_turns) >= 2.7 + and post_penetrations.max() < 0.01 + ) + else: + result["preview_checks_passed"] = False + return result + + +def sampled_post_penetration(poses: np.ndarray, rest_lengths: np.ndarray) -> np.ndarray: + """Seven axial samples per capsule against ideal finite cylinders. + + This catches gross crossings at every saved frame; it is not an exact + mesh-distance calculation or a continuous collision proof. + """ + offsets = np.linspace(-0.5, 0.5, 7) + samples = ( + poses[:, None, :3, 3] + + rest_lengths[:, None, None] * offsets[None, :, None] * poses[:, None, :3, 2] + ).reshape(-1, 3) + depths = [] + for post in POSTS: + d = np.column_stack( + ( + abs(samples[:, 0] - post[0]) - POST_HALF_LENGTH, + np.linalg.norm(samples[:, 1:] - post[1:], axis=1) - POST_RADIUS, + ) + ) + sdf = np.linalg.norm(np.maximum(d, 0), axis=1) + np.minimum(d.max(axis=1), 0) + depths.append(max(0.0, float(RADIUS - sdf.min()))) + return np.asarray(depths) + + +def write_stage(path: Path) -> None: + """Wall, normal-axis posts, visible fixed root and arm pedestal.""" + root = ET.Element("robot", name="wall_fixture") + + def link( + name: str, + kind: str, + dims: dict[str, str], + xyz: np.ndarray, + rpy: str, + color: str, + ) -> None: + body = ET.SubElement(root, "link", name=name) + inertial = ET.SubElement(body, "inertial") + ET.SubElement(inertial, "mass", value="1") + ET.SubElement( + inertial, "inertia", ixx=".1", iyy=".1", izz=".1", ixy="0", ixz="0", iyz="0" + ) + for purpose in ("visual", "collision"): + item = ET.SubElement(body, purpose) + ET.SubElement(item, "origin", xyz=" ".join(map(str, xyz)), rpy=rpy) + geo = ET.SubElement(item, "geometry") + ET.SubElement(geo, kind, **dims) + if purpose == "visual": + mat = ET.SubElement(item, "material", name=name + "_mat") + ET.SubElement(mat, "color", rgba=color) + if name != "wall": + joint = ET.SubElement(root, "joint", name=name + "_fixed", type="fixed") + ET.SubElement(joint, "parent", link="wall") + ET.SubElement(joint, "child", link=name) + + link( + "wall", + "box", + {"size": ".05 1.65 3.25"}, + np.array([0.825, 0, 1.625]), + "0 0 0", + ".17 .22 .28 1", + ) + for n, p in enumerate(POSTS, 1): + link( + f"post_{n}", + "cylinder", + {"radius": str(POST_RADIUS), "length": str(2 * POST_HALF_LENGTH)}, + p, + "0 1.570796326794897 0", + ".10 .57 .68 1", + ) + link( + "anchor", + "box", + {"size": ".06 .042 .042"}, + ANCHOR + np.array([0.015, 0, 0]), + "0 0 0", + ".96 .57 .10 1", + ) + link( + "pedestal", + "box", + {"size": f".32 .32 {BASE[2]}"}, + np.array([BASE[0], 0, BASE[2] / 2]), + "0 0 0", + ".35 .39 .43 1", + ) + ET.indent(root) + ET.ElementTree(root).write(path, encoding="unicode", xml_declaration=True) + + +def robot_fk(urdf: Path, q: np.ndarray) -> dict[str, np.ndarray]: + """Independent XML FK, with named joints and unmodified asset limits.""" + root = ET.parse(urdf).getroot() + qmap = dict(zip(JOINT_NAMES, q, strict=True)) + transforms = {"base": np.eye(4)} + transforms["base"][:3, 3] = BASE + remaining = list(root.findall("joint")) + while remaining: + progressed = False + for joint in remaining[:]: + parent = joint.find("parent").get("link") + child = joint.find("child").get("link") + if parent not in transforms: + continue + origin = joint.find("origin") + local = np.eye(4) + if origin is not None: + local[:3, 3] = np.fromstring(origin.get("xyz", "0 0 0"), sep=" ") + local[:3, :3] = Rotation.from_euler( + "xyz", np.fromstring(origin.get("rpy", "0 0 0"), sep=" ") + ).as_matrix() + motion = np.eye(4) + value = qmap.get(joint.get("name"), 0.0) + axis = joint.find("axis") + if axis is not None: + vector = np.fromstring(axis.get("xyz"), sep=" ") + if joint.get("type") == "revolute": + motion[:3, :3] = Rotation.from_rotvec(vector * value).as_matrix() + if joint.get("type") == "prismatic": + motion[:3, 3] = vector * value + transforms[child] = transforms[parent] @ local @ motion + remaining.remove(joint) + progressed = True + if not progressed: + raise RuntimeError("URDF joint tree is disconnected") + return transforms + + +def make_trajectory( + urdf: Path, out: Path, duration: float, q7: float = 0.75 +) -> tuple[np.ndarray, np.ndarray]: + """Use EmbodiChain FEP IK; independently check FK and asset joint limits.""" + import torch + from embodichain.lab.sim.motion.solvers import FEPSolverCfg + + times = np.arange(round(duration * CONTROL_FPS) + 1) / CONTROL_FPS + targets = np.array([target_at(t)[0] for t in times]) + ik = FEPSolverCfg( + urdf_path=str(urdf), + root_link_name="base", + end_link_name="fr3_hand_tcp", + redundancy_search=False, + ).init_solver("cpu") + seed = torch.tensor([[0, -0.6, 0, -2.2, 0, 1.6, q7]], dtype=torch.float32) + pose = torch.eye(4, dtype=torch.float32) + pose[:3, :3] = torch.tensor( + Rotation.from_quat(HAND_QUAT).as_matrix(), dtype=torch.float32 + ) + qs = [] + errors = [] + rot_errors = [] + for index, t in enumerate(times): + target, finger, _, _ = target_at(t) + tcp_target = target - Rotation.from_quat(HAND_QUAT).apply(TOOL_POINT) + pose[:3, 3] = torch.tensor(tcp_target - BASE, dtype=torch.float32) + valid, seed = ik.get_ik(pose, seed) + if not bool(valid.all()): + raise RuntimeError(f"FEP IK failed at {t:.3f}s") + q = np.r_[seed.numpy().reshape(-1), finger, finger] + fk = robot_fk(urdf, q)["fr3_hand_tcp"] + errors.append(np.linalg.norm(fk[:3, 3] + fk[:3, :3] @ TOOL_POINT - target)) + rot_errors.append( + ( + Rotation.from_matrix(fk[:3, :3]).inv() * Rotation.from_quat(HAND_QUAT) + ).magnitude() + ) + qs.append(q) + if index % 150 == 0: + print("IK", t, "error_m", errors[-1], flush=True) + qs = np.array(qs, dtype=np.float32) + root = ET.parse(urdf).getroot() + limits = { + j.get("name"): j.find("limit") + for j in root.findall("joint") + if j.get("name") in JOINT_NAMES + } + lower = np.array([float(limits[n].get("lower")) for n in JOINT_NAMES]) + upper = np.array([float(limits[n].get("upper")) for n in JOINT_NAMES]) + limit_violation = float(max(np.max(lower - qs), np.max(qs - upper), 0)) + joint_speed = np.abs(np.diff(qs[:, :7], axis=0)) * CONTROL_FPS + speed_limits = np.array([float(limits[n].get("velocity")) for n in JOINT_NAMES[:7]]) + stats = { + "ik_solver": f"EmbodiChain FEP, fixed q7={q7} rad", + "max_position_error_m": float(max(errors)), + "max_rotation_error_rad": float(max(rot_errors)), + "joint_limit_violation_rad": limit_violation, + "max_joint_step_rad": float(np.abs(np.diff(qs[:, :7], axis=0)).max()), + "max_joint_speed_rad_s": float(joint_speed.max()), + "max_speed_limit_ratio": float((joint_speed / speed_limits).max()), + } + (out / "ik_checks.json").write_text(json.dumps(stats, indent=2) + "\n") + np.savez( + out / "robot_trajectory.npz", + times=times, + qpos=qs, + joint_names=JOINT_NAMES, + tcp_targets=targets, + hand_quaternion=HAND_QUAT, + tool_point=TOOL_POINT, + position_error=errors, + ) + print("IK_CHECKS", stats, flush=True) + if ( + max(errors) > 0.004 + or max(rot_errors) > 0.04 + or limit_violation > 0.001 + or stats["max_speed_limit_ratio"] > 1 + ): + raise RuntimeError( + "Franka trajectory failed independent FK / joint-limit checks" + ) + return times, qs + + +def run(args: argparse.Namespace) -> None: + import dexsim, newton, warp as wp + from dexsim.engine.newton_physics import DexUniSolverCfg + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + from embodichain.lab.sim.cfg import ( + ArticulationCfg, + CableObjectCfg, + CablePhysicsCfg, + NewtonCollisionPropertiesCfg, + NewtonPhysicsCfg, + NewtonRigidBodyMaterialCfg, + RenderCfg, + DLSSCfg, + RigidBodyPhysicsCfg, + ) + from embodichain.lab.sim.cfg.articulation import LinkPhysicsOverrideCfg + from embodichain.lab.sim.sensors import CameraCfg + from embodichain.lab.sim.material import VisualMaterialCfg + from embodichain.utils.math import xyz_quat_to_4x4_matrix + + out = args.output.resolve() + out.mkdir(parents=True, exist_ok=True) + source_urdf = args.robot.resolve() + urdf = out / "franka_feed_gripper.urdf" + write_feed_gripper(source_urdf, urdf) + duration = min(args.duration, DURATION) + if args.trajectory: + saved = np.load(args.trajectory) + qs = saved["qpos"] + control_times = saved["times"] + qs[:, -2:] = np.array([target_at(t)[1] for t in control_times])[:, None] + if control_times[-1] < duration: + raise ValueError("Trajectory shorter than requested duration") + expected = np.array([target_at(t)[0] for t in control_times]) + if not np.allclose(saved["tcp_targets"], expected, atol=1e-6, rtol=0): + raise ValueError( + "Cached trajectory does not match the current winding path" + ) + if ( + "hand_quaternion" not in saved + or "tool_point" not in saved + or not np.allclose(saved["hand_quaternion"], HAND_QUAT, atol=1e-6, rtol=0) + or not np.allclose(saved["tool_point"], TOOL_POINT, atol=1e-6, rtol=0) + ): + raise ValueError( + "Cached trajectory does not match the current feed tool pose" + ) + checks = args.trajectory.parent / "ik_checks.json" + if checks.is_file(): + (out / "ik_checks.json").write_bytes(checks.read_bytes()) + else: + control_times, qs = make_trajectory(urdf, out, duration, args.ik_q7) + if args.plan_only: + return + np.savez( + out / "robot_trajectory.npz", + times=control_times, + qpos=qs, + joint_names=JOINT_NAMES, + tcp_targets=np.array([target_at(t)[0] for t in control_times]), + hand_quaternion=HAND_QUAT, + tool_point=TOOL_POINT, + ) + (out / "source_snapshot.py").write_bytes(Path(__file__).read_bytes()) + config = { + "length_m": CABLE_LENGTH, + "radius_m": RADIUS, + "post_centers": POSTS.tolist(), + "post_radius_m": POST_RADIUS, + "post_half_length_m": POST_HALF_LENGTH, + "anchor": ANCHOR.tolist(), + "franka_base": BASE.tolist(), + "hand_quaternion_xyzw": HAND_QUAT.tolist(), + "tool_point_in_tcp_m": TOOL_POINT.tolist(), + "orbit_radius_m": ORBIT_RADIUS, + "time_scale": TIME_SCALE, + "feed_finger_q_m": FEED_FINGER, + "hold_finger_q_m": CLOSED_FINGER, + "bend_stiffness": BEND_STIFFNESS, + "gripper_mu": 0.08, + "rope_mu": 0.15, + "stage_mu": 0.15, + "contact_ke": 1e5, + "joint_coefficient_scale": args.segments / 256, + "rigid_body_contact_buffer_size": 512, + "physical_tip_attachment": False, + "arguments": { + key: str(value) if isinstance(value, Path) else value + for key, value in vars(args).items() + }, + } + (out / "config.json").write_text(json.dumps(config, indent=2) + "\n") + stage = out / "wall_fixture.urdf" + write_stage(stage) + points = initial_points(args.segments) + rest_lengths = np.linalg.norm(np.diff(points, axis=0), axis=1) + np.savez(out / "inputs.npz", points=points, posts=POSTS, anchor=ANCHOR) + sim = SimulationManager( + SimulationManagerCfg( + headless=not args.viewer, + startup_summary="compact", + robot_ik_gizmo=None, + render_cfg=RenderCfg( + renderer="hybrid", spp=4, dlss=DLSSCfg(dlss_enabled=False) + ), + physics_cfg=NewtonPhysicsCfg( + physics_dt=1 / FPS, + num_substeps=args.substeps, + solver_cfg=DexUniSolverCfg( + joint_mode="kinematic", + vbd_options={ + "iterations": args.iterations, + "rigid_contact_history": True, + "rigid_body_contact_buffer_size": 512, + "friction_epsilon": 0.001, + }, + collision_options={"contact_matching": "latest"}, + ), + collision_cfg=None, + sync_to_renderer=False, + use_cuda_graph=True, + ), + ) + ) + video = camera = detail_camera = cable = robot = None + poses = [] + speeds = [] + free_tip_distances = [] + actual_tcp = [] + actual_feed_point = [] + grip_material_s = [] + grip_distances = [] + grip_segments = [] + prefix_windings = [] + post_penetrations = [] + start = time.perf_counter() + try: + material = RigidBodyPhysicsCfg.from_dict( + { + "material_props": { + "backend": "newton", + "dynamic_friction": 0.15, + "ke": 1e5, + "kd": 100.0, + }, + "collision_props": {"contact_offset": 0.001, "rest_offset": 0.0}, + } + ) + grip_material = RigidBodyPhysicsCfg.from_dict( + { + "material_props": { + "backend": "newton", + "dynamic_friction": 0.08, + "ke": 2e4, + "kd": 100.0, + }, + "collision_props": {"contact_offset": 0.001, "rest_offset": 0.0}, + } + ) + sim.add_articulation( + ArticulationCfg( + uid="wall_fixture", + fpath=str(stage), + asset_physics_mode="overlay", + attrs=material, + ) + ) + robot = sim.add_articulation( + ArticulationCfg( + uid="franka", + fpath=str(urdf), + init_pos=tuple(BASE), + init_qpos=qs[0], + asset_physics_mode="overlay", + attrs=material, + link_attrs={ + "finger_pads": LinkPhysicsOverrideCfg( + link_names_expr=[ + "fr3_leftfinger", + "fr3_rightfinger", + "fr3_.*finger_feed_pad_.*", + ], + attrs=grip_material, + ) + }, + ) + ) + sim.register_kinematic_joint_trajectory("franka", qs[None], fps=CONTROL_FPS) + sim.add_cable_object( + CableObjectCfg( + uid="cable", + points=points, + radius=RADIUS, + render_mode="capsules", + fixed_segments=(0,), + collision_filter_span=1, + attrs=CablePhysicsCfg( + density=800, + collision_props=NewtonCollisionPropertiesCfg(margin=0, gap=0.001), + material_props=NewtonRigidBodyMaterialCfg( + dynamic_friction=0.15, ke=1e5, kd=100.0 + ), + stretch_stiffness=5e5 * args.segments / 256, + shear_stiffness=5e5 * args.segments / 256, + bend_stiffness=BEND_STIFFNESS * args.segments / 256, + twist_stiffness=BEND_STIFFNESS * args.segments / 256, + stretch_damping=args.segments / 256, + shear_damping=args.segments / 256, + bend_damping=0.001 * args.segments / 256, + twist_damping=0.001 * args.segments / 256, + ), + visual_material=VisualMaterialCfg( + uid="orange_cable", + base_color=(1.0, 0.26, 0.025, 1.0), + roughness=0.6, + ), + ) + ) + if not args.no_video: + camera = sim.add_sensor( + CameraCfg( + uid="wall_view", + width=1600, + height=1200, + intrinsics=(1180.0, 1180.0, 800.0, 600.0), + extrinsics=CameraCfg.ExtrinsicsCfg( + eye=(-1.7, 1.0, 3.3), target=(0.65, 0, 2.45) + ), + ) + ) + detail_camera = sim.add_sensor( + CameraCfg( + uid="contact_detail", + width=760, + height=560, + intrinsics=(800.0, 800.0, 380.0, 280.0), + extrinsics=CameraCfg.ExtrinsicsCfg( + eye=(0.20, -0.55, 3.15), target=(0.75, 0, 2.75) + ), + ) + ) + sim.prepare() + cable = sim.get_cable_object("cable") + assert cable.data is not None + print("ROBOT_JOINT_ORDER", robot.joint_names, flush=True) + assert [x.lower() for x in robot.joint_names] == list(JOINT_NAMES) + if camera is not None: + video = cv2.VideoWriter( + str(out / "preview_raw.mp4"), + cv2.VideoWriter_fourcc(*"mp4v"), + 30, + (1600, 1200), + ) + if not video.isOpened(): + raise RuntimeError("Video writer unavailable") + frames = round(duration * FPS) + times = np.arange(frames + 1) / FPS + phase = None + for frame, t in enumerate(times): + if frame: + sim.update(step=1, render_final_step=args.viewer and camera is None) + state = cable.data.segment_state_w[0].detach().cpu() + p = xyz_quat_to_4x4_matrix(state[:, :7]).numpy() + v = state[:, 7:].numpy() + if not np.isfinite(p).all() or not np.isfinite(v).all(): + raise RuntimeError(f"Nonfinite state at {t}") + poses.append(p) + post_penetrations.append(sampled_post_penetration(p, rest_lengths)) + speeds.append(np.sqrt(np.mean(np.sum(v[:, :3] ** 2, axis=1)))) + target, finger, current, turns = target_at(t) + hand_pose = ( + robot.get_link_pose("fr3_hand_tcp", to_matrix=True) + .detach() + .cpu() + .numpy()[0] + ) + actual_tcp.append(hand_pose[:3, 3]) + feed_point = hand_pose[:3, 3] + hand_pose[:3, :3] @ TOOL_POINT + actual_feed_point.append(feed_point) + material_s, grip_distance, grip_segment = material_at_gripper( + p, rest_lengths, feed_point + ) + grip_material_s.append(material_s) + grip_distances.append(grip_distance) + grip_segments.append(grip_segment) + working_line = np.vstack((p[: grip_segment + 1, :3, 3], feed_point)) + angles = [ + np.unwrap( + np.arctan2(working_line[:, 2] - c[2], working_line[:, 1] - c[1]) + ) + for c in POSTS + ] + net = [float((x[-1] - x[0]) / (2 * np.pi)) for x in angles] + prefix_windings.append(net) + free_tip_distances.append( + float(np.linalg.norm(p[-1, :3, 3] - hand_pose[:3, 3])) + ) + if current != phase or frame % 300 == 0: + print( + f"{t:.2f}s {current} material_s={material_s:.3f} grip_axis_distance={grip_distance:.4f} working_winding={net}", + flush=True, + ) + phase = current + if camera is not None and frame % 7 == 0: + with sim.render_frame(capture_camera_images=False): + sim.sync_render_state() + camera.update() + detail_camera.update() + rgb = camera.get_data()["color"][0, ..., :3].cpu().numpy() + bgr = cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR) + detail_rgb = detail_camera.get_data()["color"][0, ..., :3].cpu().numpy() + bgr[625:1185, 825:1585] = cv2.cvtColor(detail_rgb, cv2.COLOR_RGB2BGR) + cv2.rectangle(bgr, (823, 623), (1587, 1187), (190, 190, 190), 2) + cv2.putText( + bgr, + "CONTACT DETAIL", + (845, 652), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (255, 255, 255), + 1, + cv2.LINE_AA, + ) + cv2.rectangle(bgr, (0, 0), (1600, 124), (24, 29, 35), -1) + cv2.putText( + bgr, + "P-11 | FRANKA FR3 | SLIDING FEED | 3 TURNS / POST", + (25, 36), + cv2.FONT_HERSHEY_SIMPLEX, + 0.85, + (240, 245, 250), + 2, + cv2.LINE_AA, + ) + cv2.putText( + bgr, + f"{t:06.2f}s {current} | Commanded turns: {turns[0]:.2f} / {turns[1]:.2f} | Gravity: 9.81 m/s2 | {TIME_SCALE:g}x replay", + (25, 74), + cv2.FONT_HERSHEY_SIMPLEX, + 0.63, + (180, 207, 220), + 1, + cv2.LINE_AA, + ) + grasp_label = ( + f"MATERIAL AT GRIP: {material_s:.3f} m | TAIL: {CABLE_LENGTH-material_s:.3f} m | CONTACT ONLY / NO ATTACHMENT" + if t < 36 * TIME_SCALE + else "GRIP OPEN / FREE RELEASE | ROPE MOTION FROM GRAVITY AND CONTACT" + ) + cv2.putText( + bgr, + grasp_label, + (25, 106), + cv2.FONT_HERSHEY_SIMPLEX, + 0.63, + (110, 195, 255), + 1, + cv2.LINE_AA, + ) + video.write(bgr) + if frame % 420 == 0: + cv2.imwrite(str(out / f"frame_{frame:04d}.jpg"), bgr) + wp.synchronize() + poses = np.asarray(poses) + feeding_checks = summarize_feeding( + times, + poses, + np.asarray(actual_feed_point), + np.asarray(grip_material_s), + np.asarray(grip_distances), + np.asarray(grip_segments), + np.asarray(prefix_windings), + np.asarray(post_penetrations), + ) + (out / "feeding_checks.json").write_text( + json.dumps(feeding_checks, indent=2) + "\n" + ) + np.savez_compressed( + out / "states.npz", + times=times, + poses=poses, + speed_rms=speeds, + free_tip_distance_to_tcp=free_tip_distances, + actual_tcp=actual_tcp, + actual_feed_point=actual_feed_point, + grip_material_s=grip_material_s, + grip_distance=grip_distances, + grip_segment=grip_segments, + working_prefix_winding=prefix_windings, + sampled_post_penetration_m=post_penetrations, + ) + metrics = { + "demo_only": True, + "acceptance_status": "not_validated; commanded turns are not measured rope wraps", + "feeding_checks": feeding_checks, + "robot_asset": str(urdf), + "source_robot_asset": str(source_urdf), + "robot_sha256": hashlib.sha256(urdf.read_bytes()).hexdigest(), + "dexsim_commit": dexsim.__commit_id__, + "solver": sim.physics.solver_type, + "newton": newton.__version__, + "warp": wp.__version__, + "grasp": "sliding contact in visible grooved Franka pads; free material feed", + "diagnostic_endpoint_attachment": False, + "release_time_s": 36 * TIME_SCALE, + "base_position": BASE.tolist(), + "segments": args.segments, + "length_m": float(np.linalg.norm(np.diff(points, axis=0), axis=1).sum()), + "radius_m": RADIUS, + "bend_stiffness": BEND_STIFFNESS, + "initial_material_at_grip_m": grip_material_s[0], + "final_material_at_grip_m": grip_material_s[-1], + "max_grip_centerline_distance_before_release_m": float( + np.max(np.array(grip_distances)[times <= 36 * TIME_SCALE]) + ), + "working_prefix_winding_final": prefix_windings[-1], + "dt": 1 / FPS, + "substeps": args.substeps, + "iterations": args.iterations, + "duration_s": duration, + "wall_s": time.perf_counter() - start, + "final_free_tip_distance_to_tcp_m": float(free_tip_distances[-1]), + "final_speed_rms_m_s": float(speeds[-1]), + } + (out / "manifest.json").write_text(json.dumps(metrics, indent=2) + "\n") + print(json.dumps(metrics, indent=2), flush=True) + finally: + if video is not None: + video.release() + robot = cable = camera = detail_camera = None + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + if not args.no_video: + subprocess.run( + [ + "ffmpeg", + "-y", + "-loglevel", + "error", + "-i", + str(out / "preview_raw.mp4"), + "-c:v", + "libx264", + "-crf", + "19", + "-pix_fmt", + "yuv420p", + "-movflags", + "+faststart", + str(out / "cable_wall_franka.mp4"), + ], + check=True, + ) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--robot", type=Path, required=True) + parser.add_argument("--trajectory", type=Path) + parser.add_argument("--duration", type=float, default=DURATION) + parser.add_argument("--segments", type=int, default=128) + parser.add_argument("--substeps", type=int, default=8) + parser.add_argument("--iterations", type=int, default=40) + parser.add_argument("--no-video", action="store_true") + parser.add_argument( + "--viewer", + action="store_true", + help="Open the native simulation viewer while running", + ) + parser.add_argument("--plan-only", action="store_true") + parser.add_argument("--ik-q7", type=float, default=0.75) + args = parser.parse_args() + if ( + args.duration <= 0 + or args.segments < 2 + or args.substeps < 1 + or args.iterations < 1 + ): + parser.error( + "duration, substeps and iterations must be positive; segments must be >= 2" + ) + run(args) diff --git a/extensions/embodichain_robots/__init__.py b/extensions/embodichain_robots/__init__.py index 77b38f755f..90d385c8e9 100644 --- a/extensions/embodichain_robots/__init__.py +++ b/extensions/embodichain_robots/__init__.py @@ -26,6 +26,7 @@ from typing import Any __all__ = [ + "YamI2rtCfg", "UnitreeGo1Cfg", "UnitreeGo2Cfg", "UnitreeG1Cfg", @@ -48,6 +49,7 @@ ] _EXPORTS = { + "YamI2rtCfg": ".i2rt.yam", "UnitreeGo1Cfg": ".unitree.go1", "UnitreeGo2Cfg": ".unitree.go2", "UnitreeG1Cfg": ".unitree.g1", diff --git a/extensions/embodichain_robots/_asset_catalog.py b/extensions/embodichain_robots/_asset_catalog.py index 1f3e554ce2..0a88905ef0 100644 --- a/extensions/embodichain_robots/_asset_catalog.py +++ b/extensions/embodichain_robots/_asset_catalog.py @@ -19,6 +19,10 @@ from __future__ import annotations ROBOT_ASSETS = { + "YamI2rt": { + "class_path": "embodichain_robots.i2rt.yam.assets:YamI2rt", + "category": "robot", + }, "UnitreeGo1Locomotion": { "class_path": "embodichain_robots.unitree.go1.assets:UnitreeGo1Locomotion", "category": "robot", diff --git a/extensions/embodichain_robots/_catalog.py b/extensions/embodichain_robots/_catalog.py index d42f368245..cdcf0d0412 100644 --- a/extensions/embodichain_robots/_catalog.py +++ b/extensions/embodichain_robots/_catalog.py @@ -21,6 +21,7 @@ from itertools import product _CONFIG_CLASSES = { + "YamI2rt": "embodichain_robots.i2rt.yam:YamI2rtCfg", "UnitreeGo1": "embodichain_robots.unitree.go1:UnitreeGo1Cfg", "UnitreeGo2": "embodichain_robots.unitree.go2:UnitreeGo2Cfg", "UnitreeG1": "embodichain_robots.unitree.g1:UnitreeG1Cfg", @@ -43,6 +44,10 @@ } _DEFAULT_CONTROL_INFO = { + "YamI2rt": { + "dof": 16, + "control_parts": {"right_arm": 6, "left_arm": 6, "right_eef": 2, "left_eef": 2}, + }, "UnitreeGo1": {"dof": 12, "control_parts": {"locomotion": 12}}, "UnitreeGo2": {"dof": 12, "control_parts": {"locomotion": 12}}, "UnitreeG1": {"dof": 29, "control_parts": {"locomotion": 29}}, @@ -131,6 +136,7 @@ } _CLASSIFICATION = { + "YamI2rt": ("i2rt", "fixed_manipulator", 2), "UnitreeGo1": ("unitree", "quadruped", 0), "UnitreeGo2": ("unitree", "quadruped", 0), "UnitreeG1": ("unitree", "bipedal_humanoid", 2), diff --git a/extensions/embodichain_robots/i2rt/__init__.py b/extensions/embodichain_robots/i2rt/__init__.py new file mode 100644 index 0000000000..c36a55a0bf --- /dev/null +++ b/extensions/embodichain_robots/i2rt/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""I2RT robot models and asset definitions.""" + +from __future__ import annotations + +__all__ = [] diff --git a/extensions/embodichain_robots/i2rt/yam/README.md b/extensions/embodichain_robots/i2rt/yam/README.md new file mode 100644 index 0000000000..0aad70fb60 --- /dev/null +++ b/extensions/embodichain_robots/i2rt/yam/README.md @@ -0,0 +1,13 @@ +# YAM dual-arm embodiment + +`YamI2rtCfg` and `YamI2rt` own the converted dual-arm routing embodiment. The +registered ID `YamI2rt` resolves `robot_assets/YamI2rtV1.zip`, MD5 +`23601ea820ff3e6260336c4962ef01a5`. Its URDF and collision meshes are used by +both simulation and kinematics. + +The archive retains the pinned upstream source, conversion provenance and +I2RT Robotics MIT license. `scripts/tools/prepare_yam_i2rt_assets.py` owns the +source conversion; repository `NOTICE` and `licenses/YamI2rt-MIT.txt` preserve +attribution. Model gains and initial pose describe this simulation embodiment, +not verified hardware calibration. Contact-driven routing qualification belongs +to the task, independently of successful asset loading. diff --git a/extensions/embodichain_robots/i2rt/yam/__init__.py b/extensions/embodichain_robots/i2rt/yam/__init__.py new file mode 100644 index 0000000000..bf4a73f682 --- /dev/null +++ b/extensions/embodichain_robots/i2rt/yam/__init__.py @@ -0,0 +1,36 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""YamI2rtCfg and model-owned assets; configuration loads on demand.""" + +from __future__ import annotations + +from importlib import import_module +from typing import Any + +__all__ = ["YamI2rtCfg"] + + +def __getattr__(name: str) -> Any: + if name != "YamI2rtCfg": + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") + value = import_module(".cfg", __name__).YamI2rtCfg + globals()[name] = value + return value + + +def __dir__() -> list[str]: + return sorted(set(globals()) | set(__all__)) diff --git a/extensions/embodichain_robots/i2rt/yam/assets.py b/extensions/embodichain_robots/i2rt/yam/assets.py new file mode 100644 index 0000000000..7e46fc1dca --- /dev/null +++ b/extensions/embodichain_robots/i2rt/yam/assets.py @@ -0,0 +1,66 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Versioned YAM robot assets and pinned source attribution.""" + +from __future__ import annotations + +import open3d as o3d +from embodichain.data.dataset import EmbodiChainDataset +from embodichain.data.constants import ( + EMBODICHAIN_DOWNLOAD_PREFIX, + EMBODICHAIN_DEFAULT_DATA_ROOT, +) + +__all__ = ["YamI2rt"] + +robot_assets = "robot_assets" + + +class YamI2rt(EmbodiChainDataset): + """Download the versioned YAM i2rt dual-arm robot and its collision meshes. + + The archive retains the I2RT Robotics MIT license, source provenance and + attribution notice. It contains the URDF used by both simulation and FK/IK; + ordinary downloads do not require the mesh-conversion dependencies. + + Example: + >>> from embodichain.data import get_data_path + >>> path = get_data_path("YamI2rt/yam_i2rt.urdf") + """ + + archive_md5: str = "23601ea820ff3e6260336c4962ef01a5" + + def __init__(self, data_root: str | None = None) -> None: + """Download and extract the checked archive into the selected data root. + + Args: + data_root: Cache root, or the configured default when omitted. + """ + filename = f"{robot_assets}/YamI2rtV1.zip" + urls = [f"{EMBODICHAIN_DOWNLOAD_PREFIX}{filename}"] + hub_url = ( + "https://huggingface.co/datasets/DexForceAI/embodichain_data/" + f"resolve/main/{filename}" + ) + if hub_url not in urls: + urls.append(hub_url) + descriptor = o3d.data.DataDescriptor(urls, self.archive_md5) + super().__init__( + type(self).__name__, + descriptor, + EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root, + ) diff --git a/extensions/embodichain_robots/i2rt/yam/cfg.py b/extensions/embodichain_robots/i2rt/yam/cfg.py new file mode 100644 index 0000000000..cc2e8a41fa --- /dev/null +++ b/extensions/embodichain_robots/i2rt/yam/cfg.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. +# ---------------------------------------------------------------------------- + +"""YAM i2rt dual-arm configuration, using the reference routing embodiment.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any, TYPE_CHECKING +import numpy as np +import torch +from embodichain.data import get_data_path +from embodichain.lab.sim.cfg import ( + RobotCfg, + JointDrivePropertiesCfg, + RigidBodyPhysicsCfg, + URDFCfg, +) +from embodichain.lab.sim.motion.solvers import PytorchSolverCfg +from embodichain.lab.sim.utility.cfg_utils import merge_robot_cfg +from embodichain.utils import configclass + +if TYPE_CHECKING: + import pytorch_kinematics as pk + +__all__ = ["YamI2rtCfg"] + + +@configclass +class YamI2rtCfg(RobotCfg): + """Two six-joint YAM arms with two independently driven fingers per hand. + + Assets retain their MIT license and pinned source provenance. TCP links use + the reference grasp-site frame. Runtime grasp parameters belong to tasks. + """ + + @classmethod + def from_dict(cls, init_dict: dict[str, Any]) -> YamI2rtCfg: + """Construct source defaults and apply explicit robot overrides.""" + cfg = cls() + cfg._build_defaults(init_dict) + return merge_robot_cfg(cfg, init_dict) + + def _build_defaults(self, init_dict: dict | None = None) -> None: + self.uid = "yam" + self.fpath = (init_dict or {}).get("fpath") or self.fpath + urdf_path = self._pk_urdf_path + if self.fpath: + self.fpath = urdf_path + if not Path(urdf_path).is_file(): + raise FileNotFoundError( + f"YAM robot asset not found: {urdf_path}. Run " + "`python -m embodichain data download --name YamI2rt`, " + "or set the robot's fpath to an already prepared yam_i2rt.urdf." + ) + self.urdf_cfg = URDFCfg( + components=[ + { + "component_type": "arm", + "urdf_path": urdf_path, + "transform": np.eye(4), + } + ] + ) + self.init_pos = (0.2525, 0, 0.75) + self.asset_physics_mode = "overlay" + self.control_parts = { + f"{side}_arm": [f"{side}_joint{i}" for i in range(1, 7)] + for side in ("right", "left") + } + self.control_parts.update( + { + f"{side}_eef": [f"{side}_joint7", f"{side}_joint8"] + for side in ("right", "left") + } + ) + self.solver_cfg = { + f"{side}_arm": PytorchSolverCfg( + root_link_name=f"{side}_base", + end_link_name=f"{side}_tcp", + num_samples=12, + # Cartesian retiming measures finite-difference acceleration; + # loose IK convergence produces visible joint jitter on that grid. + max_iterations=200, + pos_eps=1e-5, + rot_eps=1e-5, + ) + for side in ("right", "left") + } + self.init_qpos = [ + 0.0, + 0.0, + 1.4, + 1.4, + 1.2, + 1.2, + 0.0, + 0.0, + 1.1, + 1.1, + 0.0, + 0.0, + 0.0475, + 0.0475, + 0.0475, + 0.0475, + ] + self.joint_drive_props = JointDrivePropertiesCfg( + stiffness={ + "right_joint[1-6]": 300.0, + "left_joint[1-6]": 300.0, + ".*_joint[78]": 600.0, + }, + damping={ + "right_joint[1-6]": 30.0, + "left_joint[1-6]": 30.0, + ".*_joint[78]": 20.0, + }, + max_effort={ + ".*_joint[1-3]": 27.0, + ".*_joint[4-6]": 10.0, + ".*_joint[78]": 80.0, + }, + armature={ + ".*_joint[1-3]": 0.0192, + ".*_joint[4-6]": 0.0012, + ".*_joint[78]": 0.05, + }, + target_mode="position", + ) + self.attrs = RigidBodyPhysicsCfg.from_dict( + {"collision_props": {"contact_offset": 0.0002, "rest_offset": 0.0}} + ) + + @property + def _pk_urdf_path(self) -> str: + if self.fpath: + return str(Path(self.fpath).expanduser()) + if self.urdf_cfg is not None and "arm" in self.urdf_cfg.components: + return str(self.urdf_cfg.components["arm"]["urdf_path"]) + return get_data_path("YamI2rt/yam_i2rt.urdf") + + def build_pk_serial_chain( + self, device: torch.device = torch.device("cpu"), **kwargs: Any + ) -> dict[str, pk.SerialChain]: + """Build local arm chains from the exact simulation URDF. + + Args: + device: Kinematics device. + **kwargs: Reserved configuration hooks. + + Returns: + Six-joint chains keyed by control part. + """ + from embodichain.lab.sim.utility.solver_utils import create_pk_serial_chain + + return { + f"{side}_arm": create_pk_serial_chain( + urdf_path=self._pk_urdf_path, + device=device, + root_link_name=f"{side}_base", + end_link_name=f"{side}_tcp", + ) + for side in ("right", "left") + } diff --git a/extensions/embodichain_tasks/configs/components/embodiments/yam_i2rt.yaml b/extensions/embodichain_tasks/configs/components/embodiments/yam_i2rt.yaml new file mode 100644 index 0000000000..37dcad9475 --- /dev/null +++ b/extensions/embodichain_tasks/configs/components/embodiments/yam_i2rt.yaml @@ -0,0 +1,36 @@ +embodiment_id: yam_i2rt +simulation: + class_type: YamI2rt + uid: yam + # Preserve the source MJCF gravcomp setting for this Newton embodiment. + root_props: + newton_gravity_compensation: 1.0 + joint_drive_props: + stiffness: + right_joint[1-6]: 300.0 + left_joint[1-6]: 2000.0 + .*_joint[78]: 6000.0 + damping: + right_joint[1-6]: 30.0 + left_joint[1-6]: 90.0 + .*_joint[78]: 120.0 + link_attrs: + pads: + link_names_expr: + - (right|left)_tip_(left|right) + attrs: + material_props: + backend: newton + dynamic_friction: 60.0 + ke: 10000.0 + kd: 0.1 + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 +sensor: [] diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md new file mode 100644 index 0000000000..1ba3ec1716 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md @@ -0,0 +1,223 @@ +# Route the Cable (YAM) + +This handwritten expert routes a 600 mm anchored cable around two spools and +places its free end in a 10 mm channel. It adapts the +[robosimstudio example](https://github.com/robosimstudio/robosimstudio/tree/73739abe212f963e8a9f5862b27bca370df091a7/examples/08_demos/route_the_cable). +The task uses EmbodiChain's scene components, motion generator, Atomic Skills, +`DemoSegment` executor and trajectory recorder. Only the proximal cable segment +is fixed; grasps use collision and friction, with no runtime attachment or +in-episode state writes. + +## Requirements and entry point + +Use CUDA and a DexSim build exposing `CableDesc`, `Scene.create_cable_batch`, +`CableBatch.fetch_default_state/apply_state` and `Scene.reset_arenas`. See the +[cable example installation guide](../../../../../../../examples/README.md#newton-cable-demo) +for the development-build dependency and solver migration. Earlier routing +qualification used cable-state commit +`43d078958e6e00158eda0c3024d8af0c5682eb59` with Newton 1.6; those results do not +qualify full routing on a newer engine. A stock wheel lacking the required +cable APIs cannot run this deployment. Permanent cable attachments to articulations +with nonzero initial joint positions are rejected before scene commit: the current +engine resolves their anchors before model-initialization hooks. This task uses +contact-only grasps and does not need that unsupported combination. The file owns Newton/DexUni, dynamic +MuJoCo joints and two-way VBD cable contact; CLI `--physics` does not switch it. + +From the EmbodiChain repository: + +```bash +python -m embodichain data download --name YamI2rt +python -m embodichain list-task --category manipulation/cable +python -m embodichain run-task \ + --gym_config extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml \ + --max_episodes 1 --seed 73 --headless --record_trajectory \ + --trajectory_save_dir ./output/route_the_cable +``` + +`YamI2rtCfg` resolves `get_data_path("YamI2rt/yam_i2rt.urdf")` through the same +asset registry and cache used by Franka. The predownload command above is optional: +constructing the robot downloads and extracts the ready +`robot_assets/YamI2rtV1.zip` bundle when needed. Downloading the bundle does not +require CoACD. `EMBODICHAIN_DOWNLOAD_PREFIX` selects a mirror; the official +`DexForceAI/embodichain_data` Hugging Face dataset is the fallback. + +The bundle was +[published on 2026-11-05](https://huggingface.co/datasets/DexForceAI/embodichain_data/commit/8c0b7dcc8a80ab5a1f177c7384ad9d6950281a68) +and is publicly downloadable without a Hugging Face token. Official HF download +into an empty cache, archive checksums, extraction, cache reuse and robot +configuration/FK-chain loading were verified. This asset validation does not +qualify a complete routing episode. + +For optional reconstruction from the pinned source in a checkout, use: + +```bash +python -m pip install coacd==1.0.14 trimesh==4.12.2 scipy==1.18.1 +python scripts/tools/prepare_yam_i2rt_assets.py +``` + +The preparation script downloads the 9 STL meshes, source MJCF and MIT license +from robosimstudio commit `73739abe212f963e8a9f5862b27bca370df091a7`. It converts +the dual-arm MJCF to URDF and generates 24 left-tip and 15 right-tip collision +hulls with CoACD 1.0.14, threshold 0.06 and seed 73. Every source and generated +file must match the SHA-256 of the qualified robot before the staged directory +is installed. A valid existing bundle is verified and reused; an incomplete or +modified bundle is rejected without being overwritten. This preparation runs +on CPU and does not require the simulation renderer or CUDA. + +The download registry extracts under `/extract/YamI2rt/`; the source +script defaults to `/YamI2rt/`. Both are recognized by `get_data_path`. +The default data root is `~/.cache/embodichain_data/`. Set +`EMBODICHAIN_DATA_ROOT` before both preparation and demo execution to use another +cache root. For offline preparation, pass `--source-dir` pointing to the pinned +upstream `assets/embodiments/yam_i2rt/` directory. `--output` selects an explicit +destination; configure the robot's `fpath` to its `yam_i2rt.urdf` if this differs +from the default cache. The raw meshes, generated URDF and collision hulls are +local assets, excluded from Git and the Python wheel. + +For asset publication, `--archive /path/to/YamI2rtV1.zip` additionally builds a +deterministic archive from the verified bundle and includes its MIT license, +NOTICE and provenance. It retains the qualified geometry and all 52 original +file hashes. Archive MD5: `23601ea820ff3e6260336c4962ef01a5`; SHA-256: +`9125380a6240847f42730b79025aa61cd17b665f90a2586b3c6f061d769c503a`. +The repository's `NOTICE` and `licenses/YamI2rt-MIT.txt` record the external +asset attribution even though the binary assets are distributed separately. + +The collector accepts complete successful episodes only. A failed intermediate +validator stops its episode. A launched expert is not evidence of task success. +This expert deployment sets `visualization.allow_commands: false`: Viser is an +observer, with camera orbit/pan still available. Dragging robot controls while +the expert writes joint targets introduces a competing controller and can +invalidate grasps. The launcher preserves this read-only policy with `--viser`. +Use a separately configured, stepping scene without the expert for manual IK +experiments. Failed motion validators log planning status, TCP position error +and maximum measured grasp slip with their limits. +Feedback insertion moves may hold the same target for up to one extra second +while physical tracking converges. They retain their original position and +slip tolerances and fail when that bounded settling period is exhausted. + +## Stages and reusable boundaries + +The right arm places capsule centres at material distances 0.162, 0.282 and +0.438 m; these are the nearest capsule centres to the upstream nominal grasp +locations. The left arm grasps at 0.570 m, selects a wrist branch that can also +reach the channel staging pose, observes the distal tangent, and corrects yaw +and lateral intersection. It lowers through bounded height stages, observes the +actual free end, and accumulates tip-position corrections on the commanded pose +to compensate contact-load bias. Once the end is inside the channel or its +narrow aligned release corridor, it opens the fingers and lets the cable settle. +The final validator requires the actual end inside the channel after release, +retreat and settling. Each stage is +planned lazily from the latest cable state. Both hands return home before the +final settling period. + +The YAM IK uses 10 μm position and 1e-5 rad rotation convergence so finite- +difference joint acceleration is not dominated by numerical jitter during +Cartesian retiming. Task moves accept the planner's possibly longer control +grid; grasp-slip and physical placement thresholds are unchanged. + +The third carry targets a TCP height of 50 mm above the bench. The former +65 mm target could exhaust the 12 mm slip allowance during `raise_2`, even +with successful IK and acceptable TCP tracking. The shorter lift retains the +same firm grasp and validators. Its CUDA regression checks material-point +clearance, grasp slip, second-spool seating, and robot/spool contact throughout +raising, carrying, lowering, release and retreat. + +At each spool, the right hand releases to a 16 mm jaw aperture, retreats to +0.82 m TCP height, and only then opens fully. Opening through the full finger +stroke beside the first spool sweeps a finger through its rim. These explicit +waypoints are task geometry: joint interpolation does not provide obstacle +avoidance, and contact response is not a substitute for a clear commanded path. + +- `GraspCable`, `CableGraspGoal` and `GraspCableOptions` are registered Atomic + Skills. Planning generates descend/close/lift joint commands on the control + grid and declares no rigid-object attachment effect. +- `compute.geometry.cable` owns simulator-independent capsule endpoints, + material-point observations and TCP-relative slip measurement. +- `RouteTheCableEnv.create_demo_segments()` owns feedback, staged execution and + physical validators. Its `milestone="pinch"` or `"first_spool"` options qualify + intermediate stages through `execute_demo_episode`; they do not certify a + complete routing episode. +- `YamI2rtCfg` resolves the versioned robot bundle from the data registry/cache or an explicit + `fpath`. Its bundle retains the original MIT license and pinned provenance. + Simulation and FK use + the same URDF. Fingertips retain separate convex pieces instead of one hull + spanning their concave grasp region. + +The scene follows the pinned upstream `RodSpec` coefficients, including density, +stretch/shear, bend/twist and contact damping. Shape friction combines by a +geometric mean: cable/pad are 60, bench 0.02, and other scenery 2. The task uses +contact history and sticky matching as the upstream contact-only example does; +these values are task settings, not global physics defaults. The routing +component increases left-arm drive stiffness for insertion tracking. The grasp +profile uses the YAM finger-width calibration and cable radius to request a +small interference rather than driving an empty-hand closure against the cable. +The first carry, high third carry and free-end insertion use firm per-invocation +grasp commands through the existing `ActionControlOverrides` interface. The +first grasp needs this holding margin: its light default pinch can reach the +12 mm slip rejection limit during carry/lowering, even without browser input. +Each pinch holds the fully closed command for 0.35 s on the control grid before +lifting. This dwell is part of the Atomic Skill's `close` phase; the material +reference is sampled after closing/settling, before any lift, and is not rebased +during the grasp. + +## Validation and recordings + +Every grasp must lift the observed material point by more than 8 mm and remain +within 12 mm of its post-closure TCP-relative position. Final metrics include +both spool-seat counts, free-end location, hand clearance, speed and sampled +cable/bench, cable/spool and cable/port penetration. `reference_routed` +reproduces the upstream broad regions. Final `success` additionally requires +the free end inside the actual +channel and penetration no greater than 1 mm. Sampled SDF diagnostics do not +constitute an exact continuous collision-distance query. + +These final cable predicates do not certify the robot's swept path. The +first-spool CUDA regression samples backend robot/spool contacts throughout +approach, placement, release and retreat, rejects penetration over 1 mm, and +also requires the cable to remain seated and the hand to end open above the +spool: + +```bash +python -m pytest tests/gym/envs/tasks/test_route_the_cable_cuda.py --run-gpu -m gpu -q +``` + +On 2026-11-08 both CUDA regressions passed in separate NoRender processes with +seed 73, DexSim `0.6.0.dev11491` (dev commit +`505ee1b5600ca0b3d0a2c8235930e7684283d049`), Newton 1.7.0, MuJoCo/MuJoCo-Warp +3.15.0, Python 3.12 and an RTX 5090 D v2. Maximum grasp slip was 3.32 mm in +the first-spool test and 2.81 mm during the third carry, with no reported +robot/spool penetration during the third carry and retreat. The original +12 mm slip and 1 mm penetration limits were unchanged. See the repository's +Newton cable example instructions for the internal CI wheel; this build was +not published to the package index. These tests qualify the first-spool and +third-carry stages, not the later channel insertion or complete routing episode. + +The 2026-10-21 CUDA regressions with seed 73 passed for the first-spool +release and the third carry through release and retreat. Maximum measured +grasp slip was 8.72 mm in the first-spool test and 9.60 mm during the third +carry, with no reported robot/spool penetration during the third carry and +retreat. This qualifies the retained 50 mm third-carry change through that +intermediate stage only. Earlier 2026-10-22 development reruns passed both CUDA +regressions, but qualification of that older branch failed both routing regressions at +`carry_0`. A separate fresh-process full collector trial at seed +73 rejected `carry_0` at 12.035 mm slip and 67.38 mm TCP error. Full-episode +reliability remains unqualified, particularly later channel insertion; the +new development-build regressions above do not qualify the complete routing demo. +Success rates across seeds and reset-based batches remain unqualified. + +Recordings contain full robot commands and +`states.cables.cable.segment_state_w`, shaped `(environments, time, 50, 13)` in +world XYZ + XYZW + COM linear/angular velocity order. Segment outcomes retain +planning status and material-coordinate identities. A cable observation is not +a solver-history checkpoint. Construct the replay environment with the same +configuration and Arena origins. `ReplayWrapper(mode="dynamic")` verifies +matching full-reset recorded observations and environment counts; it replays +commands without overwriting cable states. Both `position` and +`position_velocity` expert command modes use the environment control grid; the +latter records derived joint velocities and zero velocities during holds. Cable kinematic scrubbing +and partial replay resets are rejected. The deployment defaults to one episode and +one attempt per process. Reset/episode repeatability with sticky +contact needs separate physical qualification; API state/reset tests alone do +not prove repeatable frictional routing. Use a fresh process for each trial until +that reset path is qualified. Raw trajectory recordings may include failed +attempts; select training data using the segment and final outcomes. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/catalog.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/catalog.yaml new file mode 100644 index 0000000000..03fef482bf --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/catalog.yaml @@ -0,0 +1,8 @@ +task_key: route_the_cable +title: Route the Cable +summary: Frictional, shape-feedback cable routing around two spools and into a port. +tags: [cable, deformable, manipulation, handwritten_expert] +default_deployment: yam +deployments: + yam: + config: task.yam.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml new file mode 100644 index 0000000000..4fe3737c30 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml @@ -0,0 +1,428 @@ +# Material coefficients follow the pinned upstream RodSpec and its scene-friction pass. +environment_id: route_the_cable +physics: newton +physics_config: + physics_dt: 0.004166666666666667 + num_substeps: 4 + collision_cfg: null + solver_cfg: + solver_type: dexuni + joint_mode: dynamic + coupling: two_way + contact_mode: full + vbd_options: + iterations: 20 + rigid_contact_history: true + rigid_body_contact_buffer_size: 256 + collision_options: + contact_matching: sticky +max_episode_steps: 6000 +num_envs: 1 +arena_space: 2.0 +simulation: + background: + - uid: bench + shape: + shape_type: Cube + size: + - 0.595 + - 1.3 + - 0.1 + init_pos: + - 0.6 + - 0 + - 0.7 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 0.02 + ke: 10000.0 + kd: 0.1 + - uid: spool_0_foot + shape: + shape_type: Cylinder + radius: 0.0125 + height: 0.0025 + init_pos: + - 0.525 + - -0.26 + - 0.75125 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: spool_0_shaft + shape: + shape_type: Cylinder + radius: 0.00475 + height: 0.017179 + init_pos: + - 0.525 + - -0.26 + - 0.7610895 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: spool_0_rim + shape: + shape_type: Cylinder + radius: 0.0125 + height: 0.0038210000000000015 + init_pos: + - 0.525 + - -0.26 + - 0.7715895 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: spool_1_foot + shape: + shape_type: Cylinder + radius: 0.0125 + height: 0.0025 + init_pos: + - 0.525 + - -0.02 + - 0.75125 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: spool_1_shaft + shape: + shape_type: Cylinder + radius: 0.00475 + height: 0.017179 + init_pos: + - 0.525 + - -0.02 + - 0.7610895 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: spool_1_rim + shape: + shape_type: Cylinder + radius: 0.0125 + height: 0.0038210000000000015 + init_pos: + - 0.525 + - -0.02 + - 0.7715895 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: port_wall_a + shape: + shape_type: Cube + size: + - 0.011 + - 0.032 + - 0.0125 + init_pos: + - 0.5755 + - 0.08 + - 0.75625 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: port_wall_b + shape: + shape_type: Cube + size: + - 0.011 + - 0.032 + - 0.0125 + init_pos: + - 0.5965 + - 0.08 + - 0.75625 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: port_back + shape: + shape_type: Cube + size: + - 0.01 + - 0.0064 + - 0.0125 + init_pos: + - 0.586 + - 0.0928 + - 0.75625 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: anchor + shape: + shape_type: Cube + size: + - 0.032 + - 0.032 + - 0.006 + init_pos: + - 0.42 + - -0.352 + - 0.76 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + cable_object: + - uid: cable + points: + - - 0 + - 0.0 + - 0 + - - 0 + - 0.012 + - 0 + - - 0 + - 0.024 + - 0 + - - 0 + - 0.036000000000000004 + - 0 + - - 0 + - 0.048 + - 0 + - - 0 + - 0.06 + - 0 + - - 0 + - 0.07200000000000001 + - 0 + - - 0 + - 0.084 + - 0 + - - 0 + - 0.096 + - 0 + - - 0 + - 0.108 + - 0 + - - 0 + - 0.12 + - 0 + - - 0 + - 0.132 + - 0 + - - 0 + - 0.14400000000000002 + - 0 + - - 0 + - 0.156 + - 0 + - - 0 + - 0.168 + - 0 + - - 0 + - 0.18 + - 0 + - - 0 + - 0.192 + - 0 + - - 0 + - 0.20400000000000001 + - 0 + - - 0 + - 0.216 + - 0 + - - 0 + - 0.228 + - 0 + - - 0 + - 0.24 + - 0 + - - 0 + - 0.252 + - 0 + - - 0 + - 0.264 + - 0 + - - 0 + - 0.276 + - 0 + - - 0 + - 0.28800000000000003 + - 0 + - - 0 + - 0.3 + - 0 + - - 0 + - 0.312 + - 0 + - - 0 + - 0.324 + - 0 + - - 0 + - 0.336 + - 0 + - - 0 + - 0.34800000000000003 + - 0 + - - 0 + - 0.36 + - 0 + - - 0 + - 0.372 + - 0 + - - 0 + - 0.384 + - 0 + - - 0 + - 0.396 + - 0 + - - 0 + - 0.40800000000000003 + - 0 + - - 0 + - 0.42 + - 0 + - - 0 + - 0.432 + - 0 + - - 0 + - 0.444 + - 0 + - - 0 + - 0.456 + - 0 + - - 0 + - 0.468 + - 0 + - - 0 + - 0.48 + - 0 + - - 0 + - 0.492 + - 0 + - - 0 + - 0.504 + - 0 + - - 0 + - 0.516 + - 0 + - - 0 + - 0.528 + - 0 + - - 0 + - 0.54 + - 0 + - - 0 + - 0.552 + - 0 + - - 0 + - 0.5640000000000001 + - 0 + - - 0 + - 0.5760000000000001 + - 0 + - - 0 + - 0.588 + - 0 + - - 0 + - 0.6 + - 0 + radius: 0.0035 + init_pos: + - 0.42 + - -0.36 + - 0.7535 + fixed_segments: + - 0 + attrs: + density: 350.0 + bend_stiffness: 0.00111572809234342 + bend_damping: 0.00167359213851513 + twist_stiffness: 0.00111572809234342 + twist_damping: 0.00167359213851513 + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 60.0 + ke: 10000.0 + kd: 0.1 + stretch_stiffness: 200000.0 + stretch_damping: 1.0 + shear_stiffness: 200000.0 + shear_damping: 1.0 +env: + sim_steps_per_control: 4 + ignore_terminations: true + task_instruction: Route the anchored cable around both spools and place its free + end in the port. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml new file mode 100644 index 0000000000..1a135472eb --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml @@ -0,0 +1,14 @@ +id: RouteTheCable-YAM-v1 +environment: + component: env.yaml +embodiment: + component: ../../../../components/embodiments/yam_i2rt.yaml +headless: true +render_cfg: + renderer: no-render +enable_entity_gizmo: false +visualization: + allow_commands: false +collection: + target_episodes: 1 + max_attempts: 1 diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/__init__.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/__init__.py new file mode 100644 index 0000000000..3818cfd8e4 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/__init__.py @@ -0,0 +1,19 @@ +# ---------------------------------------------------------------------------- +# 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 + +__all__ = [] diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/route_the_cable.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/route_the_cable.py new file mode 100644 index 0000000000..e2fe04e5fb --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/route_the_cable.py @@ -0,0 +1,885 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Shape-feedback expert for the anchored, two-spool cable routing board. + +The YAM deployment resolves ``YamI2rt/yam_i2rt.urdf`` through the standard data +registry, downloading and caching the versioned robot archive when needed. +Pre-download with ``python -m embodichain data download --name YamI2rt``. +For source regeneration or an offline checkout, run +``python scripts/tools/prepare_yam_i2rt_assets.py`` from the repository root; +that script retains the original source/model hashes and MIT license. +See the task config directory's README for the asset publication status, +source-conversion dependencies and cache overrides. + +Geometry and routine adapted from robosimstudio's Apache-2.0 route_the_cable +example at 73739abe212f963e8a9f5862b27bca370df091a7. Robot assets retain MIT. +https://github.com/robosimstudio/robosimstudio/tree/main/examples/08_demos/route_the_cable +""" + +from __future__ import annotations + +import math +from collections.abc import Callable, Iterable +from typing import Any +import torch +from tensordict import TensorDict +from embodichain.compute.geometry.cable import sample_cable_material, cable_endpoints +from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg, DemoSegment +from embodichain.lab.gym.envs.expert_trajectory import prepare_expert_joint_trajectory +from embodichain.lab.gym.envs.types import ControllerAction +from embodichain.lab.gym.utils.registration import register_env +from embodichain.lab.sim.atomic_actions import ( + AtomicActionEngine, + ActionInvocation, + ActionControlOverrides, + JointPositionCommand, + ControlPartCommandProfile, + MotionPolicy, + SceneSnapshot, +) +from embodichain.lab.sim.atomic_actions.primitives.grasp_cable import ( + CableGraspGoal, + GraspCableOptions, +) +from embodichain.lab.sim.atomic_actions.trajectory_ops import build_pose_plan_states +from embodichain.lab.sim.motion.motion_generator import ( + MotionGenerator, + MotionGenCfg, + MotionGenOptions, +) +from embodichain.lab.sim.motion.planners import PlanResult, TrapezoidalPlannerCfg +from embodichain.lab.sim.motion.planners.utils import interpolate_xpos_batched +from embodichain.utils import logger + +__all__ = ["RouteTheCableEnv", "measure_route"] + + +def measure_route( + state: torch.Tensor, + lengths: torch.Tensor, + hands_w: torch.Tensor, + origins: torch.Tensor, +) -> dict[str, torch.Tensor]: + """Measure the reference board goal and sampled geometry diagnostics. + + Args: + state: World cable segment states ``(B,50,13)``. + lengths: Capsule axis rest lengths ``(50,)``. + hands_w: World tool centres ``(B,2,3)``. + origins: World Arena translations ``(B,3)``. + + Returns: + Per-environment counts, speed, clearance and routing acceptance. + """ + p = state[..., :3] - origins[:, None, :] + starts, ends = cable_endpoints(state, lengths) + start_local = starts - origins[:, None, :] + end_local = ends - origins[:, None, :] + valid = torch.isfinite(state).all(dim=(1, 2)) + out = {} + for i, (y, side) in enumerate(((-0.26, -1), (-0.02, 1))): + lo, hi = sorted((0.525 + side * 0.003, 0.525 + side * 0.045)) + inside = ( + (p[:, :, 0] >= lo) + & (p[:, :, 0] <= hi) + & ((p[:, :, 1] - y).abs() <= 0.035) + & (p[:, :, 2] >= 0.75) + & (p[:, :, 2] <= 0.90) + ) + out[f"seat_{i}"] = inside.sum(-1) + in_port = ( + ((p[:, :, 0] - 0.586).abs() <= 0.045) + & ((p[:, :, 1] - 0.08).abs() <= 0.045) + & (p[:, :, 2] >= 0.75) + & (p[:, :, 2] <= 0.90) + ) + end = end_local[:, -1] + out["end_position"] = end + out["port_count"] = in_port.sum(-1) + out["end_in_port"] = ( + ((end[:, :2] - end.new_tensor([0.586, 0.08])).abs() <= 0.045).all(-1) + & (end[:, 2] >= 0.75) + & (end[:, 2] <= 0.90) + ) + out["hands_clearance"] = torch.cdist(hands_w, state[..., :3]).amin(dim=(1, 2)) + out["max_speed"] = torch.linalg.vector_norm(state[..., 7:10], dim=-1).amax(-1) + out["bench_penetration"] = ( + 0.75 + 0.0035 - torch.minimum(start_local[..., 2], end_local[..., 2]).amin(-1) + ).clamp_min(0) + # Five points per capsule axis, using the exact capped-cylinder SDF. + axis_samples = ( + start_local[:, :, None, :] + + torch.linspace(0, 1, 5, device=state.device)[None, None, :, None] + * (end_local - start_local)[:, :, None, :] + ) + penetration = torch.zeros(state.shape[0], device=state.device) + for y in (-0.26, -0.02): + for radius, z0, z1 in ( + (0.0125, 0.0, 0.0025), + (0.00475, 0.0025, 0.019679), + (0.0125, 0.019679, 0.0235), + ): + radial = ( + torch.linalg.vector_norm( + axis_samples[..., :2] - state.new_tensor([0.525, y]), dim=-1 + ) + - radius + ) + vertical = (axis_samples[..., 2] - (0.75 + (z0 + z1) / 2)).abs() - ( + z1 - z0 + ) / 2 + q = torch.stack((radial, vertical), -1) + sdf = torch.linalg.vector_norm(q.clamp_min(0), dim=-1) + q.amax( + -1 + ).clamp_max(0) + penetration = torch.maximum( + penetration, (0.0035 - sdf).clamp_min(0).amax(dim=(1, 2)) + ) + out["spool_penetration"] = penetration + port_penetration = torch.zeros_like(penetration) + for center, half_size in ( + ((0.5755, 0.08, 0.75625), (0.0055, 0.016, 0.00625)), + ((0.5965, 0.08, 0.75625), (0.0055, 0.016, 0.00625)), + ((0.586, 0.0928, 0.75625), (0.005, 0.0032, 0.00625)), + ): + q = (axis_samples - state.new_tensor(center)).abs() - state.new_tensor( + half_size + ) + sdf = torch.linalg.vector_norm(q.clamp_min(0), dim=-1) + q.amax(-1).clamp_max(0) + port_penetration = torch.maximum( + port_penetration, (0.0035 - sdf).clamp_min(0).amax(dim=(1, 2)) + ) + out["port_penetration"] = port_penetration + out["strict_port"] = ( + ((end[:, 0] - 0.586).abs() <= 0.0015) + & (end[:, 1] >= 0.064) + & (end[:, 1] <= 0.0896) + & (end[:, 2] >= 0.753) + & (end[:, 2] <= 0.759) + ) + out["release_ready"] = valid & ( + out["strict_port"] + | ( + ((end[:, 0] - 0.586).abs() <= 0.001) + & (end[:, 1] >= 0.073) + & (end[:, 1] <= 0.085) + & (end[:, 2] >= 0.753) + & (end[:, 2] <= 0.766) + ) + ) + out["reference_routed"] = ( + valid + & (out["seat_0"] >= 2) + & (out["seat_1"] >= 2) + & (out["port_count"] >= 3) + & out["end_in_port"] + & (out["hands_clearance"] > 0.05) + & (out["max_speed"] <= 0.05) + ) + return out + + +@register_env("RouteTheCable-YAM-v1", max_episode_steps=6000) +class RouteTheCableEnv(EmbodiedEnv): + """Route with physical pinches and freshly observed material geometry. + + Only the board's proximal end is fixed. No runtime latch, body teleport or + physics-state clear is used inside the expert. Every action uses env.step. + """ + + def __init__(self, cfg: EmbodiedEnvCfg | None = None, **kwargs: Any) -> None: + if cfg is None: + raise ValueError("RouteTheCable requires a configured 50-segment cable.") + cables = [obj for obj in cfg.cable_object if obj.uid == "cable"] + if len(cables) != 1: + raise ValueError("RouteTheCable requires exactly one cable named 'cable'.") + cables[0].validate() + points = torch.as_tensor(cables[0].points) + if tuple(points.shape) != (51, 3): + raise ValueError("RouteTheCable requires the 50-segment reference cable.") + self._grasp = None + self._command = None + self._max_slip = None + self._episode_slip = None + super().__init__(cfg, **kwargs) + try: + self._initialize_routing() + except Exception: + try: + self.close(exit_process=False) + except Exception as cleanup_error: + logger.log_warning( + f"Failed to close routing environment after initialization error: {cleanup_error!r}" + ) + raise + + def _initialize_routing(self) -> None: + """Bind task services only after the shared environment is ready.""" + self.cable = self.sim.get_cable_object("cable") + if self.cable is None or self.cable.data.num_segments != 50: + raise ValueError("RouteTheCable requires the 50-segment reference cable.") + self._lengths = torch.linalg.vector_norm( + torch.as_tensor( + self.cable.cfg.points, device=self.device, dtype=torch.float32 + ).diff(dim=0), + dim=-1, + ) + self._origins = torch.as_tensor( + self.sim.spawn_result.arenas.root_offsets, + device=self.device, + dtype=torch.float32, + )[list(self.cable.data.batch.arena_indices)] + self._motion = MotionGenerator( + MotionGenCfg(planner_cfg=TrapezoidalPlannerCfg(robot_uid=self.robot.uid)) + ) + self._engine = AtomicActionEngine( + self._motion, + control_profiles={ + f"{side}_eef": ControlPartCommandProfile.joint_positions( + open=torch.full((2,), 0.0475, device=self.device), + # A light distal pinch avoids wedging the tiny cable + # against the fingertips and lifting the compliant arm. + grasp=torch.full( + (2,), + max(float(self.cable.cfg.radius) - 0.0005, 0.0), + device=self.device, + ), + ) + for side in ("right", "left") + }, + ) + self._max_slip = torch.zeros(self.num_envs, device=self.device) + self._episode_slip = torch.zeros_like(self._max_slip) + self._home = self.robot.get_qpos().clone() + self._home_poses = { + side: self._tool(side).clone() for side in ("right", "left") + } + self._command = self._home.clone() + + def _extend_obs(self, obs, **kwargs): + obs = super()._extend_obs(obs, **kwargs) + cable = self.sim.get_cable_object("cable") + if cable is not None and cable.data is not None: + obs["cable_state_w"] = cable.data.segment_state_w + return obs + + def _initialize_episode(self, env_ids, **kwargs) -> None: + super()._initialize_episode(env_ids, **kwargs) + self._grasp = None + if self._max_slip is not None: + self._max_slip[env_ids] = 0 + self._episode_slip[env_ids] = 0 + self._command = self.robot.get_qpos(target=True).clone() + + def _tool(self, side: str, *, world: bool = False) -> torch.Tensor: + ids = self.robot.get_joint_ids(name=side + "_arm") + pose = self.robot.compute_fk( + self.robot.get_qpos()[:, ids], name=side + "_arm", to_matrix=True + ) + if world: + pose = pose.clone() + pose[:, :3, 3] += self._origins + return pose + + def _commanded_tool(self, side: str) -> torch.Tensor: + ids = self.robot.get_joint_ids(name=side + "_arm") + return self.robot.compute_fk( + self._command[:, ids], name=side + "_arm", to_matrix=True + ).clone() + + def _point(self, distance: float): + return sample_cable_material( + self.cable.data.segment_state_w, self._lengths, distance + ) + + def _point_in_tool(self, side: str, distance: float) -> torch.Tensor: + point = self._point(distance)[0] + tool = self._tool(side, world=True) + return torch.bmm( + tool[:, :3, :3].transpose(1, 2), (point - tool[:, :3, 3]).unsqueeze(-1) + ).squeeze(-1) + + def _update_sim_state(self, **kwargs) -> None: + super()._update_sim_state(**kwargs) + if self._grasp is not None: + side, distance, initial = self._grasp + slip = torch.linalg.vector_norm( + self._point_in_tool(side, distance) - initial, dim=-1 + ) + self._max_slip = torch.maximum(self._max_slip, slip) + self._episode_slip = torch.maximum(self._episode_slip, slip) + + def route_metrics(self) -> dict[str, torch.Tensor]: + """Return measured routing and expert-quality metrics. + + Returns: + Per-environment reference goal, strict channel, penetration and + grasp-slip measurements, including the combined success flag. + """ + hands = torch.stack( + [self._tool(s, world=True)[:, :3, 3] for s in ("right", "left")], 1 + ) + out = measure_route( + self.cable.data.segment_state_w, self._lengths, hands, self._origins + ) + out["max_grasp_slip"] = self._episode_slip.clone() + out["success"] = ( + out["reference_routed"] + & out["strict_port"] + & (out["bench_penetration"] <= 0.001) + & (out["spool_penetration"] <= 0.001) + & (out["port_penetration"] <= 0.001) + & (out["max_grasp_slip"] <= 0.012) + ) + return out + + def is_task_success(self) -> torch.Tensor: + """Return the physical routing goal combined with episode grasp quality. + + Returns: + Boolean success flags for each environment. + """ + if not hasattr(self, "_origins"): + return torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) + return self.route_metrics()["success"] + + def _validate_route(self) -> torch.Tensor: + metrics = self.route_metrics() + success = metrics["success"] + if not bool(success.all()): + values = {name: value.cpu().tolist() for name, value in metrics.items()} + logger.log_warning(f"Cable final route rejected: {values}.") + return success + + def _target( + self, point: torch.Tensor, yaw: torch.Tensor, height: float + ) -> torch.Tensor: + pose = torch.eye(4, device=self.device).repeat(self.num_envs, 1, 1) + c, s = torch.cos(yaw), torch.sin(yaw) + pose[:, 0, 0] = c + pose[:, 0, 1] = s + pose[:, 1, 0] = s + pose[:, 1, 1] = -c + pose[:, 2, 2] = -1 + pose[:, :3, 3] = point - self._origins + pose[:, 2, 3] = 0.75 + height + return pose + + def _grasp_pose( + self, distance: float, side: str, height: float, *, port: bool = False + ) -> torch.Tensor: + point, tangent, _ = self._point(distance) + yaw = torch.atan2(tangent[:, 1], tangent[:, 0]) + # Choose the reachable wrist branch without changing cable material ID. + candidates = [] + for branch, angle in enumerate((yaw, yaw + math.pi)): + pose = self._target(point, angle, height) + success, qpos = self.robot.compute_ik( + pose, + joint_seed=self.robot.get_qpos()[ + :, self.robot.get_joint_ids(name=side + "_arm") + ], + name=side + "_arm", + ) + if port: + stage = self._target( + point, + torch.full_like(yaw, math.pi / 2 if branch == 0 else -math.pi / 2), + 0.065, + ) + stage[:, 0, 3] = 0.586 + stage[:, 1, 3] = 0.047 + later, _ = self.robot.compute_ik( + stage, joint_seed=qpos, name=side + "_arm" + ) + success = success.reshape(-1).bool() & later.reshape(-1).bool() + candidates.append((success.reshape(-1).bool(), qpos, pose)) + first, second = candidates + distance0 = ( + ( + first[1] + - self.robot.get_qpos()[:, self.robot.get_joint_ids(name=side + "_arm")] + ) + .square() + .sum(-1) + ) + distance1 = ( + ( + second[1] + - self.robot.get_qpos()[:, self.robot.get_joint_ids(name=side + "_arm")] + ) + .square() + .sum(-1) + ) + choose = second[0] & (~first[0] | (distance1 < distance0)) + if port: + self._port_yaw = torch.where( + choose, + torch.full_like(yaw, -math.pi / 2), + torch.full_like(yaw, math.pi / 2), + ) + return torch.where(choose[:, None, None], second[2], first[2]) + + def _execute( + self, + trajectory: torch.Tensor, + joint_ids: list[int], + *, + closed_at: int | None = None, + grasp_ref=None, + stop_when: Callable[[], torch.Tensor] | None = None, + ) -> Iterable[ControllerAction]: + mode = self.cfg.expert_trajectory.joint_command_mode + velocities = None + if mode == "position_velocity": + # Include the current command as t=0 so velocity estimation spans + # the first executed interval on the fixed control grid. + positions = torch.cat((self._command[:, None, :], trajectory), dim=1) + intervals = torch.full( + positions.shape[:2], + self.step_dt, + dtype=torch.float64, + device=positions.device, + ) + intervals[:, 0] = 0 + prepared = prepare_expert_joint_trajectory( + PlanResult(success=True, positions=positions, dt=intervals), + control_dt=self.step_dt, + joint_command_mode=mode, + ) + trajectory = prepared.positions[:, 1:] + velocities = prepared.velocities[:, 1:] + for index, frame in enumerate(trajectory.unbind(1), start=1): + if stop_when is not None and bool(stop_when().all()): + return + if self._grasp is not None and bool((self._max_slip > 0.012).any()): + return + self._command[:, joint_ids] = frame[:, joint_ids] + if mode == "position_velocity": + velocity = torch.zeros_like(self._command) + velocity[:, joint_ids] = velocities[:, index - 1, joint_ids] + yield ControllerAction( + TensorDict( + {"qpos": self._command, "qvel": velocity}, + batch_size=[self.num_envs], + ) + ) + else: + yield ControllerAction(self._command) + if closed_at == index: + side, distance = grasp_ref + self._max_slip.zero_() + self._grasp = ( + side, + distance, + self._point_in_tool(side, distance).clone(), + ) + + def _move( + self, + side: str, + target: torch.Tensor, + name: str, + *, + speed: float = 0.05, + tolerance: float = 0.02, + goal_reached: Callable[[], torch.Tensor] | None = None, + ) -> DemoSegment: + start = self._tool(side) + distance = ( + torch.linalg.vector_norm(start[:, :3, 3] - target[:, :3, 3], dim=-1) + .max() + .item() + ) + angle = ( + torch.acos( + ( + ( + (start[:, :3, :3].transpose(1, 2) @ target[:, :3, :3]) + .diagonal(dim1=-2, dim2=-1) + .sum(-1) + - 1 + ) + / 2 + ).clamp(-1, 1) + ) + .max() + .item() + ) + count = max(3, math.ceil(max(distance / speed, angle / 0.5) / self.step_dt) + 1) + ids = list(self.robot.get_joint_ids(name=side + "_arm")) + poses = interpolate_xpos_batched(start, target, count)[:, 1:] + result = self._motion.generate( + build_pose_plan_states(poses), + options=MotionGenOptions( + strategy="ik_interp", + start_qpos=self.robot.get_qpos()[:, ids], + control_part=side + "_arm", + sample_count=count, + interpolation_dt=self.step_dt, + is_linear=True, + preserve_cartesian_samples=True, + ), + ) + # The planner may lengthen the control grid to meet joint limits. + planned_count = count if result.positions is None else result.positions.shape[1] + full = self._command[:, None, :].repeat(1, planned_count, 1) + planned = ( + torch.as_tensor(result.success, device=self.device) + .reshape(-1) + .bool() + .clone() + ) + if result.positions is None: + planned.zero_() + else: + full[:, :, ids] = result.positions + + def actions(): + if bool(planned.all()): + yield from self._execute(full, ids, stop_when=goal_reached) + yield from self._hold(0.25, stop_when=goal_reached) + if goal_reached is not None: + # Contact can delay tracking after an insertion correction. + # Allow bounded physical settling, with the same target + # and acceptance limits, before declaring a failed move. + def settled(): + return goal_reached() | ( + torch.linalg.vector_norm( + self._tool(side)[:, :3, 3] - target[:, :3, 3], + dim=-1, + ) + <= tolerance + ) + + yield from self._hold(1.0, stop_when=settled) + + def validate(): + position_error = torch.linalg.vector_norm( + self._tool(side)[:, :3, 3] - target[:, :3, 3], dim=-1 + ) + reached = position_error <= tolerance + if goal_reached is not None: + reached = reached | goal_reached() + accepted = planned & reached & (self._max_slip <= 0.012) + if not bool(accepted.all()): + logger.log_warning( + f"Cable motion segment {name!r} rejected: " + f"planning_success={planned.cpu().tolist()}, " + f"position_error_m={position_error.cpu().tolist()} " + f"(tolerance={tolerance}), " + f"max_grasp_slip_m={self._max_slip.cpu().tolist()} " + "(limit=0.012)." + ) + return accepted + + return DemoSegment( + actions=actions(), + name=name, + target_uid="cable", + metadata={"side": side, "planning_success": planned.cpu().tolist()}, + validator=validate, + ) + + def _hold( + self, seconds: float, *, stop_when: Callable[[], torch.Tensor] | None = None + ) -> Iterable[ControllerAction]: + for _ in range(max(1, math.ceil(seconds / self.step_dt))): + if stop_when is not None and bool(stop_when().all()): + return + if self._grasp is not None and bool((self._max_slip > 0.012).any()): + return + if self.cfg.expert_trajectory.joint_command_mode == "position_velocity": + yield ControllerAction( + TensorDict( + { + "qpos": self._command, + "qvel": torch.zeros_like(self._command), + }, + batch_size=[self.num_envs], + ) + ) + else: + yield ControllerAction(self._command) + + def _pinch( + self, side: str, distance: float, *, port: bool = False, firm: bool = False + ) -> Iterable[DemoSegment]: + yield self._move( + side, + self._grasp_pose(distance, side, 0.06, port=port), + f"{side}_approach_{distance}", + speed=0.12, + ) + before = self._point(distance)[0].clone() + goal = CableGraspGoal( + "cable", distance, self._grasp_pose(distance, side, 0.0115, port=port) + ) + binding = self._engine.bind_control_parts( + "grasp_cable", + {"primary": {"motion": side + "_arm", "grasp": side + "_eef"}}, + ) + plan = self._engine.plan( + ActionInvocation( + skill_id="grasp_cable", + goal=goal, + binding=binding, + motion_policy=MotionPolicy(sample_count=73), + skill_options=GraspCableOptions(close_hold_duration=0.35), + control_overrides=( + ActionControlOverrides( + endpoints={ + "primary": { + "grasp": { + "grasp": JointPositionCommand( + torch.zeros(2, device=self.device) + ) + } + } + } + ) + if firm + else ActionControlOverrides() + ), + ), + self._engine.initial_context( + scene=SceneSnapshot(timestamp=0.0, version=0, entities={}), + control_dt=self.step_dt, + ), + ) + trajectory = plan.joint_trajectory.positions + closed_at = next( + segment.stop for segment in plan.segments if segment.name == "close" + ) + ids = list(self.robot.get_joint_ids(name=side + "_arm")) + list( + self.robot.get_joint_ids(name=side + "_eef") + ) + yield DemoSegment( + actions=( + self._execute( + trajectory, ids, closed_at=closed_at, grasp_ref=(side, distance) + ) + if bool(plan.plan_success.all()) + else () + ), + name=f"{side}_pinch_lift_{distance}", + target_uid="cable", + metadata={ + "material_distance": distance, + "firm_grasp": firm, + "planning_success": plan.plan_success.cpu().tolist(), + "phase_lengths": { + segment.name: segment.stop - segment.start + for segment in plan.segments + }, + }, + validator=lambda: plan.plan_success + & (self._point(distance)[0][:, 2] - before[:, 2] > 0.008) + & (self._max_slip <= 0.012), + ) + + def _open( + self, side: str, name: str, *, finger_position: float = 0.0475 + ) -> DemoSegment: + self._grasp = None + self._max_slip.zero_() + ids = list(self.robot.get_joint_ids(name=side + "_eef")) + start = self._command.clone() + count = 36 + trajectory = start[:, None, :].repeat(1, count, 1) + trajectory[:, :, ids] = torch.lerp( + start[:, None, ids], + torch.full_like(start[:, None, ids], finger_position), + torch.linspace(0, 1, count, device=self.device)[None, :, None], + ) + return DemoSegment( + actions=self._execute(trajectory, ids), + name=name, + target_uid="cable", + validator=lambda: torch.isfinite(self.cable.data.segment_state_w).all( + dim=(1, 2) + ), + ) + + def create_demo_segments( + self, *, milestone: str = "full", **kwargs: Any + ) -> Iterable[DemoSegment]: + """Yield freshly planned stages; milestone=pinch qualifies only grasping. + + Args: + milestone: ``pinch``, ``first_spool`` or ``full``. + **kwargs: Reserved collector options. + + Yields: + Bounded action segments with measured row-wise validators. + """ + if milestone not in {"pinch", "first_spool", "full"}: + raise ValueError("Unknown routing milestone.") + self._command = self.robot.get_qpos(target=True).clone() + for stage, (distance, x, y, height) in enumerate( + ( + (0.162, 0.500, -0.255, 0.03), + (0.282, 0.528, -0.140, 0.03), + # Clear the second rim without the extra 15 mm lift that + # exhausted the physical pinch's 12 mm slip budget. + (0.438, 0.565, 0.008, 0.05), + ) + ): + yield from self._pinch("right", distance, firm=stage in {0, 2}) + if milestone == "pinch": + return + target = self._tool("right").clone() + target[:, 2, 3] = 0.75 + height + yield self._move("right", target, f"raise_{stage}") + target = target.clone() + target[:, 0, 3] = x + target[:, 1, 3] = y + yield self._move("right", target, f"carry_{stage}") + target = target.clone() + target[:, 2, 3] = 0.7645 + yield self._move("right", target, f"lower_{stage}") + # Opening fully beside the spool sweeps a finger through its rim. + # First clear the 7 mm cable with a 16 mm jaw aperture, then lift + # above the rim before opening through the full finger stroke. + yield self._open("right", f"release_{stage}", finger_position=0.008) + target = target.clone() + target[:, 2, 3] = 0.82 + yield self._move("right", target, f"retreat_{stage}", speed=0.08) + yield self._open("right", f"open_clear_of_spool_{stage}") + if milestone == "first_spool": + return + yield self._move("right", self._home_poses["right"], "home_right", speed=0.15) + yield from self._place_free_end() + + def _place_free_end(self) -> Iterable[DemoSegment]: + """Observe, align and release the distal span after routing both spools.""" + yield from self._pinch("left", 0.570, port=True, firm=True) + # Stage directly above the mouth. Feeding forward at the old lower + # staging height folded the free span before it reached the channel. + target = self._target(self._point(0.570)[0], self._port_yaw, 0.065) + target[:, :3, 3] = target.new_tensor([0.586, 0.047, 0.815]) + yield self._move("left", target, "stage_free_end") + for correction in range(2): + state = self.cable.data.segment_state_w + point = self._point(0.570)[0] + end = cable_endpoints(state, self._lengths)[1][:, -1] + direction = end - point + angle = torch.atan2(direction[:, 0], direction[:, 1]) + target = self._tool("left").clone() + c, s = torch.cos(angle), torch.sin(angle) + rotation = torch.eye(3, device=self.device).repeat(self.num_envs, 1, 1) + rotation[:, 0, 0] = c + rotation[:, 0, 1] = -s + rotation[:, 1, 0] = s + rotation[:, 1, 1] = c + target[:, :3, :3] = rotation @ target[:, :3, :3] + yield self._move("left", target, f"align_yaw_{correction}") + point = self._point(0.570)[0] - self._origins + end = ( + cable_endpoints(self.cable.data.segment_state_w, self._lengths)[1][ + :, -1 + ] + - self._origins + ) + direction = end - point + if not bool((direction[:, 1] > 1e-6).all()): + yield DemoSegment( + actions=(), + name="reject_backward_free_end", + target_uid="cable", + validator=lambda: torch.zeros( + self.num_envs, dtype=torch.bool, device=self.device + ), + ) + return + meets = point[:, 0] + direction[:, 0] / direction[:, 1].clamp_min(1e-6) * ( + 0.064 - point[:, 1] + ) + target = self._tool("left").clone() + target[:, 0, 3] += 0.586 - meets + yield self._move("left", target, f"align_lateral_{correction}", speed=0.03) + # Correct the actual tip between bounded lowering stages. Accumulate + # corrections on the previous command so contact-load bias is retained. + for level, height in enumerate((0.790, 0.780, 0.772, 0.7645)): + if bool(self._end_release_ready().all()): + break + target = self._commanded_tool("left") + target[:, 2, 3] = height + yield self._move( + "left", + target, + f"lower_channel_{level}", + speed=0.025, + tolerance=0.006, + goal_reached=self._end_release_ready, + ) + for correction in range(2): + if bool(self._end_release_ready().all()): + break + end = ( + cable_endpoints(self.cable.data.segment_state_w, self._lengths)[1][ + :, -1 + ] + - self._origins + ) + target = self._commanded_tool("left") + target[:, :2, 3] += (end.new_tensor([0.586, 0.077]) - end[:, :2]).clamp( + -0.005, 0.005 + ) + target[:, 1, 3].clamp_(max=0.047) + if level == 3: + target[:, 2, 3] += (0.754 - end[:, 2]).clamp(-0.003, 0.003) + target[:, 2, 3].clamp_(min=0.762) + yield self._move( + "left", + target, + f"align_tip_{level}_{correction}", + speed=0.02, + tolerance=0.006, + goal_reached=self._end_release_ready, + ) + + def seated(): + return self._end_release_ready() & (self._max_slip <= 0.012) + + yield DemoSegment( + actions=self._hold(self.step_dt), + name="verify_release_alignment", + target_uid="cable", + validator=seated, + ) + yield self._open("left", "release_free_end") + target = target.clone() + target[:, 1, 3] = 0.030 + target[:, 2, 3] = 0.82 + yield self._move("left", target, "retreat_free_end", speed=0.08) + yield self._move("left", self._home_poses["left"], "home_left", speed=0.15) + yield DemoSegment( + actions=self._hold(2.0), + name="settle_and_validate", + target_uid="cable", + validator=self._validate_route, + ) + + def _end_release_ready(self) -> torch.Tensor: + metrics = self.route_metrics() + return ( + metrics["release_ready"] + & (metrics["port_penetration"] <= 0.001) + & (metrics["bench_penetration"] <= 0.001) + & (metrics["spool_penetration"] <= 0.001) + ) diff --git a/licenses/YamI2rt-MIT.txt b/licenses/YamI2rt-MIT.txt new file mode 100644 index 0000000000..89d5ec3104 --- /dev/null +++ b/licenses/YamI2rt-MIT.txt @@ -0,0 +1,9 @@ +MIT License + +Copyright (c) I2RT Robotics + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/pyproject.toml b/pyproject.toml index 64d0fa8f43..9706d5621d 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -128,6 +128,7 @@ version = { file = ["VERSION"] } [tool.setuptools] include-package-data = false +license-files = ["LICEN[CS]E*", "COPYING*", "NOTICE*", "AUTHORS*", "licenses/*.txt"] [tool.black] diff --git a/scripts/tools/prepare_yam_i2rt_assets.py b/scripts/tools/prepare_yam_i2rt_assets.py new file mode 100644 index 0000000000..911c04f8db --- /dev/null +++ b/scripts/tools/prepare_yam_i2rt_assets.py @@ -0,0 +1,461 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Download pinned YAM assets and reproduce the cable demo's derived robot. + +Run with Python dependencies coacd==1.0.14, trimesh, numpy and scipy. Original +MIT assets are fetched from a fixed robosimstudio commit. Generated files are +verified against the robot used to qualify RouteTheCable before installation. +""" + +from __future__ import annotations + +import argparse +import hashlib +import importlib.metadata +import json +import os +from pathlib import Path +import shutil +import tempfile +import urllib.request +import xml.etree.ElementTree as ET +import zipfile + +__all__ = ["main"] + +_REVISION = "73739abe212f963e8a9f5862b27bca370df091a7" +_REPOSITORY = "https://github.com/robosimstudio/robosimstudio" +_SOURCE_PATH = "assets/embodiments/yam_i2rt" +_BASE_URL = f"https://raw.githubusercontent.com/robosimstudio/robosimstudio/{_REVISION}/{_SOURCE_PATH}" +_COACD_VERSION = "1.0.14" +_THRESHOLD = 0.06 +_SEED = 73 +_ASSET_NOTICE = """YAM i2rt robot assets +Copyright (c) I2RT Robotics + +The YAM arm and linear_4310 gripper models are distributed under the MIT +license; retain the accompanying LICENSE when copying or distributing them. +Upstream: https://github.com/i2rt-robotics/i2rt + +The source assembly is from RoboSimStudio, commit +73739abe212f963e8a9f5862b27bca370df091a7, whose NOTICE identifies these models +as I2RT Robotics assets under MIT. +Source: https://github.com/robosimstudio/robosimstudio + +EmbodiChain converts the MJCF to URDF, adds named grasp TCP frames and +decomposes the fingertip collision meshes. The original model copyright and +MIT license are retained; conversion details are recorded in PROVENANCE.json. +""" +_SOURCE_FILES = ( + "LICENSE", + "yam_i2rt_arms.xml", + "assets/base.stl", + "assets/link1.stl", + "assets/link2.stl", + "assets/link3.stl", + "assets/link4.stl", + "assets/link5.stl", + "assets/gripper.stl", + "assets/tip_left.stl", + "assets/tip_right.stl", +) +_EXPECTED_SHA256 = { + "LICENSE": "7cd0a0224a287ebd6c45703eb6623d264c7810f8958d65de8f99e87ccd9f4891", + "PROVENANCE.json": "1f47f75c9591b9dedc51181eb891d97fcf0f68cd1480ae2e6800d4610ac32929", + "assets/base.stl": "7d57efce4cd31db5e461e1bc83362943a7dbe0e8e101cdb94c29de0a72f3ff0f", + "assets/gripper.stl": "d949c401186197d842ca8f9b23bfcbf24bed8461b421df06093c65e4ead21f5d", + "assets/link1.stl": "2283bab1300cfdd1a04aaf09503093d7d6d19bf2144abf3429d563b596920903", + "assets/link2.stl": "4fd4df2916be71391c6c6ea6ffbdb3c270ce67c71c2bb7d42d44736c0be2af98", + "assets/link3.stl": "5b51658509771514738d0f2271df5f271cd8ae0b6a4bf3787d8463d5ab582593", + "assets/link4.stl": "f6e980b8064a1396b17e87065de50286abf616f94b55e1ccff92e889218de220", + "assets/link5.stl": "b32ece2e963cd028d68290a904223826070be52ce8f0988e3ad36bf75be3582f", + "assets/tip_left.stl": "03d94f53d7b5e9e1871ea6f3925c68d7a2f894b08e0c575fa0b9ed4024684ff7", + "assets/tip_left_collision_0.obj": "be722957fdb8eaa614c3625689373f83bf966d93250c8727acf0709d65a70941", + "assets/tip_left_collision_1.obj": "4873ebf9132048f7461dff3dd3b637ff8594faba717de2f8ee063517c411588d", + "assets/tip_left_collision_10.obj": "03c8c6f9478a1f7e0b0168f28381707f280984243e4b1df0185161f551bfeec4", + "assets/tip_left_collision_11.obj": "b24c2b17a45d1a673d9ff754245ef9b451c0d76394a9f98fb8e4b69292ea3b32", + "assets/tip_left_collision_12.obj": "b6e5dd11b5fd7718b3f5bddbd100b24cb9966b2e4ea5e4f45ba35fa9774238e5", + "assets/tip_left_collision_13.obj": "dc74139cdb72147eda6ea24d665cdf9a072117b69f25ba911ca2d9fe17ae6559", + "assets/tip_left_collision_14.obj": "9a7de3929f7f94a7bff42eaa8ab1c307108495a867a68865c7ed9fe8617aa75b", + "assets/tip_left_collision_15.obj": "02d180b7e86e5c05d405b1e686e1d75f99e971336b873a63494220f0133220fd", + "assets/tip_left_collision_16.obj": "97282f8af188b6734c4bc1735e89be10ee249c3ba486b454df10b79834befb7a", + "assets/tip_left_collision_17.obj": "100eb36dca3b7012f19d3d3e328636a0e3d3683c3aa8e581fcffe9294d7b5d9f", + "assets/tip_left_collision_18.obj": "a7754229194f4db7b7222c57e30a9b26f337b398dfec8ba2b2233de059c68eff", + "assets/tip_left_collision_19.obj": "3595435ab290e6595c08c48c34b9b44a72d61a268141b6e07cc42ee0fd9306a6", + "assets/tip_left_collision_2.obj": "6645911ab4afa24d375c1cde9cf6b086a42330a4049b3f0295c2b0cfd82c795d", + "assets/tip_left_collision_20.obj": "6afee55dda23c4864a73f9774952a8a0a292cc76130290ddaf480fe9ea2f42a0", + "assets/tip_left_collision_21.obj": "6889beacac5136b6a1aae0c284406017918d077faddc737dd76875df65bc200b", + "assets/tip_left_collision_22.obj": "54a42e94d4fc691846934c37026532479013934621759c66d45efbc1e2c199f7", + "assets/tip_left_collision_23.obj": "8f3c8d7eba37b3126426691bc1310a7ad8f1bef4bf95d6211844a1a342c112f9", + "assets/tip_left_collision_3.obj": "2c25f7c192d9c23067057d79c79ac53ef6d122d7067936dc3952b31d3ba371e1", + "assets/tip_left_collision_4.obj": "26dce0f26216956480b9ff956955780a5f06efe430f5824ffad0730380ba692e", + "assets/tip_left_collision_5.obj": "58acf4da81482604641685be978bcb39b6128b93a8f6d303bb42493397c06ba2", + "assets/tip_left_collision_6.obj": "4d5a185ceef7bcafb4316f10918b6191a407a0cc97abcbb152ab8333b09468a3", + "assets/tip_left_collision_7.obj": "3330109b6b8f025dce4716b2754e2d371cbe2da93be3cbacbb7ee7c4b6b0754f", + "assets/tip_left_collision_8.obj": "e3f11a7e79ef7a54aff3b55b079162189ce8a62a43c40bc9e6f44a011f365a78", + "assets/tip_left_collision_9.obj": "79d8d18ebadc20289d32875af9e22f608e173715999e224062a96e931e451823", + "assets/tip_right.stl": "8f620aae9c15e00e2ea4e5bf5e9a49e7edb2d23128b8955cfacc0da8da45c72f", + "assets/tip_right_collision_0.obj": "9f721ff8865f1e2c9f2b99cf815574b133595b7473c98f4e25c85c096d73fda1", + "assets/tip_right_collision_1.obj": "334dc46e059117926cd3f9e42f70d9b3dbf024d4cbe9ca79794fe5e050ae3151", + "assets/tip_right_collision_10.obj": "e7f2f9f228ca4dc0f004c9f7a0877ee57ebcd53d7b9c8a5dd2dd2a11e4769dd0", + "assets/tip_right_collision_11.obj": "fb23393715b9ff7478b9faa298b85c21b177d011eac5677e328c786c162542f6", + "assets/tip_right_collision_12.obj": "5709a388d7de54128ffeeffaa5ee554f42439167b7f30cbc4fcf8695234c046e", + "assets/tip_right_collision_13.obj": "159d95ed91cec4b1d009ddd44306d7fd07e15c10a1a45ba54c577b29ecba4c31", + "assets/tip_right_collision_14.obj": "a231b02f5ea416acd34f55d154202dec76b45a8a50ed3e7cb4f6848ceaeed822", + "assets/tip_right_collision_2.obj": "00c5ff5d494b00e64b3b432164d6be0e9704eae58bf207be67886dd099e607ab", + "assets/tip_right_collision_3.obj": "bfd94c95d04eefe319ab9c41af0b3753886fa587b2530a571c459aad8534db8d", + "assets/tip_right_collision_4.obj": "e841efda880497b5da4717baf57edc14de282213c881a7eb2e179cc014feee34", + "assets/tip_right_collision_5.obj": "8fe376e86226e5fcef11c7df975d44333f51333c8e46db52924f0319dc2f862a", + "assets/tip_right_collision_6.obj": "6ecd7c8e68b21d545fb629eebb5f82f1fa7beb8c8469488f7d9c835e77562cb0", + "assets/tip_right_collision_7.obj": "908eccc7be56d9c066d6386a519e22cc4d594d2f5616b416d179b7bc9e99ada7", + "assets/tip_right_collision_8.obj": "e07df9b12a6aa99da66125a101ec7863f2fb9d4de61095b430719fb6aaabfe67", + "assets/tip_right_collision_9.obj": "8bbaae45405779bd7733b764378c219eb7e1a24794fcc3f5d1a054a19593b3b5", + "yam_i2rt.urdf": "ea4582367a942e6d1d4c5d81cc3a69f8c0aefa55bb924726c87cfca7ff6c239b", + "yam_i2rt_arms.xml": "6146bec6973fb9b450c233b9297c76d0951182aee4883d673573127ea951bf4c", +} + + +def _sha256(path: Path) -> str: + return hashlib.sha256(path.read_bytes()).hexdigest() + + +def _verify_file(path: Path, relative: str) -> None: + if not path.is_file() or path.is_symlink(): + raise ValueError(f"Missing regular asset file: {path}") + actual = _sha256(path) + if actual != _EXPECTED_SHA256[relative]: + raise ValueError( + f"SHA-256 mismatch for {relative}: {actual}; " + f"expected {_EXPECTED_SHA256[relative]}" + ) + + +def _verify_bundle(root: Path) -> None: + for relative in _EXPECTED_SHA256: + _verify_file(root / relative, relative) + for mesh in ET.parse(root / "yam_i2rt.urdf").iter("mesh"): + relative = mesh.attrib["filename"] + if relative not in _EXPECTED_SHA256: + raise ValueError(f"Unverified URDF mesh reference: {relative}") + + +def _fetch_sources(destination: Path, source_dir: Path | None) -> None: + for relative in _SOURCE_FILES: + target = destination / relative + target.parent.mkdir(parents=True, exist_ok=True) + if source_dir is not None: + source = source_dir / relative + _verify_file(source, relative) + shutil.copyfile(source, target) + else: + request = urllib.request.Request( + f"{_BASE_URL}/{relative}", + headers={"User-Agent": "EmbodiChain-YAM-asset-preparation"}, + ) + with urllib.request.urlopen(request, timeout=60) as response: + with target.open("wb") as stream: + shutil.copyfileobj(response, stream) + _verify_file(target, relative) + print(f"Verified source: {relative}", flush=True) + + +def _generate_robot(root: Path) -> None: + import coacd + import numpy as np + from scipy.spatial.transform import Rotation + import trimesh + + version = importlib.metadata.version("coacd") + if version != _COACD_VERSION: + raise ValueError( + f"Install coacd=={_COACD_VERSION} to reproduce the qualified " + f"collision meshes; found {version}." + ) + xml = ET.parse(root / "yam_i2rt_arms.xml").getroot() + urdf = ET.Element("robot", name="yam_i2rt") + ET.SubElement(urdf, "link", name="base_link") + meshes = {mesh.attrib["name"]: mesh.attrib["file"] for mesh in xml.find("asset")} + parts = {} + for name, expected_count in (("tip_left", 24), ("tip_right", 15)): + mesh = trimesh.load_mesh(root / "assets" / meshes[name]) + coacd.set_log_level("error") + hulls = coacd.run_coacd( + coacd.Mesh(np.asarray(mesh.vertices), np.asarray(mesh.faces)), + threshold=_THRESHOLD, + seed=_SEED, + ) + if len(hulls) != expected_count: + raise ValueError( + f"{name}: expected {expected_count} collision hulls, got {len(hulls)}." + ) + paths = [] + for index, (vertices, faces) in enumerate(hulls): + path = root / "assets" / f"{name}_collision_{index}.obj" + trimesh.Trimesh(vertices, faces).export(path) + relative = f"assets/{path.name}" + _verify_file(path, relative) + paths.append(relative) + parts[name] = paths + print(f"Generated and verified {name}: {len(paths)} hulls", flush=True) + + def values(text: str | None, default: str) -> np.ndarray: + return np.array([float(value) for value in (text or default).split()]) + + def rotation(text: str | None) -> Rotation: + return Rotation.from_quat(values(text, "1 0 0 0")[[1, 2, 3, 0]]) + + def origin(parent: ET.Element, pos: str | None, quat: str | None = None) -> None: + ET.SubElement( + parent, + "origin", + xyz=" ".join(map(str, values(pos, "0 0 0"))), + rpy=" ".join(map(str, rotation(quat).as_euler("xyz"))), + ) + + def body(element: ET.Element, parent: str) -> None: + name = element.attrib["name"] + link = ET.SubElement(urdf, "link", name=name) + inertial = element.find("inertial") + if inertial is not None: + entry = ET.SubElement(link, "inertial") + origin(entry, inertial.get("pos")) + ET.SubElement(entry, "mass", value=inertial.attrib["mass"]) + matrix = rotation(inertial.get("quat")).as_matrix() + inertia = ( + matrix + @ np.diag(values(inertial.get("diaginertia"), "1e-6 1e-6 1e-6")) + @ matrix.T + ) + ET.SubElement( + entry, + "inertia", + **{ + key: str(inertia[row, col]) + for key, row, col in ( + ("ixx", 0, 0), + ("iyy", 1, 1), + ("izz", 2, 2), + ("ixy", 0, 1), + ("ixz", 0, 2), + ("iyz", 1, 2), + ) + }, + ) + source_joint = element.find("joint") + joint = ET.SubElement( + urdf, + "joint", + name=( + source_joint.attrib["name"] + if source_joint is not None + else name + "_fixed" + ), + type=( + ("prismatic" if source_joint.get("type") == "slide" else "revolute") + if source_joint is not None + else "fixed" + ), + ) + ET.SubElement(joint, "parent", link=parent) + ET.SubElement(joint, "child", link=name) + origin(joint, element.get("pos"), element.get("quat")) + if source_joint is not None: + if np.linalg.norm(values(source_joint.get("pos"), "0 0 0")) > 1e-9: + raise ValueError("Offset MJCF joint positions are unsupported.") + ET.SubElement(joint, "axis", xyz=source_joint.get("axis", "0 0 1")) + lower, upper = values(source_joint.get("range"), "-3.14 3.14") + ET.SubElement( + joint, + "limit", + lower=str(lower), + upper=str(upper), + effort=str( + max(abs(values(source_joint.get("actuatorfrcrange"), "-80 80"))) + ), + velocity="3", + ) + ET.SubElement( + joint, + "dynamics", + damping=source_joint.get("damping", "0"), + friction=source_joint.get("frictionloss", "0"), + ) + for geometry in element.findall("geom"): + mesh_name = geometry.get("mesh") + if not mesh_name: + continue + for purpose, paths in ( + ("visual", ["assets/" + meshes[mesh_name]]), + ("collision", parts.get(mesh_name, ["assets/" + meshes[mesh_name]])), + ): + if purpose == "collision" and geometry.get("contype") == "0": + continue + for path in paths: + entry = ET.SubElement(link, purpose) + origin(entry, geometry.get("pos"), geometry.get("quat")) + shape = ET.SubElement(entry, "geometry") + ET.SubElement(shape, "mesh", filename=path) + for site in element.findall("site"): + if site.get("name", "").endswith("grasp_site"): + tcp = site.attrib["name"].replace("grasp_site", "tcp") + ET.SubElement(urdf, "link", name=tcp) + fixed = ET.SubElement(urdf, "joint", name=tcp + "_fixed", type="fixed") + ET.SubElement(fixed, "parent", link=name) + ET.SubElement(fixed, "child", link=tcp) + origin(fixed, site.get("pos"), site.get("quat")) + for child in element.findall("body"): + body(child, name) + + for element in xml.find("worldbody").findall("body"): + body(element, "base_link") + ET.indent(urdf) + ET.ElementTree(urdf).write( + root / "yam_i2rt.urdf", encoding="unicode", xml_declaration=True + ) + provenance = { + "repository": _REPOSITORY, + "commit": _REVISION, + "source": f"{_SOURCE_PATH}/yam_i2rt_arms.xml", + "license": "MIT; see LICENSE", + "conversion": "MJCF local body/joint/geometry frames and rotated diagonal inertias to URDF; named TCP fixed links; fingertip convex decomposition", + "coacd_version": _COACD_VERSION, + "coacd_threshold": _THRESHOLD, + "coacd_seed": _SEED, + "fingertip_hulls": {name: len(paths) for name, paths in parts.items()}, + "source_sha256": _EXPECTED_SHA256["yam_i2rt_arms.xml"], + } + (root / "PROVENANCE.json").write_text(json.dumps(provenance, indent=2) + "\n") + + +def _prepare(output: Path, source_dir: Path | None) -> Path: + if output.exists(): + _verify_bundle(output) + print(f"Verified existing robot: {output / 'yam_i2rt.urdf'}", flush=True) + return output / "yam_i2rt.urdf" + output.parent.mkdir(parents=True, exist_ok=True) + with tempfile.TemporaryDirectory(prefix=".yam-i2rt-", dir=output.parent) as folder: + staged = Path(folder) / "YamI2rt" + staged.mkdir() + _fetch_sources(staged, source_dir) + _generate_robot(staged) + _verify_bundle(staged) + staged.rename(output) + print(f"Robot ready: {output / 'yam_i2rt.urdf'}", flush=True) + return output / "yam_i2rt.urdf" + + +def _archive(root: Path, destination: Path) -> Path: + """Package verified assets with stable metadata for the data registry.""" + _verify_bundle(root) + if destination.is_relative_to(root): + raise ValueError("The archive must be outside the asset directory.") + destination.parent.mkdir(parents=True, exist_ok=True) + with tempfile.TemporaryDirectory( + prefix=".yam-archive-", dir=destination.parent + ) as folder: + staged = Path(folder) / destination.name + with zipfile.ZipFile( + staged, "w", compression=zipfile.ZIP_DEFLATED, compresslevel=9 + ) as stream: + # Open3D requires directory entries before their children. + directories = sorted( + { + str(Path(name).parent) + "/" + for name in _EXPECTED_SHA256 + if Path(name).parent != Path(".") + } + ) + for name in directories: + info = zipfile.ZipInfo(name, date_time=(1980, 1, 1, 0, 0, 0)) + info.create_system = 3 + info.external_attr = (0o40755 << 16) | 0x10 + stream.writestr(info, b"") + for name in sorted((*_EXPECTED_SHA256, "NOTICE")): + data = ( + _ASSET_NOTICE.encode() + if name == "NOTICE" + else (root / name).read_bytes() + ) + info = zipfile.ZipInfo(name, date_time=(1980, 1, 1, 0, 0, 0)) + info.create_system = 3 + info.external_attr = 0o100644 << 16 + info.compress_type = zipfile.ZIP_DEFLATED + stream.writestr(info, data, compresslevel=9) + if destination.exists(): + if _sha256(destination) != _sha256(staged): + raise ValueError( + f"Refusing to overwrite a different archive: {destination}" + ) + else: + staged.rename(destination) + print( + f"Archive ready: {destination}; " + f"MD5={hashlib.md5(destination.read_bytes()).hexdigest()}; " + f"SHA-256={_sha256(destination)}", + flush=True, + ) + return destination + + +def main(argv: list[str] | None = None) -> int: + """Prepare verified robot assets from the pinned upstream or a local copy. + + Args: + argv: CLI arguments, or None to read the process arguments. + + Returns: + Zero after verifying and installing the complete asset bundle. + """ + data_root = Path( + os.environ.get("EMBODICHAIN_DATA_ROOT") + or Path.home() / ".cache/embodichain_data" + ) + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--output", + type=Path, + default=data_root / "YamI2rt", + help="Asset directory (default: $EMBODICHAIN_DATA_ROOT/YamI2rt, or ~/.cache/embodichain_data/YamI2rt).", + ) + parser.add_argument( + "--archive", + type=Path, + help="Also produce a deterministic asset ZIP for publication; outside --output.", + ) + parser.add_argument( + "--source-dir", + type=Path, + help="Offline directory containing the pinned LICENSE, XML and assets/*.stl.", + ) + args = parser.parse_args(argv) + try: + output = args.output.expanduser().resolve() + _prepare(output, args.source_dir) + if args.archive is not None: + _archive(output, args.archive.expanduser().resolve()) + except ( + OSError, + ValueError, + ImportError, + importlib.metadata.PackageNotFoundError, + ) as error: + parser.exit(1, f"Asset preparation failed: {error}\n") + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tests/compute/geometry/test_cable.py b/tests/compute/geometry/test_cable.py new file mode 100644 index 0000000000..9b0820cf04 --- /dev/null +++ b/tests/compute/geometry/test_cable.py @@ -0,0 +1,101 @@ +# ---------------------------------------------------------------------------- +# 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 __future__ import annotations + +import math +import pytest +import torch + +from embodichain.compute.geometry.cable import ( + cable_endpoints, + sample_cable_material, + cable_grasp_slip, +) + +pytestmark = pytest.mark.no_sim + + +def test_material_coordinates_follow_rotated_capsule_axes_and_world_origins(): + state = torch.zeros(2, 2, 13) + state[..., 3] = -math.sqrt(0.5) + state[..., 6] = math.sqrt(0.5) + state[..., :3] = torch.tensor([[0, 0.1, 1], [0, 0.3, 1]]) + state[1, :, :3] += torch.tensor([3, 5, 0]) + lengths = torch.tensor([0.2, 0.2]) + point, tangent, index = sample_cable_material(state, lengths, 0.3) + assert index == 1 + torch.testing.assert_close(point, torch.tensor([[0, 0.3, 1], [3, 5.3, 1]])) + torch.testing.assert_close( + tangent, torch.tensor([[0, 1, 0], [0, 1, 0.0]]), atol=1e-6, rtol=0 + ) + first, last = cable_endpoints(state, lengths) + torch.testing.assert_close( + first[0, 0], torch.tensor([0, 0, 1.0]), atol=1e-6, rtol=0 + ) + torch.testing.assert_close( + last[0, -1], torch.tensor([0, 0.4, 1.0]), atol=1e-6, rtol=0 + ) + + +@pytest.mark.parametrize("distance", [-0.01, 0.41, float("nan")]) +def test_material_sampling_rejects_invalid_distance(distance): + s = torch.zeros(1, 2, 13) + s[..., 6] = 1 + with pytest.raises(ValueError): + sample_cable_material(s, torch.tensor([0.2, 0.2]), distance) + + +def test_slip_uses_the_tool_frame_material_point(): + initial = torch.tensor([[0.01, 0, 0], [0, 0, 0.01]]) + current = initial + torch.tensor([[0, 0.003, 0.004], [0, 0, 0]]) + torch.testing.assert_close( + cable_grasp_slip(current, initial), torch.tensor([0.005, 0]) + ) + + +@pytest.mark.parametrize( + "lengths", + [ + torch.tensor([]), + torch.tensor([-0.2]), + torch.tensor([0.0]), + torch.tensor([float("nan")]), + ], +) +def test_endpoints_reject_invalid_rest_geometry(lengths): + state = torch.zeros(2, lengths.numel(), 13) + state[..., 6] = 1 + with pytest.raises(ValueError, match="positive vector"): + cable_endpoints(state, lengths) + + +def test_material_sampling_preserves_endpoint_identity_for_unequal_segments(): + state = torch.zeros(1, 2, 13, dtype=torch.float64) + state[..., 6] = 1 + state[0, :, 2] = torch.tensor([0.05, 0.25]) + lengths = torch.tensor([0.1, 0.3], dtype=torch.float64) + for distance, expected_z, expected_index in [ + (0.0, 0.0, 0), + (0.1, 0.1, 0), + (0.4, 0.4, 1), + ]: + point, tangent, index = sample_cable_material(state, lengths, distance) + assert index == expected_index + torch.testing.assert_close(point, state.new_tensor([[0, 0, expected_z]])) + torch.testing.assert_close(tangent, state.new_tensor([[0, 0, 1]])) diff --git a/tests/data/test_download_prefix.py b/tests/data/test_download_prefix.py index 37e47657eb..f0c277ac7b 100644 --- a/tests/data/test_download_prefix.py +++ b/tests/data/test_download_prefix.py @@ -41,6 +41,7 @@ from embodichain_robots.agilex.pika.assets import PikaGripper from embodichain_robots.sharpa.wave.assets import SharpaWave from embodichain_robots.wuji.hand.assets import WujiHand +from embodichain_robots.i2rt.yam.assets import YamI2rt urls = {} @@ -52,7 +53,7 @@ def capture(self, prefix, descriptor, path): raise Captured # stop before anything is downloaded dataset.EmbodiChainDataset.__init__ = capture -for cls in (CobotMagicArm, FlexivRizon4S, UnitreeGo2Locomotion, UniversalRobots, DexforceW1V030, PikaGripper, SharpaWave, WujiHand): +for cls in (CobotMagicArm, FlexivRizon4S, UnitreeGo2Locomotion, UniversalRobots, DexforceW1V030, PikaGripper, SharpaWave, WujiHand, YamI2rt): try: cls() except Captured: @@ -139,6 +140,10 @@ def test_assets_download_from_the_configured_prefix(prefix: str) -> None: ] # Corrected W1 visuals overwrite the existing V030 archive and cache. assert urls["DexforceW1V030"] == [f"{MIRROR}dexforce_w1/v030/w1.zip"] + assert urls["YamI2rt"] == [ + f"{MIRROR}robot_assets/YamI2rtV1.zip", + f"{HF_PREFIX}robot_assets/YamI2rtV1.zip", + ] @pytest.mark.no_sim @@ -154,3 +159,4 @@ def test_assets_do_not_repeat_the_hub_url() -> None: ] assert urls["FlexivRizon4S_v1"] == [f"{HF_PREFIX}robot_assets/FlexivRizon4S_v1.zip"] assert urls["PikaGripper"] == [f"{HF_PREFIX}eef_assets/PikaGripper.zip"] + assert urls["YamI2rt"] == [f"{HF_PREFIX}robot_assets/YamI2rtV1.zip"] diff --git a/tests/data/test_yam_i2rt_assets.py b/tests/data/test_yam_i2rt_assets.py new file mode 100644 index 0000000000..3d380bde09 --- /dev/null +++ b/tests/data/test_yam_i2rt_assets.py @@ -0,0 +1,94 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""YAM participates in the existing data registry and cache protocol.""" + +from __future__ import annotations + +from pathlib import Path +import importlib +import subprocess +import sys + +import pytest + +from embodichain.data import get_data_path +from embodichain_robots.i2rt.yam.assets import YamI2rt +from embodichain.data import dataset +from embodichain.data.download import find_asset_class, get_registry + +robot_assets = importlib.import_module("embodichain_robots.i2rt.yam.assets") + +pytestmark = pytest.mark.no_sim + + +def test_yam_is_discoverable_from_data_path_and_robot_download_cli() -> None: + assert dataset.get_data_class("YamI2rt") is YamI2rt + assert find_asset_class("YamI2rt", get_registry()) == ("robot", YamI2rt) + + +@pytest.mark.parametrize("custom_root", [False, True]) +def test_yam_uses_the_shared_checked_archive_and_cache_root( + monkeypatch: pytest.MonkeyPatch, tmp_path: Path, custom_root: bool +) -> None: + captured = {} + + class Captured(Exception): + pass + + def capture(self, prefix, descriptor, path): + captured.update(prefix=prefix, descriptor=descriptor, root=path) + raise Captured + + monkeypatch.setattr(dataset.EmbodiChainDataset, "__init__", capture) + monkeypatch.setattr(robot_assets, "EMBODICHAIN_DEFAULT_DATA_ROOT", str(tmp_path)) + root = str(tmp_path / "custom") if custom_root else None + with pytest.raises(Captured): + YamI2rt(data_root=root) + assert captured["prefix"] == "YamI2rt" + assert captured["root"] == (root or str(tmp_path)) + assert captured["descriptor"].md5 == YamI2rt.archive_md5 + assert captured["descriptor"].urls[0].endswith("robot_assets/YamI2rtV1.zip") + + +def test_prepared_offline_cache_is_reused_without_downloading( + monkeypatch: pytest.MonkeyPatch, tmp_path: Path +) -> None: + from embodichain.data import constants + + urdf = tmp_path / "YamI2rt/yam_i2rt.urdf" + urdf.parent.mkdir() + urdf.write_text('') + monkeypatch.setattr(constants, "EMBODICHAIN_DEFAULT_DATA_ROOT", str(tmp_path)) + + def no_download(*args, **kwargs): + raise AssertionError("A prepared offline cache must not download assets") + + monkeypatch.setattr(dataset, "get_data_class", no_download) + assert get_data_path("YamI2rt/yam_i2rt.urdf") == str(urdf) + + +def test_asset_import_does_not_load_robot_configuration() -> None: + result = subprocess.run( + [ + sys.executable, + "-c", + "import sys; import embodichain_robots.i2rt.yam.assets; assert 'embodichain_robots.i2rt.yam.cfg' not in sys.modules", + ], + capture_output=True, + text=True, + ) + assert result.returncode == 0, result.stderr diff --git a/tests/gym/envs/tasks/test_route_the_cable.py b/tests/gym/envs/tasks/test_route_the_cable.py new file mode 100644 index 0000000000..a5d407fbc2 --- /dev/null +++ b/tests/gym/envs/tasks/test_route_the_cable.py @@ -0,0 +1,614 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations +from pathlib import Path +from types import SimpleNamespace +from unittest.mock import Mock +import xml.etree.ElementTree as ET +import math +import yaml +import pytest +import torch +from embodichain.lab.gym.utils.gym_utils import ( + add_env_launcher_args_to_parser, + build_env_cfg_from_args, + config_to_cfg, +) +from embodichain_tasks.manipulation.cable.route_the_cable import ( + measure_route, + RouteTheCableEnv, +) +from embodichain.lab.sim.spawn.descriptors import rigid_desc_from_cfg +from embodichain.lab.sim.cfg import RigidObjectCfg +from embodichain.lab.sim.cfg import CableObjectCfg +from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg +from embodichain.lab.gym.envs.expert_trajectory import ( + ExpertTrajectoryCfg, + build_expert_action_spec, +) +from embodichain.lab.sim.shapes import CylinderCfg + +pytestmark = pytest.mark.no_sim + + +@pytest.fixture(autouse=True) +def local_yam_config_asset(tmp_path: Path, monkeypatch: pytest.MonkeyPatch) -> Path: + """Keep configuration tests independent of external model publication.""" + from embodichain_robots.i2rt.yam import cfg as yam_i2rt + + robot = ET.Element("robot", name="yam_config_fixture") + ET.SubElement(robot, "link", name="base_link") + for side in ("right", "left"): + parent = "base_link" + for child, number in [ + (f"{side}_base", None), + *[(f"{side}_link{i}", i) for i in range(1, 7)], + (f"{side}_tcp", None), + ]: + ET.SubElement(robot, "link", name=child) + joint = ET.SubElement( + robot, + "joint", + name=f"{side}_joint{number}" if number else f"{child}_fixed", + type="revolute" if number else "fixed", + ) + ET.SubElement(joint, "parent", link=parent) + ET.SubElement(joint, "child", link=child) + if number: + ET.SubElement(joint, "axis", xyz="0 0 1") + ET.SubElement( + joint, "limit", lower="-3", upper="3", effort="27", velocity="3" + ) + parent = child + path = tmp_path / "yam_i2rt.urdf" + ET.ElementTree(robot).write(path, encoding="unicode") + + def resolve(name: str) -> str: + assert name == "YamI2rt/yam_i2rt.urdf" + return str(path) + + monkeypatch.setattr(yam_i2rt, "get_data_path", resolve) + return path + + +def test_route_deployment_composes_registered_robot_cable_and_board(): + p = ( + Path(__file__).resolve().parents[4] + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml" + ) + cfg = config_to_cfg(yaml.safe_load(p.read_text()), source_path=p) + assert cfg.robot.root_props.newton_gravity_compensation == 1.0 + assert len(cfg.cable_object) == 1 and len(cfg.cable_object[0].points) == 51 + assert cfg.cable_object[0].fixed_segments == [0] + points = torch.tensor(cfg.cable_object[0].points) + torch.tensor( + cfg.cable_object[0].init_pos + ) + torch.testing.assert_close(points[:, 2], torch.full((51,), 0.7535)) + assert bool( + ( + (points[:, 0] >= 0.3025) + & (points[:, 0] <= 0.8975) + & (points[:, 1].abs() <= 0.65) + ).all() + ) + lengths = points.diff(dim=0).norm(dim=-1) + cumulative = lengths.cumsum(0) + distance = cumulative[-1] - 0.030 + index = int(torch.searchsorted(cumulative, distance)) + grasp = torch.lerp( + points[index], + points[index + 1], + (distance - cumulative[index - 1]) / lengths[index], + ) + torch.testing.assert_close( + grasp[:2], torch.tensor([0.42, 0.21]), atol=0.002, rtol=0 + ) + assert lengths.max() <= 0.0121 + assert lengths.min() > 0.0115 + assert len(cfg.background) == 11 + assert cfg.cable_object[0].attrs.density == 350.0 + assert cfg.cable_object[0].attrs.stretch_stiffness == 2e5 + assert cfg.cable_object[0].attrs.material_props.kd == 0.1 + assert cfg.cable_object[0].attrs.material_props.dynamic_friction == 60.0 + assert cfg.robot.link_attrs["pads"].attrs.material_props.dynamic_friction == 60.0 + assert all( + obj.attrs.material_props.dynamic_friction == 2.0 + for obj in cfg.background + if obj.uid.startswith("spool_") + ) + assert cfg.sim_cfg.physics_cfg.solver_cfg["coupling"] == "two_way" + assert cfg.sim_cfg.physics_cfg.solver_cfg["vbd_options"]["rigid_contact_history"] + assert cfg.robot.joint_drive_props.stiffness["right_joint[1-6]"] == 300.0 + assert cfg.robot.joint_drive_props.stiffness[".*_joint[78]"] == 6000.0 + assert cfg.robot.joint_drive_props.damping[".*_joint[78]"] == 120.0 + assert cfg.sim_cfg.render_cfg.renderer == "no-render" + assert not cfg.sim_cfg.visualization.allow_commands + assert cfg.robot.fpath is None + assert Path(cfg.robot.urdf_cfg.assemble_urdf()).is_file() + for side, chain in cfg.robot.build_pk_serial_chain().items(): + assert ( + len(chain.get_joint_parameter_names()) + == len(cfg.robot.control_parts[side]) + == 6 + ) + + +def test_route_viser_launcher_keeps_expert_deployment_read_only(): + import argparse + + path = ( + Path(__file__).resolve().parents[4] + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml" + ) + parser = argparse.ArgumentParser() + add_env_launcher_args_to_parser(parser, require_gym_config=True) + args = parser.parse_args(["--gym_config", str(path), "--viser"]) + + cfg, _, _ = build_env_cfg_from_args(args) + + assert cfg.sim_cfg.visualization.backend == "viser" + assert not cfg.sim_cfg.visualization.allow_commands + + +def _goal_state(): + state = torch.zeros(1, 50, 13) + state[..., 3] = -math.sqrt(0.5) + state[..., 6] = math.sqrt(0.5) + state[..., :3] = torch.tensor([0.42, 0, 0.7535]) + state[0, :2, :3] = torch.tensor([0.50, -0.26, 0.7535]) + state[0, 2:4, :3] = torch.tensor([0.55, -0.02, 0.7535]) + state[0, -3:, :3] = torch.tensor([0.586, 0.078, 0.7535]) + return state + + +def test_reference_goal_is_measured_and_rejects_moving_or_held_cable(): + state = _goal_state() + lengths = torch.full((50,), 0.012) + hands = torch.tensor([[[0, 0, 1], [0, 1, 1.0]]]) + origin = torch.zeros(1, 3) + assert measure_route(state, lengths, hands, origin)["reference_routed"].item() + moving = state.clone() + moving[0, 20, 7] = 0.051 + assert not measure_route(moving, lengths, hands, origin)["reference_routed"].item() + hands[:, 0] = state[:, 0, :3] + assert not measure_route(state, lengths, hands, origin)["reference_routed"].item() + + +def test_route_metrics_remove_arena_offsets_once(): + state = _goal_state().expand(2, -1, -1).clone() + origin = torch.tensor([[0, 0, 0], [3, 5, 0.0]]) + state[..., :3] += origin[:, None, :] + hands = ( + torch.tensor([[[0, 0, 1], [0, 1, 1.0]]]).expand(2, -1, -1) + origin[:, None, :] + ) + out = measure_route(state, torch.full((50,), 0.012), hands, origin) + for value in out.values(): + torch.testing.assert_close(value[0], value[1]) + + +def test_cylinder_uses_native_collision_geometry(): + desc, _ = rigid_desc_from_cfg( + RigidObjectCfg( + uid="spool", + shape=CylinderCfg(radius=0.0125, height=0.0025), + body_type="static", + ), + newton_solver_type="dexuni", + ) + assert desc.collisions[0].radius == 0.0125 + assert desc.collisions[0].half_height == 0.00125 + + +def test_cable_deployment_rejects_duplicate_component_ownership(): + p = ( + Path(__file__).resolve().parents[4] + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml" + ) + data = yaml.safe_load(p.read_text()) + data["cable_object"] = [] + with pytest.raises(ValueError, match="cable_object"): + config_to_cfg(data, source_path=p) + + +def test_strict_channel_rejects_end_that_only_meets_broad_region(): + state = _goal_state() + lengths = torch.full((50,), 0.012) + hands = torch.tensor([[[0, 0, 1], [0, 1, 1.0]]]) + origin = torch.zeros(1, 3) + assert measure_route(state, lengths, hands, origin)["strict_port"].item() + state[0, -1, 0] += 0.014 + metrics = measure_route(state, lengths, hands, origin) + assert metrics["reference_routed"].item() + assert not metrics["strict_port"].item() + + +def test_object_goal_stops_before_writing_an_unexecuted_command(): + from types import SimpleNamespace + + command = torch.full((1, 3), 0.5) + env = SimpleNamespace( + _command=command, + _grasp=None, + _max_slip=torch.zeros(1), + cfg=SimpleNamespace( + expert_trajectory=SimpleNamespace(joint_command_mode="position") + ), + ) + actions = RouteTheCableEnv._execute( + env, torch.ones(1, 5, 3), [0, 1, 2], stop_when=lambda: torch.tensor([True]) + ) + assert list(actions) == [] + torch.testing.assert_close(command, torch.full_like(command, 0.5)) + + +def _task_cfg(segments=50): + return EmbodiedEnvCfg( + cable_object=[ + CableObjectCfg( + uid="cable", + points=[[0, 0.012 * i, 0] for i in range(segments + 1)], + radius=0.0035, + ) + ] + ) + + +@pytest.mark.parametrize("segments", [0, 49, 51]) +def test_wrong_cable_topology_fails_before_environment_construction( + monkeypatch, segments +): + parent_init = Mock() + monkeypatch.setattr(EmbodiedEnv, "__init__", parent_init) + with pytest.raises(ValueError, match="50-segment"): + RouteTheCableEnv(_task_cfg(segments)) + parent_init.assert_not_called() + + +def test_missing_cable_fails_before_environment_construction(monkeypatch): + parent_init = Mock() + monkeypatch.setattr(EmbodiedEnv, "__init__", parent_init) + with pytest.raises(ValueError, match="named 'cable'"): + RouteTheCableEnv(EmbodiedEnvCfg()) + parent_init.assert_not_called() + + +@pytest.mark.parametrize("cleanup_fails", [False, True]) +def test_post_base_failure_closes_dataset_and_queues_simulator_cleanup( + monkeypatch, cleanup_fails +): + simulator, dataset = Mock(), Mock() + if cleanup_fails: + dataset.finalize.side_effect = RuntimeError("recorder failure") + + def initialize_parent(env, cfg, **kwargs): + env.cfg = cfg + env.sim = simulator + env.dataset_manager = dataset + env._profiler = Mock() + env._discard_pending_recordings = Mock() + + simulator.get_cable_object.return_value = SimpleNamespace( + data=SimpleNamespace(num_segments=49) + ) + monkeypatch.setattr(EmbodiedEnv, "__init__", initialize_parent) + with pytest.raises(ValueError, match="50-segment"): + RouteTheCableEnv(_task_cfg()) + dataset.finalize.assert_called_once_with() + simulator.destroy.assert_called_once_with(exit_process=False) + + +def _command_env(mode): + env = object.__new__(RouteTheCableEnv) + env.cfg = EmbodiedEnvCfg( + sim_steps_per_control=1, + expert_trajectory=ExpertTrajectoryCfg(joint_command_mode=mode), + ) + env.sim_cfg = SimpleNamespace(physics_dt=0.1) + env._num_envs = 2 + env.sim = SimpleNamespace(device=torch.device("cpu")) + env._command = torch.zeros(2, 4) + env._grasp = None + env._max_slip = torch.zeros(2) + env.robot = SimpleNamespace(get_qpos=lambda: torch.zeros(2, 4)) + env.active_joint_ids = [1, 3] + env.action_manager = None + env._traj_buffer = object() + env._trajectory_action_kind = "expert" + env._record_raw_actions = False + env.expert_action_spec = build_expert_action_spec( + joint_names=["a", "b"], + joint_command_mode=mode, + ) + return env + + +@pytest.mark.parametrize("mode", ["position", "position_velocity"]) +def test_motion_commands_pass_environment_preprocessing_and_recording(mode): + env = _command_env(mode) + path = torch.zeros(2, 3, 4) + path[:, :, 1] = torch.tensor([0.1, 0.2, 0.3]) + path[:, :, 3] = torch.tensor([-0.2, -0.4, -0.6]) + # Non-selected planner columns must not affect the inactive commands. + path[:, :, 0] = 9 + actions = list(env._execute(path, [1, 3])) + assert len(actions) == 3 + for i, action in enumerate(actions): + env._preprocess_action(action) + expected = path[:, i, [1, 3]] + torch.testing.assert_close(env._traj_raw_action[:, :2], expected) + if mode == "position_velocity": + velocity = ( + torch.tensor([1.0, -2.0]).expand(2, -1) if i < 2 else torch.zeros(2, 2) + ) + torch.testing.assert_close(env._traj_raw_action[:, 2:], velocity) + torch.testing.assert_close( + action.value["qvel"][:, [0, 2]], torch.zeros(2, 2) + ) + actual = action.value if mode == "position" else action.value["qpos"] + torch.testing.assert_close(actual[:, [0, 2]], torch.zeros(2, 2)) + # Yielded commands must remain independent when the next frame is emitted. + first = actions[0].value if mode == "position" else actions[0].value["qpos"] + torch.testing.assert_close(first[:, 1], torch.full((2,), 0.1)) + + +def test_hold_emits_zero_velocity_and_can_be_recorded(): + env = _command_env("position_velocity") + env._command.fill_(0.25) + actions = list(env._hold(0.2)) + assert len(actions) == 2 + for action in actions: + env._preprocess_action(action) + torch.testing.assert_close( + env._traj_raw_action, torch.tensor([[0.25, 0.25, 0, 0]]).expand(2, -1) + ) + + +def test_grasp_slip_stops_execution_before_mutating_commands(): + env = _command_env("position_velocity") + env._grasp = ("left", 0.57, torch.zeros(2, 3)) + env._max_slip[1] = 0.013 + assert list(env._execute(torch.ones(2, 3, 4), [1, 3])) == [] + torch.testing.assert_close(env._command, torch.zeros(2, 4)) + + +def test_close_callback_observes_the_executed_frame(): + env = _command_env("position") + observations = [] + env._point_in_tool = lambda *_: observations.append("observed") or torch.ones(2, 3) + actions = env._execute( + torch.ones(2, 2, 4), [1, 3], closed_at=1, grasp_ref=("right", 0.162) + ) + next(actions) + assert observations == [] and env._grasp is None + next(actions) + assert observations == ["observed"] + assert env._grasp[:2] == ("right", 0.162) + + +@pytest.mark.parametrize("invalid", ["motion", "nonfinite", "held", "empty_seat"]) +def test_reference_metrics_reject_invalid_or_incomplete_observations(invalid): + state = _goal_state() + hands = torch.ones(1, 2, 3) + if invalid == "motion": + state[0, 0, 7] = 0.051 + elif invalid == "nonfinite": + state[0, 0, 0] = float("nan") + elif invalid == "held": + hands[0, 0] = state[0, -1, :3] + else: + state[0, :2, 0] = 0.42 + assert not measure_route(state, torch.full((50,), 0.012), hands, torch.zeros(1, 3))[ + "reference_routed" + ].item() + + +def test_contact_penetration_uses_the_configured_native_back_wall(): + p = ( + Path(__file__).resolve().parents[4] + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml" + ) + cfg = config_to_cfg(yaml.safe_load(p.read_text()), source_path=p) + wall = next(obj for obj in cfg.background if obj.uid == "port_back") + inner_y = wall.init_pos[1] - wall.shape.size[1] / 2 + state = torch.zeros(1, 1, 13) + state[..., 6] = 1 + state[..., :3] = torch.tensor( + [0.586, inner_y - cfg.cable_object[0].radius + 0.002, 0.755] + ) + metrics = measure_route( + state, torch.tensor([0.001]), torch.ones(1, 2, 3), torch.zeros(1, 3) + ) + assert metrics["port_penetration"].item() == pytest.approx(0.002, abs=2e-7) + + +def test_unknown_cable_fields_are_rejected_by_production_loader(tmp_path): + p = ( + Path(__file__).resolve().parents[4] + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml" + ) + data = yaml.safe_load(p.read_text()) + physical = yaml.safe_load((p.parent / "env.yaml").read_text()) + physical["simulation"]["cable_object"][0]["unknown_cable_option"] = True + component = tmp_path / "env.yaml" + component.write_text(yaml.safe_dump(physical)) + data["environment"]["component"] = str(component) + with pytest.raises((TypeError, ValueError), match="unknown_cable_option"): + config_to_cfg(data, source_path=p) + + +@pytest.mark.parametrize( + "planning_success,tracking_error,object_goal,slip,expected", + [ + (False, 0.0, False, 0.0, False), + (False, 0.015, True, 0.0, False), + (True, 0.015, False, 0.0, False), + (True, 0.001, False, 0.0, True), + (True, 0.015, True, 0.0, True), + (True, 0.001, True, 0.013, False), + ], +) +@pytest.mark.parametrize("extra_samples", [0, 7]) +def test_insertion_validator_requires_planning_and_actual_tracking( + monkeypatch, + planning_success, + tracking_error, + object_goal, + slip, + expected, + extra_samples, +): + from types import SimpleNamespace + from unittest.mock import MagicMock + + warning = Mock() + monkeypatch.setattr( + "embodichain_tasks.manipulation.cable.route_the_cable.logger.log_warning", + warning, + ) + + actual = torch.eye(4).unsqueeze(0) + target = actual.clone() + target[:, 2, 3] += tracking_error + + def generate(states, *, options): + return SimpleNamespace( + positions=( + torch.zeros(1, options.sample_count + extra_samples, 3) + if planning_success + else None + ), + success=torch.tensor([planning_success]), + ) + + env = SimpleNamespace( + device=torch.device("cpu"), + num_envs=1, + step_dt=1 / 60, + _tool=lambda side: actual, + _command=torch.zeros(1, 3), + _max_slip=torch.tensor([slip]), + _grasp=None, + robot=SimpleNamespace( + get_joint_ids=lambda name: [0, 1, 2], get_qpos=lambda: torch.zeros(1, 3) + ), + _motion=SimpleNamespace(generate=generate), + _execute=MagicMock(return_value=iter(())), + _hold=MagicMock(return_value=iter(())), + ) + segment = RouteTheCableEnv._move( + env, + "left", + target, + "insert", + tolerance=0.003, + goal_reached=lambda: torch.tensor([object_goal]), + ) + assert bool(segment.validator().item()) is expected + if expected: + warning.assert_not_called() + else: + message = warning.call_args.args[0] + assert "'insert' rejected" in message + assert "planning_success=" in message + assert "position_error_m=" in message + assert "max_grasp_slip_m=" in message + if not planning_success: + assert list(segment.actions) == [] + env._execute.assert_not_called() + else: + list(segment.actions) + trajectory = env._execute.call_args.args[0] + assert trajectory.shape[1] >= 3 + extra_samples + assert trajectory.shape[-1] == 3 + + +@pytest.mark.parametrize("settles", [True, False]) +def test_insertion_waits_for_tracking_without_bypassing_timeout(settles): + from types import MethodType + + actual = torch.eye(4).unsqueeze(0) + target = actual.clone() + target[:, 0, 3] = 0.01 + + def generate(states, *, options): + return SimpleNamespace( + positions=torch.zeros(1, options.sample_count, 3), + success=torch.tensor([True]), + ) + + env = SimpleNamespace( + device=torch.device("cpu"), + num_envs=1, + step_dt=1 / 60, + cfg=SimpleNamespace( + expert_trajectory=SimpleNamespace(joint_command_mode="position") + ), + _tool=lambda side: actual, + _command=torch.zeros(1, 3), + _max_slip=torch.zeros(1), + _grasp=None, + robot=SimpleNamespace( + get_joint_ids=lambda name: [0, 1, 2], get_qpos=lambda: torch.zeros(1, 3) + ), + _motion=SimpleNamespace(generate=generate), + _execute=lambda *args, **kwargs: iter(()), + ) + env._hold = MethodType(RouteTheCableEnv._hold, env) + segment = RouteTheCableEnv._move( + env, + "left", + target, + "insert", + tolerance=0.003, + goal_reached=lambda: torch.tensor([False]), + ) + frames = 0 + for _ in segment.actions: + frames += 1 + # The controller reaches tolerance after the initial 0.25 s hold. + if settles and frames == 18: + actual[:, 0, 3] = 0.009 + + assert frames == (18 if settles else 75) + assert bool(segment.validator().item()) is settles + + +@pytest.mark.parametrize("success", [True, False]) +def test_final_route_validation_reports_physical_metrics(monkeypatch, success): + warning = Mock() + monkeypatch.setattr( + "embodichain_tasks.manipulation.cable.route_the_cable.logger.log_warning", + warning, + ) + metrics = { + "success": torch.tensor([success]), + "strict_port": torch.tensor([success]), + "end_position": torch.tensor([[0.586, 0.077, 0.754]]), + "max_grasp_slip": torch.tensor([0.01]), + } + env = SimpleNamespace(route_metrics=lambda: metrics) + + torch.testing.assert_close( + RouteTheCableEnv._validate_route(env), metrics["success"] + ) + if success: + warning.assert_not_called() + else: + message = warning.call_args.args[0] + assert "strict_port" in message + assert "end_position" in message + assert "max_grasp_slip" in message diff --git a/tests/gym/envs/tasks/test_route_the_cable_cuda.py b/tests/gym/envs/tasks/test_route_the_cable_cuda.py new file mode 100644 index 0000000000..55d37a6420 --- /dev/null +++ b/tests/gym/envs/tasks/test_route_the_cable_cuda.py @@ -0,0 +1,252 @@ +# ---------------------------------------------------------------------------- +# 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 collections.abc import Callable +import json +from pathlib import Path +import subprocess +import sys + +import pytest +import torch +import yaml + +from embodichain.lab.gym.utils.gym_utils import config_to_cfg +from embodichain.lab.sim import SimulationManager +from embodichain_tasks.manipulation.cable.route_the_cable import RouteTheCableEnv + +pytestmark = [ + pytest.mark.subprocess_sim, + pytest.mark.gpu, + pytest.mark.slow, + pytest.mark.skipif( + not torch.cuda.is_available(), reason="Cable routing requires CUDA" + ), + pytest.mark.usefixtures("requires_dexuni_two_way", "requires_dexuni_contact_query"), +] + + +class _SpoolReleaseCUDAScenario: + """Qualify the actual jaw sweep, rather than only cable goal predicates.""" + + def setup_method(self) -> None: + if not torch.cuda.is_available(): + pytest.skip("Cable routing requires CUDA") + path = ( + Path(__file__).resolve().parents[4] + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml" + ) + cfg = config_to_cfg(yaml.safe_load(path.read_text()), source_path=path) + cfg.seed = 73 + cfg.sim_cfg.render_cfg.renderer = "no-render" + cfg.sim_cfg.robot_ik_gizmo = None + self.env = RouteTheCableEnv(cfg) + + def teardown_method(self) -> None: + if hasattr(self, "env"): + self.env.close(exit_process=False) + SimulationManager.flush_cleanup_queue() + + def test_first_spool_release_preserves_seating_without_gripper_penetration( + self, + record_property: Callable[[str, float], None], + ) -> None: + env = self.env + scene = env.sim.spawn_result + handles = scene.handles + robot_path = next(path for path in handles if path.endswith("/yam")) + spool_paths = [path for path in handles if "/spool_" in path] + query = scene.create_contact_query( + [handles[robot_path], *(handles[path] for path in spool_paths)], + match="all", + capacity=4096, + device=env.device, + ) + spool_ids = torch.tensor( + [actor.actor_id for actor in query.actors if actor.path in spool_paths], + device=env.device, + ) + robot_ids = torch.tensor( + [actor.actor_id for actor in query.actors if actor.path == robot_path], + device=env.device, + ) + worst_distance = 0.0 + worst_segment = "" + env.reset() + for segment in env.create_demo_segments(milestone="first_spool"): + for action in segment.actions: + env.step(action) + contacts = query.fetch_solver_report() + assert contacts.dropped_count == 0 + pairs = contacts.actor_ids[: contacts.count] + robot_spool = torch.isin(pairs, robot_ids).any(-1) & torch.isin( + pairs, spool_ids + ).any(-1) + distances = contacts.distance[: contacts.count][robot_spool] + if distances.numel(): + distance = float(distances.min()) + if distance < worst_distance: + worst_distance, worst_segment = distance, segment.name + assert bool(segment.validator().all()), segment.name + + # Finite cable states and seated cable alone previously accepted a jaw + # sweep through the rim with about 16 mm reported penetration. + assert worst_distance >= -0.001, (worst_segment, worst_distance) + max_slip = float(env._episode_slip.max()) + record_property("max_grasp_slip_m", max_slip) + assert max_slip <= 0.012 + assert bool((env.route_metrics()["seat_0"] >= 2).all()) + assert bool((env._tool("right")[:, 2, 3] > 0.81).all()) + finger_ids = env.robot.get_joint_ids("right_eef") + assert bool((env.robot.get_qpos()[:, finger_ids] > 0.045).all()) + + def test_third_carry_preserves_grasp_and_clears_spools( + self, record_property: Callable[[str, float], None] + ) -> None: + env = self.env + scene = env.sim.spawn_result + paths = [ + path for path in scene.handles if path.endswith("/yam") or "/spool_" in path + ] + query = scene.create_contact_query( + [scene.handles[path] for path in paths], + match="all", + capacity=4096, + device=env.device, + ) + robot_ids = torch.tensor( + [actor.actor_id for actor in query.actors if actor.path.endswith("/yam")], + device=env.device, + ) + spool_ids = torch.tensor( + [actor.actor_id for actor in query.actors if "/spool_" in actor.path], + device=env.device, + ) + third_segments = { + "raise_2", + "carry_2", + "lower_2", + "release_2", + "retreat_2", + "open_clear_of_spool_2", + } + worst_distance = 0.0 + max_slip = 0.0 + raised = False + env.reset() + for segment in env.create_demo_segments(): + for action in segment.actions: + env.step(action) + if segment.name in third_segments: + max_slip = max(max_slip, float(env._max_slip.max())) + contacts = query.fetch_solver_report() + assert contacts.dropped_count == 0 + pairs = contacts.actor_ids[: contacts.count] + robot_spool = torch.isin(pairs, robot_ids).any(-1) & torch.isin( + pairs, spool_ids + ).any(-1) + distances = contacts.distance[: contacts.count][robot_spool] + if distances.numel(): + worst_distance = min(worst_distance, float(distances.min())) + assert bool(segment.validator().all()), segment.name + if segment.name == "raise_2": + raised = True + assert env._grasp[:2] == ("right", 0.438) + # The gripped centre must clear the 23.5 mm rim plus cable radius. + clearance_height = 0.75 + 0.0235 + env.cable.cfg.radius + assert bool((env._point(0.438)[0][:, 2] > clearance_height).all()) + if segment.name == "open_clear_of_spool_2": + assert raised + record_property("third_carry_slip_m", max_slip) + record_property("third_carry_robot_spool_distance_m", worst_distance) + assert max_slip <= 0.012 + assert worst_distance >= -0.001 + assert bool((env.route_metrics()["seat_1"] >= 2).all()) + return + pytest.fail("The expert did not complete the third carry") + + +def _run_spool_case(case_name: str, metrics_path: Path) -> None: + """Execute one unchanged acceptance case in its own NoRender world.""" + scenario = _SpoolReleaseCUDAScenario() + properties: dict[str, float] = {} + try: + scenario.setup_method() + getattr(scenario, case_name)(properties.__setitem__) + finally: + try: + scenario.teardown_method() + finally: + metrics_path.write_text(json.dumps(properties), encoding="utf-8") + + +def _assert_spool_case_in_subprocess( + case_name: str, + tmp_path: Path, + record_property: Callable[[str, float], None], +) -> None: + """Preserve acceptance metrics without initializing pytest's native world.""" + metrics_path = tmp_path / "acceptance_metrics.json" + script = ( + "import runpy, sys; from pathlib import Path; " + "module = runpy.run_path(sys.argv[1]); " + "module['_run_spool_case'](sys.argv[2], Path(sys.argv[3]))" + ) + result = subprocess.run( + [ + sys.executable, + "-c", + script, + str(Path(__file__).resolve()), + case_name, + str(metrics_path), + ], + capture_output=True, + text=True, + timeout=1800, + ) + if metrics_path.exists(): + for name, value in json.loads(metrics_path.read_text(encoding="utf-8")).items(): + record_property(name, value) + assert result.returncode == 0, result.stdout + result.stderr + + +class TestSpoolReleaseCUDA: + """Qualify the physical jaw sweep in a fresh NoRender process per case.""" + + def test_first_spool_release_preserves_seating_without_gripper_penetration( + self, + tmp_path: Path, + record_property: Callable[[str, float], None], + ) -> None: + _assert_spool_case_in_subprocess( + "test_first_spool_release_preserves_seating_without_gripper_penetration", + tmp_path, + record_property, + ) + + def test_third_carry_preserves_grasp_and_clears_spools( + self, + tmp_path: Path, + record_property: Callable[[str, float], None], + ) -> None: + _assert_spool_case_in_subprocess( + "test_third_carry_preserves_grasp_and_clears_spools", + tmp_path, + record_property, + ) diff --git a/tests/scripts/tools/test_prepare_yam_i2rt_assets.py b/tests/scripts/tools/test_prepare_yam_i2rt_assets.py new file mode 100644 index 0000000000..7d2352bf60 --- /dev/null +++ b/tests/scripts/tools/test_prepare_yam_i2rt_assets.py @@ -0,0 +1,236 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Verify pinned downloads and preserve existing assets on preparation failure.""" + +from __future__ import annotations + +import hashlib +import io +import zipfile +from pathlib import Path +from unittest.mock import Mock + +import pytest + +from scripts.tools import prepare_yam_i2rt_assets as preparation + + +@pytest.fixture +def source_payload(monkeypatch: pytest.MonkeyPatch) -> bytes: + payload = b"pinned MIT asset" + monkeypatch.setattr(preparation, "_SOURCE_FILES", ("LICENSE",)) + monkeypatch.setattr( + preparation, + "_EXPECTED_SHA256", + {"LICENSE": hashlib.sha256(payload).hexdigest()}, + ) + return payload + + +def test_download_uses_fixed_revision_and_verifies_payload( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch, source_payload: bytes +) -> None: + def open_asset(request, timeout): + assert request.full_url == ( + "https://raw.githubusercontent.com/robosimstudio/robosimstudio/" + "73739abe212f963e8a9f5862b27bca370df091a7/" + "assets/embodiments/yam_i2rt/LICENSE" + ) + assert timeout == 60 + return io.BytesIO(source_payload) + + monkeypatch.setattr(preparation.urllib.request, "urlopen", open_asset) + preparation._fetch_sources(tmp_path, None) + assert (tmp_path / "LICENSE").read_bytes() == source_payload + + +def test_bad_download_never_installs_a_partial_bundle( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch, source_payload: bytes +) -> None: + monkeypatch.setattr( + preparation.urllib.request, "urlopen", lambda *a, **kw: io.BytesIO(b"wrong") + ) + generate = Mock() + monkeypatch.setattr(preparation, "_generate_robot", generate) + output = tmp_path / "YamI2rt" + with pytest.raises(ValueError, match="SHA-256 mismatch"): + preparation._prepare(output, None) + generate.assert_not_called() + assert not output.exists() + assert list(tmp_path.iterdir()) == [] + + +def test_offline_sources_require_the_same_hash_without_network( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch, source_payload: bytes +) -> None: + upstream = tmp_path / "upstream" + upstream.mkdir() + (upstream / "LICENSE").write_bytes(source_payload) + download = Mock(side_effect=AssertionError("Offline mode used the network")) + monkeypatch.setattr(preparation.urllib.request, "urlopen", download) + preparation._fetch_sources(tmp_path / "staged", upstream) + download.assert_not_called() + assert (tmp_path / "staged/LICENSE").read_bytes() == source_payload + (upstream / "LICENSE").write_bytes(b"modified") + with pytest.raises(ValueError, match="SHA-256 mismatch"): + preparation._fetch_sources(tmp_path / "other", upstream) + + +def test_generated_mismatch_is_not_installed( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch, source_payload: bytes +) -> None: + upstream = tmp_path / "upstream" + upstream.mkdir() + (upstream / "LICENSE").write_bytes(source_payload) + + def wrong_output(staged): + (staged / "LICENSE").write_bytes(b"conversion damaged the license") + + monkeypatch.setattr(preparation, "_generate_robot", wrong_output) + output = tmp_path / "cache/YamI2rt" + with pytest.raises(ValueError, match="SHA-256 mismatch"): + preparation._prepare(output, upstream) + assert not output.exists() + assert list(output.parent.iterdir()) == [] + + +def test_existing_verified_bundle_is_reused_without_download_or_generation( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + (tmp_path / "yam_i2rt.urdf").write_text('') + monkeypatch.setattr( + preparation, + "_EXPECTED_SHA256", + {"yam_i2rt.urdf": preparation._sha256(tmp_path / "yam_i2rt.urdf")}, + ) + fetch, generate = Mock(), Mock() + monkeypatch.setattr(preparation, "_fetch_sources", fetch) + monkeypatch.setattr(preparation, "_generate_robot", generate) + assert preparation._prepare(tmp_path, None) == tmp_path / "yam_i2rt.urdf" + fetch.assert_not_called() + generate.assert_not_called() + + +def test_modified_existing_bundle_is_preserved( + tmp_path: Path, source_payload: bytes +) -> None: + (tmp_path / "LICENSE").write_bytes(b"local modification") + with pytest.raises(ValueError, match="SHA-256 mismatch"): + preparation._prepare(tmp_path, None) + assert (tmp_path / "LICENSE").read_bytes() == b"local modification" + + +def test_cli_returns_failure_for_an_invalid_asset_directory( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch, capsys +) -> None: + monkeypatch.setenv("EMBODICHAIN_DATA_ROOT", str(tmp_path)) + (tmp_path / "YamI2rt").mkdir() + with pytest.raises(SystemExit) as error: + preparation.main([]) + assert error.value.code == 1 + assert "Asset preparation failed" in capsys.readouterr().err + + +def test_robot_uses_cache_and_accepts_an_explicit_offline_urdf( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + from embodichain_robots.i2rt.yam import cfg as yam_i2rt + + cached = tmp_path / "YamI2rt/yam_i2rt.urdf" + requested = [] + + def resolve(path): + requested.append(path) + return str(cached) + + monkeypatch.setattr(yam_i2rt, "get_data_path", resolve) + with pytest.raises(FileNotFoundError, match="data download --name YamI2rt"): + yam_i2rt.YamI2rtCfg.from_dict({}) + custom = tmp_path / "offline.urdf" + custom.write_text('') + cfg = yam_i2rt.YamI2rtCfg.from_dict({"fpath": str(custom)}) + assert cfg.fpath == str(custom) + assert cfg._pk_urdf_path == str(custom) + assert cfg.urdf_cfg.components["arm"]["urdf_path"] == str(custom) + assert requested == ["YamI2rt/yam_i2rt.urdf"] + cached.parent.mkdir() + cached.write_text(custom.read_text()) + cfg = yam_i2rt.YamI2rtCfg.from_dict({}) + assert cfg.fpath is None + assert cfg._pk_urdf_path == str(cached) + assert cfg.urdf_cfg.components["arm"]["urdf_path"] == str(cached) + + +def test_archive_is_reproducible_and_preserves_license_and_sources( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + source = tmp_path / "robot" + (source / "assets").mkdir(parents=True) + files = { + "LICENSE": b"MIT Copyright I2RT Robotics", + "yam_i2rt.urdf": b'', + "assets/tip.obj": b"v 0 0 0\n", + } + for name, data in files.items(): + (source / name).write_bytes(data) + (source / "unrelated.log").write_text("do not publish") + monkeypatch.setattr( + preparation, + "_EXPECTED_SHA256", + {name: hashlib.sha256(data).hexdigest() for name, data in files.items()}, + ) + first = preparation._archive(source, tmp_path / "first.zip") + second = preparation._archive(source, tmp_path / "second.zip") + assert first.read_bytes() == second.read_bytes() + assert preparation._archive(source, first) == first + with zipfile.ZipFile(first) as stream: + assert stream.testzip() is None + assert stream.namelist().index("assets/") < stream.namelist().index( + "assets/tip.obj" + ) + assert set(stream.namelist()) == {*files, "assets/", "NOTICE"} + for name, data in files.items(): + assert stream.read(name) == data + assert b"I2RT Robotics" in stream.read("NOTICE") + assert b"MIT" in stream.read("NOTICE") + assert all( + info.date_time == (1980, 1, 1, 0, 0, 0) for info in stream.infolist() + ) + (source / "LICENSE").write_text("changed") + with pytest.raises(ValueError, match="SHA-256 mismatch"): + preparation._archive(source, tmp_path / "invalid.zip") + assert not (tmp_path / "invalid.zip").exists() + + +def test_archive_does_not_replace_an_existing_different_file( + tmp_path: Path, monkeypatch: pytest.MonkeyPatch +) -> None: + source = tmp_path / "robot" + source.mkdir() + (source / "yam_i2rt.urdf").write_text('') + monkeypatch.setattr( + preparation, + "_EXPECTED_SHA256", + {"yam_i2rt.urdf": preparation._sha256(source / "yam_i2rt.urdf")}, + ) + archive = tmp_path / "published.zip" + archive.write_bytes(b"already exists") + with pytest.raises(ValueError, match="Refusing to overwrite"): + preparation._archive(source, archive) + assert archive.read_bytes() == b"already exists" + with pytest.raises(ValueError, match="outside"): + preparation._archive(source, source / "invalid.zip") diff --git a/tests/sim/atomic_actions/test_actions.py b/tests/sim/atomic_actions/test_actions.py index 2f607c70fb..defd85271e 100644 --- a/tests/sim/atomic_actions/test_actions.py +++ b/tests/sim/atomic_actions/test_actions.py @@ -9407,3 +9407,85 @@ def test_axis_align_rejects_invalid_alignment_offsets(offset: torch.Tensor) -> N def test_pickup_rejects_invalid_per_row_approach(direction: torch.Tensor) -> None: with pytest.raises(ValueError, match="approach_direction"): PickUpOptions(approach_direction=direction) + + +@pytest.mark.parametrize("close_hold_duration", [0.0, 2.25 * CONTROL_DT]) +def test_cable_grasp_plan_preserves_other_arm_and_declares_no_attachment( + close_hold_duration, +) -> None: + from embodichain.lab.sim.atomic_actions.primitives.grasp_cable import ( + CableGraspGoal, + GraspCable, + GraspCableOptions, + ) + + generator = _dual_motion_generator() + action = _bind_action(generator, GraspCable()) + context = _dual_context() + context.robot.qpos[:, ARM_DOF:DUAL_ARM_DOF] = 0.37 + before = context.robot.qpos.clone() + plan = _plan_action( + action, + ActionInvocation( + skill_id="grasp_cable", + goal=CableGraspGoal("wire", 0.12, torch.eye(4)), + binding=_binding(action, motion="left_arm", grasp="left_hand"), + motion_policy=MotionPolicy(sample_count=5), + skill_options=GraspCableOptions( + close_steps=4, close_hold_duration=close_hold_duration + ), + ), + context, + ) + assert plan.plan_success.all() + assert [segment.name for segment in plan.segments] == ["descend", "close", "lift"] + torch.testing.assert_close(context.robot.qpos, before) + positions = plan.joint_trajectory.positions + close = next(segment for segment in plan.segments if segment.name == "close") + hold_steps = math.ceil(close_hold_duration / CONTROL_DT) + assert close.stop - close.start == 4 + hold_steps + closed_frames = positions[:, close.stop - hold_steps - 1 : close.stop] + torch.testing.assert_close( + closed_frames, closed_frames[:, -1:].expand_as(closed_frames) + ) + torch.testing.assert_close( + positions[:, :, ARM_DOF:DUAL_ARM_DOF], + before[:, None, ARM_DOF:DUAL_ARM_DOF].expand(-1, positions.shape[1], -1), + ) + lift = next(segment for segment in plan.segments if segment.name == "lift") + assert (lift.stop - lift.start) * CONTROL_DT <= 0.45 + assert context.task.get_held_object("left_arm") is None + projected = plan.expected_effects.apply(context.task, plan.plan_success) + assert projected.get_held_object("left_arm") is None + generator.robot.set_qpos.assert_not_called() + + +@pytest.mark.parametrize("distance", [-1.0, float("nan"), float("inf")]) +def test_cable_grasp_rejects_invalid_material_coordinate(distance) -> None: + from embodichain.lab.sim.atomic_actions.primitives.grasp_cable import CableGraspGoal + + with pytest.raises(ValueError): + CableGraspGoal("wire", distance, torch.eye(4)) + + +@pytest.mark.parametrize( + "options", + [ + {"close_steps": True}, + {"close_steps": 2.5}, + {"close_steps": 1}, + {"close_hold_duration": -0.1}, + {"close_hold_duration": float("nan")}, + {"close_hold_duration": float("inf")}, + {"close_hold_duration": True}, + {"lift_height": float("nan")}, + {"lift_height": 0.0}, + ], +) +def test_cable_grasp_rejects_invalid_options(options) -> None: + from embodichain.lab.sim.atomic_actions.primitives.grasp_cable import ( + GraspCableOptions, + ) + + with pytest.raises(ValueError): + GraspCableOptions(**options) diff --git a/tests/sim/robots/test_yam_i2rt.py b/tests/sim/robots/test_yam_i2rt.py new file mode 100644 index 0000000000..0147e88896 --- /dev/null +++ b/tests/sim/robots/test_yam_i2rt.py @@ -0,0 +1,132 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""YAM source assembly, serialization and per-arm FK/IK contracts.""" + +from __future__ import annotations + +from pathlib import Path +import xml.etree.ElementTree as ET + +import numpy as np +import pytest +import torch + +from embodichain_robots import YamI2rtCfg +from embodichain_robots.i2rt.yam import cfg as yam_i2rt + +pytestmark = pytest.mark.no_sim + + +@pytest.fixture +def arm_asset(tmp_path: Path) -> Path: + robot = ET.Element("robot", name="yam_fixture") + ET.SubElement(robot, "link", name="base_link") + for side in ("right", "left"): + parent = "base_link" + for name, kind, axis in [ + (f"{side}_base", "fixed", None), + *[(f"{side}_link{i}", "revolute", "0 0 1") for i in range(1, 7)], + (f"{side}_tcp", "fixed", None), + ]: + ET.SubElement(robot, "link", name=name) + number = name[-1] if kind == "revolute" else None + joint = ET.SubElement( + robot, + "joint", + name=f"{side}_joint{number}" if number else f"{name}_fixed", + type=kind, + ) + ET.SubElement(joint, "parent", link=parent) + ET.SubElement(joint, "child", link=name) + ET.SubElement(joint, "origin", xyz="0.1 0 0", rpy="0 0 0") + if axis: + ET.SubElement(joint, "axis", xyz=axis) + ET.SubElement( + joint, "limit", lower="-3", upper="3", effort="27", velocity="3" + ) + parent = name + path = tmp_path / "yam_i2rt.urdf" + ET.ElementTree(robot).write(path, encoding="unicode") + return path + + +def test_default_asset_is_resolved_once_and_shared_with_fk( + monkeypatch: pytest.MonkeyPatch, arm_asset: Path +) -> None: + calls = [] + + def resolve(name): + calls.append(name) + return str(arm_asset) + + monkeypatch.setattr(yam_i2rt, "get_data_path", resolve) + cfg = YamI2rtCfg.from_dict({}) + assert calls == ["YamI2rt/yam_i2rt.urdf"] + assert cfg.urdf_cfg.components["arm"]["urdf_path"] == str(arm_asset) + assert cfg._pk_urdf_path == str(arm_asset) + assert cfg.fpath is None + assert cfg.root_props.newton_gravity_compensation is None + restored = YamI2rtCfg.from_dict(cfg.to_dict()) + assert restored.urdf_cfg.components["arm"]["urdf_path"] == str(arm_asset) + np.testing.assert_array_equal( + restored.urdf_cfg.components["arm"]["transform"], np.eye(4) + ) + assert restored.urdf_cfg.assemble_urdf() == str(arm_asset) + assert restored.control_parts == cfg.control_parts + assert restored.root_props.newton_gravity_compensation is None + assert restored._pk_urdf_path == cfg._pk_urdf_path + + +def test_local_override_roundtrips_without_network( + monkeypatch: pytest.MonkeyPatch, arm_asset: Path +) -> None: + def no_download(*args): + raise AssertionError("Explicit offline assets must not download") + + monkeypatch.setattr(yam_i2rt, "get_data_path", no_download) + cfg = YamI2rtCfg.from_dict({"fpath": str(arm_asset)}) + restored = YamI2rtCfg.from_dict(cfg.to_dict()) + assert restored.fpath == str(arm_asset) + assert restored._pk_urdf_path == str(arm_asset) + assert restored.control_parts == cfg.control_parts + assert restored.root_props.newton_gravity_compensation is None + + +def test_per_arm_chain_joint_order_and_solver_fk_match(arm_asset: Path) -> None: + pytest.importorskip("pytorch_kinematics") + cfg = YamI2rtCfg.from_dict({"fpath": str(arm_asset)}) + chains = cfg.build_pk_serial_chain() + assert set(chains) == {"right_arm", "left_arm"} + qpos = torch.tensor([[0.0] * 6, [0.2, -0.3, 0.1, 0.4, -0.1, 0.2]]) + for part, chain in chains.items(): + assert chain.get_joint_parameter_names() == cfg.control_parts[part] + assert len(chain.get_joint_parameter_names()) == 6 + solver_cfg = cfg.solver_cfg[part] + solver_cfg.urdf_path = str(arm_asset) + solver_cfg.joint_names = cfg.control_parts[part] + solver = solver_cfg.init_solver(device=torch.device("cpu")) + torch.testing.assert_close( + solver.get_fk(qpos), chain.forward_kinematics(qpos).get_matrix() + ) + + +def test_catalog_preset_lowers_on_default_backend(arm_asset: Path) -> None: + from embodichain.lab.sim.spawn.descriptors import articulation_desc_from_cfg + + cfg = YamI2rtCfg.from_dict({"fpath": str(arm_asset)}) + descriptor = articulation_desc_from_cfg(cfg, newton_solver_type=None) + assert descriptor.newton_gravity_compensation == 0.0 diff --git a/tests/sim/spawn/test_descriptors.py b/tests/sim/spawn/test_descriptors.py index 48e2da9d29..09be9010e4 100644 --- a/tests/sim/spawn/test_descriptors.py +++ b/tests/sim/spawn/test_descriptors.py @@ -71,7 +71,7 @@ VolumeDeformableMeshingCfg, VolumeDeformableObjectCfg, ) -from embodichain.lab.sim.shapes import CubeCfg, LoadOption, MeshCfg +from embodichain.lab.sim.shapes import CubeCfg, CylinderCfg, LoadOption, MeshCfg from embodichain.lab.sim.objects import Articulation, RigidObject, RigidObjectGroup from embodichain.lab.sim.spawn.descriptors import ( articulation_desc_from_cfg, @@ -2576,3 +2576,29 @@ def test_rigid_descriptor_rejects_orientation_without_principal_moments() -> Non ) with pytest.raises(ValueError, match="requires explicit principal inertia"): rigid_desc_from_cfg(cfg) + + +def test_cylinder_descriptor_preserves_full_height_and_local_axis() -> None: + cfg = RigidObjectCfg.from_dict( + { + "uid": "spool", + "shape": {"shape_type": "Cylinder", "radius": 0.0125, "height": 0.017}, + } + ) + assert isinstance(cfg.shape, CylinderCfg) + descriptor, _ = rigid_desc_from_cfg(cfg, newton_solver_type="dexuni") + geometry = descriptor.collisions[0] + assert geometry.radius == pytest.approx(0.0125) + assert geometry.half_height == pytest.approx(0.0085) + assert geometry.axis == "z" + + +@pytest.mark.parametrize( + "radius,height", [(0.0, 1.0), (1.0, -0.1), (float("nan"), 1.0), (1.0, float("inf"))] +) +def test_cylinder_rejects_invalid_contact_geometry( + radius: float, height: float +) -> None: + cfg = RigidObjectCfg(uid="spool", shape=CylinderCfg(radius=radius, height=height)) + with pytest.raises(ValueError, match="finite and positive"): + rigid_desc_from_cfg(cfg, newton_solver_type="dexuni") diff --git a/tests/test_robot_package.py b/tests/test_robot_package.py index d2c0fad8e5..06b1a4a502 100644 --- a/tests/test_robot_package.py +++ b/tests/test_robot_package.py @@ -105,6 +105,8 @@ def test_main_distribution_stages_robot_package_without_legacy_modules(tmp_path) "embodichain_robots/__init__.py", "embodichain_robots/_catalog.py", "embodichain_robots/_asset_catalog.py", + "embodichain_robots/i2rt/yam/cfg.py", + "embodichain_robots/i2rt/yam/assets.py", "embodichain_robots/_locomotion.py", "embodichain_robots/locomotion_profiles.py", "embodichain_robots/franka/assets.py", diff --git a/tests/test_robot_registry.py b/tests/test_robot_registry.py index 13c3204a9b..3fc665d036 100644 --- a/tests/test_robot_registry.py +++ b/tests/test_robot_registry.py @@ -718,3 +718,32 @@ def test_franka_family_profiles_use_one_implementation(catalogs, local_robot_ass assert legacy.robot_type == "panda" and legacy.arm_model == "fr3" assert ROBOT_CONFIGS["FrankaPanda"]["supports_dual_arm"] assert ROBOT_CONFIGS["FrankaPanda"]["variant_of"] == "Franka" + + +def test_yam_catalog_resolves_offline_assets_and_independent_controls( + catalogs, tmp_path +): + """The new preset participates in the shared catalog without downloads.""" + from embodichain_robots import YamI2rtCfg + from embodichain_robots._catalog import ROBOT_CONFIGS + + catalogs.append(_entry("official", ROBOT_CONFIGS)) + urdf = tmp_path / "yam.urdf" + urdf.write_text('') + assert "YamI2rt" in registry.list_robot_cfgs() + cfg = registry.resolve_robot_cfg("YamI2rt", {"fpath": str(urdf)}) + assert isinstance(cfg, YamI2rtCfg) + assert cfg.fpath == cfg._pk_urdf_path == str(urdf) + spec = registry._robot_specs()["YamI2rt"] + controlled = {joint for joints in cfg.control_parts.values() for joint in joints} + assert spec["dof"] == len(controlled) == 16 + assert spec["control_parts"] == { + part: len(joints) for part, joints in cfg.control_parts.items() + } + assert not spec["mimic_joints"] + assert not spec["supports_dual_arm"] + assert (spec["vendor"], spec["morphology"], spec["arm_count"]) == ( + "i2rt", + "fixed_manipulator", + 2, + ) From edf83b9852830cab6c22458cee2ece52bbffbe20 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Wed, 11 Nov 2026 15:21:09 +0800 Subject: [PATCH 27/43] 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 28/43] 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) From df8e304f0aae689e76412a638e84f1bc3819b725 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Thu, 12 Nov 2026 18:40:17 +0800 Subject: [PATCH 29/43] docs(cable): record synchronized routing checks and source attribution --- NOTICE | 5 +++++ .../manipulation/cable/route_the_cable/README.md | 11 +++++++++++ 2 files changed, 16 insertions(+) diff --git a/NOTICE b/NOTICE index e2a23acf3d..33cc37edf2 100644 --- a/NOTICE +++ b/NOTICE @@ -1,6 +1,11 @@ EmbodiChain Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +The cable routing application includes geometry and routines adapted from +RoboSimStudio, Copyright 2026 Kaiyuan Chen, licensed under Apache-2.0. +Source: https://github.com/robosimstudio/robosimstudio +Revision: 73739abe212f963e8a9f5862b27bca370df091a7 + The external YAM i2rt model contains the YAM arm and linear_4310 gripper, Copyright (c) I2RT Robotics, under the MIT license. The source model directory and RoboSimStudio NOTICE identify this attribution and license. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md index 3946e8c195..dd209cb8ec 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md @@ -181,6 +181,17 @@ spool: python -m pytest tests/gym/envs/tasks/test_route_the_cable_cuda.py --run-gpu -m gpu -q ``` +The 2026-11-12 synchronization with cable framework commit `6a7a29d5d` passed +735 focused CPU tests and both existing CUDA regressions in separate NoRender +processes with seed 73. Maximum grasp slip was 6.07 mm in the first-spool case +and 3.92 mm in the third carry; the latter reported no robot/spool penetration. +The 12 mm slip and 1 mm penetration limits were retained. The local runtime +reports DexSim build `a3daf24edd88b489702c32ccffc7ffa405741ab2`, Torch +2.11.0+cu130, Newton 1.7.0 and Warp 1.17.0 on an RTX 5090. These checks cover +the first-spool release and third carry through release and retreat. Complete +channel insertion, multi-seed success rates and reset repeatability require +separate qualification. + On 2026-11-08 both CUDA regressions passed in separate NoRender processes with seed 73, DexSim `0.6.0.dev11491` (dev commit `505ee1b5600ca0b3d0a2c8235930e7684283d049`), Newton 1.7.0, MuJoCo/MuJoCo-Warp From 694ddda7d164b08dcebe7638e7a5dd2c119e018a Mon Sep 17 00:00:00 2001 From: yuecideng Date: Thu, 12 Nov 2026 19:58:03 +0800 Subject: [PATCH 30/43] feat(pickup): support contact grasps and unify the cable expert --- agent_context/MAP.yaml | 2 +- .../topics/atomic-actions/atomic-actions.md | 18 +- docs/source/api_reference/public_api.rst | 28 +- .../sim/atomic_skills/builtin_skills.md | 28 +- .../lab/sim/atomic_actions/__init__.py | 4 +- embodichain/lab/sim/atomic_skills/__init__.py | 8 +- embodichain/lab/sim/atomic_skills/policies.py | 16 +- .../sim/atomic_skills/primitives/__init__.py | 8 +- .../atomic_skills/primitives/grasp_cable.py | 241 --------- .../sim/atomic_skills/primitives/pick_up.py | 497 ++++++++++++------ .../cable/route_the_cable/README.md | 58 +- .../manipulation/cable/route_the_cable.py | 25 +- tests/sim/atomic_skills/test_actions.py | 324 ++++++++++-- 13 files changed, 717 insertions(+), 540 deletions(-) delete mode 100644 embodichain/lab/sim/atomic_skills/primitives/grasp_cable.py diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 3789123e26..f72c0374db 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -453,7 +453,7 @@ topics: - id: atomic-actions title: Atomic Skills aliases: [atomic skill, atomic skills, atomic action, 原子技能, 原子动作] - keywords: [PlanningBudget, PlanningRecovery, ExecutionPlanLifecycle, GraspCable, CableGraspGoal, GraspCableOptions, AtomicSkillEngine, run-atomic-skill, PlanningContext, ExecutionSession, ExecutionRunner, SkillInvocation, + keywords: [PlanningBudget, PlanningRecovery, ExecutionPlanLifecycle, PickUp, GraspGoal, ContactGraspGoal, AtomicSkillEngine, run-atomic-skill, PlanningContext, ExecutionSession, ExecutionRunner, SkillInvocation, hand_geometry_cfgs, GraspOpeningTarget, prepare_grasp_query, scoped grasp preparation, RecoveryPolicy, HeldObjectGuardRequest, PhaseEffectGateRequest, articulation_geometry, control-grid retiming, AffordanceSamplingContext, AffordancePoseCandidates, AffordanceSample, affordance sampling] diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index feb21a4e94..1b66fe65e5 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -96,13 +96,17 @@ gate, or held-object guard. Evidence acquisition stays outside planning and does not mutate symbolic task state; verification results are the only path that commits or invalidates observed effects. -`GraspCable` directly extends `AtomicSkill`: its caller -supplies an observed material-coordinate pinch pose, and its plan declares no -rigid attachment. An optional stationary closed-command hold belongs to the -`close` phase on the control grid; its end precedes lifting and is the routing -host's grasp-reference sampling boundary. The routing expert validates actual -lift and tool-frame material slip; numerical cable observations live in -`compute/geometry/cable.py`. +`PickUp` accepts a rigid `GraspGoal` or an explicit `ContactGraspGoal`. +The rigid path projects its existing held-object relation. Contact pickup +requires a free task-state resource, shares the approach/close/lift command +assembly and declares `EffectVerificationRequirement(kind="contact_grasp")` +without a rigid held-object update. Hosts supply physical evidence such as +material-point lift and tool-frame retention before accepting that boundary. +The routing host samples its material reference at the end of the `close` +phase, including `grasp_settle_steps`, before lifting. Native contact IK paths +use `MotionPolicy.interpolate_position_step` for lift sampling on the control +grid; joint limits can lengthen them. Numerical material-coordinate geometry +belongs to `compute/geometry/cable.py`. `MoveHeldObject` accepts ordered object-pose waypoints without rebasing the verified grasp. Its goal optionally frees only the final world yaw; default diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 243c03b0b9..3ad334b84e 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -3128,28 +3128,30 @@ its explicit hold or mask-only representation and cannot promote failed rows. PlanningRecovery PlanningBudget -embodichain.lab.sim.atomic_skills.primitives.grasp_cable --------------------------------------------------------- - -Plan a contact-only pinch, close and lift. The caller verifies measured lift -and material-point retention after execution. +embodichain.lab.sim.atomic_skills.primitives +--------------------------------------------- -.. currentmodule:: embodichain.lab.sim.atomic_skills.primitives.grasp_cable +.. currentmodule:: embodichain.lab.sim.atomic_skills.primitives .. autosummary:: - CableGraspGoal - GraspCableOptions - GraspCable + BUILTIN_SKILL_TYPES -embodichain.lab.sim.atomic_skills.primitives ---------------------------------------------- +embodichain.lab.sim.atomic_skills.primitives.pick_up +-------------------------------------------------- -.. currentmodule:: embodichain.lab.sim.atomic_skills.primitives +PickUp shares motion assembly between rigid-object pickup and contact-only +pickup. Contact goals require explicit physical verification and retain no +fixed object-to-tool relation. + +.. currentmodule:: embodichain.lab.sim.atomic_skills.primitives.pick_up .. autosummary:: - BUILTIN_SKILL_TYPES + GraspGoal + ContactGraspGoal + PickUp + PickUpOptions embodichain.lab.sim.atomic_skills.requirements ----------------------------------------------- diff --git a/docs/source/overview/sim/atomic_skills/builtin_skills.md b/docs/source/overview/sim/atomic_skills/builtin_skills.md index d487dd9b03..b585dd973b 100644 --- a/docs/source/overview/sim/atomic_skills/builtin_skills.md +++ b/docs/source/overview/sim/atomic_skills/builtin_skills.md @@ -195,7 +195,7 @@ The animations below are the runnable simulator demonstrations under |---|---|---|---|---|---| | `move_end_effector` | `EndEffectorPoseGoal` | `primary.motion` | none | none | none | | `move_joints` | `JointPositionGoal` | `primary.motion` | named target only: command matching `target` on `primary.motion` | none | none | -| `pick_up` | `GraspGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | semantic object/entity | attach object to the `primary.motion` target | +| `pick_up` | `GraspGoal` or `ContactGraspGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | semantic object or explicit contact pose | rigid held relation or terminal contact verification | | `axis_align` | `AxisAlignGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | unheld object with `AxisAlignAffordance` | open-loop pick and align while retaining the grasp | | `move_held_object` | `HeldObjectPoseGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `grasp` | object held exclusively by the `primary.motion` target | preserve attachment | | `pour` | `PourGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `grasp` | exclusively held object with `AxisAlignAffordance` | preserve attachment; open-loop rotate and return | @@ -410,17 +410,31 @@ named_goal = JointPositionGoal(target="home") ## `PickUp` -Plans **approach -> close hand -> lift** and declares the object attached to the -bound motion target. +Plans **approach -> close hand -> lift**. A rigid `GraspGoal` declares a held-object +relation for the bound motion target. `ContactGraspGoal(grasp_xpos=...)` uses an +observed TCP pose and requires physical contact verification, with no fixed +object-to-tool relation. | Contract | Value | |---|---| | Skill ID | `pick_up` | -| Goal | `GraspGoal(semantics=..., grasp_xpos=None, object_pose=None)` | +| Goal | `GraspGoal(semantics=..., grasp_xpos=None, object_pose=None)` or `ContactGraspGoal(grasp_xpos=...)` | | Binding contract | `primary.motion` plus disjoint `primary.grasp` | -| Precondition | When `object_pose` is omitted, `ObjectSemantics.entity_id` must resolve in the planning snapshot (the deprecated live `entity` fallback remains temporarily); an explicit `SceneEntityPose` must reference a snapshot entity; an `AntipodalAffordance` is required when neither an explicit grasp pose nor `fixed_object_to_eef` is supplied | -| Effect | write `HeldObjectState` for the bound motion target | -| Verification | the attachment effect must be verified during closed-loop execution | +| Precondition | Rigid goals resolve their object pose and require an affordance when no grasp is supplied; contact goals require a free task-state resource; late-bound poses resolve in the planning snapshot | +| Effect | Rigid goals write `HeldObjectState`; contact goals declare a physical verification requirement | +| Verification | Rigid held effects and contact-only grasp effects require physical verification during closed-loop execution | + +Contact goals require an unoccupied single/coordinated task-state resource. +Their plan declares `EffectVerificationRequirement(kind="contact_grasp")`, +which the host must resolve with physical evidence. The cable routing host +measures material-point lift and retention in the tool frame. Rigid-object +calibrations and downstream object targets belong to `GraspGoal` pickup. + +Contact pickup uses the existing `hand_interp_steps`, `grasp_settle_steps` and +`lift_height` options. Its native IK lift is sampled using +`MotionPolicy.interpolate_position_step` and the control period, then qualified +against joint limits. The motion-generator and external-nominal paths retain +their backend timing. Both goal types use the same effect assembly boundary. `grasp_xpos` may be `(4, 4)`, `(B, 4, 4)`, or a `SceneEntityPose`. A scene reference resolves the latest grasp pose and registers its entity as a recovery diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index b1862ba413..f17d002d33 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -30,9 +30,6 @@ class identity, registry state and exception types intact during migration. import warnings as _warnings __all__ = [ - "CableGraspGoal", - "GraspCable", - "GraspCableOptions", "DemoSkillObservation", "DemoSkillEvidenceProvider", "DemoSkillMonitor", @@ -135,6 +132,7 @@ class identity, registry state and exception types intact during migration. "FeedbackTerminalAcceptance", "GRASP_COMMAND", "GRASP_CAPABILITY", + "ContactGraspGoal", "GraspGoal", "HandOver", "HandOverGoal", diff --git a/embodichain/lab/sim/atomic_skills/__init__.py b/embodichain/lab/sim/atomic_skills/__init__.py index e18a73dec7..238d58ef43 100644 --- a/embodichain/lab/sim/atomic_skills/__init__.py +++ b/embodichain/lab/sim/atomic_skills/__init__.py @@ -188,9 +188,6 @@ WholeBodyPoseTrackingState, ) from .primitives import ( - CableGraspGoal, - GraspCable, - GraspCableOptions, AssembleGoal, AxisAlign, AxisAlignGoal, @@ -203,6 +200,7 @@ CoordinatedPlacementGoal, CoordinatedPlacementOptions, EndEffectorPoseGoal, + ContactGraspGoal, GraspGoal, HandOver, HandOverGoal, @@ -288,9 +286,6 @@ ) __all__ = [ - "CableGraspGoal", - "GraspCable", - "GraspCableOptions", "DemoSkillObservation", "DemoSkillEvidenceProvider", "DemoSkillMonitor", @@ -393,6 +388,7 @@ "FeedbackTerminalAcceptance", "GRASP_COMMAND", "GRASP_CAPABILITY", + "ContactGraspGoal", "GraspGoal", "HandOver", "HandOverGoal", diff --git a/embodichain/lab/sim/atomic_skills/policies.py b/embodichain/lab/sim/atomic_skills/policies.py index 22fb610e65..bbf51381d4 100644 --- a/embodichain/lab/sim/atomic_skills/policies.py +++ b/embodichain/lab/sim/atomic_skills/policies.py @@ -75,6 +75,13 @@ class MotionPolicy: plan_opts: PlanOptions | None = None """Optional typed planner-specific options.""" + interpolate_position_step: float = field(default=0.002, kw_only=True) + """Cartesian sampling distance in metres, forwarded to motion generation. + + PickUp contact motions also use this distance to sample their lift on the + control grid. Joint-limit qualification can lengthen the resulting path. + """ + velocity_limit: float | None = None """Optional scalar speed request, capped by effective Robot joint limits.""" @@ -205,9 +212,13 @@ def __post_init__(self) -> None: raise ValueError("velocity_targets must be auto or zero.") if self.sample_count < 2: raise ValueError("sample_count must be at least 2.") - for name in ("velocity_limit", "acceleration_limit"): + for name in ( + "velocity_limit", + "acceleration_limit", + "interpolate_position_step", + ): value = getattr(self, name) - if value is None: + if value is None and name != "interpolate_position_step": if ( name == "velocity_limit" or self.strategy == "motion_gen" @@ -301,6 +312,7 @@ def to_motion_gen_options( interpolation_dt=interpolation_dt, is_linear=cartesian_linear or cartesian_keyframes, preserve_cartesian_samples=cartesian_linear, + interpolate_position_step=self.interpolate_position_step, cartesian_path_sampler=cartesian_path_sampler, interpolation_mode=self.interpolation_mode, ik_seed_mode=self.ik_seed_mode, diff --git a/embodichain/lab/sim/atomic_skills/primitives/__init__.py b/embodichain/lab/sim/atomic_skills/primitives/__init__.py index aeb70324e1..cf6f9878a6 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/__init__.py +++ b/embodichain/lab/sim/atomic_skills/primitives/__init__.py @@ -44,7 +44,7 @@ ) from .move_joints import JointPositionGoal, MoveJoints, MoveJointsOptions from .open_door import OpenDoor, OpenDoorGoal, OpenDoorOptions -from .pick_up import GraspGoal, PickUp, PickUpOptions +from .pick_up import ContactGraspGoal, GraspGoal, PickUp, PickUpOptions from .place import AssembleGoal, Place, PlaceGoal, PlaceOptions from .press import ( Press, @@ -59,7 +59,6 @@ PushObjectOptions, PushObjectToolCalibration, ) -from .grasp_cable import CableGraspGoal, GraspCable, GraspCableOptions from .pour import Pour, PourGoal, PourOptions, PourTarget from .slide import ( Slide, @@ -71,7 +70,6 @@ BUILTIN_SKILL_TYPES: tuple[type[AtomicSkill], ...] = ( ExecuteDemoSkill, - GraspCable, MoveEndEffector, MoveJoints, PickUp, @@ -98,9 +96,6 @@ "AxisAlignGoal", "AxisAlignOptions", "BUILTIN_SKILL_TYPES", - "CableGraspGoal", - "GraspCable", - "GraspCableOptions", "CoordinatedPickGoal", "CoordinatedPickment", "CoordinatedPickmentOptions", @@ -108,6 +103,7 @@ "CoordinatedPlacementGoal", "CoordinatedPlacementOptions", "EndEffectorPoseGoal", + "ContactGraspGoal", "GraspGoal", "HandOver", "HandOverGoal", diff --git a/embodichain/lab/sim/atomic_skills/primitives/grasp_cable.py b/embodichain/lab/sim/atomic_skills/primitives/grasp_cable.py deleted file mode 100644 index 4c0e4fb9cb..0000000000 --- a/embodichain/lab/sim/atomic_skills/primitives/grasp_cable.py +++ /dev/null @@ -1,241 +0,0 @@ -# ---------------------------------------------------------------------------- -# 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. -# ---------------------------------------------------------------------------- - -"""Plan a frictional cable pinch without a rigid-object attachment effect.""" - -from __future__ import annotations - -from dataclasses import dataclass, replace -import math -from typing import ClassVar -import torch -from embodichain.lab.sim.atomic_skills.core import AtomicSkill -from embodichain.lab.sim.atomic_skills.invocation import ( - SkillOptions, - ResolvedSkillRequest, -) -from embodichain.lab.sim.atomic_skills.plans import SkillPlan -from embodichain.lab.sim.atomic_skills.state import PlanningContext -from embodichain.lab.sim.atomic_skills.bindings import JointPositionTarget -from embodichain.lab.sim.atomic_skills.requirements import ( - SkillBindingContract, - CARTESIAN_POSE_CAPABILITY, - FORWARD_KINEMATICS_CAPABILITY, -) -from embodichain.lab.sim.atomic_skills.control import ( - OPEN_COMMAND, - GRASP_COMMAND, - JointPositionCommand, -) -from embodichain.lab.sim.atomic_skills.primitives._binding_contracts import ( - make_manipulation_slot, -) -from embodichain.lab.sim.atomic_skills.trajectory_ops import ( - build_pose_plan_states, - resolve_pose_target, - to_full_robot_trajectory, -) -from embodichain.lab.sim.motion.planners.utils import interpolate_xpos_batched -from embodichain.lab.sim.atomic_skills.plans import TimedTrajectory - -__all__ = ["CableGraspGoal", "GraspCableOptions", "GraspCable"] - - -@dataclass(frozen=True, slots=True, eq=False) -class CableGraspGoal: - """Resolved pinch pose plus a stable material-coordinate identity. - - Args: - cable_uid: Cable identity retained for the caller's physical validator. - material_distance: Resolved arc length from the proximal end, in metres. - grasp_pose: ``(4,4)`` or ``(B,4,4)`` TCP pose in Robot FK's Arena-local - frame. The caller converts world cable observations before planning. - """ - - goal_kind: ClassVar[str] = "cable_grasp" - cable_uid: str - material_distance: float - grasp_pose: torch.Tensor - - def __post_init__(self) -> None: - from embodichain.lab.sim.atomic_skills.goals import validate_pose_goal - - if ( - not isinstance(self.cable_uid, str) - or not self.cable_uid - or not math.isfinite(self.material_distance) - or self.material_distance < 0 - ): - raise ValueError( - "Cable identity and finite nonnegative material distance are required." - ) - validate_pose_goal(self.grasp_pose, "grasp_pose", allow_waypoints=False) - - -@dataclass(frozen=True, slots=True, eq=False) -class GraspCableOptions(SkillOptions): - """Pinch dwell and world-Z lift behavior; timing grid is engine-owned.""" - - close_steps: int = 48 - close_hold_duration: float = 0.0 - """Seconds to maintain the closed command before lifting. - - Rounded up to the control grid and included in the ``close`` phase. - """ - lift_height: float = 0.0185 - lift_speed: float = 0.05 - """World-Z lift speed in metres per second, sampled on the control grid.""" - - def __post_init__(self) -> None: - if ( - isinstance(self.close_steps, bool) - or not isinstance(self.close_steps, int) - or self.close_steps < 2 - or isinstance(self.close_hold_duration, bool) - or not math.isfinite(self.close_hold_duration) - or self.close_hold_duration < 0 - or not math.isfinite(self.lift_height) - or self.lift_height <= 0 - or not math.isfinite(self.lift_speed) - or self.lift_speed <= 0 - ): - raise ValueError( - "Cable grasp requires finite positive lift height/speed, " - "nonnegative close_hold_duration and integer close_steps >= 2." - ) - - -class GraspCable(AtomicSkill[CableGraspGoal, GraspCableOptions]): - """Descend, close and lift using contact alone. - - The caller supplies a fresh cable-derived pose and verifies material-point - lift/slip after execution. Planning does not step physics or assert a grasp. - """ - - skill_id: ClassVar[str] = "grasp_cable" - GoalType: ClassVar[type] = CableGraspGoal - OptionsType: ClassVar[type] = GraspCableOptions - binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( - slots=( - make_manipulation_slot( - "primary", - motion_capabilities=frozenset( - {CARTESIAN_POSE_CAPABILITY, FORWARD_KINEMATICS_CAPABILITY} - ), - grasp_commands={ - OPEN_COMMAND: JointPositionCommand, - GRASP_COMMAND: JointPositionCommand, - }, - ), - ) - ) - - def _plan( - self, - request: ResolvedSkillRequest[CableGraspGoal, GraspCableOptions], - context: PlanningContext, - ) -> SkillPlan: - goal = self.require_goal(request) - motion = request.binding.endpoint("primary", "motion").require_target( - JointPositionTarget - ) - hand = request.binding.endpoint("primary", "grasp") - fingers = list(hand.require_target(JointPositionTarget).joint_ids) - ids = list(motion.joint_ids) - open_q = hand.joint_positions( - OPEN_COMMAND, - num_envs=context.batch_size, - device=self.device, - dtype=context.robot.qpos.dtype, - ) - closed_q = hand.joint_positions( - GRASP_COMMAND, - num_envs=context.batch_size, - device=self.device, - dtype=context.robot.qpos.dtype, - ) - target = resolve_pose_target( - goal.grasp_pose, num_envs=context.batch_size, device=self.device - ).to(context.robot.qpos) - - def move(start_full: torch.Tensor, end: torch.Tensor, *, sample_count: int): - start = self.robot.compute_fk( - start_full[:, ids], name=motion.control_part, to_matrix=True - ) - poses = interpolate_xpos_batched(start, end, sample_count)[:, 1:] - result = self.motion_generator.generate( - build_pose_plan_states(poses), - options=replace( - request.motion_policy, sample_count=sample_count - ).to_motion_gen_options( - start_qpos=start_full[:, ids], - control_part=motion.control_part, - interpolation_dt=context.require_control_dt(), - cartesian_linear=True, - ), - ) - success, trajectory = to_full_robot_trajectory( - result, - base_qpos=start_full, - joint_ids=ids, - env_ids=context.env_ids, - control_dt=context.require_control_dt(), - ) - return success, trajectory.positions.clone() - - ok1, down = move( - context.robot.qpos, target, sample_count=request.motion_policy.sample_count - ) - down[:, :, fingers] = open_q[:, None, :] - close = down[:, -1:].expand(-1, request.skill_options.close_steps, -1).clone() - weights = torch.linspace( - 0, 1, request.skill_options.close_steps, device=self.device - )[None, :, None] - close[:, :, fingers] = ( - open_q[:, None, :] * (1 - weights) + closed_q[:, None, :] * weights - ) - hold_steps = math.ceil( - request.skill_options.close_hold_duration / context.require_control_dt() - ) - if hold_steps: - close = torch.cat((close, close[:, -1:].expand(-1, hold_steps, -1)), dim=1) - lifted = target.clone() - lifted[:, 2, 3] += request.skill_options.lift_height - lift_samples = max( - 2, - math.ceil( - request.skill_options.lift_height - / (request.skill_options.lift_speed * context.require_control_dt()) - ) - + 1, - ) - ok2, up = move(close[:, -1], lifted, sample_count=lift_samples) - up[:, :, fingers] = closed_q[:, None, :] - return self.build_plan( - request, - context, - success=ok1 & ok2, - trajectory=TimedTrajectory.from_uniform_step( - torch.cat((down, close, up), 1), - env_ids=context.env_ids, - step_dt=context.require_control_dt(), - ), - segment_lengths={ - "descend": down.shape[1], - "close": close.shape[1], - "lift": up.shape[1], - }, - ) diff --git a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py index 9c3a5f7822..3851789e4f 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py @@ -24,6 +24,7 @@ import torch +from embodichain.lab.sim.motion.planners.utils import interpolate_xpos_batched from embodichain.utils import logger from embodichain.utils.math import ( axis_angle_to_rotation_matrix, @@ -65,6 +66,7 @@ from embodichain.lab.sim.atomic_skills.nominal import NominalMotionSource from embodichain.lab.sim.atomic_skills.plans import ( SkillPlan, + EffectVerificationRequirement, PlannerDiagnostics, TimedTrajectory, normalize_success_mask, @@ -83,6 +85,7 @@ resolve_pose_target, split_three_segments, translate_pose_world, + to_full_robot_trajectory, ) from embodichain.lab.sim.atomic_skills.primitives._binding_contracts import ( make_manipulation_slot, @@ -128,6 +131,27 @@ def __post_init__(self) -> None: validate_pose_goal(self.grasp_xpos, "grasp_xpos", allow_waypoints=False) +@dataclass(frozen=True, slots=True, eq=False) +class ContactGraspGoal: + """Contact-only pickup at an observed TCP pose in the planning frame. + + Args: + grasp_xpos: Explicit or late-bound grasp pose. The host supplies physical + evidence that the target remains pinched during lift. The goal does + not project a fixed object-to-tool relationship. + """ + + goal_kind: ClassVar[str] = "contact_grasp" + grasp_xpos: PoseGoalValue + + def __post_init__(self) -> None: + validate_pose_goal(self.grasp_xpos, "grasp_xpos", allow_waypoints=False) + if isinstance(self.grasp_xpos, torch.Tensor) and not bool( + torch.isfinite(self.grasp_xpos).all() + ): + raise ValueError("grasp_xpos must be finite.") + + def _validate_single_se3(value: torch.Tensor, name: str) -> None: """Validate one finite, proper SE(3) transform.""" validate_pose_goal(value, name, allow_waypoints=False) @@ -307,11 +331,11 @@ def __post_init__(self) -> None: ) -class PickUp(AtomicSkill[GraspGoal, PickUpOptions]): +class PickUp(AtomicSkill[GraspGoal | ContactGraspGoal, PickUpOptions]): """Approach a grasp pose, close the gripper, lift.""" skill_id: ClassVar[str] = "pick_up" - GoalType: ClassVar[type] = GraspGoal + GoalType: ClassVar[tuple[type, ...]] = (GraspGoal, ContactGraspGoal) OptionsType: ClassVar[type] = PickUpOptions binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( @@ -334,10 +358,12 @@ class PickUp(AtomicSkill[GraspGoal, PickUpOptions]): def _scene_dependencies( self, - request: ResolvedSkillRequest[GraspGoal, PickUpOptions], + request: ResolvedSkillRequest[GraspGoal | ContactGraspGoal, PickUpOptions], ) -> tuple[str, ...]: """Monitor the acquired object and late-bound downstream targets.""" dependencies = set(super()._scene_dependencies(request)) + if isinstance(request.goal, ContactGraspGoal): + return tuple(sorted(dependencies)) entity_id = request.goal.semantics.entity_id # An explicit object pose is a scene-independent planning input. Do # not retain the semantic entity as a dynamic dependency in that @@ -364,6 +390,9 @@ def _get_full_pickup_trajectory( hand_open_qpos: torch.Tensor, hand_grasp_qpos: torch.Tensor, interpolation_dt: float, + *, + contact_grasp: bool = False, + env_ids: torch.Tensor | None = None, ) -> tuple[torch.Tensor, torch.Tensor, dict[str, int]]: if options.approach_along_grasp_axis: approach_direction = grasp_xpos[:, :3, 2] @@ -371,58 +400,149 @@ def _get_full_pickup_trajectory( grasp_xpos, -approach_direction * options.pre_grasp_distance ) - n_approach, n_close, n_lift = split_three_segments( - motion_policy.sample_count, - options.hand_interp_steps, - first_segment_name="approach", - third_segment_name="lift", - ) + if contact_grasp and motion_policy.strategy == "ik_interp": - lift_xpos = translate_pose_world( - grasp_xpos, - torch.tensor([0, 0, 1], device=self.device) * options.lift_height, - ) - motion_options = motion_policy.to_motion_gen_options( - start_qpos=start_arm_qpos, - control_part=manipulator.control_part, - sample_count=n_approach + n_lift, - interpolation_dt=interpolation_dt, - ) - if motion_policy.strategy == "motion_gen": - # Keep the native combined path for the pose-based split. The - # motion generator resolves backend defaults when no explicit - # planner options were supplied. - motion_options.sample_count = None - motion_result = self.motion_generator.generate( - build_pose_plan_states( - torch.stack([pre_grasp_xpos, grasp_xpos, lift_xpos], dim=1) - ), - options=motion_options, - ) - assert isinstance(motion_result.success, torch.Tensor) - assert motion_result.positions is not None - assert motion_result.dt is not None - approach_arm, lift_arm = split_joint_trajectory_at_pose( - motion_result.positions, - grasp_xpos, - robot=self.robot, - control_part=manipulator.control_part, - first_sample_count=n_approach, - second_sample_count=n_lift, - dt=motion_result.dt, - control_dt=interpolation_dt, - arrival_indices=motion_result.waypoint_indices, - split_target_index=1, - env_ids=self.planning_services.env_ids, - ) - n_approach = approach_arm.shape[1] - n_lift = lift_arm.shape[1] - grasp_arm_qpos = approach_arm[:, -1, :] - is_success = motion_result.success + def sampled_move( + start_full: torch.Tensor, target_pose: torch.Tensor, sample_count: int + ) -> tuple[torch.Tensor, torch.Tensor]: + arm_ids = list(manipulator.joint_ids) + start_pose = self.robot.compute_fk( + start_full[:, arm_ids], + name=manipulator.control_part, + to_matrix=True, + **( + {"env_ids": self.planning_services.env_ids} + if self.planning_services.env_ids is not None + else {} + ), + ) + poses = interpolate_xpos_batched(start_pose, target_pose, sample_count)[ + :, 1: + ] + result = self.motion_generator.generate( + build_pose_plan_states(poses), + options=replace( + motion_policy, sample_count=sample_count + ).to_motion_gen_options( + start_qpos=start_full[:, arm_ids], + control_part=manipulator.control_part, + interpolation_dt=interpolation_dt, + cartesian_linear=True, + ), + ) + valid, trajectory = to_full_robot_trajectory( + result, + base_qpos=start_full, + joint_ids=arm_ids, + env_ids=env_ids, + control_dt=interpolation_dt, + ) + return valid, trajectory.positions[:, :, arm_ids] - hand_close_path = interpolate_hand_qpos( - hand_open_qpos, hand_grasp_qpos, n_waypoints=n_close - ) + approach_start = last_qpos + prefix = None + prefix_valid = torch.ones( + self.num_envs, dtype=torch.bool, device=self.device + ) + if options.pre_grasp_distance > 0: + prefix_valid, prefix = sampled_move( + approach_start, pre_grasp_xpos, motion_policy.sample_count + ) + approach_start = last_qpos.clone() + approach_start[:, list(manipulator.joint_ids)] = prefix[:, -1] + is_success, approach_arm = sampled_move( + approach_start, grasp_xpos, motion_policy.sample_count + ) + if prefix is not None: + approach_arm = torch.cat((prefix, approach_arm), dim=1) + lift_start_qpos = last_qpos.clone() + lift_start_qpos[:, list(manipulator.joint_ids)] = approach_arm[:, -1] + lift_start_qpos[:, list(end_effector.joint_ids)] = hand_grasp_qpos + lift_xpos = translate_pose_world( + grasp_xpos, grasp_xpos.new_tensor([0, 0, options.lift_height]) + ) + lift_samples = max( + 2, + math.ceil(options.lift_height / motion_policy.interpolate_position_step) + + 1, + ) + lift_success, lift_arm = sampled_move( + lift_start_qpos, lift_xpos, lift_samples + ) + is_success = prefix_valid & is_success & lift_success + n_approach, n_close, n_lift = ( + approach_arm.shape[1], + options.hand_interp_steps, + lift_arm.shape[1], + ) + grasp_arm_qpos = approach_arm[:, -1] + else: + n_approach, n_close, n_lift = split_three_segments( + motion_policy.sample_count, + options.hand_interp_steps, + first_segment_name="approach", + third_segment_name="lift", + ) + + lift_xpos = translate_pose_world( + grasp_xpos, + torch.tensor([0, 0, 1], device=self.device) * options.lift_height, + ) + motion_options = motion_policy.to_motion_gen_options( + start_qpos=start_arm_qpos, + control_part=manipulator.control_part, + sample_count=n_approach + n_lift, + interpolation_dt=interpolation_dt, + ) + if motion_policy.strategy == "motion_gen": + # Keep the native combined path for the pose-based split. The + # motion generator resolves backend defaults when no explicit + # planner options were supplied. + motion_options.sample_count = None + motion_result = self.motion_generator.generate( + build_pose_plan_states( + torch.stack([pre_grasp_xpos, grasp_xpos, lift_xpos], dim=1) + ), + options=motion_options, + ) + assert isinstance(motion_result.success, torch.Tensor) + assert motion_result.positions is not None + assert motion_result.dt is not None + approach_arm, lift_arm = split_joint_trajectory_at_pose( + motion_result.positions, + grasp_xpos, + robot=self.robot, + control_part=manipulator.control_part, + first_sample_count=n_approach, + second_sample_count=n_lift, + dt=motion_result.dt, + control_dt=interpolation_dt, + arrival_indices=motion_result.waypoint_indices, + split_target_index=1, + env_ids=self.planning_services.env_ids, + ) + n_approach = approach_arm.shape[1] + n_lift = lift_arm.shape[1] + grasp_arm_qpos = approach_arm[:, -1, :] + is_success = motion_result.success + + if contact_grasp: + # Keep endpoint-weighted contact commands on their original grid. + weights = ( + torch.linspace( + 0, 1, n_close, device=self.device, dtype=hand_open_qpos.dtype + ) + if n_close > 1 + else torch.ones(1, device=self.device, dtype=hand_open_qpos.dtype) + )[None, :, None] + hand_close_path = ( + hand_open_qpos[:, None] * (1 - weights) + + hand_grasp_qpos[:, None] * weights + ) + else: + hand_close_path = interpolate_hand_qpos( + hand_open_qpos, hand_grasp_qpos, n_waypoints=n_close + ) n_settle = options.grasp_settle_steps close_start = n_approach settle_start = close_start + n_close @@ -445,7 +565,9 @@ def _get_full_pickup_trajectory( n_waypoints=n_approach, ) full[:, :n_approach, arm_joint_ids] = approach_arm - full[:, :n_approach, hand_joint_ids] = approach_hand + full[:, :n_approach, hand_joint_ids] = ( + hand_open_qpos.unsqueeze(1) if contact_grasp else approach_hand + ) full[:, close_start:settle_start, arm_joint_ids] = grasp_arm_qpos.unsqueeze(1) full[:, close_start:settle_start, hand_joint_ids] = hand_close_path if n_settle: @@ -469,11 +591,13 @@ def _get_full_pickup_trajectory( def _plan( self, - request: ResolvedSkillRequest[GraspGoal, PickUpOptions], + request: ResolvedSkillRequest[GraspGoal | ContactGraspGoal, PickUpOptions], context: PlanningContext, ) -> SkillPlan: """Plan approach, close, and lift segments without committing attachment.""" target = self.require_goal(request) + if isinstance(target, ContactGraspGoal): + self._validate_contact_goal(request, context) options = replace( request.skill_options, downstream_object_target_poses=tuple( @@ -521,76 +645,85 @@ def _plan( dtype=context.robot.qpos.dtype, ) state = context - sem = target.semantics - if target.object_pose is None: - object_pose = _resolve_object_pose( - sem, - context, - name="pickup_object_pose", - ) - else: - object_pose = resolve_pose_target( - resolve_pose_goal( - target.object_pose, - context, - name="object_pose", - ), + start_arm_qpos = arm_qpos_from_state(state, list(manipulator.joint_ids)) + if isinstance(target, ContactGraspGoal): + object_pose = None + affordance_sample = None + grasp_xpos = resolve_pose_target( + resolve_pose_goal(target.grasp_xpos, context, name="grasp_xpos"), num_envs=self.num_envs, device=self.device, ) - if ( - target.grasp_xpos is None - and options.fixed_object_to_eef is None - and not isinstance(sem.affordance, AntipodalAffordance) - ): - raise ValueError( - "PickUp requires an AntipodalAffordance when neither grasp_xpos " - "nor fixed_object_to_eef is set." - ) - start_arm_qpos = arm_qpos_from_state( - state, - list(manipulator.joint_ids), - ) - affordance_sample: AffordanceSample | None = None - if target.grasp_xpos is None: - if options.fixed_object_to_eef is None: - affordance_sample = self._resolve_grasp_pose( + is_success = torch.ones(self.num_envs, dtype=torch.bool, device=self.device) + else: + sem = target.semantics + if target.object_pose is None: + object_pose = _resolve_object_pose( sem, - object_pose, - start_arm_qpos, - manipulator, - end_effector.target_id, - options, - approach_direction, context, - sample_key=(request.invocation_id or self.skill_id) + ":grasp", + name="pickup_object_pose", ) - is_success = affordance_sample.success - grasp_xpos = affordance_sample.poses else: - object_to_eef = options.fixed_object_to_eef.to( + object_pose = resolve_pose_target( + resolve_pose_goal( + target.object_pose, + context, + name="object_pose", + ), + num_envs=self.num_envs, device=self.device, - dtype=object_pose.dtype, ) - grasp_xpos = torch.matmul(object_pose, object_to_eef) - is_success = torch.ones( - self.num_envs, - dtype=torch.bool, + if ( + target.grasp_xpos is None + and options.fixed_object_to_eef is None + and not isinstance(sem.affordance, AntipodalAffordance) + ): + raise ValueError( + "PickUp requires an AntipodalAffordance when neither grasp_xpos " + "nor fixed_object_to_eef is set." + ) + affordance_sample: AffordanceSample | None = None + if target.grasp_xpos is None: + if options.fixed_object_to_eef is None: + affordance_sample = self._resolve_grasp_pose( + sem, + object_pose, + start_arm_qpos, + manipulator, + end_effector.target_id, + options, + approach_direction, + context, + sample_key=(request.invocation_id or self.skill_id) + ":grasp", + ) + is_success = affordance_sample.success + grasp_xpos = affordance_sample.poses + else: + object_to_eef = options.fixed_object_to_eef.to( + device=self.device, + dtype=object_pose.dtype, + ) + grasp_xpos = torch.matmul(object_pose, object_to_eef) + is_success = torch.ones( + self.num_envs, + dtype=torch.bool, + device=self.device, + ) + else: + grasp_xpos = resolve_pose_target( + resolve_pose_goal(target.grasp_xpos, context, name="grasp_xpos"), + num_envs=self.num_envs, device=self.device, ) - else: - grasp_xpos = resolve_pose_target( - resolve_pose_goal(target.grasp_xpos, context, name="grasp_xpos"), - num_envs=self.num_envs, - device=self.device, - ) - if options.rotate_upright is not None: - grasp_xpos = self._upright_adjusted_grasp_poses( - grasp_xpos, - object_pose, - options, + if options.rotate_upright is not None: + grasp_xpos = self._upright_adjusted_grasp_poses( + grasp_xpos, + object_pose, + options, + ) + is_success = torch.ones( + self.num_envs, dtype=torch.bool, device=self.device ) - is_success = torch.ones(self.num_envs, dtype=torch.bool, device=self.device) grasp_success = normalize_success_mask( is_success, num_envs=self.num_envs, @@ -615,6 +748,8 @@ def _plan( hand_open_qpos, hand_grasp_qpos, context.require_control_dt(), + contact_grasp=isinstance(target, ContactGraspGoal), + env_ids=context.env_ids, ) success_mask = grasp_success & normalize_success_mask( trajectory_success, @@ -639,14 +774,49 @@ def _plan( provenance=None, ) + def _validate_contact_goal( + self, + request: ResolvedSkillRequest[GraspGoal | ContactGraspGoal, PickUpOptions], + context: PlanningContext, + ) -> None: + options = request.skill_options + if ( + options.fixed_object_to_eef is not None + or options.downstream_object_target_poses + or options.rotate_upright is not None + or options.obj_upright_direction is not None + ): + raise ValueError( + "Contact pickup requires an explicit grasp pose without rigid-object options." + ) + motion = request.binding.endpoint("primary", "motion") + hand = request.binding.endpoint("primary", "grasp") + key = require_shared_task_state_key( + motion, hand, participant="PickUp primary participant" + ) + held = context.task.get_held_object(key) + occupied = held is not None and ( + held.env_mask is None or bool(held.env_mask.any()) + ) + coordinated = any( + key in pair and (value.env_mask is None or bool(value.env_mask.any())) + for pair, value in context.coordinated_held_objects.items() + ) + if occupied or coordinated: + raise ValueError( + "Contact pickup requires an unoccupied task-state resource." + ) + def _plan_from_nominal( self, - request: ResolvedSkillRequest[GraspGoal, PickUpOptions], + request: ResolvedSkillRequest[GraspGoal | ContactGraspGoal, PickUpOptions], context: PlanningContext, nominal_source: NominalMotionSource, ) -> SkillPlan: """Assemble a Pick from an explicitly supplied external motion.""" target = self.require_goal(request) + if isinstance(target, ContactGraspGoal): + self._validate_contact_goal(request, context) motion = self._validate_nominal_motion( request, context, nominal_source, segments=("approach", "close", "lift") ) @@ -699,27 +869,36 @@ def _plan_from_nominal( raise ValueError("external Pick grasp FK is unavailable") else: grasp_xpos = motion.grasp_pose.to(context.robot.qpos) - if target.object_pose is None: - object_pose = _resolve_object_pose( - target.semantics, context, name="pickup_object_pose" - ) - else: - object_pose = resolve_pose_target( - resolve_pose_goal(target.object_pose, context, name="object_pose"), - num_envs=context.batch_size, - device=self.device, - ) - expected_grasp = None - if target.grasp_xpos is not None: + if isinstance(target, ContactGraspGoal): + object_pose = None expected_grasp = resolve_pose_target( resolve_pose_goal(target.grasp_xpos, context, name="grasp_xpos"), - num_envs=context.batch_size, + num_envs=self.num_envs, device=self.device, ) - elif request.skill_options.fixed_object_to_eef is not None: - expected_grasp = object_pose @ request.skill_options.fixed_object_to_eef.to( - object_pose - ) + else: + if target.object_pose is None: + object_pose = _resolve_object_pose( + target.semantics, context, name="pickup_object_pose" + ) + else: + object_pose = resolve_pose_target( + resolve_pose_goal(target.object_pose, context, name="object_pose"), + num_envs=context.batch_size, + device=self.device, + ) + expected_grasp = None + if target.grasp_xpos is not None: + expected_grasp = resolve_pose_target( + resolve_pose_goal(target.grasp_xpos, context, name="grasp_xpos"), + num_envs=context.batch_size, + device=self.device, + ) + elif request.skill_options.fixed_object_to_eef is not None: + expected_grasp = ( + object_pose + @ request.skill_options.fixed_object_to_eef.to(object_pose) + ) if expected_grasp is not None and not torch.allclose( grasp_xpos, expected_grasp, atol=2e-3, rtol=0 ): @@ -752,13 +931,13 @@ def _plan_from_nominal( def _assemble_pick_plan( self, - request: ResolvedSkillRequest[GraspGoal, PickUpOptions], + request: ResolvedSkillRequest[GraspGoal | ContactGraspGoal, PickUpOptions], context: PlanningContext, *, success: torch.Tensor, trajectory: TimedTrajectory, grasp_xpos: torch.Tensor, - object_pose: torch.Tensor, + object_pose: torch.Tensor | None, segment_lengths: dict[str, int], affordance_sample: AffordanceSample | None, replannable: bool, @@ -772,31 +951,45 @@ def _assemble_pick_plan( task_state_key = require_shared_task_state_key( motion, grasp, participant="PickUp primary participant" ) - sem = target.semantics - state = context - object_to_eef = torch.bmm(pose_inv(object_pose), grasp_xpos) - held = HeldObjectState( - semantics=sem, object_to_eef=object_to_eef, grasp_xpos=grasp_xpos - ) - coordinated_updates = { - key: None for key in state.coordinated_held_objects if task_state_key in key - } - if target.object_pose is None: - monitored_object_id = sem.entity_id - elif isinstance(target.object_pose, SceneEntityPose): - monitored_object_id = target.object_pose.entity_id + requirement = None + if isinstance(target, ContactGraspGoal): + effects = StateDelta() + monitored_object_id = ( + target.grasp_xpos.entity_id + if isinstance(target.grasp_xpos, SceneEntityPose) + else None + ) + requirement = EffectVerificationRequirement(kind="contact_grasp") else: - monitored_object_id = None + sem = target.semantics + state = context + object_to_eef = torch.bmm(pose_inv(object_pose), grasp_xpos) + held = HeldObjectState( + semantics=sem, object_to_eef=object_to_eef, grasp_xpos=grasp_xpos + ) + coordinated_updates = { + key: None + for key in state.coordinated_held_objects + if task_state_key in key + } + if target.object_pose is None: + monitored_object_id = sem.entity_id + elif isinstance(target.object_pose, SceneEntityPose): + monitored_object_id = target.object_pose.entity_id + else: + monitored_object_id = None + effects = StateDelta( + held_object_updates={task_state_key: held}, + coordinated_held_object_updates=coordinated_updates, + ) return self.build_plan( request, context, success=success, trajectory=trajectory, replannable=replannable, - expected_effects=StateDelta( - held_object_updates={task_state_key: held}, - coordinated_held_object_updates=coordinated_updates, - ), + expected_effects=effects, + effect_verification=requirement, diagnostics=PlannerDiagnostics( backend=( self.planning_services.planner_name @@ -1118,4 +1311,4 @@ def _upright_adjusted_grasp_poses( return adjusted_grasp_xpos -__all__ = ["GraspGoal", "PickUp", "PickUpOptions"] +__all__ = ["GraspGoal", "ContactGraspGoal", "PickUp", "PickUpOptions"] diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md index dd209cb8ec..96aa2ce05a 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md @@ -128,9 +128,11 @@ stroke beside the first spool sweeps a finger through its rim. These explicit waypoints are task geometry: joint interpolation does not provide obstacle avoidance, and contact response is not a substitute for a clear commanded path. -- `GraspCable`, `CableGraspGoal` and `GraspCableOptions` are registered Atomic - Skills. Planning generates descend/close/lift joint commands on the control - grid and declares no rigid-object attachment effect. +- `PickUp` accepts `ContactGraspGoal` for the observed pinch pose. It shares + approach/close/lift command assembly with rigid pickup and requests physical + contact verification. The routing host verifies material-point lift and slip. + `PickUpOptions` retains the 48-frame close, 0.35-second closed hold and + 18.5 mm lift; `MotionPolicy` samples the lift at the previous 0.05 m/s grid. - `compute.geometry.cable` owns simulator-independent capsule endpoints, material-point observations and TCP-relative slip measurement. - `RouteTheCableEnv.create_demo_segments()` owns feedback, staged execution and @@ -181,40 +183,22 @@ spool: python -m pytest tests/gym/envs/tasks/test_route_the_cable_cuda.py --run-gpu -m gpu -q ``` -The 2026-11-12 synchronization with cable framework commit `6a7a29d5d` passed -735 focused CPU tests and both existing CUDA regressions in separate NoRender -processes with seed 73. Maximum grasp slip was 6.07 mm in the first-spool case -and 3.92 mm in the third carry; the latter reported no robot/spool penetration. -The 12 mm slip and 1 mm penetration limits were retained. The local runtime -reports DexSim build `a3daf24edd88b489702c32ccffc7ffa405741ab2`, Torch -2.11.0+cu130, Newton 1.7.0 and Warp 1.17.0 on an RTX 5090. These checks cover -the first-spool release and third carry through release and retreat. Complete -channel insertion, multi-seed success rates and reset repeatability require -separate qualification. - -On 2026-11-08 both CUDA regressions passed in separate NoRender processes with -seed 73, DexSim `0.6.0.dev11491` (dev commit -`505ee1b5600ca0b3d0a2c8235930e7684283d049`), Newton 1.7.0, MuJoCo/MuJoCo-Warp -3.15.0, Python 3.12 and an RTX 5090 D v2. Maximum grasp slip was 3.32 mm in -the first-spool test and 2.81 mm during the third carry, with no reported -robot/spool penetration during the third carry and retreat. The original -12 mm slip and 1 mm penetration limits were unchanged. These historical measurements used an internal wheel. The current runtime -installation follows the published DexSim dependency and locks inherited from -the cable framework. These tests qualify the first-spool and -third-carry stages, not the later channel insertion or complete routing episode. - -The 2026-10-21 CUDA regressions with seed 73 passed for the first-spool -release and the third carry through release and retreat. Maximum measured -grasp slip was 8.72 mm in the first-spool test and 9.60 mm during the third -carry, with no reported robot/spool penetration during the third carry and -retreat. This qualifies the retained 50 mm third-carry change through that -intermediate stage only. Earlier 2026-10-22 development reruns passed both CUDA -regressions, but qualification of that older branch failed both routing regressions at -`carry_0`. A separate fresh-process full collector trial at seed -73 rejected `carry_0` at 12.035 mm slip and 67.38 mm TCP error. Full-episode -reliability remains unqualified, particularly later channel insertion; the -new development-build regressions above do not qualify the complete routing demo. -Success rates across seeds and reset-based batches remain unqualified. +The contact-pickup migration is covered by 1349 CPU tests spanning the Atomic +Skill and semantic-planning contracts and by both existing NoRender CUDA +regressions with seed 73. Maximum grasp slip was 5.26 mm in the first-spool +case and 4.37 mm in the third carry, with no reported robot/spool penetration +in the latter. The original 12 mm slip and 1 mm penetration limits are retained. +The local runtime reports DexSim build `a3daf24edd88b489702c32ccffc7ffa405741ab2`, +Torch 2.11.0+cu130, Newton 1.7.0 and Warp 1.17.0 on an RTX 5090. + +The synchronized FastRT comparison runs the old `GraspCable` implementation at +`df8e304f0` and the unified `PickUp(ContactGraspGoal)` with identical seed 73, +physical configuration, camera and control settings. Its old run stops during +the left-hand pinch at 12.28 mm episode slip; the new run reaches final settling +with 6.96 mm slip but fails the strict port predicate after release. Both report +no robot/spool penetration. These are individual full-task trials. Complete +channel insertion, multi-seed success rates and repeated-reset reliability +require further work. Recordings contain full robot commands and `states.cables.cable.segment_state_w`, shaped `(environments, time, 50, 13)` in diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/route_the_cable.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/route_the_cable.py index dd1d83bac1..25c889593c 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/route_the_cable.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/route_the_cable.py @@ -51,9 +51,9 @@ MotionPolicy, SceneSnapshot, ) -from embodichain.lab.sim.atomic_skills.primitives.grasp_cable import ( - CableGraspGoal, - GraspCableOptions, +from embodichain.lab.sim.atomic_skills.primitives.pick_up import ( + ContactGraspGoal, + PickUpOptions, ) from embodichain.lab.sim.atomic_skills.trajectory_ops import build_pose_plan_states from embodichain.lab.sim.motion.motion_generator import ( @@ -626,20 +626,25 @@ def _pinch( speed=0.12, ) before = self._point(distance)[0].clone() - goal = CableGraspGoal( - "cable", distance, self._grasp_pose(distance, side, 0.0115, port=port) - ) + goal = ContactGraspGoal(self._grasp_pose(distance, side, 0.0115, port=port)) binding = self._engine.bind_control_parts( - "grasp_cable", + "pick_up", {"primary": {"motion": side + "_arm", "grasp": side + "_eef"}}, ) plan = self._engine.plan( SkillInvocation( - skill_id="grasp_cable", + skill_id="pick_up", goal=goal, binding=binding, - motion_policy=MotionPolicy(sample_count=73), - skill_options=GraspCableOptions(close_hold_duration=0.35), + motion_policy=MotionPolicy( + sample_count=73, interpolate_position_step=0.05 * self.step_dt + ), + skill_options=PickUpOptions( + hand_interp_steps=48, + grasp_settle_steps=math.ceil(0.35 / self.step_dt), + lift_height=0.0185, + pre_grasp_distance=0.0, + ), control_overrides=( SkillControlOverrides( endpoints={ diff --git a/tests/sim/atomic_skills/test_actions.py b/tests/sim/atomic_skills/test_actions.py index be3348fa1f..af9c80e93e 100644 --- a/tests/sim/atomic_skills/test_actions.py +++ b/tests/sim/atomic_skills/test_actions.py @@ -945,7 +945,9 @@ def _plan_segment_contract_case(case_id: str) -> SkillPlan: def test_builtin_descriptors_expose_strict_goal_types() -> None: assert MoveEndEffector.GoalType is EndEffectorPoseGoal assert MoveJoints.GoalType is JointPositionGoal - assert PickUp.GoalType is GraspGoal + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + + assert PickUp.GoalType == (GraspGoal, ContactGraspGoal) assert AxisAlign.GoalType is AxisAlignGoal assert MoveHeldObject.GoalType is HeldObjectPoseGoal assert Place.GoalType == (PlaceGoal, AssembleGoal) @@ -2333,7 +2335,12 @@ def test_pick_holds_fully_closed_before_lifting_when_configured() -> None: ) -def test_pick_combined_motion_gen_preserves_backend_samples_before_split() -> None: +@pytest.mark.parametrize("contact", [False, True]) +def test_pick_combined_motion_gen_preserves_backend_samples_before_split( + contact, +) -> None: + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + sample_count = 20 backend_sample_count = 6 generator = _motion_generator() @@ -2351,9 +2358,13 @@ def test_pick_combined_motion_gen_preserves_backend_samples_before_split() -> No action, SkillInvocation( skill_id="pick_up", - goal=GraspGoal( - semantics=_semantics(entity_id="target"), - grasp_xpos=torch.eye(4), + goal=( + ContactGraspGoal(torch.eye(4)) + if contact + else GraspGoal( + semantics=_semantics(entity_id="target"), + grasp_xpos=torch.eye(4), + ) ), binding=_binding(action), motion_policy=MotionPolicy( @@ -2370,6 +2381,10 @@ def test_pick_combined_motion_gen_preserves_backend_samples_before_split() -> No assert motion_options.plan_opts is None assert plan.commands.frame_count == sample_count + if contact: + assert plan.expected_effects.is_empty + assert plan.effect_verification.kind == "contact_grasp" + def test_place_releases_only_exclusively_held_rows() -> None: generator = _motion_generator() @@ -5246,7 +5261,6 @@ def test_push_object_plans_contact_and_target_directed_planar_motion() -> None: assert plan.plan_success.tolist() == [True, True] assert plan.expected_effects.is_empty assert plan.scene_dependencies == ("slot", "utensil") - assert plan.scene_dependency_end_segment == "approach" assert [segment.name for segment in plan.segments] == [ "close", "approach", @@ -9475,83 +9489,283 @@ def test_pickup_rejects_invalid_per_row_approach(direction: torch.Tensor) -> Non PickUpOptions(approach_direction=direction) -@pytest.mark.parametrize("close_hold_duration", [0.0, 2.25 * CONTROL_DT]) -def test_cable_grasp_plan_preserves_other_arm_and_declares_no_attachment( - close_hold_duration, -) -> None: - from embodichain.lab.sim.atomic_skills.primitives.grasp_cable import ( - CableGraspGoal, - GraspCable, - GraspCableOptions, - ) +@pytest.mark.parametrize("settle_steps", [0, 3]) +def test_contact_pickup_preserves_other_arm_and_requires_physical_verification( + settle_steps, +): + from embodichain.lab.sim.atomic_skills import ContactGraspGoal generator = _dual_motion_generator() - action = _bind_action(generator, GraspCable()) + skill = _bind_action(generator, PickUp()) context = _dual_context() context.robot.qpos[:, ARM_DOF:DUAL_ARM_DOF] = 0.37 before = context.robot.qpos.clone() plan = _plan_action( - action, + skill, SkillInvocation( - skill_id="grasp_cable", - goal=CableGraspGoal("wire", 0.12, torch.eye(4)), - binding=_binding(action, motion="left_arm", grasp="left_hand"), - motion_policy=MotionPolicy(sample_count=5), - skill_options=GraspCableOptions( - close_steps=4, close_hold_duration=close_hold_duration + skill_id="pick_up", + goal=ContactGraspGoal(torch.eye(4)), + binding=_binding(skill, motion="left_arm", grasp="left_hand"), + motion_policy=MotionPolicy( + sample_count=12, interpolate_position_step=0.05 * CONTROL_DT + ), + skill_options=PickUpOptions( + hand_interp_steps=4, + grasp_settle_steps=settle_steps, + lift_height=0.0185, + pre_grasp_distance=0, ), ), context, ) assert plan.plan_success.all() - assert [segment.name for segment in plan.segments] == ["descend", "close", "lift"] + assert [segment.name for segment in plan.segments] == ["approach", "close", "lift"] torch.testing.assert_close(context.robot.qpos, before) positions = plan.joint_trajectory.positions close = next(segment for segment in plan.segments if segment.name == "close") - hold_steps = math.ceil(close_hold_duration / CONTROL_DT) - assert close.stop - close.start == 4 + hold_steps - closed_frames = positions[:, close.stop - hold_steps - 1 : close.stop] - torch.testing.assert_close( - closed_frames, closed_frames[:, -1:].expand_as(closed_frames) - ) + assert close.stop - close.start == 4 + settle_steps + closed = positions[:, close.stop - settle_steps - 1 : close.stop] + torch.testing.assert_close(closed, closed[:, -1:].expand_as(closed)) torch.testing.assert_close( positions[:, :, ARM_DOF:DUAL_ARM_DOF], before[:, None, ARM_DOF:DUAL_ARM_DOF].expand(-1, positions.shape[1], -1), ) - lift = next(segment for segment in plan.segments if segment.name == "lift") - assert (lift.stop - lift.start) * CONTROL_DT <= 0.45 - assert context.task.get_held_object("left_arm") is None + assert plan.expected_effects.is_empty + assert plan.effect_verification.kind == "contact_grasp" + assert plan.requires_effect_verification + assert plan.scene_dependencies == () projected = plan.expected_effects.apply(context.task, plan.plan_success) assert projected.get_held_object("left_arm") is None generator.robot.set_qpos.assert_not_called() -@pytest.mark.parametrize("distance", [-1.0, float("nan"), float("inf")]) -def test_cable_grasp_rejects_invalid_material_coordinate(distance) -> None: - from embodichain.lab.sim.atomic_skills.primitives.grasp_cable import CableGraspGoal +@pytest.mark.parametrize("occupied", ["held", "coordinated"]) +def test_contact_pickup_rejects_occupied_task_resource(occupied): + from embodichain.lab.sim.atomic_skills import ( + ContactGraspGoal, + CoordinatedHeldObjectState, + ) - with pytest.raises(ValueError): - CableGraspGoal("wire", distance, torch.eye(4)) + generator = _dual_motion_generator() + generator.generate = Mock(wraps=generator.generate) + skill = _bind_action(generator, PickUp()) + pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects=( + {"left_arm": HeldObjectState(_semantics(), pose, pose)} + if occupied == "held" + else {} + ), + coordinated_held_objects=( + { + ("left_arm", "right_arm"): CoordinatedHeldObjectState( + _semantics(), pose, pose, pose, pose + ) + } + if occupied == "coordinated" + else {} + ), + ) + with pytest.raises(ValueError, match="unoccupied"): + _plan_action( + skill, + SkillInvocation( + skill_id="pick_up", + goal=ContactGraspGoal(torch.eye(4)), + binding=_binding(skill, motion="left_arm", grasp="left_hand"), + ), + _dual_context(task), + ) + generator.generate.assert_not_called() @pytest.mark.parametrize( - "options", - [ - {"close_steps": True}, - {"close_steps": 2.5}, - {"close_steps": 1}, - {"close_hold_duration": -0.1}, - {"close_hold_duration": float("nan")}, - {"close_hold_duration": float("inf")}, - {"close_hold_duration": True}, - {"lift_height": float("nan")}, - {"lift_height": 0.0}, - ], + "bad_pose", [torch.zeros(3, 3), torch.full((4, 4), float("nan"))] ) -def test_cable_grasp_rejects_invalid_options(options) -> None: - from embodichain.lab.sim.atomic_skills.primitives.grasp_cable import ( - GraspCableOptions, - ) +def test_contact_pickup_rejects_invalid_goal_pose(bad_pose): + from embodichain.lab.sim.atomic_skills import ContactGraspGoal with pytest.raises(ValueError): - GraspCableOptions(**options) + ContactGraspGoal(bad_pose) + + +@pytest.mark.parametrize("step", [0.0, -0.01, float("nan"), float("inf"), None, True]) +def test_motion_policy_rejects_invalid_cartesian_sampling_distance(step): + with pytest.raises(ValueError, match="interpolate_position_step"): + MotionPolicy(interpolate_position_step=step) + + +def test_contact_pickup_external_motion_uses_same_verification_contract(): + from embodichain.lab.sim.atomic_skills import ( + ContactGraspGoal, + InstantiatedSkillMotion, + RecoveryPolicy, + ) + + generator = _motion_generator() + generator.robot.num_instances = 1 + get_qpos = generator.robot.get_qpos.side_effect + generator.robot.get_qpos.side_effect = lambda name=None: get_qpos(name)[:1] + generator.robot.get_qpos_limits.return_value = torch.tensor([-2.0, 2.0]).repeat( + 1, ARM_DOF, 1 + ) + generator.robot.get_qvel_limits.side_effect = None + generator.robot.get_qvel_limits.return_value = torch.full((1, ARM_DOF), 10.0) + generator.generate = Mock(wraps=generator.generate) + skill = _bind_action(generator, PickUp()) + qpos = torch.zeros(1, ROBOT_DOF) + context = PlanningContext( + robot=RobotObservation(timestamp=0.0, qpos=qpos, qvel=qpos.clone()), + task=TaskState.empty(1, "cpu"), + scene=SceneSnapshot.empty(), + env_ids=torch.tensor([0]), + control_dt=CONTROL_DT, + ) + hand_ids = list(generator.robot.get_joint_ids("hand")) + + class Source: + def instantiate(self, request, current): + positions = current.robot.qpos[:, None].repeat(1, 6, 1) + positions[:, 3:, hand_ids] = 1 + return InstantiatedSkillMotion( + trajectory=TimedTrajectory.from_uniform_step( + positions, env_ids=current.env_ids, step_dt=CONTROL_DT + ), + success=torch.ones(1, dtype=torch.bool), + segment_lengths={"approach": 2, "close": 2, "lift": 2}, + initial_qpos=current.robot.qpos, + source_id="contact_fixture", + source_revision="1", + unit_id="pickup", + origin="test", + grasp_pose=torch.eye(4).repeat(1, 1, 1), + joint_names=tuple(generator.robot.joint_names), + ) + + invocation = SkillInvocation( + skill_id="pick_up", + goal=ContactGraspGoal(torch.eye(4)), + binding=_binding(skill), + skill_options=PickUpOptions(lift_height=0), + recovery_policy=RecoveryPolicy(max_replans=0, max_skill_retries=0), + ) + plan = _ACTION_ENGINES[id(skill)].plan(invocation, context, nominal_source=Source()) + assert plan.plan_success.all() + assert plan.expected_effects.is_empty + assert plan.effect_verification.kind == "contact_grasp" + assert plan.requires_effect_verification + generator.generate.assert_not_called() + + +def test_contact_pickup_keeps_failed_rows_at_observed_state(): + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + + generator = _dual_motion_generator() + original_ik = generator.robot.compute_ik.side_effect + + def fail_second_row(*args, **kwargs): + valid, qpos = original_ik(*args, **kwargs) + valid[1] = False + return valid, qpos + + generator.robot.compute_ik.side_effect = fail_second_row + skill = _bind_action(generator, PickUp()) + context = _dual_context() + plan = _plan_action( + skill, + SkillInvocation( + skill_id="pick_up", + goal=ContactGraspGoal(torch.eye(4).repeat(NUM_ENVS, 1, 1)), + binding=_binding(skill, motion="left_arm", grasp="left_hand"), + skill_options=PickUpOptions(pre_grasp_distance=0), + ), + context, + ) + assert plan.plan_success.tolist() == [True, False] + torch.testing.assert_close( + plan.joint_trajectory.positions[1], + context.robot.qpos[1].expand(plan.joint_trajectory.waypoint_count, -1), + ) + + +def test_contact_pickup_monitors_late_bound_pose_until_acquisition(): + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + + generator = _dual_motion_generator() + skill = _bind_action(generator, PickUp()) + pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + context = _dual_context(scene=_target_scene(pose, timestamp=0.0, version=0)) + plan = _plan_action( + skill, + SkillInvocation( + skill_id="pick_up", + goal=ContactGraspGoal(SceneEntityPose("target")), + binding=_binding(skill, motion="left_arm", grasp="left_hand"), + skill_options=PickUpOptions(pre_grasp_distance=0), + ), + context, + ) + assert plan.scene_dependencies == ("target",) + assert ( + plan.scene_dependency_monitor_until["target"] == plan.segment("approach").stop + ) + + +def test_contact_pickup_propagates_nonprefix_environment_selection(): + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + + generator = _dual_motion_generator() + original_fk = generator.robot.compute_fk.side_effect + + def selected_fk(qpos, *, name, to_matrix, env_ids=None): + assert tuple(env_ids) == (1,) + return original_fk(qpos, name=name, to_matrix=to_matrix) + + generator.robot.compute_fk.side_effect = selected_fk + skill = _bind_action(generator, PickUp(), env_ids=(1,)) + full_context = _dual_context() + qpos = full_context.robot.qpos[[1]] + context = PlanningContext( + robot=RobotObservation(0.0, qpos, torch.zeros_like(qpos)), + task=TaskState.empty(1, "cpu"), + scene=SceneSnapshot.empty(), + env_ids=torch.tensor([1]), + control_dt=CONTROL_DT, + ) + plan = _plan_action( + skill, + SkillInvocation( + skill_id="pick_up", + goal=ContactGraspGoal(torch.eye(4)), + binding=_binding(skill, motion="left_arm", grasp="left_hand"), + skill_options=PickUpOptions(pre_grasp_distance=0), + ), + context, + ) + assert plan.plan_success.tolist() == [True] + assert plan.joint_trajectory.env_ids.tolist() == [1] + assert plan.joint_trajectory.positions.shape[0] == 1 + + +def test_contact_pickup_rejects_rigid_options_before_scene_lookup(): + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + + generator = _dual_motion_generator() + generator.generate = Mock(wraps=generator.generate) + skill = _bind_action(generator, PickUp()) + with pytest.raises(ValueError, match="without rigid-object options"): + _plan_action( + skill, + SkillInvocation( + skill_id="pick_up", + goal=ContactGraspGoal(torch.eye(4)), + binding=_binding(skill, motion="left_arm", grasp="left_hand"), + skill_options=PickUpOptions( + downstream_object_target_poses=(SceneEntityPose("unavailable"),) + ), + ), + _dual_context(), + ) + generator.generate.assert_not_called() From 1a8bc308d905c174a25da007045a48723038ead9 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Thu, 12 Nov 2026 21:39:03 +0800 Subject: [PATCH 31/43] feat(tasks): route cables through configured Task Program Replace the handwritten routing environment with configuration-owned registration and a bounded 49-stage program. Reuse PickUp contact grasps, sampled native motions and the standard Gym bridge; keep DexUni physics and strict measured final acceptance. Scope material references and slip evidence to each acknowledged execution and preserve inactive integration identities. Validation: 2,468 related CPU regressions; 204 observer lifecycle rechecks; two NoRender CUDA spool/contact qualifications; deployment inspector, API coverage, context checks and Black 26.10.1. The paired seed-73 FastRT run remains unsuccessful at strict insertion: the baseline stops on slip, while the Task Program stops on final lowering TCP error. --- .../topics/task-programs/configuration.md | 19 + embodichain/compute/geometry/_cable_route.py | 178 +++ embodichain/lab/evaluation/_predicates.py | 35 + embodichain/lab/evaluation/_recording.py | 16 +- embodichain/lab/evaluation/observations.py | 2 + .../lab/gym/envs/managers/observations.py | 50 + .../lab/gym/envs/task_program/bridge.py | 13 + .../lab/gym/utils/_component_composition.py | 1 + embodichain/lab/gym/utils/trajectory_state.py | 2 +- .../primitives/move_end_effector.py | 17 + .../atomic_skills/primitives/move_joints.py | 43 +- .../integrations/_configured_composition.py | 8 + .../lab/task_program/integrations/catalog.py | 17 + .../task_program/integrations/configured.py | 93 +- .../integrations/simulation/bindings.py | 19 + .../integrations/simulation/cable.py | 861 +++++++++++++ .../integrations/simulation/environment.py | 51 +- .../integrations/simulation/policies.py | 18 + .../lab/task_program/runtime/executor.py | 11 + .../components/embodiments/yam_i2rt.yaml | 70 +- .../cable/route_the_cable/README.md | 44 +- .../cable/route_the_cable/catalog.yaml | 9 +- .../cable/route_the_cable/env.yaml | 2 +- .../cable/route_the_cable/task.yam.yaml | 6 + .../task_program/evaluation.yaml | 104 ++ .../task_program/execution_policy.yaml | 24 + .../task_program/integration.yaml | 710 +++++++++++ .../route_the_cable/task_program/program.yaml | 1106 +++++++++++++++++ .../manipulation/cable/route_the_cable.py | 890 ------------- tests/gym/envs/task_program/test_bridge.py | 10 + .../envs/task_program/test_cable_material.py | 259 ++++ .../test_configured_integration.py | 10 +- tests/gym/envs/tasks/test_route_the_cable.py | 353 +----- .../envs/tasks/test_route_the_cable_cuda.py | 38 +- tests/lab/evaluation/test_rule_graph.py | 24 + tests/sim/atomic_skills/test_actions.py | 52 + 36 files changed, 3886 insertions(+), 1279 deletions(-) create mode 100644 embodichain/compute/geometry/_cable_route.py create mode 100644 embodichain/lab/task_program/integrations/simulation/cable.py create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/evaluation.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.yaml delete mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/route_the_cable.py create mode 100644 tests/gym/envs/task_program/test_cable_material.py diff --git a/agent_context/topics/task-programs/configuration.md b/agent_context/topics/task-programs/configuration.md index 9af92453ad..f0646794ea 100644 --- a/agent_context/topics/task-programs/configuration.md +++ b/agent_context/topics/task-programs/configuration.md @@ -183,6 +183,13 @@ the closed-field decoder. Omitting `velocity_limit` preserves the Robot's ordered per-joint speed caps; an explicit scalar request is intersected with those caps rather than replacing them. +Configured motion accepts `interpolate_position_step` for native Cartesian +sampling. `MoveEndEffectorOptions.preserve_cartesian_samples` retains authored +Cartesian samples; `MoveJointsOptions.preserve_samples` consumes explicitly +sampled joint commands at the host cadence using `ik_interp`. These declarations +retain normal Atomic velocity qualification and do not certify collision-free +motion. Inactive sampling additions preserve existing integration fingerprints. + Configured motion also accepts explicit `interpolation_mode` and `ik_seed_mode`. `fixed_grid` requires `acceleration_limit: null` and no scalar speed override; omission retains the existing adaptive policy and acceleration default. @@ -197,6 +204,18 @@ declarations in the integration and timing in the execution policy, rather than placing a precomputed bottle world pose in the program. Omitting `target` retains the original internal-axis pouring behavior. +`scene_binding.cables` declares material-state native identities without rigid +object semantics or root-pose providers. The allowlisted +`runtime_services.cable_material` binds those identities to measured material +coordinates and closed target declarations. Its calls lower to existing +`PickUp(ContactGraspGoal)`, `MoveEndEffector`, and `MoveJoints` skills; the program +owns stage ordering. The bridge captures contact references only after an +acknowledged close frame and checks material slip after every consumed action. +References are fresh per bridge, guarded by its episode scope, and mapped from +selected robot rows to native cable arenas. Evaluation consumes qualified +`cables//` channels, including strict endpoint geometry and +episode slip. Keep routing acceptance in the deployment's named evaluation rules. + The configured lowerer allowlist also exposes profile-owned Park, AxisAlign, relative Place and coordinated transport. Their declarations in `integrations/_configured_services.py` select exact Atomic Skill descriptors; diff --git a/embodichain/compute/geometry/_cable_route.py b/embodichain/compute/geometry/_cable_route.py new file mode 100644 index 0000000000..57aa374e85 --- /dev/null +++ b/embodichain/compute/geometry/_cable_route.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. +# ---------------------------------------------------------------------------- + +"""Pure cable routing measurements for declared board geometry. + +Geometry measurements adapted from robosimstudio's Apache-2.0 example at +73739abe212f963e8a9f5862b27bca370df091a7; see the repository NOTICE. +https://github.com/robosimstudio/robosimstudio/tree/73739abe212f963e8a9f5862b27bca370df091a7/examples/08_demos/route_the_cable +""" + +from __future__ import annotations + +import torch +from .cable import cable_endpoints + +__all__: list[str] = [] + +_CABLE_METRICS = ( + "seat_0", + "seat_1", + "port_count", + "end_in_port", + "strict_port", + "release_ready", + "reference_routed", + "hands_clearance", + "max_speed", + "bench_penetration", + "spool_penetration", + "port_penetration", + "max_grasp_slip", + "end_position", +) + + +_CABLE_METRIC_UNITS = { + name: ( + "m/s" + if name == "max_speed" + else ( + "m" + if name + in { + "end_position", + "hands_clearance", + "bench_penetration", + "spool_penetration", + "port_penetration", + "max_grasp_slip", + } + else "unitless" + ) + ) + for name in _CABLE_METRICS +} + + +def _measure_cable_route( + state: torch.Tensor, + lengths: torch.Tensor, + hands_w: torch.Tensor, + origins: torch.Tensor, + geometry: dict[str, object], + radius: float, +) -> dict[str, torch.Tensor]: + """Measure the reference board goal and sampled geometry diagnostics. + + Args: + state: World cable segment states ``(B,50,13)``. + lengths: Capsule axis rest lengths ``(50,)``. + hands_w: World tool centres ``(B,2,3)``. + origins: World Arena translations ``(B,3)``. + + Returns: + Per-environment counts, speed, clearance and routing acceptance. + """ + bench = geometry["bench_height"] + port_x, port_y = geometry["port_center"] + p = state[..., :3] - origins[:, None, :] + starts, ends = cable_endpoints(state, lengths) + start_local = starts - origins[:, None, :] + end_local = ends - origins[:, None, :] + valid = torch.isfinite(state).all(dim=(1, 2)) & ( + state[..., 3:7].norm(dim=-1) > 1e-8 + ).all(-1) + out = {} + for i, (x, y, side) in enumerate(geometry["seats"]): + lo, hi = sorted((x + side * 0.003, x + side * 0.045)) + inside = ( + (p[:, :, 0] >= lo) + & (p[:, :, 0] <= hi) + & ((p[:, :, 1] - y).abs() <= 0.035) + & (p[:, :, 2] >= bench) + & (p[:, :, 2] <= bench + 0.15) + ) + out[f"seat_{i}"] = inside.sum(-1) + in_port = ( + ((p[:, :, 0] - port_x).abs() <= 0.045) + & ((p[:, :, 1] - port_y).abs() <= 0.045) + & (p[:, :, 2] >= bench) + & (p[:, :, 2] <= bench + 0.15) + ) + end = end_local[:, -1] + out["end_position"] = end + out["port_count"] = in_port.sum(-1) + out["end_in_port"] = ( + ((end[:, :2] - end.new_tensor([port_x, port_y])).abs() <= 0.045).all(-1) + & (end[:, 2] >= bench) + & (end[:, 2] <= bench + 0.15) + ) + out["hands_clearance"] = torch.cdist(hands_w, state[..., :3]).amin(dim=(1, 2)) + out["max_speed"] = torch.linalg.vector_norm(state[..., 7:10], dim=-1).amax(-1) + out["bench_penetration"] = ( + bench + radius - torch.minimum(start_local[..., 2], end_local[..., 2]).amin(-1) + ).clamp_min(0) + # Five points per capsule axis, using the exact capped-cylinder SDF. + axis_samples = ( + start_local[:, :, None, :] + + torch.linspace(0, 1, 5, device=state.device)[None, None, :, None] + * (end_local - start_local)[:, :, None, :] + ) + penetration = torch.zeros(state.shape[0], device=state.device) + for x, y, cylinder_radius, z0, z1 in geometry["cylinders"]: + radius_at_spool = cylinder_radius + radial = ( + torch.linalg.vector_norm( + axis_samples[..., :2] - state.new_tensor([x, y]), dim=-1 + ) + - radius_at_spool + ) + vertical = (axis_samples[..., 2] - (bench + (z0 + z1) / 2)).abs() - ( + z1 - z0 + ) / 2 + q = torch.stack((radial, vertical), -1) + sdf = torch.linalg.vector_norm(q.clamp_min(0), dim=-1) + q.amax(-1).clamp_max(0) + penetration = torch.maximum( + penetration, (radius - sdf).clamp_min(0).amax(dim=(1, 2)) + ) + out["spool_penetration"] = penetration + port_penetration = torch.zeros_like(penetration) + for center, half_size in geometry["boxes"]: + q = (axis_samples - state.new_tensor(center)).abs() - state.new_tensor( + half_size + ) + sdf = torch.linalg.vector_norm(q.clamp_min(0), dim=-1) + q.amax(-1).clamp_max(0) + port_penetration = torch.maximum( + port_penetration, (radius - sdf).clamp_min(0).amax(dim=(1, 2)) + ) + out["port_penetration"] = port_penetration + strict_lo, strict_hi = (state.new_tensor(v) for v in geometry["strict_region"]) + release_lo, release_hi = (state.new_tensor(v) for v in geometry["release_region"]) + out["strict_port"] = ((end >= strict_lo) & (end <= strict_hi)).all(-1) + out["release_ready"] = valid & ( + out["strict_port"] | ((end >= release_lo) & (end <= release_hi)).all(-1) + ) + out["reference_routed"] = ( + valid + & (out["seat_0"] >= 2) + & (out["seat_1"] >= 2) + & (out["port_count"] >= 3) + & out["end_in_port"] + & (out["hands_clearance"] > 0.05) + & (out["max_speed"] <= 0.05) + ) + return out diff --git a/embodichain/lab/evaluation/_predicates.py b/embodichain/lab/evaluation/_predicates.py index d02eb551dd..bb1a9012d5 100644 --- a/embodichain/lab/evaluation/_predicates.py +++ b/embodichain/lab/evaluation/_predicates.py @@ -477,10 +477,45 @@ def _region(arguments, params): ) +def _scalar_range_params(params): + _keys(params, ("minimum", "maximum"), ("minimum_inclusive", "maximum_inclusive")) + lo, hi = (_finite(params[key], key) for key in ("minimum", "maximum")) + if lo > hi: + raise ValueError("Scalar range minimum exceeds maximum.") + flags = { + key: params.get(key, True) for key in ("minimum_inclusive", "maximum_inclusive") + } + if any(type(value) is not bool for value in flags.values()): + raise ValueError("Scalar range boundary flags must be booleans.") + return {"minimum": lo, "maximum": hi, **flags} + + +def _scalar_range(arguments, params): + value = arguments["value"] + if value.ndim != 2 or value.shape[1] != 1: + raise ValueError("Scalar observations must have shape (B, 1).") + value = value[:, 0] + lo, hi = params["minimum"], params["maximum"] + import torch + + return PredicateMeasurement( + ((value >= lo) if params["minimum_inclusive"] else (value > lo)) + & ((value <= hi) if params["maximum_inclusive"] else (value < hi)), + torch.isfinite(value), + {"value": value}, + ) + + def builtin_predicates() -> PredicateRegistry: """Return an independent registry with the shared built-in predicates.""" registry = PredicateRegistry() for kind, definition in ( + ( + "scalar_range", + PredicateDefinition( + 1, ("value",), _scalar_range_params, _scalar_range, ("scalar",) + ), + ), ("near_target", _NEAR), ( "near_relative_target", diff --git a/embodichain/lab/evaluation/_recording.py b/embodichain/lab/evaluation/_recording.py index c2e2f6742d..2a5f9c8923 100644 --- a/embodichain/lab/evaluation/_recording.py +++ b/embodichain/lab/evaluation/_recording.py @@ -289,6 +289,8 @@ def evaluate_recorded_rules( def _matches_field(field, uid, quantity): + if field.startswith("cables/"): + return field == f"cables/{uid}/{quantity}" if field.startswith("rigid_objects/"): if _is_segment_start_field(field): return ( @@ -355,7 +357,19 @@ def get(name, default=None): expected = expected_names[field] if expected is None or tuple(names) != tuple(expected): return None - if quantity in ("pose", "segment_start_pose"): + if field.startswith("cables/"): + from embodichain.compute.geometry._cable_route import ( + _CABLE_METRICS as _METRICS, + _CABLE_METRIC_UNITS, + ) + + if quantity not in _METRICS: + return None + if quantity == "end_position": + contract = ("arena", "xyz", ("m",), (3,)) + else: + contract = ("arena", "scalar", (_CABLE_METRIC_UNITS[quantity],), (1,)) + elif quantity in ("pose", "segment_start_pose"): contract = ("arena", "xyz_xyzw", ("m", "unitless"), (7,)) elif quantity in ("lin_vel", "ang_vel"): unit = "m/s" if quantity == "lin_vel" else "rad/s" diff --git a/embodichain/lab/evaluation/observations.py b/embodichain/lab/evaluation/observations.py index be495570f3..1fded6ff48 100644 --- a/embodichain/lab/evaluation/observations.py +++ b/embodichain/lab/evaluation/observations.py @@ -41,6 +41,8 @@ def _raw_field_kind(field: str) -> str: owner, _, channel = field.partition("/") quantity = channel.rsplit("/", 1)[1] + if owner == "cables": + return "position" if quantity == "end_position" else "scalar" if owner == "rigid_objects": return { "pose": "position", diff --git a/embodichain/lab/gym/envs/managers/observations.py b/embodichain/lab/gym/envs/managers/observations.py index 9e515cb85e..62efd854d3 100644 --- a/embodichain/lab/gym/envs/managers/observations.py +++ b/embodichain/lab/gym/envs/managers/observations.py @@ -231,6 +231,56 @@ def _get_evaluation_state( env: EmbodiedEnv, obs: EnvObs, *, field: str ) -> ObservationSample: parts = field.split("/") + if parts[0] == "cables": + from embodichain.compute.geometry._cable_route import ( + _CABLE_METRICS as _METRICS, + _CABLE_METRIC_UNITS, + ) + + _, uid, quantity = parts + if quantity not in _METRICS: + raise ValueError(f"Unknown cable observation {quantity!r}.") + bridge = getattr(env, "_active_task_program_bridge", None) + service = getattr( + getattr(bridge, "post_policy_port", None), "_cable_runtime", None + ) + shape = (3,) if quantity == "end_position" else (1,) + if service is None or service.cfg.simulation_uid != uid: + values = torch.zeros((env.num_envs, *shape), device=env.device) + valid = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + else: + measured = ( + service.metrics()[quantity] + .float() + .reshape(len(service.env_ids), *shape) + ) + values = torch.zeros((env.num_envs, *shape), device=env.device) + valid = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + values[service.env_ids] = measured + state = service.state() + valid[service.env_ids] = ( + torch.isfinite(measured).all(-1) + & torch.isfinite(state).all(dim=(1, 2)) + & (state[..., 3:7].norm(dim=-1) > 1e-8).all(-1) + ) + descriptor = ObservationDescriptor( + uid, + quantity, + "arena", + "xyz" if shape == (3,) else "scalar", + (_CABLE_METRIC_UNITS[quantity],), + shape, + values.dtype, + ) + return ObservationSample( + descriptor, + values, + valid, + tuple( + None if v else "Cable material session is unavailable." + for v in valid.tolist() + ), + ) if parts[0] in ("links", "robot_links"): owner = parts[0] link = parts[-2] diff --git a/embodichain/lab/gym/envs/task_program/bridge.py b/embodichain/lab/gym/envs/task_program/bridge.py index 38e69c3a54..13547897ec 100644 --- a/embodichain/lab/gym/envs/task_program/bridge.py +++ b/embodichain/lab/gym/envs/task_program/bridge.py @@ -1778,6 +1778,19 @@ def _yield_and_advance( self._record_consumed_action(action) self._clock.advance_after_env_step() lifecycle.pending_action = None + observer = getattr( + getattr(self, "_validator_port", None), "_observe_consumed_action", None + ) + decision = None if observer is None else observer(action, runtime=self._runtime) + if decision is not None: + rejected, hold_qpos, reason = decision + if self._eligible_mask is not None: + rejected &= self._eligible_mask + if bool(rejected.any()): + hold = ControllerAction(self._sink.encode_post_policy_hold(hold_qpos)) + self.request_row_rejection( + tuple(self.env_ids[rejected].tolist()), hold, reason=reason + ) process = getattr(self, "_process_rejection", None) if process is not None: self._process_rejection = None diff --git a/embodichain/lab/gym/utils/_component_composition.py b/embodichain/lab/gym/utils/_component_composition.py index d1a61046a9..d2b9274c63 100644 --- a/embodichain/lab/gym/utils/_component_composition.py +++ b/embodichain/lab/gym/utils/_component_composition.py @@ -414,6 +414,7 @@ def _validate_scene_binding_targets( ) -> None: """Fail before runtime when semantic roots select absent native entities.""" physical_ids = { + "cables": _physical_scene_uids(simulation, field_names=("cable_object",)), "rigid_objects": _physical_scene_uids( simulation, field_names=("background", "rigid_object"), diff --git a/embodichain/lab/gym/utils/trajectory_state.py b/embodichain/lab/gym/utils/trajectory_state.py index 9012442701..7d3b98bffc 100644 --- a/embodichain/lab/gym/utils/trajectory_state.py +++ b/embodichain/lab/gym/utils/trajectory_state.py @@ -285,7 +285,7 @@ def _evaluation_observation_cfgs( name, uid, "qpos" ) for name in sorted(fields): - if name.startswith(("links/", "robot/", "robot_links/")): + if name.startswith(("links/", "robot/", "robot_links/", "cables/")): result[f"recording:{name}"] = ObservationCfg( func=_get_evaluation_state, mode="add", diff --git a/embodichain/lab/sim/atomic_skills/primitives/move_end_effector.py b/embodichain/lab/sim/atomic_skills/primitives/move_end_effector.py index 3dde684606..5685f152d4 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/move_end_effector.py +++ b/embodichain/lab/sim/atomic_skills/primitives/move_end_effector.py @@ -63,6 +63,13 @@ def __post_init__(self) -> None: class MoveEndEffectorOptions(SkillOptions): """Per-invocation behavior for :class:`MoveEndEffector`.""" + preserve_cartesian_samples: bool = False + """Execute the supplied Cartesian samples on the control grid.""" + + def __post_init__(self) -> None: + if type(self.preserve_cartesian_samples) is not bool: + raise TypeError("preserve_cartesian_samples must be a bool.") + class MoveEndEffector(AtomicSkill[EndEffectorPoseGoal, MoveEndEffectorOptions]): """Plan a free-space move for a bound manipulator.""" @@ -96,6 +103,10 @@ def _plan( num_envs=context.batch_size, device=self.device, ) + if request.skill_options.preserve_cartesian_samples and move_xpos.ndim != 4: + raise ValueError( + "preserve_cartesian_samples requires batched pose samples." + ) start_qpos = context.robot.qpos[:, joint_ids] result = self.motion_generator.generate( build_pose_plan_states(move_xpos), @@ -103,6 +114,12 @@ def _plan( start_qpos=start_qpos, control_part=control_part, interpolation_dt=context.control_dt, + cartesian_linear=request.skill_options.preserve_cartesian_samples, + sample_count=( + move_xpos.shape[1] + 1 + if request.skill_options.preserve_cartesian_samples + else None + ), ), ) success, trajectory = to_full_robot_trajectory( diff --git a/embodichain/lab/sim/atomic_skills/primitives/move_joints.py b/embodichain/lab/sim/atomic_skills/primitives/move_joints.py index 7f76815918..dce15adb41 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_skills/primitives/move_joints.py @@ -75,6 +75,13 @@ def __post_init__(self) -> None: class MoveJointsOptions(SkillOptions): """Per-invocation behavior for :class:`MoveJoints`.""" + preserve_samples: bool = False + """Consume explicitly sampled joint positions at the host control cadence.""" + + def __post_init__(self) -> None: + if type(self.preserve_samples) is not bool: + raise TypeError("preserve_samples must be a bool.") + class MoveJoints(AtomicSkill[JointPositionGoal, MoveJointsOptions]): """Plan joint motion from the observed state to one or more waypoints.""" @@ -116,14 +123,34 @@ def _plan( device=self.device, ) start_qpos = context.robot.qpos[:, joint_ids] - result = self.motion_generator.generate( - build_joint_plan_states(target_qpos), - options=request.motion_policy.to_motion_gen_options( - start_qpos=start_qpos, - control_part=control_part, - interpolation_dt=context.control_dt, - ), - ) + if request.skill_options.preserve_samples: + if request.motion_policy.strategy != "ik_interp": + raise ValueError("preserve_samples requires strategy='ik_interp'.") + from embodichain.lab.sim.motion.planners import PlanResult + + if target_qpos.ndim != 3: + raise ValueError("preserve_samples requires batched joint samples.") + dt = torch.full( + target_qpos.shape[:2], + context.require_control_dt(), + dtype=torch.float64, + device=target_qpos.device, + ) + dt[:, 0] = 0 + result = PlanResult( + success=torch.isfinite(target_qpos).all(dim=(1, 2)), + positions=target_qpos, + dt=dt, + ) + else: + result = self.motion_generator.generate( + build_joint_plan_states(target_qpos), + options=request.motion_policy.to_motion_gen_options( + start_qpos=start_qpos, + control_part=control_part, + interpolation_dt=context.control_dt, + ), + ) success, trajectory = to_full_robot_trajectory( result, base_qpos=context.robot.qpos, diff --git a/embodichain/lab/task_program/integrations/_configured_composition.py b/embodichain/lab/task_program/integrations/_configured_composition.py index c256f64891..385366264e 100644 --- a/embodichain/lab/task_program/integrations/_configured_composition.py +++ b/embodichain/lab/task_program/integrations/_configured_composition.py @@ -58,6 +58,7 @@ "registered_semantic_lowerers", "control_part_evidence", "settle_presets", + "cable_material", } ) _TASK_RUNTIME_SERVICE_FIELDS = _RUNTIME_SERVICE_FIELDS | frozenset( @@ -265,6 +266,12 @@ def _merge_runtime_services( evidence = task_evidence if task_evidence is not None else skill_profile_evidence if evidence is not None: result["control_part_evidence"] = deepcopy(evidence) + if "cable_material" in skill_profile and "cable_material" in task: + raise ValueError("cable_material must have exactly one owner.") + if "cable_material" in task or "cable_material" in skill_profile: + result["cable_material"] = deepcopy( + task.get("cable_material", skill_profile.get("cable_material")) + ) if "nominal_motion_sources" in task: result["nominal_motion_sources"] = _owned_sequence( task["nominal_motion_sources"], @@ -651,6 +658,7 @@ def _load_configured_task_program_deployment( optional=frozenset( { "rigid_objects", + "cables", "rigidized_articulations", "articulations", "links", diff --git a/embodichain/lab/task_program/integrations/catalog.py b/embodichain/lab/task_program/integrations/catalog.py index a8ef7c8d69..5d4e6f4511 100644 --- a/embodichain/lab/task_program/integrations/catalog.py +++ b/embodichain/lab/task_program/integrations/catalog.py @@ -226,6 +226,23 @@ def _canonical_value(value: object) -> object: metadata.pop("supports_segment_motion_recovery") if type(value) is MotionPolicy and value.final_velocity_rejection == "hold": metadata.pop("final_velocity_rejection") + # Inactive additions do not change an existing integration's identity. + from embodichain.lab.sim.atomic_skills.primitives.move_joints import ( + MoveJointsOptions, + ) + from embodichain.lab.sim.atomic_skills.primitives.move_end_effector import ( + MoveEndEffectorOptions, + ) + + if type(value) is SimulationSceneBinding and not value.cables: + metadata.pop("cables") + if type(value) is MoveJointsOptions and not value.preserve_samples: + metadata.pop("preserve_samples") + if ( + type(value) is MoveEndEffectorOptions + and not value.preserve_cartesian_samples + ): + metadata.pop("preserve_cartesian_samples") if type(value) is RecoveryPolicy and value.retry_all_failed_plans is True: metadata.pop("retry_all_failed_plans") return {"type": _qualified_name(value), "fields": metadata} diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index 2df41efafb..d66a1a905a 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -823,6 +823,7 @@ def _decode_scene(value: object) -> SimulationSceneBinding: optional=frozenset( { "rigid_objects", + "cables", "rigidized_articulations", "articulations", "links", @@ -831,6 +832,23 @@ def _decode_scene(value: object) -> SimulationSceneBinding: ), ) + from .simulation.bindings import _SimulationCableBinding + + cables = [] + for index, value in enumerate( + _sequence(config.get("cables", ()), path=f"{path}.cables") + ): + item = _mapping( + value, + path=f"{path}.cables[{index}]", + required=frozenset({"entity_id", "simulation_uid"}), + ) + cables.append( + _SimulationCableBinding( + _identifier(item["entity_id"], path=path), + _identifier(item["simulation_uid"], path=path), + ) + ) antipodal_grasps: list[AntipodalGraspAffordanceBinding] = [] rigidized_articulation_grasps: list[RigidizedArticulationAntipodalGraspBinding] = [] support_surfaces: list[SupportSurfaceAffordanceBinding] = [] @@ -879,6 +897,7 @@ def decode_entities(field_name: str, decoder): return SimulationSceneBinding( registry_id=_identifier(config["registry_id"], path=f"{path}.registry_id"), rigid_objects=rigid_objects, + cables=tuple(cables), rigidized_articulations=rigidized_articulations, articulations=articulations, links=links, @@ -1013,6 +1032,8 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: optional=frozenset( { "hand_interp_steps", + "preserve_samples", + "preserve_cartesian_samples", "hold_steps", "release", "release_at_target", @@ -1256,15 +1277,27 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: _mapping(value, path=path, required=frozenset({"kind"})) return MoveHeldObjectOptions() if kind == "move_joints": - _mapping( + config = _mapping( value, path=path, required=frozenset({"kind"}), + optional=frozenset({"preserve_samples"}), + ) + return MoveJointsOptions( + preserve_samples=_boolean(config.get("preserve_samples", False), path=path) ) - return MoveJointsOptions() if kind == "move_end_effector": - _mapping(value, path=path, required=frozenset({"kind"})) - return MoveEndEffectorOptions() + config = _mapping( + value, + path=path, + required=frozenset({"kind"}), + optional=frozenset({"preserve_cartesian_samples"}), + ) + return MoveEndEffectorOptions( + preserve_cartesian_samples=_boolean( + config.get("preserve_cartesian_samples", False), path=path + ) + ) if kind == "coordinated_pickment": config = _mapping( value, @@ -1681,6 +1714,7 @@ def _decode_motion_policy(value: object, *, path: str) -> MotionPolicy: "velocity_targets", "plan_opts", "interpolation_mode", + "interpolate_position_step", "ik_seed_mode", "sample_count_retries", "segment_sample_count_retries", @@ -1748,7 +1782,11 @@ def _decode_motion_policy(value: object, *, path: str) -> MotionPolicy: path=f"{path}.dynamic_collision_mode", ), } - for field_name in ("velocity_limit", "acceleration_limit"): + for field_name in ( + "velocity_limit", + "acceleration_limit", + "interpolate_position_step", + ): if field_name in config: kwargs[field_name] = ( None @@ -2962,6 +3000,7 @@ class _DecodedRuntimeServices: ] = () control_part_evidence: _JointPositionConstraintEvidenceProviderFactory | None = None nominal_motion_sources: tuple[_ConfiguredPrototypeSource, ...] = () + cable_material: object | None = None settle_presets: ( tuple[ tuple[ @@ -3078,6 +3117,7 @@ def _decode_runtime_services( "registered_semantic_lowerers", "control_part_evidence", "nominal_motion_sources", + "cable_material", "settle_presets", } ), @@ -3128,6 +3168,19 @@ def _decode_runtime_services( ) ) ) + from .simulation.cable import ( + _decode_cable_service, + _CablePickFactory, + _CableMoveFactory, + _CableJointFactory, + _CableCheckFactory, + ) + + cable = ( + None + if "cable_material" not in config + else _decode_cable_service(config["cable_material"]) + ) lowerers = tuple( _decode_registered_lowerer( item, @@ -3140,6 +3193,16 @@ def _decode_runtime_services( ) ) ) + if cable is not None: + lowerers += tuple( + factory(cable) + for factory in ( + _CablePickFactory, + _CableMoveFactory, + _CableJointFactory, + _CableCheckFactory, + ) + ) lowerer_call_ids = tuple(factory.call_id for factory in lowerers) if len(set(lowerer_call_ids)) != len(lowerer_call_ids): raise ValueError( @@ -3296,6 +3359,7 @@ def _decode_runtime_services( hand_geometries=hand_geometries, handover_pose_providers=handover, registered_semantic_lowerers=lowerers, + cable_material=cable, control_part_evidence=evidence, nominal_motion_sources=tuple(nominal), settle_presets=( @@ -3346,6 +3410,7 @@ def _decode_evaluation( robot_profile: SimulationRobotSkillProfileBinding | None = None, robot_links: Mapping[str, str] | None = None, registry: PredicateRegistry | None = None, + cable_material: object | None = None, ) -> tuple[dict[str, CompiledEvaluation], tuple[str, ...], tuple[str, ...]]: if value is None: return {}, (), () @@ -3361,6 +3426,13 @@ def _decode_evaluation( for binding in scene.rigid_objects for quantity in ("pose", "frame", "segment_start_frame", "lin_vel", "ang_vel") } + if cable_material is not None: + from .simulation.cable import _METRICS + + for quantity in _METRICS: + channels[f"cables/{cable_material.entity_id}/{quantity}"] = ( + f"cables/{cable_material.simulation_uid}/{quantity}" + ) articulations = { binding.entity_id: binding.simulation_uid for binding in scene.articulations } @@ -3494,6 +3566,16 @@ def _decode_configured_task_program_integration( config.get("runtime_services", {}), robot_profile_binding=robot_profile_binding ) + if services.cable_material is not None: + cable = services.cable_material + if not any( + binding.entity_id == cable.entity_id + and binding.simulation_uid == cable.simulation_uid + for binding in scene_binding.cables + ): + raise ValueError( + "The cable material service must match one declared scene cable binding." + ) call_catalog = builtin_semantic_call_catalog( include_transport=any( "move_held_object" in preset.skill_option_templates @@ -3514,6 +3596,7 @@ def _decode_configured_task_program_integration( robot_profile=robot_profile_binding, robot_links=evaluation_robot_links, registry=evaluation_registry, + cable_material=services.cable_material, ) registration = SimulationTaskProgramRegistration( scene_binding=scene_binding, diff --git a/embodichain/lab/task_program/integrations/simulation/bindings.py b/embodichain/lab/task_program/integrations/simulation/bindings.py index 0c3e51b2ac..44c855adc8 100644 --- a/embodichain/lab/task_program/integrations/simulation/bindings.py +++ b/embodichain/lab/task_program/integrations/simulation/bindings.py @@ -202,6 +202,18 @@ def _validate_scene_classification( raise TypeError("collision_role must be a SceneCollisionRole value.") +@dataclass(frozen=True, slots=True) +class _SimulationCableBinding: + """Material-state identity, deliberately without a rigid object pose.""" + + entity_id: str + simulation_uid: str + + def __post_init__(self) -> None: + _identifier(self.entity_id, field_name="entity_id") + _identifier(self.simulation_uid, field_name="simulation_uid") + + @dataclass(frozen=True, slots=True) class SimulationRigidObjectBinding: """Explicit binding for one simulation rigid object.""" @@ -813,6 +825,7 @@ class SimulationSceneBinding: """Build one authoritative registry from explicit simulation bindings.""" registry_id: str + cables: tuple[_SimulationCableBinding, ...] = field(default=(), kw_only=True) rigid_objects: tuple[SimulationRigidObjectBinding, ...] = () rigidized_articulations: tuple[ SimulationRigidizedArticulationObjectBinding, ... @@ -830,6 +843,7 @@ class SimulationSceneBinding: def __post_init__(self) -> None: _identifier(self.registry_id, field_name="registry_id") expected_types = { + "cables": _SimulationCableBinding, "rigid_objects": SimulationRigidObjectBinding, "rigidized_articulations": SimulationRigidizedArticulationObjectBinding, "articulations": SimulationArticulationBinding, @@ -1056,6 +1070,11 @@ def build(self, simulation: SimulationManager) -> SceneRegistry: Returns: Immutable registry with typed roots, links, and affordances. """ + for binding in self.cables: + if simulation.get_cable_object(binding.simulation_uid) is None: + raise ValueError( + f"Cable {binding.simulation_uid!r} is absent from the live scene." + ) objects = {item.entity_id: item for item in self.rigid_objects} rigidized_articulations = { item.entity_id: item for item in self.rigidized_articulations diff --git a/embodichain/lab/task_program/integrations/simulation/cable.py b/embodichain/lab/task_program/integrations/simulation/cable.py new file mode 100644 index 0000000000..6cd657ab89 --- /dev/null +++ b/embodichain/lab/task_program/integrations/simulation/cable.py @@ -0,0 +1,861 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Material-coordinate grounding and measured contact for configured programs. + +This service owns no task sequence, controller, motion generator or simulator +step. Program calls use the ordinary Atomic Skills and Gym command bridge. +""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass +import math +from typing import ClassVar, Any + +import torch + +from embodichain.compute.geometry.cable import sample_cable_material, cable_endpoints +from embodichain.compute.geometry._cable_route import _measure_cable_route +from embodichain.lab.sim.atomic_skills import ( + AtomicSkillEngine, + SkillControlOverrides, + JointPositionCommand, + SkillDescriptor, + PlanningContext, + SkillOptions, +) +from embodichain.lab.sim.atomic_skills.primitives.pick_up import ( + PickUp, + ContactGraspGoal, +) +from embodichain.lab.sim.atomic_skills.primitives.move_end_effector import ( + MoveEndEffector, + EndEffectorPoseGoal, +) +from embodichain.lab.sim.atomic_skills.primitives.move_joints import ( + MoveJoints, + JointPositionGoal, +) +from embodichain.lab.sim.motion.planners.utils import interpolate_xpos_batched +from embodichain.lab.task_program.compiler.lowering import ( + RegisteredSemanticLowerer, + SemanticLowering, +) +from embodichain.lab.task_program.language.semantic_contracts import ( + SemanticCallContract, +) +from embodichain.lab.task_program.semantics import ( + RegisteredSemanticCall, + BoundSemanticCall, + SceneRegistry, +) +from ..extensions import RegisteredSemanticLowererFactory + +__all__: list[str] = [] + +from embodichain.compute.geometry._cable_route import _CABLE_METRICS as _METRICS + + +@dataclass(frozen=True, slots=True) +class _CableTarget: + target_id: str + kind: str + material_distance: float | None = None + position: tuple[float | None, float | None, float | None] = (None, None, None) + speed: float = 0.05 + tolerance: float = 0.02 + orientation_tolerance: float = 0.25 + port_screen: bool = False + firm: bool = False + stop_when_seated: bool = False + maximum_correction: float = 0.005 + maximum_z_correction: float = 0.003 + minimum_z: float | None = None + maximum_y: float | None = None + samples: int = 2 + finger_position: float = 0.0475 + condition: str | None = None + release_part: str | None = None + + +@dataclass(frozen=True, slots=True) +class _CableService: + entity_id: str + simulation_uid: str + targets: tuple[_CableTarget, ...] + geometry: tuple[tuple[str, object], ...] + motion_parts: tuple[str, ...] + slip_limit: float = 0.012 + + def target(self, target_id: str) -> _CableTarget: + for target in self.targets: + if target.target_id == target_id: + return target + raise ValueError(f"Unknown cable target {target_id!r}.") + + +class _CableRuntime: + """One bridge's references and metrics, in explicit native arena order.""" + + def __init__( + self, cfg: _CableService, simulation: Any, robot: Any, env_ids: torch.Tensor + ) -> None: + self.cfg, self.simulation, self.robot = cfg, simulation, robot + self.env_ids = env_ids.clone() + self.cable = simulation.get_cable_object(cfg.simulation_uid) + if self.cable is None: + raise ValueError(f"Cable {cfg.simulation_uid!r} is absent.") + arenas = tuple(self.cable.data.batch.arena_indices) + by_arena = {int(value): row for row, value in enumerate(arenas)} + try: + selected_arenas = [ + simulation.spawn_result.arenas.index( + robot.body_data.entities[int(value)].arena_name + ) + for value in env_ids.tolist() + ] + rows = [by_arena[value] for value in selected_arenas] + except KeyError as exc: + raise ValueError( + "The cable does not cover the selected environment rows." + ) from exc + self.rows = torch.tensor(rows, device=env_ids.device) + points = torch.as_tensor( + self.cable.cfg.points, device=env_ids.device, dtype=torch.float32 + ) + self.lengths = points.diff(dim=0).norm(dim=-1) + origins = torch.as_tensor( + simulation.spawn_result.arenas.root_offsets, + device=env_ids.device, + dtype=torch.float32, + ) + self.origins = origins.index_select( + 0, torch.tensor(selected_arenas, device=env_ids.device) + ) + self.geometry = dict(cfg.geometry) + self._scope_guard = lambda: None + self.home = {part: self.tool(part).clone() for part in cfg.motion_parts} + self.pending_pick: tuple[str, _CableTarget] | None = None + self.hand_to_motion: dict[str, str] = {} + self.grasps: dict[str, tuple[float, torch.Tensor]] = {} + self.max_slip = torch.zeros(len(rows), device=env_ids.device) + self.grasp_slip: dict[str, torch.Tensor] = {} + self.last_targets: dict[str, torch.Tensor] = {} + self.pick_initial: dict[str, tuple[float, torch.Tensor]] = {} + self.port_yaw: dict[str, torch.Tensor] = {} + self.closed_reference_samples: list[dict[str, object]] = [] + self._metric_cache_time: float | None = None + self._metric_cache: dict[str, torch.Tensor] = {} + for target in cfg.targets: + if target.material_distance is not None: + # Configuration errors fail before the first command. + sample_cable_material( + self.state(), self.lengths, target.material_distance + ) + + def state(self) -> torch.Tensor: + self._scope_guard() + return self.cable.data.segment_state_w.index_select(0, self.rows) + + def tool( + self, part: str, *, commanded: bool = False, world: bool = False + ) -> torch.Tensor: + self._scope_guard() + joints = self.robot.get_joint_ids(name=part) + qpos = self.robot.get_qpos(target=commanded).index_select(0, self.env_ids)[ + :, joints + ] + pose = self.robot.compute_fk( + qpos, name=part, to_matrix=True, env_ids=self.env_ids + ).clone() + if world: + pose[:, :3, 3] += self.origins + return pose + + def point(self, distance: float) -> tuple[torch.Tensor, torch.Tensor, int]: + return sample_cable_material(self.state(), self.lengths, distance) + + def point_in_tool(self, part: str, distance: float) -> torch.Tensor: + point = self.point(distance)[0] + tool = self.tool(part, world=True) + return ( + tool[:, :3, :3].transpose(1, 2) @ (point - tool[:, :3, 3]).unsqueeze(-1) + ).squeeze(-1) + + def observe(self) -> torch.Tensor: + for part, (distance, baseline) in self.grasps.items(): + slip = (self.point_in_tool(part, distance) - baseline).norm(dim=-1) + self.grasp_slip[part] = torch.maximum(self.grasp_slip[part], slip) + self.max_slip = torch.maximum(self.max_slip, slip) + state = self.state() + invalid = ~torch.isfinite(state).all(dim=(1, 2)) | ~( + state[..., 3:7].norm(dim=-1) > 1e-8 + ).all(-1) + return (self.max_slip > self.cfg.slip_limit) | invalid + + def metrics(self) -> dict[str, torch.Tensor]: + self._scope_guard() + timestamp = getattr(self.simulation, "simulation_time", None) + if timestamp is not None and timestamp == self._metric_cache_time: + return {key: value.clone() for key, value in self._metric_cache.items()} + self.observe() + hands = torch.stack( + [self.tool(part, world=True)[:, :3, 3] for part in self.cfg.motion_parts], 1 + ) + result = _measure_cable_route( + self.state(), + self.lengths, + hands, + self.origins, + self.geometry, + float(self.cable.cfg.radius), + ) + result["max_grasp_slip"] = self.max_slip.clone() + if timestamp is not None: + self._metric_cache_time = timestamp + self._metric_cache = {key: value.clone() for key, value in result.items()} + return result + + def seated(self) -> torch.Tensor: + measurements = self.metrics() + return measurements["release_ready"] & torch.stack( + [ + measurements[key] <= 0.001 + for key in ( + "port_penetration", + "bench_penetration", + "spool_penetration", + ) + ] + ).all(0) + + def after_action(self, action: Any, runtime: Any) -> tuple[torch.Tensor, str]: + """Capture only after Gym acknowledges the final physical close step.""" + self._scope_guard() + phase = ( + runtime._last_sent_command_phase() + if hasattr(runtime, "_last_sent_command_phase") + else None + ) + if ( + self.pending_pick is not None + and action.metadata.get("bridge_action_kind") == "runtime_command" + and phase is not None + ): + if phase[0] == "close" and phase[1]: + part, target = self.pending_pick + distance = target.material_distance + assert distance is not None + self.grasps[part] = ( + distance, + self.point_in_tool(part, distance).clone(), + ) + self.grasp_slip[part] = torch.zeros_like(self.max_slip) + self.pending_pick = None + self.closed_reference_samples.append( + { + "part": part, + "material_distance": distance, + "phase": "close", + "sample_index": phase[2], + } + ) + return self.observe(), "cable_material_state_or_slip_guard_failed" + + def _material_pose(self, target: _CableTarget, part: str) -> torch.Tensor: + assert target.material_distance is not None + point, tangent, _ = self.point(target.material_distance) + yaw = torch.atan2(tangent[:, 1], tangent[:, 0]) + candidates = [] + joints = self.robot.get_joint_ids(name=part) + seed = self.robot.get_qpos().index_select(0, self.env_ids)[:, joints] + for branch, angle in enumerate((yaw, yaw + math.pi)): + pose = torch.eye(4, device=point.device).repeat(len(point), 1, 1) + c, s = torch.cos(angle), torch.sin(angle) + pose[:, 0, 0], pose[:, 0, 1] = c, s + pose[:, 1, 0], pose[:, 1, 1] = s, -c + pose[:, 2, 2] = -1 + pose[:, :3, 3] = point - self.origins + if target.position[2] is not None: + pose[:, 2, 3] = target.position[2] + success, qpos = self.robot.compute_ik( + pose, joint_seed=seed, name=part, env_ids=self.env_ids + ) + if target.port_screen: + stage = pose.clone() + port_angle = math.pi / 2 if branch == 0 else -math.pi / 2 + stage[:, 0, 0], stage[:, 0, 1] = math.cos(port_angle), math.sin( + port_angle + ) + stage[:, 1, 0], stage[:, 1, 1] = math.sin(port_angle), -math.cos( + port_angle + ) + stage[:, :3, 3] = point.new_tensor(self.geometry["port_stage"]) + later, _ = self.robot.compute_ik( + stage, joint_seed=qpos, name=part, env_ids=self.env_ids + ) + success = success.reshape(-1).bool() & later.reshape(-1).bool() + candidates.append((success.reshape(-1).bool(), qpos, pose)) + first, second = candidates + choose = second[0] & ( + ~first[0] + | ((second[1] - seed).square().sum(-1) < (first[1] - seed).square().sum(-1)) + ) + if not bool((first[0] | second[0]).all()): + raise ValueError("No reachable wrist branch for the cable material target.") + if target.port_screen: + self.port_yaw[part] = torch.where(choose, -math.pi / 2, math.pi / 2) + return torch.where(choose[:, None, None], second[2], first[2]) + + def pose(self, target: _CableTarget, part: str) -> torch.Tensor: + if target.kind == "material": + return self._material_pose(target, part) + measured = self.tool(part) + if target.kind == "home": + return self.home[part].clone() + pose = self.tool(part, commanded=target.kind in {"command", "tip_align"}) + if target.kind == "port_stage": + angle = self.port_yaw[part] + c, s = torch.cos(angle), torch.sin(angle) + pose[:, 0, 0], pose[:, 0, 1] = c, s + pose[:, 1, 0], pose[:, 1, 1] = s, -c + pose[:, 2, 2] = -1 + if target.kind in {"yaw_align", "ray_align"}: + assert target.material_distance is not None + point = self.point(target.material_distance)[0] - self.origins + end = cable_endpoints(self.state(), self.lengths)[1][:, -1] - self.origins + direction = end - point + if target.kind == "yaw_align": + angle = torch.atan2(direction[:, 0], direction[:, 1]) + c, s = torch.cos(angle), torch.sin(angle) + rotation = torch.eye(3, device=pose.device).repeat(len(pose), 1, 1) + rotation[:, 0, 0], rotation[:, 0, 1] = c, -s + rotation[:, 1, 0], rotation[:, 1, 1] = s, c + pose[:, :3, :3] = rotation @ pose[:, :3, :3] + else: + if not bool((direction[:, 1] > 1e-6).all()): + raise ValueError( + "The distal cable span points away from the channel." + ) + mouth_x, mouth_y = self.geometry["port_mouth"] + meets = point[:, 0] + direction[:, 0] / direction[:, 1] * ( + mouth_y - point[:, 1] + ) + pose[:, 0, 3] += mouth_x - meets + elif target.kind == "tip_align": + end = cable_endpoints(self.state(), self.lengths)[1][:, -1] - self.origins + for axis, value in enumerate(target.position): + if value is not None: + pose[:, axis, 3] += (value - end[:, axis]).clamp( + -( + target.maximum_z_correction + if axis == 2 + else target.maximum_correction + ), + ( + target.maximum_z_correction + if axis == 2 + else target.maximum_correction + ), + ) + if target.maximum_y is not None: + pose[:, 1, 3].clamp_(max=target.maximum_y) + if target.minimum_z is not None: + pose[:, 2, 3].clamp_(min=target.minimum_z) + else: + for axis, value in enumerate(target.position): + if value is not None: + pose[:, axis, 3] = value + if target.stop_when_seated: + pose = torch.where(self.seated()[:, None, None], measured, pose) + return pose + + +class _CableLowerer(RegisteredSemanticLowerer): + preserves_symbolic_state: ClassVar[bool] = True + + def __init__(self, cfg: _CableService) -> None: + self.cfg = cfg + self.runtime: _CableRuntime | None = None + + def _target( + self, call: RegisteredSemanticCall + ) -> tuple[_CableRuntime, _CableTarget]: + if dict(call.arguments).keys() != {"target"}: + raise ValueError( + "Cable calls require exactly one declared target identifier." + ) + if self.runtime is None: + raise RuntimeError("Cable service was not bound to this runtime assembly.") + return self.runtime, self.cfg.target(call.arguments["target"]) + + @staticmethod + def _part(bound: BoundSemanticCall, endpoint: str = "motion") -> str: + return ( + bound.binding.resources["primary"] + .endpoints[endpoint] + .runtime_target.control_part + ) + + +class _CablePickLowerer(_CableLowerer): + call_id: ClassVar[str] = "simulation.cable_pick" + target_descriptor: ClassVar[SkillDescriptor] = PickUp.descriptor() + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: SkillOptions, + ) -> SemanticLowering: + service, target = self._target(call) + if target.kind != "material": + raise ValueError("Cable pickup requires a material target.") + part = self._part(bound) + service.pending_pick = (part, target) + service.hand_to_motion[self._part(bound, "grasp")] = part + assert target.material_distance is not None + service.pick_initial[part] = ( + target.material_distance, + service.point(target.material_distance)[0][:, 2].clone(), + ) + overrides = SkillControlOverrides() + if target.firm: + grasp_part = self._part(bound, "grasp") + count = len(service.robot.get_joint_ids(name=grasp_part)) + overrides = SkillControlOverrides( + endpoints={ + "primary": { + "grasp": { + "grasp": JointPositionCommand( + torch.zeros(count, device=context.env_ids.device) + ) + } + } + } + ) + return SemanticLowering( + goal=ContactGraspGoal(service.pose(target, part)), + control_overrides=overrides, + ) + + +class _CableMoveLowerer(_CableLowerer): + call_id: ClassVar[str] = "simulation.cable_move" + target_descriptor: ClassVar[SkillDescriptor] = MoveEndEffector.descriptor() + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: SkillOptions, + ) -> SemanticLowering: + service, target = self._target(call) + part = self._part(bound) + start, end = service.tool(part), service.pose(target, part) + distance = (start[:, :3, 3] - end[:, :3, 3]).norm(dim=-1).max().item() + trace = ( + (start[:, :3, :3].transpose(1, 2) @ end[:, :3, :3]) + .diagonal(dim1=-2, dim2=-1) + .sum(-1) + ) + angle = torch.acos(((trace - 1) / 2).clamp(-1, 1)).max().item() + count = max( + 3, + math.ceil( + max(distance / target.speed, angle / 0.5) / context.require_control_dt() + ) + + 1, + ) + service.last_targets[part] = end.clone() + return SemanticLowering( + goal=EndEffectorPoseGoal(interpolate_xpos_batched(start, end, count)[:, 1:]) + ) + + +class _CableJointLowerer(_CableLowerer): + call_id: ClassVar[str] = "simulation.cable_joint" + target_descriptor: ClassVar[SkillDescriptor] = MoveJoints.descriptor() + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: SkillOptions, + ) -> SemanticLowering: + service, target = self._target(call) + part = self._part(bound) + ids = service.robot.get_joint_ids(name=part) + start = service.robot.get_qpos(target=True).index_select(0, context.env_ids)[ + :, ids + ] + path = start[:, None, :].repeat(1, target.samples, 1) + if target.kind == "open": + # Only release the grasp whose configured gripper is being opened. + motion_part = target.release_part + if ( + motion_part not in self.cfg.motion_parts + or service.hand_to_motion.get(part) != motion_part + ): + raise ValueError("Opening a hand requires its declared motion part.") + service.grasps.pop(motion_part, None) + service.pending_pick = None + path = torch.lerp( + start[:, None], + torch.full_like(start[:, None], target.finger_position), + torch.linspace(0, 1, target.samples, device=start.device)[ + None, :, None + ], + ) + elif target.kind != "hold": + raise ValueError("Cable joint targets must be open or hold declarations.") + return SemanticLowering(goal=JointPositionGoal(path)) + + +class _CableCheckLowerer(_CableLowerer): + call_id: ClassVar[str] = "simulation.cable_check" + target_descriptor: ClassVar[SkillDescriptor] = MoveJoints.descriptor() + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: SkillOptions, + ) -> SemanticLowering: + service, target = self._target(call) + part = self._part(bound) + passed = ~service.observe() + details = {} + if target.condition == "pose": + expected, actual = service.last_targets[part], service.tool(part) + error = (actual[:, :3, 3] - expected[:, :3, 3]).norm(dim=-1) + trace = ( + (actual[:, :3, :3].transpose(1, 2) @ expected[:, :3, :3]) + .diagonal(dim1=-2, dim2=-1) + .sum(-1) + ) + angle = torch.acos(((trace - 1) / 2).clamp(-1, 1)) + passed &= (error <= target.tolerance) & ( + angle <= target.orientation_tolerance + ) + if target.stop_when_seated: + passed |= service.seated() & ~service.observe() + details = { + "position_error_m": error.tolist(), + "orientation_error_rad": angle.tolist(), + } + elif target.condition == "lift": + distance, before = service.pick_initial[part] + lift = service.point(distance)[0][:, 2] - before + passed &= (lift > 0.008) & (part in service.grasps) + details = {"material_lift_m": lift.tolist()} + elif target.condition == "seated": + passed &= service.seated() + else: + raise ValueError("Unknown declared cable precondition.") + if not bool(passed.all()): + raise ValueError( + f"Cable condition {target.target_id!r} failed in envs {context.env_ids[~passed].tolist()}: {details}; max_slip_m={service.max_slip.tolist()}" + ) + ids = service.robot.get_joint_ids(name=part) + qpos = service.robot.get_qpos(target=True).index_select(0, context.env_ids)[ + :, ids + ] + return SemanticLowering(goal=JointPositionGoal(qpos[:, None].repeat(1, 2, 1))) + + +@dataclass(frozen=True, slots=True) +class _CableFactory(RegisteredSemanticLowererFactory): + cfg: _CableService + revision: ClassVar[str] = "1" + lowerer_type: ClassVar[type[_CableLowerer]] + + @property + def argument_contract(self) -> SemanticCallContract: + return SemanticCallContract( + required={ + "target": tuple( + target.target_id + for target in self.cfg.targets + if self._supports(target) + ) + }, + optional={}, + ) + + def _supports(self, target: _CableTarget) -> bool: + if self.call_id == "simulation.cable_pick": + return target.kind == "material" + if self.call_id == "simulation.cable_joint": + return target.kind in {"open", "hold"} + if self.call_id == "simulation.cable_check": + return target.kind == "check" + return target.kind not in {"open", "hold", "check"} + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicSkillEngine, + ) -> RegisteredSemanticLowerer: + if engine.robot is not robot: + raise ValueError("Cable services must use the engine's exact robot.") + lowerer = self.lowerer_type(self.cfg) + selected = engine.planning_services.env_ids + ids = ( + torch.arange(robot.get_qpos().shape[0], device=engine.device) + if selected is None + else torch.tensor(selected, dtype=torch.long, device=engine.device) + ) + lowerer.runtime = _CableRuntime(self.cfg, simulation, robot, ids) + return lowerer + + +@dataclass(frozen=True, slots=True) +class _CablePickFactory(_CableFactory): + call_id: ClassVar[str] = _CablePickLowerer.call_id + target_descriptor: ClassVar[SkillDescriptor] = PickUp.descriptor() + lowerer_type: ClassVar[type[_CableLowerer]] = _CablePickLowerer + + +@dataclass(frozen=True, slots=True) +class _CableMoveFactory(_CableFactory): + call_id: ClassVar[str] = _CableMoveLowerer.call_id + target_descriptor: ClassVar[SkillDescriptor] = MoveEndEffector.descriptor() + lowerer_type: ClassVar[type[_CableLowerer]] = _CableMoveLowerer + + +@dataclass(frozen=True, slots=True) +class _CableJointFactory(_CableFactory): + call_id: ClassVar[str] = _CableJointLowerer.call_id + target_descriptor: ClassVar[SkillDescriptor] = MoveJoints.descriptor() + lowerer_type: ClassVar[type[_CableLowerer]] = _CableJointLowerer + + +@dataclass(frozen=True, slots=True) +class _CableCheckFactory(_CableFactory): + call_id: ClassVar[str] = _CableCheckLowerer.call_id + target_descriptor: ClassVar[SkillDescriptor] = MoveJoints.descriptor() + lowerer_type: ClassVar[type[_CableLowerer]] = _CableCheckLowerer + + +def _decode_cable_service(value: object) -> _CableService: + from ..configured import _mapping, _identifier, _real, _integer, _boolean, _sequence + + config = _mapping( + value, + path="cable_material", + required=frozenset( + {"entity_id", "simulation_uid", "motion_parts", "geometry", "targets"} + ), + optional=frozenset({"slip_limit"}), + ) + targets = [] + fields = { + field + for field in _CableTarget.__dataclass_fields__ + if field not in {"target_id", "kind"} + } + for name, item in _mapping( + config["targets"], + path="cable_material.targets", + required=frozenset(), + optional=frozenset(config["targets"]), + ).items(): + path = f"cable_material.targets.{name}" + row = _mapping( + item, path=path, required=frozenset({"kind"}), optional=frozenset(fields) + ) + kind = _identifier(row["kind"], path=path) + motion_fields = {"speed", "tolerance", "orientation_tolerance"} + allowed = { + "material": motion_fields + | {"material_distance", "position", "port_screen", "firm"}, + "measured": motion_fields | {"position", "stop_when_seated"}, + "command": motion_fields | {"position", "stop_when_seated"}, + "home": motion_fields, + "port_stage": motion_fields | {"position"}, + "yaw_align": motion_fields | {"material_distance"}, + "ray_align": motion_fields | {"material_distance"}, + "tip_align": motion_fields + | { + "position", + "maximum_correction", + "maximum_z_correction", + "minimum_z", + "maximum_y", + "stop_when_seated", + }, + "hold": {"samples"}, + "open": {"samples", "finger_position", "release_part"}, + "check": { + "condition", + "tolerance", + "orientation_tolerance", + "stop_when_seated", + }, + } + if kind in allowed and set(row) - {"kind"} - allowed[kind]: + raise ValueError(f"Unsupported fields for cable target kind {kind!r}.") + if kind not in { + "material", + "measured", + "command", + "home", + "port_stage", + "yaw_align", + "ray_align", + "tip_align", + "hold", + "open", + "check", + }: + raise ValueError(f"Unsupported cable target kind {kind!r}") + kwargs = {"target_id": _identifier(name, path=path), "kind": kind} + for key, val in row.items(): + if key == "kind": + continue + if key == "position": + values = _sequence(val, path=path) + if len(values) != 3: + raise ValueError("Cable position needs exactly three axes.") + kwargs[key] = tuple( + None if v is None else _real(v, path=path) for v in values + ) + elif key in {"port_screen", "firm", "stop_when_seated"}: + kwargs[key] = _boolean(val, path=path) + elif key == "samples": + kwargs[key] = _integer(val, path=path, minimum=1) + elif key in {"condition", "release_part"}: + kwargs[key] = _identifier(val, path=path) + else: + kwargs[key] = _real(val, path=path) + target = _CableTarget(**kwargs) + if ( + target.speed <= 0 + or target.tolerance <= 0 + or target.orientation_tolerance <= 0 + or target.maximum_correction <= 0 + ): + raise ValueError("Cable motion limits must be positive.") + if kind in {"material", "yaw_align", "ray_align"} and ( + target.material_distance is None or target.material_distance < 0 + ): + raise ValueError("A cable material coordinate must be nonnegative.") + if kind == "open" and target.release_part is None: + raise ValueError("An opening target requires its explicit release_part.") + if kind == "check" and target.condition not in {"pose", "lift", "seated"}: + raise ValueError("Unknown cable check condition.") + targets.append(target) + geometry = _mapping( + config["geometry"], + path="cable_material.geometry", + required=frozenset( + { + "bench_height", + "port_center", + "port_stage", + "port_mouth", + "seats", + "cylinders", + "boxes", + "strict_region", + "release_region", + } + ), + ) + + def freeze(value: object) -> object: + if isinstance(value, (list, tuple)): + return tuple(freeze(item) for item in value) + return _real(value, path="cable_material.geometry") + + frozen_geometry = {key: freeze(value) for key, value in geometry.items()} + expected_widths = {"port_center": 2, "port_stage": 3, "port_mouth": 2} + for key, width in expected_widths.items(): + if ( + not isinstance(frozen_geometry[key], tuple) + or len(frozen_geometry[key]) != width + ): + raise ValueError(f"Cable geometry {key!r} requires {width} coordinates.") + for key in ("strict_region", "release_region"): + bounds = frozen_geometry[key] + if ( + not isinstance(bounds, tuple) + or len(bounds) != 2 + or any(not isinstance(row, tuple) or len(row) != 3 for row in bounds) + or any(lo > hi for lo, hi in zip(*bounds)) + ): + raise ValueError(f"Cable geometry {key!r} requires ordered XYZ bounds.") + for key, width in (("seats", 3), ("cylinders", 5)): + if not isinstance(frozen_geometry[key], tuple) or any( + not isinstance(row, tuple) or len(row) != width + for row in frozen_geometry[key] + ): + raise ValueError(f"Cable geometry {key!r} has invalid rows.") + if any( + radius <= 0 or z0 >= z1 for _, _, radius, z0, z1 in frozen_geometry["cylinders"] + ): + raise ValueError("Cable obstacle cylinders require positive radii and heights.") + if len(frozen_geometry["seats"]) != 2: + raise ValueError( + "The two-seat cable route measurements require exactly two seats." + ) + if not isinstance(frozen_geometry["boxes"], tuple) or any( + not isinstance(row, tuple) + or len(row) != 2 + or any(not isinstance(vector, tuple) or len(vector) != 3 for vector in row) + or any(v <= 0 for v in row[1]) + for row in frozen_geometry["boxes"] + ): + raise ValueError( + "Cable obstacle boxes require centres and positive half sizes." + ) + parts = tuple( + _identifier(part, path="cable_material.motion_parts") + for part in _sequence( + config["motion_parts"], path="cable_material.motion_parts" + ) + ) + if not parts or len(set(parts)) != len(parts): + raise ValueError("Cable motion parts must be nonempty and unique.") + return _CableService( + entity_id=_identifier(config["entity_id"], path="cable_material.entity_id"), + simulation_uid=_identifier( + config["simulation_uid"], path="cable_material.simulation_uid" + ), + targets=tuple(targets), + geometry=tuple(frozen_geometry.items()), + motion_parts=parts, + slip_limit=_real( + config.get("slip_limit", 0.012), + path="cable_material.slip_limit", + minimum=0.0, + strict_minimum=True, + ), + ) diff --git a/embodichain/lab/task_program/integrations/simulation/environment.py b/embodichain/lab/task_program/integrations/simulation/environment.py index 6e1628d5f5..fd6eea752e 100644 --- a/embodichain/lab/task_program/integrations/simulation/environment.py +++ b/embodichain/lab/task_program/integrations/simulation/environment.py @@ -35,7 +35,7 @@ from copy import deepcopy from dataclasses import dataclass, fields import math -from weakref import ReferenceType, ref +from weakref import ReferenceType, ref, proxy from types import MappingProxyType from typing import Literal, Protocol, TYPE_CHECKING @@ -915,7 +915,13 @@ def validate_runtime_bridge_creation(self) -> None: Non-support factories retain their existing independent assembly behavior. """ self._owner_token.require_open() - if self._support_registry is None or self._runtime_bridge_ref is None: + has_cable = any( + factory.call_id.startswith("simulation.cable_") + for factory in self._registration.registered_semantic_lowerer_factories + ) + if ( + self._support_registry is None and not has_cable + ) or self._runtime_bridge_ref is None: return previous = self._runtime_bridge_ref() if previous is not None and not previous.execution_finished: @@ -938,7 +944,10 @@ def bind_runtime_bridge(self, bridge: TaskProgramDemoBridge) -> None: raise ValueError( "The runtime bridge must use this factory's assembled clock." ) - if self._support_registry is not None: + cable = getattr(self, "_pending_cable_runtime", None) + if cable is not None: + bridge.post_policy_port._cable_runtime = cable + if self._support_registry is not None or cable is not None: self._runtime_bridge_ref = ref(bridge) def close_runtime_resources(self) -> None: @@ -1104,13 +1113,38 @@ def create_registered_semantic_lowerers( engine: AtomicSkillEngine, ) -> tuple[RegisteredSemanticLowerer, ...]: """Create fresh registration-owned lowerers for this runtime assembly.""" - return self._registration.create_registered_semantic_lowerers( + self.validate_runtime_bridge_creation() + lowerers = self._registration.create_registered_semantic_lowerers( simulation=self._simulation, robot=self._robot, scene_registry=scene_registry, engine=engine, ) + from .cable import _CableLowerer, _CableRuntime + + cable_lowerers = tuple( + value for value in lowerers if isinstance(value, _CableLowerer) + ) + self._pending_cable_runtime = None + if cable_lowerers: + cfg = cable_lowerers[0].cfg + if any(value.cfg != cfg for value in cable_lowerers): + raise ValueError( + "One bridge must use one consistent cable service declaration." + ) + session = cable_lowerers[0].runtime + if session is None or not torch.equal(session.env_ids, self._env_ids): + raise ValueError( + "Cable service rows must match this factory's explicit selection." + ) + for lowerer in cable_lowerers: + # Registration remembers lowerer identities; it must not retain + # every episode's live cable service through that history. + lowerer.runtime = proxy(session) + self._pending_cable_runtime = session + return lowerers + def create_scene_registry(self) -> SceneRegistry: """Build one fresh authoritative registry from the task registration.""" self._owner_token.require_open() @@ -1196,6 +1230,11 @@ def create_planning_observation_provider( require_open=self._owner_token.require_open, runtime_scope_guard=runtime_scope_guard, ) + cable = getattr(self, "_pending_cable_runtime", None) + if cable is not None: + cable._scope_guard = _runtime_scope_validator( + self._owner_token.require_open, runtime_scope_guard + ) self._segment_policy_port.bind_clock(clock) self._support_clock = clock return SimulationPlanningObservationProvider( @@ -1206,7 +1245,9 @@ def create_planning_observation_provider( owner_token=self._owner_token, runtime_scope_guard=runtime_scope_guard, capture_root_pose=self._capture_root_pose, - hold_qpos_source=self._hold_qpos_source, + hold_qpos_source=( + "drive_target" if cable is not None else self._hold_qpos_source + ), ) def create_effect_evidence_providers( diff --git a/embodichain/lab/task_program/integrations/simulation/policies.py b/embodichain/lab/task_program/integrations/simulation/policies.py index 339d6185ba..cd6e14306b 100644 --- a/embodichain/lab/task_program/integrations/simulation/policies.py +++ b/embodichain/lab/task_program/integrations/simulation/policies.py @@ -450,6 +450,23 @@ def settle_preset_ids(self) -> tuple[str, ...]: """Return installed post-policy preset IDs in declaration order.""" return tuple(self._settle_presets) + def _observe_consumed_action( + self, action: Any, *, runtime: Any + ) -> tuple[torch.Tensor, torch.Tensor, str] | None: + """Measure provider-owned invariants after a host-acknowledged command. + + Returns row rejection and a proposed hold. The bridge remains the only + owner of command encoding and dispatch; this port never steps physics. + """ + cable = getattr(self, "_cable_runtime", None) + if cable is None: + return None + rejected, reason = cable.after_action(action, runtime) + if not bool(rejected.any()): + return None + hold = self._robot.get_qpos(target=True).index_select(0, self._env_ids) + return rejected, hold, reason + def new_task_program_session(self) -> SimulationSegmentPolicyPort: """Create fresh episode state while retaining the validated scene bindings. @@ -460,6 +477,7 @@ def new_task_program_session(self) -> SimulationSegmentPolicyPort: self._require_open() session = copy(self) session._hold_observer = None + session._cable_runtime = None session._target_observer = None session._post_policy_results = {} session._post_policy_success = {} diff --git a/embodichain/lab/task_program/runtime/executor.py b/embodichain/lab/task_program/runtime/executor.py index c0e0686cda..e6460e15f8 100644 --- a/embodichain/lab/task_program/runtime/executor.py +++ b/embodichain/lab/task_program/runtime/executor.py @@ -492,6 +492,17 @@ def last_sent_plan_reference(self) -> tuple[int, str | None, int] | None: """ return self._last_sent_plan_reference + def _last_sent_command_phase(self) -> tuple[str, bool, int] | None: + reference = self._last_sent_plan_reference + if reference is None or self._runner is None: + return None + plan = self._runner.session._plan + if plan is None: + return None + index = reference[2] + segment = plan.segment_at(index) + return segment.name, index == segment.stop - 1, index + @property def result(self) -> SemanticExecutionResult: """Return an immutable snapshot of the current workflow.""" diff --git a/extensions/embodichain_tasks/configs/components/embodiments/yam_i2rt.yaml b/extensions/embodichain_tasks/configs/components/embodiments/yam_i2rt.yaml index 37dcad9475..ca423d557a 100644 --- a/extensions/embodichain_tasks/configs/components/embodiments/yam_i2rt.yaml +++ b/extensions/embodichain_tasks/configs/components/embodiments/yam_i2rt.yaml @@ -2,7 +2,6 @@ embodiment_id: yam_i2rt simulation: class_type: YamI2rt uid: yam - # Preserve the source MJCF gravcomp setting for this Newton embodiment. root_props: newton_gravity_compensation: 1.0 joint_drive_props: @@ -34,3 +33,72 @@ simulation: ke: 10000.0 kd: 0.1 sensor: [] +skill_profile: + contract_id: dual_arm_parallel_gripper + profile_id: yam_i2rt + resources: + - resource_id: right_manipulator + endpoints: + - endpoint_id: motion + control_part: right_arm + capabilities: + - kinematics.batch_inverse + - motion.cartesian_pose + - kinematics.forward + - motion.joint_position + - endpoint_id: grasp + control_part: right_eef + capabilities: + - interaction.grasp + - motion.joint_position + command_preset: right_jaw + - resource_id: right_hand + endpoints: + - endpoint_id: motion + control_part: right_eef + capabilities: + - motion.joint_position + members: + - right_manipulator + - resource_id: left_manipulator + endpoints: + - endpoint_id: motion + control_part: left_arm + capabilities: + - kinematics.batch_inverse + - motion.cartesian_pose + - kinematics.forward + - motion.joint_position + - endpoint_id: grasp + control_part: left_eef + capabilities: + - interaction.grasp + - motion.joint_position + command_preset: left_jaw + - resource_id: left_hand + endpoints: + - endpoint_id: motion + control_part: left_eef + capabilities: + - motion.joint_position + members: + - left_manipulator + command_presets: + - preset_id: right_jaw + control_part: right_eef + commands: + open: + - 0.0475 + - 0.0475 + grasp: + - 0.003 + - 0.003 + - preset_id: left_jaw + control_part: left_eef + commands: + open: + - 0.0475 + - 0.0475 + grasp: + - 0.003 + - 0.003 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md index 96aa2ce05a..3a641b768a 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md @@ -1,10 +1,11 @@ # Route the Cable (YAM) -This handwritten expert routes a 600 mm anchored cable around two spools and +This configured Task Program routes a 600 mm anchored cable around two spools and places its free end in a 10 mm channel. It adapts the [robosimstudio example](https://github.com/robosimstudio/robosimstudio/tree/73739abe212f963e8a9f5862b27bca370df091a7/examples/08_demos/route_the_cable). -The task uses EmbodiChain's scene components, motion generator, Atomic Skills, -`DemoSegment` executor and trajectory recorder. Only the proximal cable segment +The task uses the common `EmbodiedEnv`, configuration-owned registration, +Semantic Call lowering to existing Atomic Skills, the standard `DemoSegment` +bridge and trajectory recorder. Only the proximal cable segment is fixed; grasps use collision and friction, with no runtime attachment or in-episode state writes. @@ -91,9 +92,10 @@ invalidate grasps. The launcher preserves this read-only policy with `--viser`. Use a separately configured, stepping scene without the expert for manual IK experiments. Failed motion validators log planning status, TCP position error and maximum measured grasp slip with their limits. -Feedback insertion moves may hold the same target for up to one extra second -while physical tracking converges. They retain their original position and -slip tolerances and fail when that bounded settling period is exhausted. +Every motion is followed by an explicit bounded hold and a measured TCP +position/orientation check. Insertion corrections read the latest material +geometry when grounded; the finite correction slots hold the current pose once +the release corridor is reached. They do not run an unbounded Python loop. ## Stages and reusable boundaries @@ -135,10 +137,19 @@ avoidance, and contact response is not a substitute for a clear commanded path. 18.5 mm lift; `MotionPolicy` samples the lift at the previous 0.05 m/s grid. - `compute.geometry.cable` owns simulator-independent capsule endpoints, material-point observations and TCP-relative slip measurement. -- `RouteTheCableEnv.create_demo_segments()` owns feedback, staged execution and - physical validators. Its `milestone="pinch"` or `"first_spool"` options qualify - intermediate stages through `execute_demo_episode`; they do not certify a - complete routing episode. +- `task_program/program.yaml` owns the ordered routing and insertion stages. + `integration.yaml` binds declared material targets and the board geometry. + `execution_policy.yaml` owns the adaptive Cartesian sampling policy. + `evaluation.yaml` requires both spool seats, the strict final channel, + clearance, speed, penetration and the 12 mm episode slip limit. +- The allowlisted `cable_material` service resolves material-coordinate TCP + targets. Calls lower to `PickUp(ContactGraspGoal)`, `MoveEndEffector` and + `MoveJoints`; no cable-specific Atomic Skill or environment subclass exists. + References are captured after the acknowledged close frame, before lift, + and belong to one bridge/episode and explicit environment rows. +- The deployment still uses the original `RouteTheCable-YAM-v1` ID. Loading + `task.yam.yaml` registers it as the common `EmbodiedEnv`; importing the task + package alone does not register a configuration-owned deployment. - `YamI2rtCfg` resolves the versioned robot bundle from the data registry/cache or an explicit `fpath`. Its bundle retains the original MIT license and pinned provenance. Simulation and FK use @@ -218,3 +229,16 @@ contact needs separate physical qualification; API state/reset tests alone do not prove repeatable frictional routing. Use a fresh process for each trial until that reset path is qualified. Raw trajectory recordings may include failed attempts; select training data using the segment and final outcomes. + +## Task Program migration qualification + +The migration preserves the Newton/DexUni two-way cable solver, scene and robot +contact coefficients. The shared execution policy applies its joint acceleration +budget to all Cartesian moves, so stage durations may be longer than the former +handwritten direct-motion path. The episode budget is 12,000 control steps. + +Static composition/catalog preflight and intermediate physical qualification do +not certify a complete cable task. Full acceptance remains strict after release, +retreat and settling. The comparison report records unsuccessful samples and +their measured stopping stage; changing orchestration does not by itself solve +physical insertion. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/catalog.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/catalog.yaml index 03fef482bf..4711589548 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/catalog.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/catalog.yaml @@ -1,7 +1,12 @@ task_key: route_the_cable title: Route the Cable -summary: Frictional, shape-feedback cable routing around two spools and into a port. -tags: [cable, deformable, manipulation, handwritten_expert] +summary: Task Program with material-coordinate pinches, measured slip and bounded + routing feedback. +tags: +- cable +- deformable +- manipulation +- task_program_expert default_deployment: yam deployments: yam: diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml index 4fe3737c30..160c882820 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml @@ -16,7 +16,7 @@ physics_config: rigid_body_contact_buffer_size: 256 collision_options: contact_matching: sticky -max_episode_steps: 6000 +max_episode_steps: 12000 num_envs: 1 arena_space: 2.0 simulation: diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml index 1a135472eb..34dad4c17c 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml @@ -12,3 +12,9 @@ visualization: collection: target_episodes: 1 max_attempts: 1 +task_program: + program: task_program/program.yaml + integration: task_program/integration.yaml + execution_policy: task_program/execution_policy.yaml +evaluation: + component: task_program/evaluation.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/evaluation.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/evaluation.yaml new file mode 100644 index 0000000000..6d47a2728d --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/evaluation.yaml @@ -0,0 +1,104 @@ +graph: + roots: + - route_success + rules: + seat_0: + kind: scalar_range + bindings: + value: cables/cable/seat_0 + params: + minimum: 2 + maximum: 50 + seat_1: + kind: scalar_range + bindings: + value: cables/cable/seat_1 + params: + minimum: 2 + maximum: 50 + port_count: + kind: scalar_range + bindings: + value: cables/cable/port_count + params: + minimum: 3 + maximum: 50 + end_in_port: + kind: scalar_range + bindings: + value: cables/cable/end_in_port + params: + minimum: 1 + maximum: 1 + hands_clearance: + kind: scalar_range + bindings: + value: cables/cable/hands_clearance + params: + minimum: 0.05 + maximum: 100 + minimum_inclusive: false + max_speed: + kind: scalar_range + bindings: + value: cables/cable/max_speed + params: + minimum: 0 + maximum: 0.05 + bench_penetration: + kind: scalar_range + bindings: + value: cables/cable/bench_penetration + params: + minimum: 0 + maximum: 0.001 + spool_penetration: + kind: scalar_range + bindings: + value: cables/cable/spool_penetration + params: + minimum: 0 + maximum: 0.001 + port_penetration: + kind: scalar_range + bindings: + value: cables/cable/port_penetration + params: + minimum: 0 + maximum: 0.001 + max_grasp_slip: + kind: scalar_range + bindings: + value: cables/cable/max_grasp_slip + params: + minimum: 0 + maximum: 0.012 + strict_port: + kind: inside_region + bindings: + position: cables/cable/end_position + params: + lower: + - 0.5845 + - 0.064 + - 0.753 + upper: + - 0.5875 + - 0.0896 + - 0.759 + route_success: + kind: all + refs: + - seat_0 + - seat_1 + - port_count + - end_in_port + - hands_clearance + - max_speed + - bench_penetration + - spool_penetration + - port_penetration + - max_grasp_slip + - strict_port +final_acceptance: +- route_success diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.yaml new file mode 100644 index 0000000000..38c6d2bc7a --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.yaml @@ -0,0 +1,24 @@ +policy_id: cable_sampled_contact_v1 +preset_id: cable +requires: + embodiment_contract: dual_arm_parallel_gripper +planner: + type: trapezoidal +motion: + strategy: ik_interp + velocity_targets: auto + sample_count: 73 + interpolate_position_step: 0.0008333333333333334 + dynamic_collision_mode: 'off' +tracking: + kind: timed +recovery: + max_replans: 0 + max_skill_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/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml new file mode 100644 index 0000000000..f440a0ec6c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml @@ -0,0 +1,710 @@ +integration_id: route_the_cable_v1 +program_id: route_the_cable +requires: + scene_contract: cable_board_v1 + embodiment_contract: dual_arm_parallel_gripper +scene_binding: + contract_id: cable_board_v1 + registry_id: cable_board + cables: + - entity_id: cable + simulation_uid: cable +profile: + defaults: + pick_up: + primary: right_manipulator + move_end_effector: + primary: right_manipulator + move_joints: + primary: right_manipulator + skill_options: + simulation.cable_pick: + kind: pick_up + hand_interp_steps: 48 + grasp_settle_steps: 21 + lift_height: 0.0185 + pre_grasp_distance: 0.0 + simulation.cable_move: + kind: move_end_effector + preserve_cartesian_samples: true + simulation.cable_joint: + kind: move_joints + preserve_samples: true + simulation.cable_check: + kind: move_joints + preserve_samples: true + effect_monitors: {} +runtime_services: + cable_material: + entity_id: cable + simulation_uid: cable + motion_parts: + - right_arm + - left_arm + slip_limit: 0.012 + geometry: + bench_height: 0.75 + port_center: + - 0.586 + - 0.08 + port_stage: + - 0.586 + - 0.047 + - 0.815 + port_mouth: + - 0.586 + - 0.064 + seats: + - - 0.525 + - -0.26 + - -1 + - - 0.525 + - -0.02 + - 1 + cylinders: + - - 0.525 + - -0.26 + - 0.0125 + - 0 + - 0.0025 + - - 0.525 + - -0.26 + - 0.00475 + - 0.0025 + - 0.019679 + - - 0.525 + - -0.26 + - 0.0125 + - 0.019679 + - 0.0235 + - - 0.525 + - -0.02 + - 0.0125 + - 0 + - 0.0025 + - - 0.525 + - -0.02 + - 0.00475 + - 0.0025 + - 0.019679 + - - 0.525 + - -0.02 + - 0.0125 + - 0.019679 + - 0.0235 + boxes: + - - - 0.5755 + - 0.08 + - 0.75625 + - - 0.0055 + - 0.016 + - 0.00625 + - - - 0.5965 + - 0.08 + - 0.75625 + - - 0.0055 + - 0.016 + - 0.00625 + - - - 0.586 + - 0.0928 + - 0.75625 + - - 0.005 + - 0.0032 + - 0.00625 + strict_region: + - - 0.5845 + - 0.064 + - 0.753 + - - 0.5875 + - 0.0896 + - 0.759 + release_region: + - - 0.585 + - 0.073 + - 0.753 + - - 0.587 + - 0.085 + - 0.766 + targets: + right_approach_0.162: + kind: material + material_distance: 0.162 + position: + - null + - null + - 0.81 + speed: 0.12 + port_screen: false + settle_motion: + kind: hold + samples: 15 + right_approach_0.162_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + right_grasp_0.162: + kind: material + material_distance: 0.162 + position: + - null + - null + - 0.7615 + firm: true + port_screen: false + right_lifted_0.162: + kind: check + condition: lift + raise_0: + kind: measured + position: + - null + - null + - 0.78 + raise_0_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + carry_0: + kind: command + position: + - 0.5 + - -0.255 + - null + carry_0_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + lower_0: + kind: command + position: + - null + - null + - 0.7645 + lower_0_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + release_0: + kind: open + samples: 36 + finger_position: 0.008 + release_part: right_arm + retreat_0: + kind: command + position: + - null + - null + - 0.82 + speed: 0.08 + retreat_0_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + open_clear_of_spool_0: + kind: open + samples: 36 + release_part: right_arm + right_approach_0.282: + kind: material + material_distance: 0.282 + position: + - null + - null + - 0.81 + speed: 0.12 + port_screen: false + right_approach_0.282_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + right_grasp_0.282: + kind: material + material_distance: 0.282 + position: + - null + - null + - 0.7615 + firm: false + port_screen: false + right_lifted_0.282: + kind: check + condition: lift + raise_1: + kind: measured + position: + - null + - null + - 0.78 + raise_1_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + carry_1: + kind: command + position: + - 0.528 + - -0.14 + - null + carry_1_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + lower_1: + kind: command + position: + - null + - null + - 0.7645 + lower_1_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + release_1: + kind: open + samples: 36 + finger_position: 0.008 + release_part: right_arm + retreat_1: + kind: command + position: + - null + - null + - 0.82 + speed: 0.08 + retreat_1_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + open_clear_of_spool_1: + kind: open + samples: 36 + release_part: right_arm + right_approach_0.438: + kind: material + material_distance: 0.438 + position: + - null + - null + - 0.81 + speed: 0.12 + port_screen: false + right_approach_0.438_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + right_grasp_0.438: + kind: material + material_distance: 0.438 + position: + - null + - null + - 0.7615 + firm: true + port_screen: false + right_lifted_0.438: + kind: check + condition: lift + raise_2: + kind: measured + position: + - null + - null + - 0.8 + raise_2_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + carry_2: + kind: command + position: + - 0.565 + - 0.008 + - null + carry_2_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + lower_2: + kind: command + position: + - null + - null + - 0.7645 + lower_2_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + release_2: + kind: open + samples: 36 + finger_position: 0.008 + release_part: right_arm + retreat_2: + kind: command + position: + - null + - null + - 0.82 + speed: 0.08 + retreat_2_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + open_clear_of_spool_2: + kind: open + samples: 36 + release_part: right_arm + home_right: + kind: home + speed: 0.15 + home_right_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + left_approach_0.57: + kind: material + material_distance: 0.57 + position: + - null + - null + - 0.81 + speed: 0.12 + port_screen: true + left_approach_0.57_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + left_grasp_0.57: + kind: material + material_distance: 0.57 + position: + - null + - null + - 0.7615 + firm: true + port_screen: true + left_lifted_0.57: + kind: check + condition: lift + stage_free_end: + kind: port_stage + position: + - 0.586 + - 0.047 + - 0.815 + stage_free_end_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + align_yaw_0: + kind: yaw_align + material_distance: 0.57 + align_yaw_0_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + align_lateral_0: + kind: ray_align + material_distance: 0.57 + speed: 0.03 + align_lateral_0_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + align_yaw_1: + kind: yaw_align + material_distance: 0.57 + align_yaw_1_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + align_lateral_1: + kind: ray_align + material_distance: 0.57 + speed: 0.03 + align_lateral_1_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + lower_channel_0: + kind: command + position: + - null + - null + - 0.79 + speed: 0.025 + tolerance: 0.006 + stop_when_seated: true + lower_channel_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_0_0: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.047 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_0_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_0_1: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.047 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_0_1_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + lower_channel_1: + kind: command + position: + - null + - null + - 0.78 + speed: 0.025 + tolerance: 0.006 + stop_when_seated: true + lower_channel_1_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_1_0: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.047 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_1_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_1_1: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.047 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_1_1_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + lower_channel_2: + kind: command + position: + - null + - null + - 0.772 + speed: 0.025 + tolerance: 0.006 + stop_when_seated: true + lower_channel_2_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_2_0: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.047 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_2_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_2_1: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.047 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_2_1_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + lower_channel_3: + kind: command + position: + - null + - null + - 0.7645 + speed: 0.025 + tolerance: 0.006 + stop_when_seated: true + lower_channel_3_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_3_0: + kind: tip_align + position: + - 0.586 + - 0.077 + - 0.754 + maximum_correction: 0.005 + maximum_y: 0.047 + minimum_z: 0.762 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_3_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_3_1: + kind: tip_align + position: + - 0.586 + - 0.077 + - 0.754 + maximum_correction: 0.005 + maximum_y: 0.047 + minimum_z: 0.762 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_3_1_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + release_alignment: + kind: check + condition: seated + release_free_end: + kind: open + samples: 36 + release_part: left_arm + retreat_free_end: + kind: command + position: + - null + - 0.03 + - 0.82 + speed: 0.08 + retreat_free_end_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + home_left: + kind: home + speed: 0.15 + home_left_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + settle_and_validate: + kind: hold + samples: 120 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.yaml new file mode 100644 index 0000000000..bebe87453d --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.yaml @@ -0,0 +1,1106 @@ +program_id: route_the_cable +instruction: Route the anchored cable around two spools and seat its free end in the + port. +targets: {} +program: +- segment: right_approach_0.162 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: right_approach_0.162 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: right_approach_0.162_tracking + resources: + primary: right_manipulator +- segment: right_pinch_lift_0.162 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pick + arguments: + target: right_grasp_0.162 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: right_lifted_0.162 + resources: + primary: right_manipulator +- segment: raise_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: raise_0 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: raise_0_tracking + resources: + primary: right_manipulator +- segment: carry_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: carry_0 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: carry_0_tracking + resources: + primary: right_manipulator +- segment: lower_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lower_0 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: lower_0_tracking + resources: + primary: right_manipulator +- segment: release_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: release_0 + resources: + primary: right_hand +- segment: retreat_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: retreat_0 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: retreat_0_tracking + resources: + primary: right_manipulator +- segment: open_clear_of_spool_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: open_clear_of_spool_0 + resources: + primary: right_hand +- segment: right_approach_0.282 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: right_approach_0.282 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: right_approach_0.282_tracking + resources: + primary: right_manipulator +- segment: right_pinch_lift_0.282 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pick + arguments: + target: right_grasp_0.282 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: right_lifted_0.282 + resources: + primary: right_manipulator +- segment: raise_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: raise_1 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: raise_1_tracking + resources: + primary: right_manipulator +- segment: carry_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: carry_1 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: carry_1_tracking + resources: + primary: right_manipulator +- segment: lower_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lower_1 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: lower_1_tracking + resources: + primary: right_manipulator +- segment: release_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: release_1 + resources: + primary: right_hand +- segment: retreat_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: retreat_1 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: retreat_1_tracking + resources: + primary: right_manipulator +- segment: open_clear_of_spool_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: open_clear_of_spool_1 + resources: + primary: right_hand +- segment: right_approach_0.438 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: right_approach_0.438 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: right_approach_0.438_tracking + resources: + primary: right_manipulator +- segment: right_pinch_lift_0.438 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pick + arguments: + target: right_grasp_0.438 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: right_lifted_0.438 + resources: + primary: right_manipulator +- segment: raise_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: raise_2 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: raise_2_tracking + resources: + primary: right_manipulator +- segment: carry_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: carry_2 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: carry_2_tracking + resources: + primary: right_manipulator +- segment: lower_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lower_2 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: lower_2_tracking + resources: + primary: right_manipulator +- segment: release_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: release_2 + resources: + primary: right_hand +- segment: retreat_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: retreat_2 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: retreat_2_tracking + resources: + primary: right_manipulator +- segment: open_clear_of_spool_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: open_clear_of_spool_2 + resources: + primary: right_hand +- segment: home_right + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: home_right + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: home_right_tracking + resources: + primary: right_manipulator +- segment: left_approach_0.57 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: left_approach_0.57 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: left_approach_0.57_tracking + resources: + primary: left_manipulator +- segment: left_pinch_lift_0.57 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pick + arguments: + target: left_grasp_0.57 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: left_lifted_0.57 + resources: + primary: left_manipulator +- segment: stage_free_end + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: stage_free_end + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: stage_free_end_tracking + resources: + primary: left_manipulator +- segment: align_yaw_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_yaw_0 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_yaw_0_tracking + resources: + primary: left_manipulator +- segment: align_lateral_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_lateral_0 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_lateral_0_tracking + resources: + primary: left_manipulator +- segment: align_yaw_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_yaw_1 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_yaw_1_tracking + resources: + primary: left_manipulator +- segment: align_lateral_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_lateral_1 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_lateral_1_tracking + resources: + primary: left_manipulator +- segment: lower_channel_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lower_channel_0 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: lower_channel_0_tracking + resources: + primary: left_manipulator +- segment: align_tip_0_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_0_0 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_0_0_tracking + resources: + primary: left_manipulator +- segment: align_tip_0_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_0_1 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_0_1_tracking + resources: + primary: left_manipulator +- segment: lower_channel_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lower_channel_1 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: lower_channel_1_tracking + resources: + primary: left_manipulator +- segment: align_tip_1_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_1_0 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_1_0_tracking + resources: + primary: left_manipulator +- segment: align_tip_1_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_1_1 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_1_1_tracking + resources: + primary: left_manipulator +- segment: lower_channel_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lower_channel_2 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: lower_channel_2_tracking + resources: + primary: left_manipulator +- segment: align_tip_2_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_2_0 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_2_0_tracking + resources: + primary: left_manipulator +- segment: align_tip_2_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_2_1 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_2_1_tracking + resources: + primary: left_manipulator +- segment: lower_channel_3 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lower_channel_3 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: lower_channel_3_tracking + resources: + primary: left_manipulator +- segment: align_tip_3_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_3_0 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_3_0_tracking + resources: + primary: left_manipulator +- segment: align_tip_3_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_3_1 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_3_1_tracking + resources: + primary: left_manipulator +- segment: verify_release_alignment + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: release_alignment + resources: + primary: left_manipulator +- segment: release_free_end + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: release_free_end + resources: + primary: left_hand +- segment: retreat_free_end + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: retreat_free_end + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: retreat_free_end_tracking + resources: + primary: left_manipulator +- segment: home_left + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: home_left + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: home_left_tracking + resources: + primary: left_manipulator +- segment: settle_and_validate + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_and_validate + resources: + primary: left_manipulator + validators: + - kind: condition + ref: route_success diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/route_the_cable.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/route_the_cable.py deleted file mode 100644 index 25c889593c..0000000000 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/route_the_cable.py +++ /dev/null @@ -1,890 +0,0 @@ -# ---------------------------------------------------------------------------- -# 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. -# ---------------------------------------------------------------------------- - -"""Shape-feedback expert for the anchored, two-spool cable routing board. - -The YAM deployment resolves ``YamI2rt/yam_i2rt.urdf`` through the standard data -registry, downloading and caching the versioned robot archive when needed. -Pre-download with ``python -m embodichain data download --name YamI2rt``. -For source regeneration or an offline checkout, run -``python scripts/tools/prepare_yam_i2rt_assets.py`` from the repository root; -that script retains the original source/model hashes and MIT license. -See the task config directory's README for the asset publication status, -source-conversion dependencies and cache overrides. - -Geometry and routine adapted from robosimstudio's Apache-2.0 route_the_cable -example at 73739abe212f963e8a9f5862b27bca370df091a7. Robot assets retain MIT. -https://github.com/robosimstudio/robosimstudio/tree/main/examples/08_demos/route_the_cable -""" - -from __future__ import annotations - -import math -from collections.abc import Callable, Iterable -from typing import Any -import torch -from tensordict import TensorDict -from embodichain.compute.geometry.cable import sample_cable_material, cable_endpoints -from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg, DemoSegment -from embodichain.lab.gym.envs.expert_trajectory import prepare_expert_joint_trajectory -from embodichain.lab.gym.envs.types import ControllerAction -from embodichain.lab.gym.utils.registration import register_env -from embodichain.lab.sim.atomic_skills import ( - AtomicSkillEngine, - SkillInvocation, - SkillControlOverrides, - JointPositionCommand, - ControlPartCommandProfile, - MotionPolicy, - SceneSnapshot, -) -from embodichain.lab.sim.atomic_skills.primitives.pick_up import ( - ContactGraspGoal, - PickUpOptions, -) -from embodichain.lab.sim.atomic_skills.trajectory_ops import build_pose_plan_states -from embodichain.lab.sim.motion.motion_generator import ( - MotionGenerator, - MotionGenCfg, - MotionGenOptions, -) -from embodichain.lab.sim.motion.planners import PlanResult, TrapezoidalPlannerCfg -from embodichain.lab.sim.motion.planners.utils import interpolate_xpos_batched -from embodichain.utils import logger - -__all__ = ["RouteTheCableEnv", "measure_route"] - - -def measure_route( - state: torch.Tensor, - lengths: torch.Tensor, - hands_w: torch.Tensor, - origins: torch.Tensor, -) -> dict[str, torch.Tensor]: - """Measure the reference board goal and sampled geometry diagnostics. - - Args: - state: World cable segment states ``(B,50,13)``. - lengths: Capsule axis rest lengths ``(50,)``. - hands_w: World tool centres ``(B,2,3)``. - origins: World Arena translations ``(B,3)``. - - Returns: - Per-environment counts, speed, clearance and routing acceptance. - """ - p = state[..., :3] - origins[:, None, :] - starts, ends = cable_endpoints(state, lengths) - start_local = starts - origins[:, None, :] - end_local = ends - origins[:, None, :] - valid = torch.isfinite(state).all(dim=(1, 2)) - out = {} - for i, (y, side) in enumerate(((-0.26, -1), (-0.02, 1))): - lo, hi = sorted((0.525 + side * 0.003, 0.525 + side * 0.045)) - inside = ( - (p[:, :, 0] >= lo) - & (p[:, :, 0] <= hi) - & ((p[:, :, 1] - y).abs() <= 0.035) - & (p[:, :, 2] >= 0.75) - & (p[:, :, 2] <= 0.90) - ) - out[f"seat_{i}"] = inside.sum(-1) - in_port = ( - ((p[:, :, 0] - 0.586).abs() <= 0.045) - & ((p[:, :, 1] - 0.08).abs() <= 0.045) - & (p[:, :, 2] >= 0.75) - & (p[:, :, 2] <= 0.90) - ) - end = end_local[:, -1] - out["end_position"] = end - out["port_count"] = in_port.sum(-1) - out["end_in_port"] = ( - ((end[:, :2] - end.new_tensor([0.586, 0.08])).abs() <= 0.045).all(-1) - & (end[:, 2] >= 0.75) - & (end[:, 2] <= 0.90) - ) - out["hands_clearance"] = torch.cdist(hands_w, state[..., :3]).amin(dim=(1, 2)) - out["max_speed"] = torch.linalg.vector_norm(state[..., 7:10], dim=-1).amax(-1) - out["bench_penetration"] = ( - 0.75 + 0.0035 - torch.minimum(start_local[..., 2], end_local[..., 2]).amin(-1) - ).clamp_min(0) - # Five points per capsule axis, using the exact capped-cylinder SDF. - axis_samples = ( - start_local[:, :, None, :] - + torch.linspace(0, 1, 5, device=state.device)[None, None, :, None] - * (end_local - start_local)[:, :, None, :] - ) - penetration = torch.zeros(state.shape[0], device=state.device) - for y in (-0.26, -0.02): - for radius, z0, z1 in ( - (0.0125, 0.0, 0.0025), - (0.00475, 0.0025, 0.019679), - (0.0125, 0.019679, 0.0235), - ): - radial = ( - torch.linalg.vector_norm( - axis_samples[..., :2] - state.new_tensor([0.525, y]), dim=-1 - ) - - radius - ) - vertical = (axis_samples[..., 2] - (0.75 + (z0 + z1) / 2)).abs() - ( - z1 - z0 - ) / 2 - q = torch.stack((radial, vertical), -1) - sdf = torch.linalg.vector_norm(q.clamp_min(0), dim=-1) + q.amax( - -1 - ).clamp_max(0) - penetration = torch.maximum( - penetration, (0.0035 - sdf).clamp_min(0).amax(dim=(1, 2)) - ) - out["spool_penetration"] = penetration - port_penetration = torch.zeros_like(penetration) - for center, half_size in ( - ((0.5755, 0.08, 0.75625), (0.0055, 0.016, 0.00625)), - ((0.5965, 0.08, 0.75625), (0.0055, 0.016, 0.00625)), - ((0.586, 0.0928, 0.75625), (0.005, 0.0032, 0.00625)), - ): - q = (axis_samples - state.new_tensor(center)).abs() - state.new_tensor( - half_size - ) - sdf = torch.linalg.vector_norm(q.clamp_min(0), dim=-1) + q.amax(-1).clamp_max(0) - port_penetration = torch.maximum( - port_penetration, (0.0035 - sdf).clamp_min(0).amax(dim=(1, 2)) - ) - out["port_penetration"] = port_penetration - out["strict_port"] = ( - ((end[:, 0] - 0.586).abs() <= 0.0015) - & (end[:, 1] >= 0.064) - & (end[:, 1] <= 0.0896) - & (end[:, 2] >= 0.753) - & (end[:, 2] <= 0.759) - ) - out["release_ready"] = valid & ( - out["strict_port"] - | ( - ((end[:, 0] - 0.586).abs() <= 0.001) - & (end[:, 1] >= 0.073) - & (end[:, 1] <= 0.085) - & (end[:, 2] >= 0.753) - & (end[:, 2] <= 0.766) - ) - ) - out["reference_routed"] = ( - valid - & (out["seat_0"] >= 2) - & (out["seat_1"] >= 2) - & (out["port_count"] >= 3) - & out["end_in_port"] - & (out["hands_clearance"] > 0.05) - & (out["max_speed"] <= 0.05) - ) - return out - - -@register_env("RouteTheCable-YAM-v1", max_episode_steps=6000) -class RouteTheCableEnv(EmbodiedEnv): - """Route with physical pinches and freshly observed material geometry. - - Only the board's proximal end is fixed. No runtime latch, body teleport or - physics-state clear is used inside the expert. Every action uses env.step. - """ - - def __init__(self, cfg: EmbodiedEnvCfg | None = None, **kwargs: Any) -> None: - if cfg is None: - raise ValueError("RouteTheCable requires a configured 50-segment cable.") - cables = [obj for obj in cfg.cable_object if obj.uid == "cable"] - if len(cables) != 1: - raise ValueError("RouteTheCable requires exactly one cable named 'cable'.") - cables[0].validate() - points = torch.as_tensor(cables[0].points) - if tuple(points.shape) != (51, 3): - raise ValueError("RouteTheCable requires the 50-segment reference cable.") - self._grasp = None - self._command = None - self._max_slip = None - self._episode_slip = None - super().__init__(cfg, **kwargs) - try: - self._initialize_routing() - except Exception: - try: - self.close(exit_process=False) - except Exception as cleanup_error: - logger.log_warning( - f"Failed to close routing environment after initialization error: {cleanup_error!r}" - ) - raise - - def _initialize_routing(self) -> None: - """Bind task services only after the shared environment is ready.""" - self.cable = self.sim.get_cable_object("cable") - if self.cable is None or self.cable.data.num_segments != 50: - raise ValueError("RouteTheCable requires the 50-segment reference cable.") - self._lengths = torch.linalg.vector_norm( - torch.as_tensor( - self.cable.cfg.points, device=self.device, dtype=torch.float32 - ).diff(dim=0), - dim=-1, - ) - self._origins = torch.as_tensor( - self.sim.spawn_result.arenas.root_offsets, - device=self.device, - dtype=torch.float32, - )[list(self.cable.data.batch.arena_indices)] - self._motion = MotionGenerator( - MotionGenCfg(planner_cfg=TrapezoidalPlannerCfg(robot_uid=self.robot.uid)) - ) - self._engine = AtomicSkillEngine( - self._motion, - control_profiles={ - f"{side}_eef": ControlPartCommandProfile.joint_positions( - open=torch.full((2,), 0.0475, device=self.device), - # A light distal pinch avoids wedging the tiny cable - # against the fingertips and lifting the compliant arm. - grasp=torch.full( - (2,), - max(float(self.cable.cfg.radius) - 0.0005, 0.0), - device=self.device, - ), - ) - for side in ("right", "left") - }, - ) - self._max_slip = torch.zeros(self.num_envs, device=self.device) - self._episode_slip = torch.zeros_like(self._max_slip) - self._home = self.robot.get_qpos().clone() - self._home_poses = { - side: self._tool(side).clone() for side in ("right", "left") - } - self._command = self._home.clone() - - def _extend_obs(self, obs, **kwargs): - obs = super()._extend_obs(obs, **kwargs) - cable = self.sim.get_cable_object("cable") - if cable is not None and cable.data is not None: - obs["cable_state_w"] = cable.data.segment_state_w - return obs - - def _initialize_episode(self, env_ids, **kwargs) -> None: - super()._initialize_episode(env_ids, **kwargs) - self._grasp = None - if self._max_slip is not None: - self._max_slip[env_ids] = 0 - self._episode_slip[env_ids] = 0 - self._command = self.robot.get_qpos(target=True).clone() - - def _tool(self, side: str, *, world: bool = False) -> torch.Tensor: - ids = self.robot.get_joint_ids(name=side + "_arm") - pose = self.robot.compute_fk( - self.robot.get_qpos()[:, ids], name=side + "_arm", to_matrix=True - ) - if world: - pose = pose.clone() - pose[:, :3, 3] += self._origins - return pose - - def _commanded_tool(self, side: str) -> torch.Tensor: - ids = self.robot.get_joint_ids(name=side + "_arm") - return self.robot.compute_fk( - self._command[:, ids], name=side + "_arm", to_matrix=True - ).clone() - - def _point(self, distance: float): - return sample_cable_material( - self.cable.data.segment_state_w, self._lengths, distance - ) - - def _point_in_tool(self, side: str, distance: float) -> torch.Tensor: - point = self._point(distance)[0] - tool = self._tool(side, world=True) - return torch.bmm( - tool[:, :3, :3].transpose(1, 2), (point - tool[:, :3, 3]).unsqueeze(-1) - ).squeeze(-1) - - def _update_sim_state(self, **kwargs) -> None: - super()._update_sim_state(**kwargs) - if self._grasp is not None: - side, distance, initial = self._grasp - slip = torch.linalg.vector_norm( - self._point_in_tool(side, distance) - initial, dim=-1 - ) - self._max_slip = torch.maximum(self._max_slip, slip) - self._episode_slip = torch.maximum(self._episode_slip, slip) - - def route_metrics(self) -> dict[str, torch.Tensor]: - """Return measured routing and expert-quality metrics. - - Returns: - Per-environment reference goal, strict channel, penetration and - grasp-slip measurements, including the combined success flag. - """ - hands = torch.stack( - [self._tool(s, world=True)[:, :3, 3] for s in ("right", "left")], 1 - ) - out = measure_route( - self.cable.data.segment_state_w, self._lengths, hands, self._origins - ) - out["max_grasp_slip"] = self._episode_slip.clone() - out["success"] = ( - out["reference_routed"] - & out["strict_port"] - & (out["bench_penetration"] <= 0.001) - & (out["spool_penetration"] <= 0.001) - & (out["port_penetration"] <= 0.001) - & (out["max_grasp_slip"] <= 0.012) - ) - return out - - def is_task_success(self) -> torch.Tensor: - """Return the physical routing goal combined with episode grasp quality. - - Returns: - Boolean success flags for each environment. - """ - if not hasattr(self, "_origins"): - return torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) - return self.route_metrics()["success"] - - def _validate_route(self) -> torch.Tensor: - metrics = self.route_metrics() - success = metrics["success"] - if not bool(success.all()): - values = {name: value.cpu().tolist() for name, value in metrics.items()} - logger.log_warning(f"Cable final route rejected: {values}.") - return success - - def _target( - self, point: torch.Tensor, yaw: torch.Tensor, height: float - ) -> torch.Tensor: - pose = torch.eye(4, device=self.device).repeat(self.num_envs, 1, 1) - c, s = torch.cos(yaw), torch.sin(yaw) - pose[:, 0, 0] = c - pose[:, 0, 1] = s - pose[:, 1, 0] = s - pose[:, 1, 1] = -c - pose[:, 2, 2] = -1 - pose[:, :3, 3] = point - self._origins - pose[:, 2, 3] = 0.75 + height - return pose - - def _grasp_pose( - self, distance: float, side: str, height: float, *, port: bool = False - ) -> torch.Tensor: - point, tangent, _ = self._point(distance) - yaw = torch.atan2(tangent[:, 1], tangent[:, 0]) - # Choose the reachable wrist branch without changing cable material ID. - candidates = [] - for branch, angle in enumerate((yaw, yaw + math.pi)): - pose = self._target(point, angle, height) - success, qpos = self.robot.compute_ik( - pose, - joint_seed=self.robot.get_qpos()[ - :, self.robot.get_joint_ids(name=side + "_arm") - ], - name=side + "_arm", - ) - if port: - stage = self._target( - point, - torch.full_like(yaw, math.pi / 2 if branch == 0 else -math.pi / 2), - 0.065, - ) - stage[:, 0, 3] = 0.586 - stage[:, 1, 3] = 0.047 - later, _ = self.robot.compute_ik( - stage, joint_seed=qpos, name=side + "_arm" - ) - success = success.reshape(-1).bool() & later.reshape(-1).bool() - candidates.append((success.reshape(-1).bool(), qpos, pose)) - first, second = candidates - distance0 = ( - ( - first[1] - - self.robot.get_qpos()[:, self.robot.get_joint_ids(name=side + "_arm")] - ) - .square() - .sum(-1) - ) - distance1 = ( - ( - second[1] - - self.robot.get_qpos()[:, self.robot.get_joint_ids(name=side + "_arm")] - ) - .square() - .sum(-1) - ) - choose = second[0] & (~first[0] | (distance1 < distance0)) - if port: - self._port_yaw = torch.where( - choose, - torch.full_like(yaw, -math.pi / 2), - torch.full_like(yaw, math.pi / 2), - ) - return torch.where(choose[:, None, None], second[2], first[2]) - - def _execute( - self, - trajectory: torch.Tensor, - joint_ids: list[int], - *, - closed_at: int | None = None, - grasp_ref=None, - stop_when: Callable[[], torch.Tensor] | None = None, - ) -> Iterable[ControllerAction]: - mode = self.cfg.expert_trajectory.joint_command_mode - velocities = None - if mode == "position_velocity": - # Include the current command as t=0 so velocity estimation spans - # the first executed interval on the fixed control grid. - positions = torch.cat((self._command[:, None, :], trajectory), dim=1) - intervals = torch.full( - positions.shape[:2], - self.step_dt, - dtype=torch.float64, - device=positions.device, - ) - intervals[:, 0] = 0 - prepared = prepare_expert_joint_trajectory( - PlanResult(success=True, positions=positions, dt=intervals), - control_dt=self.step_dt, - joint_command_mode=mode, - ) - trajectory = prepared.positions[:, 1:] - velocities = prepared.velocities[:, 1:] - for index, frame in enumerate(trajectory.unbind(1), start=1): - if stop_when is not None and bool(stop_when().all()): - return - if self._grasp is not None and bool((self._max_slip > 0.012).any()): - return - self._command[:, joint_ids] = frame[:, joint_ids] - if mode == "position_velocity": - velocity = torch.zeros_like(self._command) - velocity[:, joint_ids] = velocities[:, index - 1, joint_ids] - yield ControllerAction( - TensorDict( - {"qpos": self._command, "qvel": velocity}, - batch_size=[self.num_envs], - ) - ) - else: - yield ControllerAction(self._command) - if closed_at == index: - side, distance = grasp_ref - self._max_slip.zero_() - self._grasp = ( - side, - distance, - self._point_in_tool(side, distance).clone(), - ) - - def _move( - self, - side: str, - target: torch.Tensor, - name: str, - *, - speed: float = 0.05, - tolerance: float = 0.02, - goal_reached: Callable[[], torch.Tensor] | None = None, - ) -> DemoSegment: - start = self._tool(side) - distance = ( - torch.linalg.vector_norm(start[:, :3, 3] - target[:, :3, 3], dim=-1) - .max() - .item() - ) - angle = ( - torch.acos( - ( - ( - (start[:, :3, :3].transpose(1, 2) @ target[:, :3, :3]) - .diagonal(dim1=-2, dim2=-1) - .sum(-1) - - 1 - ) - / 2 - ).clamp(-1, 1) - ) - .max() - .item() - ) - count = max(3, math.ceil(max(distance / speed, angle / 0.5) / self.step_dt) + 1) - ids = list(self.robot.get_joint_ids(name=side + "_arm")) - poses = interpolate_xpos_batched(start, target, count)[:, 1:] - result = self._motion.generate( - build_pose_plan_states(poses), - options=MotionGenOptions( - strategy="ik_interp", - start_qpos=self.robot.get_qpos()[:, ids], - control_part=side + "_arm", - sample_count=count, - interpolation_dt=self.step_dt, - is_linear=True, - preserve_cartesian_samples=True, - ), - ) - # The planner may lengthen the control grid to meet joint limits. - planned_count = count if result.positions is None else result.positions.shape[1] - full = self._command[:, None, :].repeat(1, planned_count, 1) - planned = ( - torch.as_tensor(result.success, device=self.device) - .reshape(-1) - .bool() - .clone() - ) - if result.positions is None: - planned.zero_() - else: - full[:, :, ids] = result.positions - - def actions(): - if bool(planned.all()): - yield from self._execute(full, ids, stop_when=goal_reached) - yield from self._hold(0.25, stop_when=goal_reached) - if goal_reached is not None: - # Contact can delay tracking after an insertion correction. - # Allow bounded physical settling, with the same target - # and acceptance limits, before declaring a failed move. - def settled(): - return goal_reached() | ( - torch.linalg.vector_norm( - self._tool(side)[:, :3, 3] - target[:, :3, 3], - dim=-1, - ) - <= tolerance - ) - - yield from self._hold(1.0, stop_when=settled) - - def validate(): - position_error = torch.linalg.vector_norm( - self._tool(side)[:, :3, 3] - target[:, :3, 3], dim=-1 - ) - reached = position_error <= tolerance - if goal_reached is not None: - reached = reached | goal_reached() - accepted = planned & reached & (self._max_slip <= 0.012) - if not bool(accepted.all()): - logger.log_warning( - f"Cable motion segment {name!r} rejected: " - f"planning_success={planned.cpu().tolist()}, " - f"position_error_m={position_error.cpu().tolist()} " - f"(tolerance={tolerance}), " - f"max_grasp_slip_m={self._max_slip.cpu().tolist()} " - "(limit=0.012)." - ) - return accepted - - return DemoSegment( - actions=actions(), - name=name, - target_uid="cable", - metadata={"side": side, "planning_success": planned.cpu().tolist()}, - validator=validate, - ) - - def _hold( - self, seconds: float, *, stop_when: Callable[[], torch.Tensor] | None = None - ) -> Iterable[ControllerAction]: - for _ in range(max(1, math.ceil(seconds / self.step_dt))): - if stop_when is not None and bool(stop_when().all()): - return - if self._grasp is not None and bool((self._max_slip > 0.012).any()): - return - if self.cfg.expert_trajectory.joint_command_mode == "position_velocity": - yield ControllerAction( - TensorDict( - { - "qpos": self._command, - "qvel": torch.zeros_like(self._command), - }, - batch_size=[self.num_envs], - ) - ) - else: - yield ControllerAction(self._command) - - def _pinch( - self, side: str, distance: float, *, port: bool = False, firm: bool = False - ) -> Iterable[DemoSegment]: - yield self._move( - side, - self._grasp_pose(distance, side, 0.06, port=port), - f"{side}_approach_{distance}", - speed=0.12, - ) - before = self._point(distance)[0].clone() - goal = ContactGraspGoal(self._grasp_pose(distance, side, 0.0115, port=port)) - binding = self._engine.bind_control_parts( - "pick_up", - {"primary": {"motion": side + "_arm", "grasp": side + "_eef"}}, - ) - plan = self._engine.plan( - SkillInvocation( - skill_id="pick_up", - goal=goal, - binding=binding, - motion_policy=MotionPolicy( - sample_count=73, interpolate_position_step=0.05 * self.step_dt - ), - skill_options=PickUpOptions( - hand_interp_steps=48, - grasp_settle_steps=math.ceil(0.35 / self.step_dt), - lift_height=0.0185, - pre_grasp_distance=0.0, - ), - control_overrides=( - SkillControlOverrides( - endpoints={ - "primary": { - "grasp": { - "grasp": JointPositionCommand( - torch.zeros(2, device=self.device) - ) - } - } - } - ) - if firm - else SkillControlOverrides() - ), - ), - self._engine.initial_context( - scene=SceneSnapshot(timestamp=0.0, version=0, entities={}), - control_dt=self.step_dt, - ), - ) - trajectory = plan.joint_trajectory.positions - closed_at = next( - segment.stop for segment in plan.segments if segment.name == "close" - ) - ids = list(self.robot.get_joint_ids(name=side + "_arm")) + list( - self.robot.get_joint_ids(name=side + "_eef") - ) - yield DemoSegment( - actions=( - self._execute( - trajectory, ids, closed_at=closed_at, grasp_ref=(side, distance) - ) - if bool(plan.plan_success.all()) - else () - ), - name=f"{side}_pinch_lift_{distance}", - target_uid="cable", - metadata={ - "material_distance": distance, - "firm_grasp": firm, - "planning_success": plan.plan_success.cpu().tolist(), - "phase_lengths": { - segment.name: segment.stop - segment.start - for segment in plan.segments - }, - }, - validator=lambda: plan.plan_success - & (self._point(distance)[0][:, 2] - before[:, 2] > 0.008) - & (self._max_slip <= 0.012), - ) - - def _open( - self, side: str, name: str, *, finger_position: float = 0.0475 - ) -> DemoSegment: - self._grasp = None - self._max_slip.zero_() - ids = list(self.robot.get_joint_ids(name=side + "_eef")) - start = self._command.clone() - count = 36 - trajectory = start[:, None, :].repeat(1, count, 1) - trajectory[:, :, ids] = torch.lerp( - start[:, None, ids], - torch.full_like(start[:, None, ids], finger_position), - torch.linspace(0, 1, count, device=self.device)[None, :, None], - ) - return DemoSegment( - actions=self._execute(trajectory, ids), - name=name, - target_uid="cable", - validator=lambda: torch.isfinite(self.cable.data.segment_state_w).all( - dim=(1, 2) - ), - ) - - def create_demo_segments( - self, *, milestone: str = "full", **kwargs: Any - ) -> Iterable[DemoSegment]: - """Yield freshly planned stages; milestone=pinch qualifies only grasping. - - Args: - milestone: ``pinch``, ``first_spool`` or ``full``. - **kwargs: Reserved collector options. - - Yields: - Bounded action segments with measured row-wise validators. - """ - if milestone not in {"pinch", "first_spool", "full"}: - raise ValueError("Unknown routing milestone.") - self._command = self.robot.get_qpos(target=True).clone() - for stage, (distance, x, y, height) in enumerate( - ( - (0.162, 0.500, -0.255, 0.03), - (0.282, 0.528, -0.140, 0.03), - # Clear the second rim without the extra 15 mm lift that - # exhausted the physical pinch's 12 mm slip budget. - (0.438, 0.565, 0.008, 0.05), - ) - ): - yield from self._pinch("right", distance, firm=stage in {0, 2}) - if milestone == "pinch": - return - target = self._tool("right").clone() - target[:, 2, 3] = 0.75 + height - yield self._move("right", target, f"raise_{stage}") - target = target.clone() - target[:, 0, 3] = x - target[:, 1, 3] = y - yield self._move("right", target, f"carry_{stage}") - target = target.clone() - target[:, 2, 3] = 0.7645 - yield self._move("right", target, f"lower_{stage}") - # Opening fully beside the spool sweeps a finger through its rim. - # First clear the 7 mm cable with a 16 mm jaw aperture, then lift - # above the rim before opening through the full finger stroke. - yield self._open("right", f"release_{stage}", finger_position=0.008) - target = target.clone() - target[:, 2, 3] = 0.82 - yield self._move("right", target, f"retreat_{stage}", speed=0.08) - yield self._open("right", f"open_clear_of_spool_{stage}") - if milestone == "first_spool": - return - yield self._move("right", self._home_poses["right"], "home_right", speed=0.15) - yield from self._place_free_end() - - def _place_free_end(self) -> Iterable[DemoSegment]: - """Observe, align and release the distal span after routing both spools.""" - yield from self._pinch("left", 0.570, port=True, firm=True) - # Stage directly above the mouth. Feeding forward at the old lower - # staging height folded the free span before it reached the channel. - target = self._target(self._point(0.570)[0], self._port_yaw, 0.065) - target[:, :3, 3] = target.new_tensor([0.586, 0.047, 0.815]) - yield self._move("left", target, "stage_free_end") - for correction in range(2): - state = self.cable.data.segment_state_w - point = self._point(0.570)[0] - end = cable_endpoints(state, self._lengths)[1][:, -1] - direction = end - point - angle = torch.atan2(direction[:, 0], direction[:, 1]) - target = self._tool("left").clone() - c, s = torch.cos(angle), torch.sin(angle) - rotation = torch.eye(3, device=self.device).repeat(self.num_envs, 1, 1) - rotation[:, 0, 0] = c - rotation[:, 0, 1] = -s - rotation[:, 1, 0] = s - rotation[:, 1, 1] = c - target[:, :3, :3] = rotation @ target[:, :3, :3] - yield self._move("left", target, f"align_yaw_{correction}") - point = self._point(0.570)[0] - self._origins - end = ( - cable_endpoints(self.cable.data.segment_state_w, self._lengths)[1][ - :, -1 - ] - - self._origins - ) - direction = end - point - if not bool((direction[:, 1] > 1e-6).all()): - yield DemoSegment( - actions=(), - name="reject_backward_free_end", - target_uid="cable", - validator=lambda: torch.zeros( - self.num_envs, dtype=torch.bool, device=self.device - ), - ) - return - meets = point[:, 0] + direction[:, 0] / direction[:, 1].clamp_min(1e-6) * ( - 0.064 - point[:, 1] - ) - target = self._tool("left").clone() - target[:, 0, 3] += 0.586 - meets - yield self._move("left", target, f"align_lateral_{correction}", speed=0.03) - # Correct the actual tip between bounded lowering stages. Accumulate - # corrections on the previous command so contact-load bias is retained. - for level, height in enumerate((0.790, 0.780, 0.772, 0.7645)): - if bool(self._end_release_ready().all()): - break - target = self._commanded_tool("left") - target[:, 2, 3] = height - yield self._move( - "left", - target, - f"lower_channel_{level}", - speed=0.025, - tolerance=0.006, - goal_reached=self._end_release_ready, - ) - for correction in range(2): - if bool(self._end_release_ready().all()): - break - end = ( - cable_endpoints(self.cable.data.segment_state_w, self._lengths)[1][ - :, -1 - ] - - self._origins - ) - target = self._commanded_tool("left") - target[:, :2, 3] += (end.new_tensor([0.586, 0.077]) - end[:, :2]).clamp( - -0.005, 0.005 - ) - target[:, 1, 3].clamp_(max=0.047) - if level == 3: - target[:, 2, 3] += (0.754 - end[:, 2]).clamp(-0.003, 0.003) - target[:, 2, 3].clamp_(min=0.762) - yield self._move( - "left", - target, - f"align_tip_{level}_{correction}", - speed=0.02, - tolerance=0.006, - goal_reached=self._end_release_ready, - ) - - def seated(): - return self._end_release_ready() & (self._max_slip <= 0.012) - - yield DemoSegment( - actions=self._hold(self.step_dt), - name="verify_release_alignment", - target_uid="cable", - validator=seated, - ) - yield self._open("left", "release_free_end") - target = target.clone() - target[:, 1, 3] = 0.030 - target[:, 2, 3] = 0.82 - yield self._move("left", target, "retreat_free_end", speed=0.08) - yield self._move("left", self._home_poses["left"], "home_left", speed=0.15) - yield DemoSegment( - actions=self._hold(2.0), - name="settle_and_validate", - target_uid="cable", - validator=self._validate_route, - ) - - def _end_release_ready(self) -> torch.Tensor: - metrics = self.route_metrics() - return ( - metrics["release_ready"] - & (metrics["port_penetration"] <= 0.001) - & (metrics["bench_penetration"] <= 0.001) - & (metrics["spool_penetration"] <= 0.001) - ) diff --git a/tests/gym/envs/task_program/test_bridge.py b/tests/gym/envs/task_program/test_bridge.py index 608f28cb77..6daa82efb0 100644 --- a/tests/gym/envs/task_program/test_bridge.py +++ b/tests/gym/envs/task_program/test_bridge.py @@ -980,6 +980,14 @@ def test_execution_trace_records_only_consumed_actions_and_rejects_unknown_frame bridge._row_rejection = None bridge._row_hold = None bridge._pending_rejected_ids = () + from types import SimpleNamespace + + observed = [] + bridge._validator_port = SimpleNamespace( + _observe_consumed_action=lambda action, runtime: observed.append(action) + ) + bridge._runtime = object() + bridge._eligible_mask = None lifecycle = bridge_module._SegmentLifecycle() action = ControllerAction( torch.zeros(1, 1), @@ -995,8 +1003,10 @@ def test_execution_trace_records_only_consumed_actions_and_rejects_unknown_frame pending = bridge._yield_and_advance(action, lifecycle) next(pending) assert bridge.execution_trace == () + assert observed == [] with pytest.raises(StopIteration): next(pending) + assert len(observed) == 1 assert bridge.execution_trace == ( { "step": 0, diff --git a/tests/gym/envs/task_program/test_cable_material.py b/tests/gym/envs/task_program/test_cable_material.py new file mode 100644 index 0000000000..4d9a409e69 --- /dev/null +++ b/tests/gym/envs/task_program/test_cable_material.py @@ -0,0 +1,259 @@ +# ---------------------------------------------------------------------------- +# 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 copy import deepcopy +from pathlib import Path +from types import SimpleNamespace +import math + +import pytest +import torch +import yaml + +from embodichain.lab.gym.utils.composition import resolve_gym_components +from embodichain.lab.task_program.integrations import ( + load_configured_task_program_deployment, +) +from embodichain.lab.task_program.integrations.simulation.cable import ( + _decode_cable_service, + _CableRuntime, + _CableCheckLowerer, + _CablePickFactory, +) +from embodichain.lab.task_program.semantics import RegisteredSemanticCall +from embodichain.lab.sim.atomic_skills import MoveJointsOptions + +pytestmark = pytest.mark.no_sim +_ROOT = Path(__file__).resolve().parents[4] +_TASK = ( + _ROOT + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable" +) + + +def _declaration(): + return yaml.safe_load((_TASK / "task_program/integration.yaml").read_text())[ + "runtime_services" + ]["cable_material"] + + +def test_cable_deployment_preflights_all_calls_and_measured_final_acceptance(): + path = _TASK / "task.yam.yaml" + raw = yaml.safe_load(path.read_text()) + physical = resolve_gym_components(raw, base_dir=path.parent) + deployment = load_configured_task_program_deployment( + task_program=physical.config["task_program"], + skill_profile=physical.embodiment_skill_profile, + evaluation=physical.evaluation, + base_dir=path.parent, + ) + physical.validate_scene_bindings(deployment.scene_binding) + registration = deployment.integration.registration + compiled = registration.catalog.preflight(deployment.load_program()) + assert compiled.segment_count == 49 + assert registration.evaluation_final_refs == ("route_success",) + assert registration.scene_binding.declare().entries == () + factories = registration.registered_semantic_lowerer_factories + assert {factory.target_descriptor.skill_id for factory in factories} == { + "pick_up", + "move_end_effector", + "move_joints", + } + for factory in factories: + assert factory.argument_contract.required["target"] + + +@pytest.mark.parametrize( + "change", ["unknown", "negative_coordinate", "bad_geometry", "executable"] +) +def test_cable_configuration_rejects_invalid_closed_payload(change): + value = _declaration() + if change == "unknown": + value["teleport"] = True + elif change == "negative_coordinate": + value["targets"]["right_grasp_0.162"]["material_distance"] = -1 + elif change == "bad_geometry": + value["geometry"]["strict_region"] = [[0, 0, 0]] + else: + value["targets"]["right_grasp_0.162"]["class_type"] = "os.system" + with pytest.raises((ValueError, TypeError)): + _decode_cable_service(value) + + +def test_cable_binding_rejects_missing_physical_uid_before_simulation(): + from embodichain.lab.gym.utils._component_composition import ( + _validate_scene_binding_targets, + ) + + with pytest.raises(ValueError, match="not declared"): + _validate_scene_binding_targets( + {"cables": [{"entity_id": "cable", "simulation_uid": "missing"}]}, + simulation={"cable_object": [{"uid": "cable"}]}, + ) + + +def _runtime(): + cfg = _decode_cable_service(_declaration()) + qpos = torch.zeros(4, 3) + origins = torch.tensor( + [[0, 0, 0], [10, 0, 0], [20, 0, 0], [30, 0, 0], [40, 0, 0]], dtype=torch.float32 + ) + state = torch.zeros(4, 50, 13) + state[..., 4] = math.sqrt(0.5) + state[..., 6] = math.sqrt(0.5) + state[..., 0] = (torch.arange(50) + 0.5) * 0.012 + state[..., :3] += origins[1:, None] + + class Arenas: + root_offsets = origins.numpy() + + def index(self, name): + return int(name[5:]) + + def fk(q, **_): + result = torch.eye(4).repeat(len(q), 1, 1) + result[:, :3, 3] = q + return result + + robot = SimpleNamespace( + body_data=SimpleNamespace( + entities=[SimpleNamespace(arena_name=f"arena{i}") for i in range(1, 5)] + ), + get_joint_ids=lambda **_: [0, 1, 2], + get_qpos=lambda **_: qpos, + compute_fk=fk, + ) + cable = SimpleNamespace( + cfg=SimpleNamespace( + points=[[i * 0.012, 0, 0] for i in range(51)], radius=0.0035 + ), + data=SimpleNamespace( + segment_state_w=state, batch=SimpleNamespace(arena_indices=(1, 2, 3, 4)) + ), + ) + sim = SimpleNamespace( + spawn_result=SimpleNamespace(arenas=Arenas()), get_cable_object=lambda _: cable + ) + service = _CableRuntime(cfg, sim, robot, torch.tensor([3, 1])) + return service, state, qpos + + +def test_material_observation_maps_selected_rows_and_arena_origins_once(): + service, _, _ = _runtime() + torch.testing.assert_close( + service.origins, torch.tensor([[40.0, 0, 0], [20.0, 0, 0]]) + ) + torch.testing.assert_close( + service.point(0.162)[0] - service.origins, + torch.tensor([[0.162, 0, 0], [0.162, 0, 0]]), + atol=4e-6, + rtol=0, + ) + + +def test_material_reference_is_captured_after_close_and_tracks_slip_per_row(): + service, state, qpos = _runtime() + service.pending_pick = ("right_arm", service.cfg.target("right_grasp_0.162")) + action = SimpleNamespace(metadata={"bridge_action_kind": "runtime_command"}) + phase = ["close", False, 5] + runtime = SimpleNamespace(_last_sent_command_phase=lambda: phase) + service.after_action(action, runtime) + assert not service.grasps + # This represents the just-consumed close frame, before the first lift. + qpos[3, 0] = 0.004 + phase[1] = True + service.after_action(action, runtime) + baseline = service.grasps["right_arm"][1].clone() + torch.testing.assert_close( + baseline[:, 0], torch.tensor([0.158, 0.162]), atol=4e-6, rtol=0 + ) + state[3, :, 0] += 0.013 + rejected, _ = service.after_action(action, runtime) + assert rejected.tolist() == [True, False] + torch.testing.assert_close(service.grasps["right_arm"][1], baseline) + fresh, _, _ = _runtime() + assert not fresh.grasps and fresh.max_slip.tolist() == [0, 0] + + +def test_cable_tracking_rejects_wrong_orientation_at_same_position(): + service, _, _ = _runtime() + target = service.tool("left_arm") + target[:, :2, :2] *= -1 + service.last_targets["left_arm"] = target + lowerer = _CableCheckLowerer(service.cfg) + lowerer.runtime = service + endpoint = SimpleNamespace(runtime_target=SimpleNamespace(control_part="left_arm")) + bound = SimpleNamespace( + binding=SimpleNamespace( + resources={"primary": SimpleNamespace(endpoints={"motion": endpoint})} + ) + ) + call = RegisteredSemanticCall( + call_id="simulation.cable_check", + arguments={"target": "stage_free_end_tracking"}, + ) + with pytest.raises(ValueError, match="orientation_error_rad"): + lowerer.lower( + call, + context=SimpleNamespace(env_ids=service.env_ids), + bound=bound, + option_template=MoveJointsOptions(), + ) + + +def test_cable_observation_rejects_invalid_quaternion_per_row(): + service, state, _ = _runtime() + state[1, :, 3:7] = 0 + assert service.observe().tolist() == [False, True] + + +def test_cable_session_scope_is_checked_before_cached_measurements(): + service, _, _ = _runtime() + service.simulation.simulation_time = 1.0 + service.metrics() + + def invalid_scope(): + raise RuntimeError("episode changed") + + service._scope_guard = invalid_scope + with pytest.raises(RuntimeError, match="episode changed"): + service.metrics() + + +def test_cable_factories_return_fresh_ready_lowerers(): + service, _, _ = _runtime() + factory = _CablePickFactory(service.cfg) + engine = SimpleNamespace( + robot=service.robot, + planning_services=SimpleNamespace(env_ids=(3, 1)), + device=torch.device("cpu"), + ) + first = factory.create( + simulation=service.simulation, + robot=service.robot, + scene_registry=None, + engine=engine, + ) + second = factory.create( + simulation=service.simulation, + robot=service.robot, + scene_registry=None, + engine=engine, + ) + assert first is not second and first.runtime is not second.runtime + assert first.runtime.env_ids.tolist() == [3, 1] diff --git a/tests/gym/envs/task_program/test_configured_integration.py b/tests/gym/envs/task_program/test_configured_integration.py index 367738577d..ef55d96a55 100644 --- a/tests/gym/envs/task_program/test_configured_integration.py +++ b/tests/gym/envs/task_program/test_configured_integration.py @@ -4547,22 +4547,22 @@ def test_invalid_evaluation_deployment_fails_before_registration( assert config["id"] not in gym_registry -def test_legacy_task_program_identities_keep_pre_evaluation_values(): +def test_unrelated_task_program_identities_keep_pre_cable_program_values(): from embodichain.lab.task_program.integrations import task_program_fingerprint deployment = _deployment_from_path(_config_path("hand_over")) integration = deployment.integration registration = integration.registration assert "evaluation" not in registration.fingerprint_payload() - # Independently recomputed on upstream main ac714ec8 after its robot/profile - # migrations. The evaluation-free payload and program are unchanged here. + # Independently recomputed at 694ddda7d before the cable Task Program + # migration. Contact-capable PickUp had already changed the old main golden. assert ( registration.fingerprint - == "61cf38ce9128dfcc7063cec3933d900674863facbc6b244f31dec07ea24ca730" + == "0b5253d265d1d88dc165138e915f56f3596f52525c9ae620943f68a3ecc1d79f" ) assert ( integration.integration_fingerprint - == "d8d328c33f7149a5ed76b861bead1155977ed7f4ef213c9ceae560b36c20034e" + == "400ee8ebb00e416a8c9b783f5a681b7f9566da92cef4cbc55ec966cd5c20b2c4" ) compiled = registration.catalog.preflight(deployment.load_program()) assert ( diff --git a/tests/gym/envs/tasks/test_route_the_cable.py b/tests/gym/envs/tasks/test_route_the_cable.py index a5d407fbc2..3a47b12bab 100644 --- a/tests/gym/envs/tasks/test_route_the_cable.py +++ b/tests/gym/envs/tasks/test_route_the_cable.py @@ -16,8 +16,6 @@ from __future__ import annotations from pathlib import Path -from types import SimpleNamespace -from unittest.mock import Mock import xml.etree.ElementTree as ET import math import yaml @@ -28,23 +26,27 @@ build_env_cfg_from_args, config_to_cfg, ) -from embodichain_tasks.manipulation.cable.route_the_cable import ( - measure_route, - RouteTheCableEnv, -) +from embodichain.compute.geometry._cable_route import _measure_cable_route + + from embodichain.lab.sim.spawn.descriptors import rigid_desc_from_cfg from embodichain.lab.sim.cfg import RigidObjectCfg -from embodichain.lab.sim.cfg import CableObjectCfg -from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg -from embodichain.lab.gym.envs.expert_trajectory import ( - ExpertTrajectoryCfg, - build_expert_action_spec, -) from embodichain.lab.sim.shapes import CylinderCfg pytestmark = pytest.mark.no_sim +def measure_route(state, lengths, hands, origins): + path = ( + Path(__file__).resolve().parents[4] + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml" + ) + geometry = yaml.safe_load(path.read_text())["runtime_services"]["cable_material"][ + "geometry" + ] + return _measure_cable_route(state, lengths, hands, origins, geometry, 0.0035) + + @pytest.fixture(autouse=True) def local_yam_config_asset(tmp_path: Path, monkeypatch: pytest.MonkeyPatch) -> Path: """Keep configuration tests independent of external model publication.""" @@ -236,169 +238,6 @@ def test_strict_channel_rejects_end_that_only_meets_broad_region(): assert not metrics["strict_port"].item() -def test_object_goal_stops_before_writing_an_unexecuted_command(): - from types import SimpleNamespace - - command = torch.full((1, 3), 0.5) - env = SimpleNamespace( - _command=command, - _grasp=None, - _max_slip=torch.zeros(1), - cfg=SimpleNamespace( - expert_trajectory=SimpleNamespace(joint_command_mode="position") - ), - ) - actions = RouteTheCableEnv._execute( - env, torch.ones(1, 5, 3), [0, 1, 2], stop_when=lambda: torch.tensor([True]) - ) - assert list(actions) == [] - torch.testing.assert_close(command, torch.full_like(command, 0.5)) - - -def _task_cfg(segments=50): - return EmbodiedEnvCfg( - cable_object=[ - CableObjectCfg( - uid="cable", - points=[[0, 0.012 * i, 0] for i in range(segments + 1)], - radius=0.0035, - ) - ] - ) - - -@pytest.mark.parametrize("segments", [0, 49, 51]) -def test_wrong_cable_topology_fails_before_environment_construction( - monkeypatch, segments -): - parent_init = Mock() - monkeypatch.setattr(EmbodiedEnv, "__init__", parent_init) - with pytest.raises(ValueError, match="50-segment"): - RouteTheCableEnv(_task_cfg(segments)) - parent_init.assert_not_called() - - -def test_missing_cable_fails_before_environment_construction(monkeypatch): - parent_init = Mock() - monkeypatch.setattr(EmbodiedEnv, "__init__", parent_init) - with pytest.raises(ValueError, match="named 'cable'"): - RouteTheCableEnv(EmbodiedEnvCfg()) - parent_init.assert_not_called() - - -@pytest.mark.parametrize("cleanup_fails", [False, True]) -def test_post_base_failure_closes_dataset_and_queues_simulator_cleanup( - monkeypatch, cleanup_fails -): - simulator, dataset = Mock(), Mock() - if cleanup_fails: - dataset.finalize.side_effect = RuntimeError("recorder failure") - - def initialize_parent(env, cfg, **kwargs): - env.cfg = cfg - env.sim = simulator - env.dataset_manager = dataset - env._profiler = Mock() - env._discard_pending_recordings = Mock() - - simulator.get_cable_object.return_value = SimpleNamespace( - data=SimpleNamespace(num_segments=49) - ) - monkeypatch.setattr(EmbodiedEnv, "__init__", initialize_parent) - with pytest.raises(ValueError, match="50-segment"): - RouteTheCableEnv(_task_cfg()) - dataset.finalize.assert_called_once_with() - simulator.destroy.assert_called_once_with(exit_process=False) - - -def _command_env(mode): - env = object.__new__(RouteTheCableEnv) - env.cfg = EmbodiedEnvCfg( - sim_steps_per_control=1, - expert_trajectory=ExpertTrajectoryCfg(joint_command_mode=mode), - ) - env.sim_cfg = SimpleNamespace(physics_dt=0.1) - env._num_envs = 2 - env.sim = SimpleNamespace(device=torch.device("cpu")) - env._command = torch.zeros(2, 4) - env._grasp = None - env._max_slip = torch.zeros(2) - env.robot = SimpleNamespace(get_qpos=lambda: torch.zeros(2, 4)) - env.active_joint_ids = [1, 3] - env.action_manager = None - env._traj_buffer = object() - env._trajectory_action_kind = "expert" - env._record_raw_actions = False - env.expert_action_spec = build_expert_action_spec( - joint_names=["a", "b"], - joint_command_mode=mode, - ) - return env - - -@pytest.mark.parametrize("mode", ["position", "position_velocity"]) -def test_motion_commands_pass_environment_preprocessing_and_recording(mode): - env = _command_env(mode) - path = torch.zeros(2, 3, 4) - path[:, :, 1] = torch.tensor([0.1, 0.2, 0.3]) - path[:, :, 3] = torch.tensor([-0.2, -0.4, -0.6]) - # Non-selected planner columns must not affect the inactive commands. - path[:, :, 0] = 9 - actions = list(env._execute(path, [1, 3])) - assert len(actions) == 3 - for i, action in enumerate(actions): - env._preprocess_action(action) - expected = path[:, i, [1, 3]] - torch.testing.assert_close(env._traj_raw_action[:, :2], expected) - if mode == "position_velocity": - velocity = ( - torch.tensor([1.0, -2.0]).expand(2, -1) if i < 2 else torch.zeros(2, 2) - ) - torch.testing.assert_close(env._traj_raw_action[:, 2:], velocity) - torch.testing.assert_close( - action.value["qvel"][:, [0, 2]], torch.zeros(2, 2) - ) - actual = action.value if mode == "position" else action.value["qpos"] - torch.testing.assert_close(actual[:, [0, 2]], torch.zeros(2, 2)) - # Yielded commands must remain independent when the next frame is emitted. - first = actions[0].value if mode == "position" else actions[0].value["qpos"] - torch.testing.assert_close(first[:, 1], torch.full((2,), 0.1)) - - -def test_hold_emits_zero_velocity_and_can_be_recorded(): - env = _command_env("position_velocity") - env._command.fill_(0.25) - actions = list(env._hold(0.2)) - assert len(actions) == 2 - for action in actions: - env._preprocess_action(action) - torch.testing.assert_close( - env._traj_raw_action, torch.tensor([[0.25, 0.25, 0, 0]]).expand(2, -1) - ) - - -def test_grasp_slip_stops_execution_before_mutating_commands(): - env = _command_env("position_velocity") - env._grasp = ("left", 0.57, torch.zeros(2, 3)) - env._max_slip[1] = 0.013 - assert list(env._execute(torch.ones(2, 3, 4), [1, 3])) == [] - torch.testing.assert_close(env._command, torch.zeros(2, 4)) - - -def test_close_callback_observes_the_executed_frame(): - env = _command_env("position") - observations = [] - env._point_in_tool = lambda *_: observations.append("observed") or torch.ones(2, 3) - actions = env._execute( - torch.ones(2, 2, 4), [1, 3], closed_at=1, grasp_ref=("right", 0.162) - ) - next(actions) - assert observations == [] and env._grasp is None - next(actions) - assert observations == ["observed"] - assert env._grasp[:2] == ("right", 0.162) - - @pytest.mark.parametrize("invalid", ["motion", "nonfinite", "held", "empty_seat"]) def test_reference_metrics_reject_invalid_or_incomplete_observations(invalid): state = _goal_state() @@ -448,167 +287,3 @@ def test_unknown_cable_fields_are_rejected_by_production_loader(tmp_path): data["environment"]["component"] = str(component) with pytest.raises((TypeError, ValueError), match="unknown_cable_option"): config_to_cfg(data, source_path=p) - - -@pytest.mark.parametrize( - "planning_success,tracking_error,object_goal,slip,expected", - [ - (False, 0.0, False, 0.0, False), - (False, 0.015, True, 0.0, False), - (True, 0.015, False, 0.0, False), - (True, 0.001, False, 0.0, True), - (True, 0.015, True, 0.0, True), - (True, 0.001, True, 0.013, False), - ], -) -@pytest.mark.parametrize("extra_samples", [0, 7]) -def test_insertion_validator_requires_planning_and_actual_tracking( - monkeypatch, - planning_success, - tracking_error, - object_goal, - slip, - expected, - extra_samples, -): - from types import SimpleNamespace - from unittest.mock import MagicMock - - warning = Mock() - monkeypatch.setattr( - "embodichain_tasks.manipulation.cable.route_the_cable.logger.log_warning", - warning, - ) - - actual = torch.eye(4).unsqueeze(0) - target = actual.clone() - target[:, 2, 3] += tracking_error - - def generate(states, *, options): - return SimpleNamespace( - positions=( - torch.zeros(1, options.sample_count + extra_samples, 3) - if planning_success - else None - ), - success=torch.tensor([planning_success]), - ) - - env = SimpleNamespace( - device=torch.device("cpu"), - num_envs=1, - step_dt=1 / 60, - _tool=lambda side: actual, - _command=torch.zeros(1, 3), - _max_slip=torch.tensor([slip]), - _grasp=None, - robot=SimpleNamespace( - get_joint_ids=lambda name: [0, 1, 2], get_qpos=lambda: torch.zeros(1, 3) - ), - _motion=SimpleNamespace(generate=generate), - _execute=MagicMock(return_value=iter(())), - _hold=MagicMock(return_value=iter(())), - ) - segment = RouteTheCableEnv._move( - env, - "left", - target, - "insert", - tolerance=0.003, - goal_reached=lambda: torch.tensor([object_goal]), - ) - assert bool(segment.validator().item()) is expected - if expected: - warning.assert_not_called() - else: - message = warning.call_args.args[0] - assert "'insert' rejected" in message - assert "planning_success=" in message - assert "position_error_m=" in message - assert "max_grasp_slip_m=" in message - if not planning_success: - assert list(segment.actions) == [] - env._execute.assert_not_called() - else: - list(segment.actions) - trajectory = env._execute.call_args.args[0] - assert trajectory.shape[1] >= 3 + extra_samples - assert trajectory.shape[-1] == 3 - - -@pytest.mark.parametrize("settles", [True, False]) -def test_insertion_waits_for_tracking_without_bypassing_timeout(settles): - from types import MethodType - - actual = torch.eye(4).unsqueeze(0) - target = actual.clone() - target[:, 0, 3] = 0.01 - - def generate(states, *, options): - return SimpleNamespace( - positions=torch.zeros(1, options.sample_count, 3), - success=torch.tensor([True]), - ) - - env = SimpleNamespace( - device=torch.device("cpu"), - num_envs=1, - step_dt=1 / 60, - cfg=SimpleNamespace( - expert_trajectory=SimpleNamespace(joint_command_mode="position") - ), - _tool=lambda side: actual, - _command=torch.zeros(1, 3), - _max_slip=torch.zeros(1), - _grasp=None, - robot=SimpleNamespace( - get_joint_ids=lambda name: [0, 1, 2], get_qpos=lambda: torch.zeros(1, 3) - ), - _motion=SimpleNamespace(generate=generate), - _execute=lambda *args, **kwargs: iter(()), - ) - env._hold = MethodType(RouteTheCableEnv._hold, env) - segment = RouteTheCableEnv._move( - env, - "left", - target, - "insert", - tolerance=0.003, - goal_reached=lambda: torch.tensor([False]), - ) - frames = 0 - for _ in segment.actions: - frames += 1 - # The controller reaches tolerance after the initial 0.25 s hold. - if settles and frames == 18: - actual[:, 0, 3] = 0.009 - - assert frames == (18 if settles else 75) - assert bool(segment.validator().item()) is settles - - -@pytest.mark.parametrize("success", [True, False]) -def test_final_route_validation_reports_physical_metrics(monkeypatch, success): - warning = Mock() - monkeypatch.setattr( - "embodichain_tasks.manipulation.cable.route_the_cable.logger.log_warning", - warning, - ) - metrics = { - "success": torch.tensor([success]), - "strict_port": torch.tensor([success]), - "end_position": torch.tensor([[0.586, 0.077, 0.754]]), - "max_grasp_slip": torch.tensor([0.01]), - } - env = SimpleNamespace(route_metrics=lambda: metrics) - - torch.testing.assert_close( - RouteTheCableEnv._validate_route(env), metrics["success"] - ) - if success: - warning.assert_not_called() - else: - message = warning.call_args.args[0] - assert "strict_port" in message - assert "end_position" in message - assert "max_grasp_slip" in message diff --git a/tests/gym/envs/tasks/test_route_the_cable_cuda.py b/tests/gym/envs/tasks/test_route_the_cable_cuda.py index 55d37a6420..d5f2802e84 100644 --- a/tests/gym/envs/tasks/test_route_the_cable_cuda.py +++ b/tests/gym/envs/tasks/test_route_the_cable_cuda.py @@ -28,7 +28,8 @@ from embodichain.lab.gym.utils.gym_utils import config_to_cfg from embodichain.lab.sim import SimulationManager -from embodichain_tasks.manipulation.cable.route_the_cable import RouteTheCableEnv +import gymnasium as gym +import embodichain_tasks pytestmark = [ pytest.mark.subprocess_sim, @@ -41,6 +42,10 @@ ] +def _material_runtime(env): + return env._active_task_program_bridge.post_policy_port._cable_runtime + + class _SpoolReleaseCUDAScenario: """Qualify the actual jaw sweep, rather than only cable goal predicates.""" @@ -55,7 +60,7 @@ def setup_method(self) -> None: cfg.seed = 73 cfg.sim_cfg.render_cfg.renderer = "no-render" cfg.sim_cfg.robot_ik_gizmo = None - self.env = RouteTheCableEnv(cfg) + self.env = gym.make("RouteTheCable-YAM-v1", cfg=cfg).unwrapped def teardown_method(self) -> None: if hasattr(self, "env"): @@ -88,7 +93,7 @@ def test_first_spool_release_preserves_seating_without_gripper_penetration( worst_distance = 0.0 worst_segment = "" env.reset() - for segment in env.create_demo_segments(milestone="first_spool"): + for segment in env.create_demo_segments(): for action in segment.actions: env.step(action) contacts = query.fetch_solver_report() @@ -103,15 +108,17 @@ def test_first_spool_release_preserves_seating_without_gripper_penetration( if distance < worst_distance: worst_distance, worst_segment = distance, segment.name assert bool(segment.validator().all()), segment.name + if segment.name == "open_clear_of_spool_0": + break # Finite cable states and seated cable alone previously accepted a jaw # sweep through the rim with about 16 mm reported penetration. assert worst_distance >= -0.001, (worst_segment, worst_distance) - max_slip = float(env._episode_slip.max()) + max_slip = float(_material_runtime(env).max_slip.max()) record_property("max_grasp_slip_m", max_slip) assert max_slip <= 0.012 - assert bool((env.route_metrics()["seat_0"] >= 2).all()) - assert bool((env._tool("right")[:, 2, 3] > 0.81).all()) + assert bool((_material_runtime(env).metrics()["seat_0"] >= 2).all()) + assert bool((_material_runtime(env).tool("right_arm")[:, 2, 3] > 0.81).all()) finger_ids = env.robot.get_joint_ids("right_eef") assert bool((env.robot.get_qpos()[:, finger_ids] > 0.045).all()) @@ -153,7 +160,10 @@ def test_third_carry_preserves_grasp_and_clears_spools( for action in segment.actions: env.step(action) if segment.name in third_segments: - max_slip = max(max_slip, float(env._max_slip.max())) + max_slip = max( + max_slip, + float(_material_runtime(env).max_slip.max()), + ) contacts = query.fetch_solver_report() assert contacts.dropped_count == 0 pairs = contacts.actor_ids[: contacts.count] @@ -166,17 +176,23 @@ def test_third_carry_preserves_grasp_and_clears_spools( assert bool(segment.validator().all()), segment.name if segment.name == "raise_2": raised = True - assert env._grasp[:2] == ("right", 0.438) + assert _material_runtime(env).grasps["right_arm"][0] == 0.438 # The gripped centre must clear the 23.5 mm rim plus cable radius. - clearance_height = 0.75 + 0.0235 + env.cable.cfg.radius - assert bool((env._point(0.438)[0][:, 2] > clearance_height).all()) + clearance_height = ( + 0.75 + 0.0235 + env.sim.get_cable_object("cable").cfg.radius + ) + assert bool( + ( + _material_runtime(env).point(0.438)[0][:, 2] > clearance_height + ).all() + ) if segment.name == "open_clear_of_spool_2": assert raised record_property("third_carry_slip_m", max_slip) record_property("third_carry_robot_spool_distance_m", worst_distance) assert max_slip <= 0.012 assert worst_distance >= -0.001 - assert bool((env.route_metrics()["seat_1"] >= 2).all()) + assert bool((_material_runtime(env).metrics()["seat_1"] >= 2).all()) return pytest.fail("The expert did not complete the third carry") diff --git a/tests/lab/evaluation/test_rule_graph.py b/tests/lab/evaluation/test_rule_graph.py index 297fe938fa..df3e2dbe49 100644 --- a/tests/lab/evaluation/test_rule_graph.py +++ b/tests/lab/evaluation/test_rule_graph.py @@ -659,3 +659,27 @@ def test_replay_preserves_row_ends_and_rejects_missing_endpoint(): assert beyond.step == 10 assert beyond.revisions.tolist() == [-1, -1] assert beyond.rules["closed"].status.tolist() == [Status.UNAVAILABLE] * 2 + + +def test_scalar_range_preserves_open_boundary_and_missing_evidence(): + graph = builtin_predicates().compile( + EvaluationSpec( + { + "clear": PredicateSpec( + "scalar_range", + {"value": "clearance"}, + {"minimum": 0.05, "maximum": 100, "minimum_inclusive": False}, + ) + }, + ("clear",), + ), + field_kinds={"clearance": "scalar"}, + ) + result = graph.evaluate( + {"clearance": torch.tensor([[0.05], [0.051], [float("nan")]])}, batch_size=3 + ) + assert result["clear"].status.tolist() == [ + Status.FAILED, + Status.SATISFIED, + Status.UNAVAILABLE, + ] diff --git a/tests/sim/atomic_skills/test_actions.py b/tests/sim/atomic_skills/test_actions.py index af9c80e93e..bf5fe8a35c 100644 --- a/tests/sim/atomic_skills/test_actions.py +++ b/tests/sim/atomic_skills/test_actions.py @@ -9769,3 +9769,55 @@ def test_contact_pickup_rejects_rigid_options_before_scene_lookup(): _dual_context(), ) generator.generate.assert_not_called() + + +def test_move_joints_preserves_authored_samples_and_cadence() -> None: + generator = _motion_generator() + action = _bind_action(generator, MoveJoints()) + samples = ( + torch.tensor([0.0, 0.001, 0.004, 0.005]) + .view(1, 4, 1) + .expand(NUM_ENVS, -1, ARM_DOF) + .clone() + ) + invocation = _invocation(action, JointPositionGoal(samples), sample_count=90) + invocation = replace( + invocation, skill_options=MoveJointsOptions(preserve_samples=True) + ) + plan = _plan_action(action, invocation, _context()) + torch.testing.assert_close(_joint_command_positions(plan, "arm"), samples) + assert _joint_trajectory(plan).duration.tolist() == pytest.approx( + [3 * CONTROL_DT] * NUM_ENVS + ) + + +def test_move_end_effector_preserves_authored_cartesian_samples() -> None: + generator = _motion_generator() + calls = [] + generate = generator.generate + + def trace(*args, **kwargs): + calls.append(kwargs["options"]) + return generate(*args, **kwargs) + + generator.generate = trace + action = _bind_action(generator, MoveEndEffector()) + poses = torch.eye(4).repeat(NUM_ENVS, 5, 1, 1) + invocation = replace( + _invocation(action, EndEffectorPoseGoal(poses), sample_count=90), + skill_options=MoveEndEffectorOptions(preserve_cartesian_samples=True), + ) + _plan_action(action, invocation, _context()) + assert calls[0].preserve_cartesian_samples + assert calls[0].sample_count == 6 + + +def test_presampled_joint_commands_cannot_bypass_motion_gen_backend(): + action = _bind_action(_motion_generator(), MoveJoints()) + invocation = replace( + _invocation(action, JointPositionGoal(torch.zeros(NUM_ENVS, 2, ARM_DOF))), + motion_policy=MotionPolicy(strategy="motion_gen"), + skill_options=MoveJointsOptions(preserve_samples=True), + ) + with pytest.raises(ValueError, match="requires strategy"): + _plan_action(action, invocation, _context()) From 0a2b25f02ca8e365a70057d3e2295f1cba853162 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Fri, 13 Nov 2026 10:41:52 +0800 Subject: [PATCH 32/43] fix(sim): register CableObject facades for USD cables --- .../topics/simulation-system/deformables.md | 7 +- embodichain/lab/sim/sim_manager.py | 48 ++++++---- embodichain/lab/sim/spawn/usd_import.py | 56 +++++++++++- embodichain/lab/sim/spawn/usd_result.py | 26 +++++- scripts/validation/accept_usd_import.py | 11 ++- tests/gym/envs/test_cable_recording_cuda.py | 61 +++++++++++-- tests/sim/objects/test_cable_object.py | 5 +- tests/sim/objects/test_cable_object_cuda.py | 91 +++++++++++++++++++ tests/sim/spawn/test_usd_scene_overrides.py | 15 ++- 9 files changed, 280 insertions(+), 40 deletions(-) diff --git a/agent_context/topics/simulation-system/deformables.md b/agent_context/topics/simulation-system/deformables.md index 165ad49dfe..f59bbc9339 100644 --- a/agent_context/topics/simulation-system/deformables.md +++ b/agent_context/topics/simulation-system/deformables.md @@ -108,8 +108,11 @@ Focused coverage: `tests/sim/test_deformable_cfg.py`, `CableObjectCfg` and `CableObject(BatchEntity)` are independent of particle-set deformables. `SimulationManager.add_cable_object()` lowers through `spawn/descriptors.py` into `CableDesc`, declares in the existing `SpawnScene`, -and binds the same facade in place during `prepare()`. Backend capabilities -reject Default, CPU, other Newton solvers and gradient mode; use DexUni on CUDA. +and binds the same facade in place during `prepare()`. `add_usd(UsdImportCfg)` +also returns a `CableObject` per imported cable under its source resource key, +with the same UID lookup, segment state and reset as a procedural declaration. +The parsed `CableDesc` remains the source of cable physics and visuals. +Cable declarations require DexUni on CUDA with gradient mode disabled. With this backend, an arena containing a cable cannot also contain fluid, cloth or soft-body particles: cable episode reset does not support those particles. Spawn checks the native cable and particle descriptors for direct diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index 6eae9b3cc4..d31af9c874 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -2755,7 +2755,13 @@ def add_usd( ) -> ( UsdImportResult | dict[ - str, VisualObject | RigidObject | Articulation | Robot | DeformableObject + str, + VisualObject + | RigidObject + | Articulation + | Robot + | DeformableObject + | CableObject, ] ): """Declare a USD import through a validated configuration recipe. @@ -2777,7 +2783,8 @@ def add_usd( asset_cfgs: Legacy per-asset overrides; use ``cfg.asset_cfgs``. Returns: - Source-path mapping of declared facades and imported resource UIDs. + Source-path mapping of declared facades, including USD cables, + and imported resource UIDs. Legacy argument forms retain the mutable asset dictionary result. """ from .cfg.usd import _normalize_usd_import @@ -2816,6 +2823,7 @@ def add_usd( ), "soft_object": self._deformable_objects, "cloth_object": self._deformable_objects, + "cable_object": self._cable_objects, }[kind] registry[descriptor.name] = facade assets[source_path] = facade @@ -3180,22 +3188,8 @@ def get_fluid_diagnostics(self, uid: str) -> dict[str, object]: raise RuntimeError("Fluid diagnostics require prepare().") return self.physics.fluid_diagnostics(fluid) - def add_cable_object(self, cfg: CableObjectCfg) -> CableObject: - """Declare a cable before ``prepare()`` using DexSim's public Spawn API. - - Args: - cfg: Typed cable geometry, four-mode material and visual configuration. - - Returns: - A declared facade whose data becomes available after ``prepare()``. - - Raises: - NotImplementedError: Backend/device/solver/gradient mode is unsupported, - or the physical scene has already been finalized. - ValueError: Required configuration or the asset UID is invalid. - """ - if not isinstance(cfg, CableObjectCfg): - raise TypeError("Cable objects require CableObjectCfg.") + def _require_cable_declaration(self) -> None: + """Check the shared runtime requirements for declaring cables.""" if not self.physics.supports_cables: raise NotImplementedError("Cable objects require the Newton backend.") if self.device.type != "cuda": @@ -3212,6 +3206,24 @@ def add_cable_object(self, cfg: CableObjectCfg) -> CableObject: raise NotImplementedError( "Declare cable objects before scene finalization." ) + + def add_cable_object(self, cfg: CableObjectCfg) -> CableObject: + """Declare a cable before ``prepare()`` using DexSim's public Spawn API. + + Args: + cfg: Typed cable geometry, four-mode material and visual configuration. + + Returns: + A declared facade whose data becomes available after ``prepare()``. + + Raises: + NotImplementedError: Backend/device/solver/gradient mode is unsupported, + or the physical scene has already been finalized. + ValueError: Required configuration or the asset UID is invalid. + """ + if not isinstance(cfg, CableObjectCfg): + raise TypeError("Cable objects require CableObjectCfg.") + self._require_cable_declaration() descriptor, materials = cable_desc_from_cfg(cfg, per_env=True) if descriptor.name in self.asset_uids or descriptor.name in self._spawn_scene: raise ValueError( diff --git a/embodichain/lab/sim/spawn/usd_import.py b/embodichain/lab/sim/spawn/usd_import.py index 933b8fd573..87d7d038f3 100644 --- a/embodichain/lab/sim/spawn/usd_import.py +++ b/embodichain/lab/sim/spawn/usd_import.py @@ -27,6 +27,7 @@ from dexsim.spawn import ( ArticulationDesc, + CableDesc, ConstraintDesc, MeshObjectDesc, SoftBodyDesc, @@ -36,6 +37,9 @@ from embodichain.lab.sim.cfg import ( ArticulationCfg, ArticulationRootPropertiesCfg, + CableAttachmentCfg, + CableObjectCfg, + CablePhysicsCfg, RigidObjectCfg, RobotCfg, VisualObjectCfg, @@ -46,6 +50,7 @@ ) from embodichain.lab.sim.objects import ( Articulation, + CableObject, RigidObject, Robot, VisualObject, @@ -108,6 +113,8 @@ def _compile_usd_import( manager._spawn_scene._require_compatible_particles( cables=bool(source.cables), particles=bool(source.particle_sets) ) + if source.cables: + manager._require_cable_declaration() resources = list(source.iter_entities()) source_paths_by_id = {id(item): _source_path(item) for item in resources} counts = Counter(source_paths_by_id.values()) @@ -342,6 +349,37 @@ def _compile_usd_import( cfg.body_type = None cfg.body_scale = None pose_mode = "preserve" + elif isinstance(descriptor, CableDesc): + if original_name in occupied: + raise ValueError(f"USD asset UID already exists: {original_name!r}.") + occupied.add(original_name) + if root_pose is not None: + descriptor.pose = root_pose @ descriptor.pose + # Keep the parsed descriptor as the source of physics and rendering. + # The facade config exposes the resolved segment and placement data. + frames = descriptor.segment_quaternions + cfg = CableObjectCfg( + uid=original_name, + points=descriptor.points, + radius=descriptor.radius, + init_local_pose=descriptor.pose, + segment_quaternions=None if frames is None else frames[:, [1, 2, 3, 0]], + closed=descriptor.closed, + fixed_segments=descriptor.fixed_segments, + attrs=CablePhysicsCfg( + **{ + item.name: getattr(descriptor.physics, item.name) + for item in fields(descriptor.physics) + } + ), + collision_filter_span=descriptor.collision_filter_span, + render_mode=descriptor.render_mode, + ) + facade = CableObject(cfg, device=manager.device) + declarations.append( + (resource_keys[id(descriptor)], "cable_object", descriptor, facade) + ) + continue else: if descriptor.name in occupied: raise ValueError(f"USD asset UID already exists: {descriptor.name!r}.") @@ -481,6 +519,22 @@ def renamed_endpoint(endpoint: str) -> str: owner, separator, suffix = endpoint.partition("/") return renamed.get(owner, owner) + separator + suffix + for _, kind, descriptor, facade in declarations: + if kind == "cable_object": + for name in ("start_attachment", "end_attachment"): + anchor = getattr(descriptor, name) + if anchor is not None: + anchor.target = renamed.get(anchor.target, anchor.target) + setattr( + facade.cfg, + name, + CableAttachmentCfg( + target=anchor.target, + position=anchor.position.copy(), + link_name=anchor.link_name, + ), + ) + for constraint in source.constraints: if constraint.body0 is not None: constraint.body0 = renamed_endpoint(constraint.body0) @@ -511,6 +565,7 @@ def renamed_endpoint(endpoint: str) -> str: public_kinds = { "soft_object": "volume_deformable", "cloth_object": "surface_deformable", + "cable_object": "cable", } resource_types = { path: "robot" if isinstance(facade, Robot) else public_kinds.get(kind, kind) @@ -520,7 +575,6 @@ def renamed_endpoint(endpoint: str) -> str: ("cameras", "camera"), ("lights", "light"), ("records", "record"), - ("cables", "cable"), ("constraints", "constraint"), ): resource_types.update( diff --git a/embodichain/lab/sim/spawn/usd_result.py b/embodichain/lab/sim/spawn/usd_result.py index dd562cb593..198281319e 100644 --- a/embodichain/lab/sim/spawn/usd_result.py +++ b/embodichain/lab/sim/spawn/usd_result.py @@ -25,6 +25,7 @@ if TYPE_CHECKING: from embodichain.lab.sim.objects import ( Articulation, + CableObject, DeformableObject, RigidObject, Robot, @@ -35,11 +36,15 @@ class UsdImportResult( - Mapping[str, "VisualObject | RigidObject | Articulation | Robot | DeformableObject"] + Mapping[ + str, + "VisualObject | RigidObject | Articulation | Robot | DeformableObject | CableObject", + ] ): """Map source resource keys to stable, declared EmbodiChain facades. - Facades bind in place at ``SimulationManager.prepare()``. Resource maps + Facades, including imported cables, bind in place at + ``SimulationManager.prepare()``. Resource maps include native-only scene resources and constraints; their values are logical Spawn UIDs, not native handles or builder indices. A unique source keeps its prim path as the resource key. When a source prim produces several @@ -51,7 +56,13 @@ class UsdImportResult( def __init__( self, assets: Mapping[ - str, VisualObject | RigidObject | Articulation | Robot | DeformableObject + str, + VisualObject + | RigidObject + | Articulation + | Robot + | DeformableObject + | CableObject, ], *, resource_uids: Mapping[str, str], @@ -83,7 +94,14 @@ def __init__( def __getitem__( self, source_path: str - ) -> VisualObject | RigidObject | Articulation | Robot | DeformableObject: + ) -> ( + VisualObject + | RigidObject + | Articulation + | Robot + | DeformableObject + | CableObject + ): return self._assets[source_path] def __iter__(self) -> Iterator[str]: diff --git a/scripts/validation/accept_usd_import.py b/scripts/validation/accept_usd_import.py index dd7b09a757..37429f7eca 100644 --- a/scripts/validation/accept_usd_import.py +++ b/scripts/validation/accept_usd_import.py @@ -169,7 +169,10 @@ def _accept_case(case: dict[str, Any], output: Path) -> dict[str, Any]: "An imported entity was lost or a source path is duplicated." ) expected_assets = ( - len(raw.mesh_objects) + len(raw.articulations) + len(raw.particle_sets) + len(raw.mesh_objects) + + len(raw.articulations) + + len(raw.particle_sets) + + len(raw.cables) ) record["resources"] = dict(imported.resource_uids) record["resource_source_paths"] = dict(imported.resource_source_paths) @@ -286,6 +289,12 @@ def check_state() -> None: ) elif type(asset).__name__ in ("Articulation", "Robot"): value = asset.get_qpos() + elif type(asset).__name__ == "CableObject": + value = asset.data.segment_state_w + if value.numel() == 0: + raise AssertionError( + f"Cable {asset.uid} has no simulation segments." + ) else: value = asset.get_local_pose() if not torch.isfinite(value).all(): diff --git a/tests/gym/envs/test_cable_recording_cuda.py b/tests/gym/envs/test_cable_recording_cuda.py index 81c39b5eae..9edbfed986 100644 --- a/tests/gym/envs/test_cable_recording_cuda.py +++ b/tests/gym/envs/test_cable_recording_cuda.py @@ -30,7 +30,7 @@ pytestmark = [pytest.mark.subprocess_sim, pytest.mark.gpu] -def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( +def test_usd_and_procedural_cables_record_and_replay_across_arenas( tmp_path: Path, ) -> None: # A fresh process preserves NoRender without the shared test engine's @@ -44,8 +44,32 @@ def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( """, encoding="utf-8", ) + usd = tmp_path / "wire.usda" + usd.write_text( + """#usda 1.0 +( + defaultPrim = "World" + metersPerUnit = 1 + upAxis = "Z" +) + +def Xform "World" +{ + def BasisCurves "wire" ( + prepend apiSchemas = ["PhysicsCurvesDeformableSimAPI"] + ) + { + int[] curveVertexCounts = [3] + point3f[] points = [(0, 0, 1), (0.1, 0, 1), (0.2, 0, 1)] + uniform token type = "linear" + uniform token wrap = "nonperiodic" + } +} +""", + encoding="utf-8", + ) result = subprocess.run( - [sys.executable, str(Path(__file__).resolve()), str(urdf)], + [sys.executable, str(Path(__file__).resolve()), str(urdf), str(usd)], env=os.environ.copy(), capture_output=True, text=True, @@ -54,7 +78,7 @@ def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( assert result.returncode == 0, result.stdout + result.stderr -def _run_recording(urdf: str) -> None: +def _run_recording(urdf: str, usd: str) -> None: import torch from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg @@ -66,8 +90,18 @@ def _run_recording(urdf: str) -> None: NewtonPhysicsCfg, RenderCfg, RobotCfg, + UsdImportCfg, + UsdSourceCfg, ) + class CableRecordingEnv(EmbodiedEnv): + def _prepare_scene(self, **kwargs) -> None: + super()._prepare_scene(**kwargs) + result = self.sim.add_usd( + UsdImportCfg(name="scene", source=UsdSourceCfg(file_path=usd)) + ) + assert result["/World/wire"] is self.sim.get_cable_object("wire") + cfg = EmbodiedEnvCfg( num_envs=2, max_episode_steps=4, @@ -98,12 +132,11 @@ def _run_recording(urdf: str) -> None: ), cable_object=[ CableObjectCfg( - uid=uid, - points=[[0.1 * index, 0, 0] for index in range(segments + 1)], + uid="long_wire", + points=[[0.1 * index, 0, 0] for index in range(4)], radius=0.005, - init_pos=(0.0, y, 1.0), + init_pos=(0.0, 0.5, 1.0), ) - for uid, segments, y in (("wire", 2, 0.0), ("long_wire", 3, 0.5)) ], observations={ "wire": ObservationCfg( @@ -120,12 +153,14 @@ def _run_recording(urdf: str) -> None: ) env = None try: - env = EmbodiedEnv(cfg) + env = CableRecordingEnv(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} + assert cables["wire"].data.num_segments == 2 + assert cables["long_wire"].data.num_segments == 3 fields = {uid: f"cables/{uid}/segment_state_w" for uid in cables} episodes = env.episode_context.episode_ids expected = { @@ -225,6 +260,12 @@ def _run_recording(urdf: str) -> None: assert saved["meta"]["arena_origins_w"] == [source_origin.tolist()] for uid, field in fields.items(): assert saved["raw_observations"]["fields"][field]["values"].shape[1] == 4 + torch.testing.assert_close( + saved["terminal_states"]["cables", uid, "segment_state_w"].cpu(), + latest.values[field][[1]], + rtol=0, + atol=0, + ) env.close(exit_process=False) env = None @@ -236,7 +277,7 @@ def _run_recording(urdf: str) -> None: cfg.num_envs = 1 cfg.sim_cfg.num_envs = 1 cfg.record_trajectory = False - env = EmbodiedEnv(cfg) + env = CableRecordingEnv(cfg) replay = ReplayWrapper(env, saved) replay.reset() for _ in range(saved["meta"]["lengths"][0]): @@ -258,4 +299,4 @@ def _run_recording(urdf: str) -> None: if __name__ == "__main__": faulthandler.enable() faulthandler.dump_traceback_later(120) - _run_recording(sys.argv[1]) + _run_recording(sys.argv[1], sys.argv[2]) diff --git a/tests/sim/objects/test_cable_object.py b/tests/sim/objects/test_cable_object.py index d8a4d98b36..8c78e4cc6c 100644 --- a/tests/sim/objects/test_cable_object.py +++ b/tests/sim/objects/test_cable_object.py @@ -390,11 +390,10 @@ def add_cable(): source=UsdSourceCfg(file_path=str(_write_cable_usd(tmp_path))), ) ) - assert dict(imported) == {} + cable = imported["/Cable"] assert list(imported.resource_types.values()) == ["cable"] assert len(builder.cables) == 1 - assert not sim._cable_objects and not scene._assets - return None + return cable if cable_first: first = add_cable() diff --git a/tests/sim/objects/test_cable_object_cuda.py b/tests/sim/objects/test_cable_object_cuda.py index 2a25e8594f..7904112f56 100644 --- a/tests/sim/objects/test_cable_object_cuda.py +++ b/tests/sim/objects/test_cable_object_cuda.py @@ -226,6 +226,92 @@ def test_imported_particles_survive_rejected_cable(self, tmp_path: Path) -> None torch.testing.assert_close(reset[0], moved[0], rtol=0, atol=0) torch.testing.assert_close(reset[1], initial[1]) + def test_usd_cable_state_selective_reset_and_viser(self, tmp_path: Path) -> None: + from embodichain.lab.visualization import SceneExporter, VisualizationCfg + + path = tmp_path / "wire.usda" + path.write_text( + """#usda 1.0 +( + defaultPrim = "World" + metersPerUnit = 1 + upAxis = "Z" +) +def Xform "World" { + def BasisCurves "Wire" ( + prepend apiSchemas = ["PhysicsCurvesDeformableSimAPI"] + ) { + int[] curveVertexCounts = [3] + point3f[] points = [(0, 0, 0), (0.1, 0, 0), (0.3, 0, 0)] + uniform token type = "linear" + uniform token wrap = "nonperiodic" + float[] physics:thicknesses = [0.02] + token physics:thicknesses:elementType = "constant" + double3 xformOp:translate = (0.25, 0.5, 1) + uniform token[] xformOpOrder = ["xformOp:translate"] + } +} +""", + encoding="utf-8", + ) + imported = self.sim.add_usd( + UsdImportCfg(name="wire", source=UsdSourceCfg(file_path=str(path))) + ) + key = "/World/Wire" + cable = imported[key] + uid = imported.resource_uids[key] + assert self.sim.get_asset(uid) is self.sim.get_cable_object(uid) is cable + assert cable.num_instances == 2 and cable.data is None + + self.sim.prepare() + initial = cable.data.default_segment_state_w + assert initial.shape == (2, 2, 13) + expected_centres = initial.new_tensor([[0.3, 0.5, 1], [0.45, 0.5, 1]]) + torch.testing.assert_close( + initial[..., :3] - self.sim.arena_offsets[:, None, :], + expected_centres.expand(2, 2, 3), + atol=1e-6, + rtol=0, + ) + + self.sim.update(step=3, render_final_step=False) + moved = cable.data.segment_state_w + assert torch.all(moved[..., 2] < initial[..., 2]) + self.sim.reset_objects_state(env_ids=[1]) + reset = cable.data.segment_state_w + torch.testing.assert_close(reset[0], moved[0], rtol=0, atol=0) + torch.testing.assert_close(reset[1], initial[1], rtol=0, atol=0) + + state = reset[[1]].clone() + state[..., 0] += 0.125 + cable.write_segment_state_to_sim(state, env_ids=[1]) + torch.testing.assert_close( + cable.data.segment_state_w[0], moved[0], rtol=0, atol=0 + ) + torch.testing.assert_close(cable.data.segment_state_w[[1]], state) + + exporter = SceneExporter( + self.sim, VisualizationCfg(backend="viser", env_ids=[1]) + ) + manifest = exporter.build_manifest() + frame = exporter.capture(sim_step=3, sim_time=self.sim.simulation_time).frame + nodes = [node for node in manifest.nodes if node.kind == "cable_segment"] + assert len(nodes) == 2 + geometries = { + geometry.geometry_id: geometry for geometry in manifest.geometries + } + for segment_index, node in enumerate(nodes): + assert node.env_id == 1 + assert exporter.resolve_node_target(node.node_id) == (uid, "cable") + index = frame.node_ids.index(node.node_id) + pose = state[0, segment_index].cpu().numpy() + np.testing.assert_allclose(frame.positions[index], pose[:3], atol=1e-6) + np.testing.assert_allclose(frame.wxyz[index], pose[[6, 3, 4, 5]], atol=1e-6) + vertices = geometries[node.geometry_id].vertices + np.testing.assert_allclose( + np.ptp(vertices, axis=0), [0.02, 0.02, (0.12, 0.22)[segment_index]] + ) + def test_attached_articulation_expert_trajectory_repeats_after_reset( self, tmp_path: Path ) -> None: @@ -451,6 +537,11 @@ def test_imported_particles_survive_rejected_cable( "test_imported_particles_survive_rejected_cable", volume_particle_usd.parent ) + def test_usd_cable_state_selective_reset_and_viser(self, tmp_path: Path) -> None: + _assert_cable_case_in_subprocess( + "test_usd_cable_state_selective_reset_and_viser", tmp_path + ) + @pytest.mark.usefixtures("requires_dexuni_two_way") def test_attached_articulation_expert_trajectory_repeats_after_reset( self, tmp_path: Path diff --git a/tests/sim/spawn/test_usd_scene_overrides.py b/tests/sim/spawn/test_usd_scene_overrides.py index 6b488add07..e140ea39c7 100644 --- a/tests/sim/spawn/test_usd_scene_overrides.py +++ b/tests/sim/spawn/test_usd_scene_overrides.py @@ -43,6 +43,7 @@ from embodichain.lab.sim.sim_manager import SimulationManager from embodichain.lab.sim.spawn.scene import SpawnScene from embodichain.lab.sim.cfg import UsdImportCfg, UsdSourceCfg +from embodichain.lab.sim.objects import CableObject pytestmark = pytest.mark.no_sim @@ -61,6 +62,7 @@ def _manager(): "_rigid_object_groups", "_deformable_objects", "_fluid_objects", + "_cable_objects", "_articulations", ): setattr(manager, name, {}) @@ -666,6 +668,11 @@ def test_curve_prim_resources_keep_all_uids_and_stable_source_keys( ) for namespace in (None, "copy"): manager = _manager() + manager.device = torch.device("cuda:0") + manager.physics = SimpleNamespace( + name="newton", solver_type="dexuni", supports_cables=True + ) + manager.sim_config.physics_cfg = SimpleNamespace(requires_grad=False) result = manager.add_usd( UsdImportCfg( name="scene", @@ -681,7 +688,13 @@ def test_curve_prim_resources_keep_all_uids_and_stable_source_keys( assert dict(result.resource_source_paths) == { key: "/World/Cable" for key in expected_keys } - assert len(result) == 0 + assert list(result) == expected_keys + for key in expected_keys: + cable = result[key] + uid = result.resource_uids[key] + assert isinstance(cable, CableObject) + assert cable is manager.get_cable_object(uid) is manager.get_asset(uid) + assert cable.is_declared and cable.data is None with pytest.raises(TypeError): result.resource_source_paths[expected_keys[0]] = "/Other" From 2e502cf793f468a6c8a8a32b6f66e00d98cb8433 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Fri, 13 Nov 2026 20:54:05 +0800 Subject: [PATCH 33/43] fix(task): stabilize contact cable routing and release --- embodichain/compute/geometry/_cable_route.py | 8 +- .../integrations/simulation/cable.py | 45 +++- .../cable/route_the_cable/README.md | 182 +++++++++----- .../cable/route_the_cable/env.yaml | 5 +- .../task_program/integration.yaml | 195 +++++++++------ .../route_the_cable/task_program/program.yaml | 116 ++++++--- .../envs/task_program/test_cable_material.py | 230 +++++++++++++++++- tests/gym/envs/tasks/test_route_the_cable.py | 74 +++++- .../envs/tasks/test_route_the_cable_cuda.py | 156 +++++++++++- 9 files changed, 815 insertions(+), 196 deletions(-) diff --git a/embodichain/compute/geometry/_cable_route.py b/embodichain/compute/geometry/_cable_route.py index 57aa374e85..b05c6ae098 100644 --- a/embodichain/compute/geometry/_cable_route.py +++ b/embodichain/compute/geometry/_cable_route.py @@ -163,9 +163,11 @@ def _measure_cable_route( strict_lo, strict_hi = (state.new_tensor(v) for v in geometry["strict_region"]) release_lo, release_hi = (state.new_tensor(v) for v in geometry["release_region"]) out["strict_port"] = ((end >= strict_lo) & (end <= strict_hi)).all(-1) - out["release_ready"] = valid & ( - out["strict_port"] | ((end >= release_lo) & (end <= release_hi)).all(-1) - ) + inside_release = ((end >= release_lo) & (end <= release_hi)).all(-1) + # An endpoint already seated deeply inside the physical channel need not + # be pushed toward the narrower corridor used for release from above. + seated_deeply = out["strict_port"] & (end[:, 1] >= release_lo[1]) + out["release_ready"] = valid & (inside_release | seated_deeply) out["reference_routed"] = ( valid & (out["seat_0"] >= 2) diff --git a/embodichain/lab/task_program/integrations/simulation/cable.py b/embodichain/lab/task_program/integrations/simulation/cable.py index 6cd657ab89..117be006a3 100644 --- a/embodichain/lab/task_program/integrations/simulation/cable.py +++ b/embodichain/lab/task_program/integrations/simulation/cable.py @@ -76,6 +76,7 @@ class _CableTarget: target_id: str kind: str material_distance: float | None = None + retract_distance: float = 0.0 position: tuple[float | None, float | None, float | None] = (None, None, None) speed: float = 0.05 tolerance: float = 0.02 @@ -325,13 +326,15 @@ def _material_pose(self, target: _CableTarget, part: str) -> torch.Tensor: def pose(self, target: _CableTarget, part: str) -> torch.Tensor: if target.kind == "material": return self._material_pose(target, part) - measured = self.tool(part) if target.kind == "home": return self.home[part].clone() pose = self.tool(part, commanded=target.kind in {"command", "tip_align"}) if target.kind == "port_stage": angle = self.port_yaw[part] c, s = torch.cos(angle), torch.sin(angle) + # Replace the complete rotation; measured tilt is not part of this + # downward staging orientation and would make it non-orthogonal. + pose[:, :3, :3] = 0 pose[:, 0, 0], pose[:, 0, 1] = c, s pose[:, 1, 0], pose[:, 1, 1] = s, -c pose[:, 2, 2] = -1 @@ -357,6 +360,17 @@ def pose(self, target: _CableTarget, part: str) -> torch.Tensor: mouth_y - point[:, 1] ) pose[:, 0, 3] += mouth_x - meets + elif target.kind == "material_lift": + assert target.material_distance is not None + assert target.position[2] is not None + tangent = self.point(target.material_distance)[1][:, :2] + horizontal_norm = tangent.norm(dim=-1, keepdim=True) + if not bool((horizontal_norm > 1e-6).all()): + raise ValueError( + "Material lift requires a cable tangent with a horizontal direction." + ) + pose[:, :2, 3] -= target.retract_distance * tangent / horizontal_norm + pose[:, 2, 3] = target.position[2] elif target.kind == "tip_align": end = cable_endpoints(self.state(), self.lengths)[1][:, -1] - self.origins for axis, value in enumerate(target.position): @@ -382,7 +396,10 @@ def pose(self, target: _CableTarget, part: str) -> torch.Tensor: if value is not None: pose[:, axis, 3] = value if target.stop_when_seated: - pose = torch.where(self.seated()[:, None, None], measured, pose) + # Retain the loading target after seating; holding the measured + # pose would unload contact forces before the configured release. + commanded = self.tool(part, commanded=True) + pose = torch.where(self.seated()[:, None, None], commanded, pose) return pose @@ -472,6 +489,10 @@ def lower( service, target = self._target(call) part = self._part(bound) start, end = service.tool(part), service.pose(target, part) + if target.stop_when_seated: + # Seated rows retain the loaded command throughout the path; + # interpolating from their measured pose would first unload it. + start = torch.where(service.seated()[:, None, None], end, start) distance = (start[:, :3, 3] - end[:, :3, 3]).norm(dim=-1).max().item() trace = ( (start[:, :3, :3].transpose(1, 2) @ end[:, :3, :3]) @@ -562,7 +583,13 @@ def lower( angle <= target.orientation_tolerance ) if target.stop_when_seated: - passed |= service.seated() & ~service.observe() + # Above-channel release alignment still requires ordinary TCP + # tracking. Only an inserted endpoint may retain contact bias. + passed |= ( + service.seated() + & service.metrics()["strict_port"] + & ~service.observe() + ) details = { "position_error_m": error.tolist(), "orientation_error_rad": angle.tolist(), @@ -696,6 +723,8 @@ def _decode_cable_service(value: object) -> _CableService: allowed = { "material": motion_fields | {"material_distance", "position", "port_screen", "firm"}, + "material_lift": motion_fields + | {"material_distance", "retract_distance", "position"}, "measured": motion_fields | {"position", "stop_when_seated"}, "command": motion_fields | {"position", "stop_when_seated"}, "home": motion_fields, @@ -724,6 +753,7 @@ def _decode_cable_service(value: object) -> _CableService: raise ValueError(f"Unsupported fields for cable target kind {kind!r}.") if kind not in { "material", + "material_lift", "measured", "command", "home", @@ -763,10 +793,17 @@ def _decode_cable_service(value: object) -> _CableService: or target.maximum_correction <= 0 ): raise ValueError("Cable motion limits must be positive.") - if kind in {"material", "yaw_align", "ray_align"} and ( + if kind in {"material", "material_lift", "yaw_align", "ray_align"} and ( target.material_distance is None or target.material_distance < 0 ): raise ValueError("A cable material coordinate must be nonnegative.") + if kind == "material_lift": + if target.retract_distance < 0: + raise ValueError("Cable retraction distance must be nonnegative.") + if target.position[:2] != (None, None) or target.position[2] is None: + raise ValueError( + "Material lift position must specify only its Z height." + ) if kind == "open" and target.release_part is None: raise ValueError("An opening target requires its explicit release_part.") if kind == "check" and target.condition not in {"pose", "lift", "seated"}: diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md index 3a641b768a..7e8235a769 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md @@ -14,15 +14,14 @@ in-episode state writes. Use CUDA and a DexSim build exposing `CableDesc`, `Scene.create_cable_batch`, `CableBatch.fetch_default_state/apply_state` and `Scene.reset_arenas`. See the [cable example installation guide](../../../../../../../examples/README.md#newton-cable-demo) -for the development-build dependency and solver migration. Earlier routing -qualification used cable-state commit -`43d078958e6e00158eda0c3024d8af0c5682eb59` with Newton 1.6; those results do not -qualify full routing on a newer engine. A stock wheel lacking the required -cable APIs cannot run this deployment. Permanent cable attachments to articulations -with nonzero initial joint positions are rejected before scene commit: the current -engine resolves their anchors before model-initialization hooks. This task uses -contact-only grasps and does not need that unsupported combination. The file owns Newton/DexUni, dynamic -MuJoCo joints and two-way VBD cable contact; CLI `--physics` does not switch it. +for the development-build dependency and solver migration. The current +revalidation runtime uses DexSim `0.6.0.dev12034`, build +`b040478aa5cb4902b26a19a611eff768d229548b`, Newton 1.7.0 and +Torch 2.11.0+cu130. Full-episode revalidation of the revised path is still in +progress; results from earlier engine builds do not qualify this configuration. +A wheel lacking the required cable APIs cannot run this deployment. The +environment file owns Newton/DexUni, dynamic MuJoCo joints and two-way VBD cable +contact; CLI `--physics` does not switch it. From the EmbodiChain repository: @@ -94,39 +93,68 @@ experiments. Failed motion validators log planning status, TCP position error and maximum measured grasp slip with their limits. Every motion is followed by an explicit bounded hold and a measured TCP position/orientation check. Insertion corrections read the latest material -geometry when grounded; the finite correction slots hold the current pose once -the release corridor is reached. They do not run an unbounded Python loop. +geometry when grounded; the finite correction slots retain the commanded pose +once the release condition is reached. They do not run an unbounded Python loop. ## Stages and reusable boundaries +The program contains 51 segments and 129 Semantic Call invocations. + The right arm places capsule centres at material distances 0.162, 0.282 and 0.438 m; these are the nearest capsule centres to the upstream nominal grasp -locations. The left arm grasps at 0.570 m, selects a wrist branch that can also -reach the channel staging pose, observes the distal tangent, and corrects yaw -and lateral intersection. It lowers through bounded height stages, observes the -actual free end, and accumulates tip-position corrections on the commanded pose -to compensate contact-load bias. Once the end is inside the channel or its -narrow aligned release corridor, it opens the fingers and lets the cable settle. -The final validator requires the actual end inside the channel after release, -retreat and settling. Each stage is -planned lazily from the latest cable state. Both hands return home before the -final settling period. +locations. The left arm grasps at 0.57 m, leaving a 30 mm free span. All four +pinches use a 0.7605 m TCP height. `PickUp` closes and settles with +`lift_height: 0`; a separate `material_lift` target lowers to `MoveEndEffector` +and raises the held material while retracting opposite its observed tangent +projected onto XY. This supplies upstream slack as the cable lifts from the +bench. The lift preserves the measured tool orientation and keeps the original +post-closure slip reference. + +| Grasp material distance | Retraction | Lift TCP height | +| --- | --- | --- | +| Right, 0.162 m | 1.5 mm | 0.774 m | +| Right, 0.282 m | 3 mm | 0.780 m | +| Right, 0.438 m | 8 mm | 0.800 m | +| Left, 0.570 m | 3 mm | 0.780 m | + +The left arm selects a wrist branch that can reach the staging TCP position +`[0.586, 0.055, 0.805]` m above the open channel. The staging target constructs a +complete orthonormal rotation, including when the measured tool has pitch or +roll. Four bounded XY corrections aim the observed free end at +`[0.586, 0.077]` m. Subsequent TCP height stages are 0.795, 0.7875, 0.780 and +0.7655 m, each followed by two correction slots. Each correction is limited to +5 mm and keeps TCP Y at or below 0.05 m so the hand stays outside the channel +mouth while lowering. Final vertical corrections are limited to 3 mm and retain +a 0.7655 m TCP height floor. Corrections accumulate on the commanded pose to +compensate for contact-load bias. + +Release requires the observed endpoint to be inside the strict channel with +`y >= 0.072` m. The configured release region is +`[0.5845, 0.072, 0.753]` to `[0.5875, 0.083, 0.758]` m, entirely within the +channel; readiness also accepts an endpoint already seated in the strict final +region at the same minimum depth. The X bounds account for the 3.5 mm cable +radius between the channel's inner walls. Merely crossing the channel mouth +does not stop the remaining corrections. Release also requires sampled cable +penetration no greater than 1 mm. Once ready, subsequent correction slots retain +the commanded pose and its contact load. Ordinary measured TCP tracking remains +required until the endpoint is seated inside the strict channel. + +The left hand first releases with an 8 mm joint target per finger, then retreats +to TCP `[same X, 0.03, 0.82]` m. `open_clear_of_port` opens the hand fully only +after that retreat, avoiding a full finger sweep through the upstream cable at +the insertion pose. The final validator requires the actual end inside the +strict channel after release, retreat and settling. Each stage is planned +lazily from the latest cable state. Both hands return home before the final +settling period. The YAM IK uses 10 μm position and 1e-5 rad rotation convergence so finite- difference joint acceleration is not dominated by numerical jitter during Cartesian retiming. Task moves accept the planner's possibly longer control grid; grasp-slip and physical placement thresholds are unchanged. -The third carry targets a TCP height of 50 mm above the bench. The former -65 mm target could exhaust the 12 mm slip allowance during `raise_2`, even -with successful IK and acceptable TCP tracking. The shorter lift retains the -same firm grasp and validators. Its CUDA regression checks material-point -clearance, grasp slip, second-spool seating, and robot/spool contact throughout -raising, carrying, lowering, release and retreat. - -At each spool, the right hand releases to a 16 mm jaw aperture, retreats to -0.82 m TCP height, and only then opens fully. Opening through the full finger -stroke beside the first spool sweeps a finger through its rim. These explicit +At each spool, the right hand releases with an 8 mm joint target per finger, +retreats to 0.82 m TCP height, and only then opens fully. Opening through the full +finger stroke beside the first spool sweeps a finger through its rim. These explicit waypoints are task geometry: joint interpolation does not provide obstacle avoidance, and contact response is not a substitute for a clear commanded path. @@ -134,7 +162,8 @@ avoidance, and contact response is not a substitute for a clear commanded path. approach/close/lift command assembly with rigid pickup and requests physical contact verification. The routing host verifies material-point lift and slip. `PickUpOptions` retains the 48-frame close, 0.35-second closed hold and - 18.5 mm lift; `MotionPolicy` samples the lift at the previous 0.05 m/s grid. + zero lift height. The following `material_lift` invocation performs the + retraction and lift before the material-point lift validator runs. - `compute.geometry.cable` owns simulator-independent capsule endpoints, material-point observations and TCP-relative slip measurement. - `task_program/program.yaml` owns the ordered routing and insertion stages. @@ -159,16 +188,28 @@ avoidance, and contact response is not a substitute for a clear commanded path. The scene follows the pinned upstream `RodSpec` coefficients, including density, stretch/shear, bend/twist and contact damping. Shape friction combines by a geometric mean: cable/pad are 60, bench 0.02, and other scenery 2. The task uses -contact history and sticky matching as the upstream contact-only example does; -these values are task settings, not global physics defaults. The routing -component increases left-arm drive stiffness for insertion tracking. The grasp -profile uses the YAM finger-width calibration and cable radius to request a -small interference rather than driving an empty-hand closure against the cable. -The first carry, high third carry and free-end insertion use firm per-invocation -grasp commands through the existing `ActionControlOverrides` interface. The -first grasp needs this holding margin: its light default pinch can reach the -12 mm slip rejection limit during carry/lowering, even without browser input. -Each pinch holds the fully closed command for 0.35 s on the control grid before +`rigid_contact_history: true` with `contact_matching: latest`. Both +`rigid_contact_stick_freeze_translation_eps` and +`rigid_contact_stick_freeze_angular_eps` are set to `0.0`. Contact history still +retains the contact multipliers and local sticking anchors. The zero thresholds +disable VBD's body-level world-space motion freeze for dynamic sticking contacts +without a static/kinematic anchor contact. For sufficiently small motion, that +operation would otherwise restore the previous body pose before computing +velocity. Disabling it allows slowly moving gripper proxies and cable segments +to retain their motion while frictional sticking remains active. +These are task-specific solver settings; the strict final channel bounds and +12 mm slip limit remain unchanged. + +The routing component increases left-arm drive stiffness for insertion tracking. +The grasp profile specifies finger joint positions, not the physical gap between the +collision pads. At the grasp depth, two 3 mm joint targets give an unloaded +inner pad gap of approximately 4.45 mm for the 7 mm diameter cable; the nominal +target therefore has about 2.55 mm of interference. Actual finger positions +depend on the drive and contact response. The first and third right-hand +grasps and the left-hand grasp use zero-position firm commands through the +existing `SkillControlOverrides` interface; the second right-hand grasp uses +the shared 3 mm targets. +Each pinch holds its grasp command for 0.35 s on the control grid before lifting. This dwell is part of the Atomic Skill's `close` phase; the material reference is sampled after closing/settling, before any lift, and is not rebased during the grasp. @@ -180,9 +221,11 @@ within 12 mm of its post-closure TCP-relative position. Final metrics include both spool-seat counts, free-end location, hand clearance, speed and sampled cable/bench, cable/spool and cable/port penetration. `reference_routed` reproduces the upstream broad regions. Final `success` additionally requires -the free end inside the actual -channel and penetration no greater than 1 mm. Sampled SDF diagnostics do not -constitute an exact continuous collision-distance query. +the free end inside the strict region +`[0.5845, 0.064, 0.753]` to `[0.5875, 0.0896, 0.759]` m and penetration no greater +than 1 mm. These final bounds and the original lift/slip limits are unchanged. +Sampled SDF diagnostics do not constitute an exact continuous collision-distance +query. These final cable predicates do not certify the robot's swept path. The first-spool CUDA regression samples backend robot/spool contacts throughout @@ -194,22 +237,30 @@ spool: python -m pytest tests/gym/envs/tasks/test_route_the_cable_cuda.py --run-gpu -m gpu -q ``` -The contact-pickup migration is covered by 1349 CPU tests spanning the Atomic -Skill and semantic-planning contracts and by both existing NoRender CUDA -regressions with seed 73. Maximum grasp slip was 5.26 mm in the first-spool -case and 4.37 mm in the third carry, with no reported robot/spool penetration -in the latter. The original 12 mm slip and 1 mm penetration limits are retained. -The local runtime reports DexSim build `a3daf24edd88b489702c32ccffc7ffa405741ab2`, -Torch 2.11.0+cu130, Newton 1.7.0 and Warp 1.17.0 on an RTX 5090. - -The synchronized FastRT comparison runs the old `GraspCable` implementation at -`df8e304f0` and the unified `PickUp(ContactGraspGoal)` with identical seed 73, -physical configuration, camera and control settings. Its old run stops during -the left-hand pinch at 12.28 mm episode slip; the new run reaches final settling -with 6.96 mm slip but fails the strict port predicate after release. Both report -no robot/spool penetration. These are individual full-task trials. Complete -channel insertion, multi-seed success rates and repeated-reset reliability -require further work. +The three NoRender CUDA regressions pass on the development build listed above, +using seed 73 and a fresh process per case. The full-route case naturally +exhausts all Task Program segments, validates every stage, then requires +`task_success=True` after release, retreat and two seconds of physical settling. + +| Case | Maximum material-point displacement | Reported robot/obstacle penetration | +| --- | --- | --- | +| First-spool release | 0.080 mm | 0 mm | +| Third carry through release and retreat | 0.379 mm | 0 mm | +| Complete route after the main integration | 0.306 mm | 0 mm | + +The full route was repeated successfully after integrating main `1955c640` +and finished at endpoint `[0.58459103, 0.07624141, 0.75500649]` m, +inside the unchanged strict channel. Sampled cable/bench, cable/spool and +cable/port penetration were all zero. The complete-route regression checks raw +robot/bench, robot/spool and robot/port contacts after every control step; it also +checks the final bounds independently of the configured evaluator. The original +12 mm displacement and 1 mm penetration limits remain active. + +The production deployment inspector and 45 focused CPU cases cover the new +material lift, staging orientation, commanded hold, release-depth gating and +acceptance behavior. Diagnostic suffix fixtures are excluded from complete-route +qualification. These individual fresh-process cases do not establish multi-seed +success rates or repeated-reset reliability. Recordings contain full robot commands and `states.cables.cable.segment_state_w`, shaped `(environments, time, 50, 13)` in @@ -224,8 +275,8 @@ commands without overwriting cable states. Both `position` and `position_velocity` expert command modes use the environment control grid; the latter records derived joint velocities and zero velocities during holds. Cable kinematic scrubbing and partial replay resets are rejected. The deployment defaults to one episode and -one attempt per process. Reset/episode repeatability with sticky -contact needs separate physical qualification; API state/reset tests alone do +one attempt per process. Reset/episode repeatability with contact history +needs separate physical qualification; API state/reset tests alone do not prove repeatable frictional routing. Use a fresh process for each trial until that reset path is qualified. Raw trajectory recordings may include failed attempts; select training data using the segment and final outcomes. @@ -239,6 +290,5 @@ handwritten direct-motion path. The episode budget is 12,000 control steps. Static composition/catalog preflight and intermediate physical qualification do not certify a complete cable task. Full acceptance remains strict after release, -retreat and settling. The comparison report records unsuccessful samples and -their measured stopping stage; changing orchestration does not by itself solve -physical insertion. +retreat and settling. Failed samples retain their measured stopping stage; +changing orchestration does not by itself establish physical insertion success. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml index 160c882820..ed779b6f44 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml @@ -13,9 +13,12 @@ physics_config: vbd_options: iterations: 20 rigid_contact_history: true + # Keep moving cable/pad anchors without snapping slow bodies in world space. + rigid_contact_stick_freeze_translation_eps: 0.0 + rigid_contact_stick_freeze_angular_eps: 0.0 rigid_body_contact_buffer_size: 256 collision_options: - contact_matching: sticky + contact_matching: latest max_episode_steps: 12000 num_envs: 1 arena_space: 2.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml index f440a0ec6c..452524ba31 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml @@ -22,7 +22,7 @@ profile: kind: pick_up hand_interp_steps: 48 grasp_settle_steps: 21 - lift_height: 0.0185 + lift_height: 0.0 pre_grasp_distance: 0.0 simulation.cable_move: kind: move_end_effector @@ -49,8 +49,8 @@ runtime_services: - 0.08 port_stage: - 0.586 - - 0.047 - - 0.815 + - 0.055 + - 0.805 port_mouth: - 0.586 - 0.064 @@ -119,12 +119,12 @@ runtime_services: - 0.0896 - 0.759 release_region: - - - 0.585 - - 0.073 + - - 0.5845 + - 0.072 - 0.753 - - - 0.587 - - 0.085 - - 0.766 + - - 0.5875 + - 0.083 + - 0.758 targets: right_approach_0.162: kind: material @@ -150,18 +150,20 @@ runtime_services: position: - null - null - - 0.7615 + - 0.7605 firm: true port_screen: false right_lifted_0.162: kind: check condition: lift raise_0: - kind: measured + kind: material_lift position: - null - null - - 0.78 + - 0.774 + material_distance: 0.162 + retract_distance: 0.0015 raise_0_tracking: kind: check condition: pose @@ -235,18 +237,20 @@ runtime_services: position: - null - null - - 0.7615 + - 0.7605 firm: false port_screen: false right_lifted_0.282: kind: check condition: lift raise_1: - kind: measured + kind: material_lift position: - null - null - 0.78 + material_distance: 0.282 + retract_distance: 0.003 raise_1_tracking: kind: check condition: pose @@ -320,18 +324,20 @@ runtime_services: position: - null - null - - 0.7615 + - 0.7605 firm: true port_screen: false right_lifted_0.438: kind: check condition: lift raise_2: - kind: measured + kind: material_lift position: - null - null - 0.8 + material_distance: 0.438 + retract_distance: 0.008 raise_2_tracking: kind: check condition: pose @@ -414,7 +420,7 @@ runtime_services: position: - null - null - - 0.7615 + - 0.7605 firm: true port_screen: true left_lifted_0.57: @@ -424,58 +430,20 @@ runtime_services: kind: port_stage position: - 0.586 - - 0.047 - - 0.815 + - 0.055 + - 0.805 stage_free_end_tracking: kind: check condition: pose tolerance: 0.02 orientation_tolerance: 0.25 stop_when_seated: false - align_yaw_0: - kind: yaw_align - material_distance: 0.57 - align_yaw_0_tracking: - kind: check - condition: pose - tolerance: 0.02 - orientation_tolerance: 0.25 - stop_when_seated: false - align_lateral_0: - kind: ray_align - material_distance: 0.57 - speed: 0.03 - align_lateral_0_tracking: - kind: check - condition: pose - tolerance: 0.02 - orientation_tolerance: 0.25 - stop_when_seated: false - align_yaw_1: - kind: yaw_align - material_distance: 0.57 - align_yaw_1_tracking: - kind: check - condition: pose - tolerance: 0.02 - orientation_tolerance: 0.25 - stop_when_seated: false - align_lateral_1: - kind: ray_align - material_distance: 0.57 - speed: 0.03 - align_lateral_1_tracking: - kind: check - condition: pose - tolerance: 0.02 - orientation_tolerance: 0.25 - stop_when_seated: false lower_channel_0: kind: command position: - null - null - - 0.79 + - 0.795 speed: 0.025 tolerance: 0.006 stop_when_seated: true @@ -492,7 +460,7 @@ runtime_services: - 0.077 - null maximum_correction: 0.005 - maximum_y: 0.047 + maximum_y: 0.05 speed: 0.02 tolerance: 0.006 stop_when_seated: true @@ -510,7 +478,7 @@ runtime_services: - 0.077 - null maximum_correction: 0.005 - maximum_y: 0.047 + maximum_y: 0.05 speed: 0.02 tolerance: 0.006 stop_when_seated: true @@ -526,7 +494,7 @@ runtime_services: position: - null - null - - 0.78 + - 0.7875 speed: 0.025 tolerance: 0.006 stop_when_seated: true @@ -543,7 +511,7 @@ runtime_services: - 0.077 - null maximum_correction: 0.005 - maximum_y: 0.047 + maximum_y: 0.05 speed: 0.02 tolerance: 0.006 stop_when_seated: true @@ -561,7 +529,7 @@ runtime_services: - 0.077 - null maximum_correction: 0.005 - maximum_y: 0.047 + maximum_y: 0.05 speed: 0.02 tolerance: 0.006 stop_when_seated: true @@ -577,7 +545,7 @@ runtime_services: position: - null - null - - 0.772 + - 0.78 speed: 0.025 tolerance: 0.006 stop_when_seated: true @@ -594,7 +562,7 @@ runtime_services: - 0.077 - null maximum_correction: 0.005 - maximum_y: 0.047 + maximum_y: 0.05 speed: 0.02 tolerance: 0.006 stop_when_seated: true @@ -612,7 +580,7 @@ runtime_services: - 0.077 - null maximum_correction: 0.005 - maximum_y: 0.047 + maximum_y: 0.05 speed: 0.02 tolerance: 0.006 stop_when_seated: true @@ -628,7 +596,7 @@ runtime_services: position: - null - null - - 0.7645 + - 0.7655 speed: 0.025 tolerance: 0.006 stop_when_seated: true @@ -645,8 +613,8 @@ runtime_services: - 0.077 - 0.754 maximum_correction: 0.005 - maximum_y: 0.047 - minimum_z: 0.762 + maximum_y: 0.05 + minimum_z: 0.7655 speed: 0.02 tolerance: 0.006 stop_when_seated: true @@ -664,8 +632,8 @@ runtime_services: - 0.077 - 0.754 maximum_correction: 0.005 - maximum_y: 0.047 - minimum_z: 0.762 + maximum_y: 0.05 + minimum_z: 0.7655 speed: 0.02 tolerance: 0.006 stop_when_seated: true @@ -683,6 +651,7 @@ runtime_services: kind: open samples: 36 release_part: left_arm + finger_position: 0.008 retreat_free_end: kind: command position: @@ -708,3 +677,89 @@ runtime_services: settle_and_validate: kind: hold samples: 120 + align_over_port_0: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.05 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + align_over_port_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_over_port_1: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.05 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + align_over_port_1_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_over_port_2: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.05 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + align_over_port_2_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_over_port_3: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.05 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + align_over_port_3_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + raise_free_end: + kind: material_lift + material_distance: 0.57 + retract_distance: 0.003 + position: + - null + - null + - 0.78 + raise_free_end_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: false + open_clear_of_port: + kind: open + samples: 36 + release_part: left_arm diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.yaml index bebe87453d..b1cd0dea7e 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.yaml @@ -29,7 +29,7 @@ program: target: right_approach_0.162_tracking resources: primary: right_manipulator -- segment: right_pinch_lift_0.162 +- segment: right_pinch_0.162 steps: - kind: invoke call: @@ -39,14 +39,6 @@ program: target: right_grasp_0.162 resources: primary: right_manipulator - - kind: invoke - call: - kind: registered - call_id: simulation.cable_check - arguments: - target: right_lifted_0.162 - resources: - primary: right_manipulator - segment: raise_0 steps: - kind: invoke @@ -73,6 +65,14 @@ program: target: raise_0_tracking resources: primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: right_lifted_0.162 + resources: + primary: right_manipulator - segment: carry_0 steps: - kind: invoke @@ -197,7 +197,7 @@ program: target: right_approach_0.282_tracking resources: primary: right_manipulator -- segment: right_pinch_lift_0.282 +- segment: right_pinch_0.282 steps: - kind: invoke call: @@ -207,14 +207,6 @@ program: target: right_grasp_0.282 resources: primary: right_manipulator - - kind: invoke - call: - kind: registered - call_id: simulation.cable_check - arguments: - target: right_lifted_0.282 - resources: - primary: right_manipulator - segment: raise_1 steps: - kind: invoke @@ -241,6 +233,14 @@ program: target: raise_1_tracking resources: primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: right_lifted_0.282 + resources: + primary: right_manipulator - segment: carry_1 steps: - kind: invoke @@ -365,7 +365,7 @@ program: target: right_approach_0.438_tracking resources: primary: right_manipulator -- segment: right_pinch_lift_0.438 +- segment: right_pinch_0.438 steps: - kind: invoke call: @@ -375,14 +375,6 @@ program: target: right_grasp_0.438 resources: primary: right_manipulator - - kind: invoke - call: - kind: registered - call_id: simulation.cable_check - arguments: - target: right_lifted_0.438 - resources: - primary: right_manipulator - segment: raise_2 steps: - kind: invoke @@ -409,6 +401,14 @@ program: target: raise_2_tracking resources: primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: right_lifted_0.438 + resources: + primary: right_manipulator - segment: carry_2 steps: - kind: invoke @@ -559,7 +559,7 @@ program: target: left_approach_0.57_tracking resources: primary: left_manipulator -- segment: left_pinch_lift_0.57 +- segment: left_pinch_0.57 steps: - kind: invoke call: @@ -569,6 +569,32 @@ program: target: left_grasp_0.57 resources: primary: left_manipulator +- segment: raise_free_end + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: raise_free_end + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: raise_free_end_tracking + resources: + primary: left_manipulator - kind: invoke call: kind: registered @@ -603,14 +629,14 @@ program: target: stage_free_end_tracking resources: primary: left_manipulator -- segment: align_yaw_0 +- segment: align_over_port_0 steps: - kind: invoke call: kind: registered call_id: simulation.cable_move arguments: - target: align_yaw_0 + target: align_over_port_0 resources: primary: left_manipulator - kind: invoke @@ -626,17 +652,17 @@ program: kind: registered call_id: simulation.cable_check arguments: - target: align_yaw_0_tracking + target: align_over_port_0_tracking resources: primary: left_manipulator -- segment: align_lateral_0 +- segment: align_over_port_1 steps: - kind: invoke call: kind: registered call_id: simulation.cable_move arguments: - target: align_lateral_0 + target: align_over_port_1 resources: primary: left_manipulator - kind: invoke @@ -652,17 +678,17 @@ program: kind: registered call_id: simulation.cable_check arguments: - target: align_lateral_0_tracking + target: align_over_port_1_tracking resources: primary: left_manipulator -- segment: align_yaw_1 +- segment: align_over_port_2 steps: - kind: invoke call: kind: registered call_id: simulation.cable_move arguments: - target: align_yaw_1 + target: align_over_port_2 resources: primary: left_manipulator - kind: invoke @@ -678,17 +704,17 @@ program: kind: registered call_id: simulation.cable_check arguments: - target: align_yaw_1_tracking + target: align_over_port_2_tracking resources: primary: left_manipulator -- segment: align_lateral_1 +- segment: align_over_port_3 steps: - kind: invoke call: kind: registered call_id: simulation.cable_move arguments: - target: align_lateral_1 + target: align_over_port_3 resources: primary: left_manipulator - kind: invoke @@ -704,7 +730,7 @@ program: kind: registered call_id: simulation.cable_check arguments: - target: align_lateral_1_tracking + target: align_over_port_3_tracking resources: primary: left_manipulator - segment: lower_channel_0 @@ -1065,6 +1091,16 @@ program: target: retreat_free_end_tracking resources: primary: left_manipulator +- segment: open_clear_of_port + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: open_clear_of_port + resources: + primary: left_hand - segment: home_left steps: - kind: invoke diff --git a/tests/gym/envs/task_program/test_cable_material.py b/tests/gym/envs/task_program/test_cable_material.py index 4d9a409e69..07e8a5f305 100644 --- a/tests/gym/envs/task_program/test_cable_material.py +++ b/tests/gym/envs/task_program/test_cable_material.py @@ -17,6 +17,7 @@ from __future__ import annotations from copy import deepcopy +from dataclasses import replace from pathlib import Path from types import SimpleNamespace import math @@ -33,10 +34,11 @@ _decode_cable_service, _CableRuntime, _CableCheckLowerer, + _CableMoveLowerer, _CablePickFactory, ) from embodichain.lab.task_program.semantics import RegisteredSemanticCall -from embodichain.lab.sim.atomic_skills import MoveJointsOptions +from embodichain.lab.sim.atomic_skills import MoveEndEffectorOptions, MoveJointsOptions pytestmark = pytest.mark.no_sim _ROOT = Path(__file__).resolve().parents[4] @@ -65,7 +67,7 @@ def test_cable_deployment_preflights_all_calls_and_measured_final_acceptance(): physical.validate_scene_bindings(deployment.scene_binding) registration = deployment.integration.registration compiled = registration.catalog.preflight(deployment.load_program()) - assert compiled.segment_count == 49 + assert compiled.segment_count == 51 assert registration.evaluation_final_refs == ("route_success",) assert registration.scene_binding.declare().entries == () factories = registration.registered_semantic_lowerer_factories @@ -95,6 +97,35 @@ def test_cable_configuration_rejects_invalid_closed_payload(change): _decode_cable_service(value) +@pytest.mark.parametrize( + "changes", + [ + {"material_distance": None}, + {"material_distance": -0.01}, + {"retract_distance": -0.001}, + {"retract_distance": float("inf")}, + {"position": [0.5, None, 0.78]}, + {"position": [None, None, None]}, + {"position": [None, None, float("nan")]}, + {"firm": True}, + {"kind": "material", "retract_distance": 0.005}, + ], +) +def test_material_lift_rejects_invalid_geometry_and_inapplicable_fields(changes): + value = _declaration() + target = { + "kind": "material_lift", + "material_distance": 0.162, + "position": [None, None, 0.78], + } + target.update(changes) + if target.get("material_distance") is None: + target.pop("material_distance") + value["targets"] = {"lift": target} + with pytest.raises((ValueError, TypeError)): + _decode_cable_service(value) + + def test_cable_binding_rejects_missing_physical_uid_before_simulation(): from embodichain.lab.gym.utils._component_composition import ( _validate_scene_binding_targets, @@ -166,6 +197,65 @@ def test_material_observation_maps_selected_rows_and_arena_origins_once(): ) +@pytest.mark.parametrize("retract_distance", [0.0, 0.008]) +def test_material_lift_retracts_from_measured_tool_along_horizontal_material_tangent( + retract_distance, +): + service, state, measured = _runtime() + value = _declaration() + target = { + "kind": "material_lift", + "material_distance": 0.162, + "position": [None, None, 0.78], + } + if retract_distance: + target["retract_distance"] = retract_distance + value["targets"] = {"lift": target} + target = _decode_cable_service(value).target("lift") + measured[3] = torch.tensor([0.50, 0.02, 0.76]) + measured[1] = torch.tensor([0.60, 0.03, 0.77]) + commanded = measured + 0.01 + service.robot.get_qpos = lambda *, target=False: commanded if target else measured + original_fk = service.robot.compute_fk + measured_rotation = torch.tensor( + [[0.0, 1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, -1.0]] + ) + + def rotated_fk(qpos, **kwargs): + pose = original_fk(qpos, **kwargs) + pose[:, :3, :3] = measured_rotation + return pose + + service.robot.compute_fk = rotated_fk + # Native rows 3 and 1 belong to arenas at X=40 and X=20. The first + # tangent also tilts upward, so the XY projection must be normalized. + for row, direction in ((3, (0.0, 0.6, 0.8)), (1, (-0.6, 0.8, 0.0))): + x, y, z = direction + quaternion = torch.tensor([-y, x, 0.0, 1 + z]) + state[row, 13, 3:7] = quaternion / quaternion.norm() + pose = service.pose(target, "left_arm") + expected = torch.tensor([[0.50, 0.02, 0.78], [0.60, 0.03, 0.78]]) + expected[:, :2] -= retract_distance * torch.tensor([[0.0, 1.0], [-0.6, 0.8]]) + torch.testing.assert_close(pose[:, :3, 3], expected, atol=2e-6, rtol=0) + torch.testing.assert_close(pose[:, :3, :3], measured_rotation.repeat(2, 1, 1)) + + +def test_material_lift_rejects_vertical_tangent_without_horizontal_retraction_axis(): + service, state, _ = _runtime() + value = _declaration() + value["targets"] = { + "lift": { + "kind": "material_lift", + "material_distance": 0.162, + "position": [None, None, 0.78], + "retract_distance": 0.008, + } + } + state[1, 13, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0]) + with pytest.raises(ValueError, match="horizontal direction"): + service.pose(_decode_cable_service(value).target("lift"), "left_arm") + + def test_material_reference_is_captured_after_close_and_tracks_slip_per_row(): service, state, qpos = _runtime() service.pending_pick = ("right_arm", service.cfg.target("right_grasp_0.162")) @@ -190,6 +280,93 @@ def test_material_reference_is_captured_after_close_and_tracks_slip_per_row(): assert not fresh.grasps and fresh.max_slip.tolist() == [0, 0] +def test_port_stage_replaces_measured_tilt_with_selected_downward_orientation(): + service, _, _ = _runtime() + original_fk = service.robot.compute_fk + cosine = math.sqrt(0.5) + # A physical tool tilted by 45 degrees in both pitch and roll. + measured_rotation = torch.tensor( + [[cosine, 0.5, 0.5], [0.0, cosine, -cosine], [-cosine, 0.5, 0.5]] + ) + + def tilted_fk(qpos, **kwargs): + pose = original_fk(qpos, **kwargs) + pose[:, :3, :3] = measured_rotation + return pose + + service.robot.compute_fk = tilted_fk + service.port_yaw["left_arm"] = torch.tensor([math.pi / 2, -math.pi / 2]) + target = replace( + service.cfg.target("stage_free_end"), position=(0.586, 0.047, 0.815) + ) + pose = service.pose(target, "left_arm") + rotation = pose[:, :3, :3] + # The two reachable wrist branches point down with opposite channel yaw. + expected_rotation = torch.tensor( + [ + [[0.0, 1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, -1.0]], + [[0.0, -1.0, 0.0], [-1.0, 0.0, 0.0], [0.0, 0.0, -1.0]], + ] + ) + torch.testing.assert_close(rotation, expected_rotation) + torch.testing.assert_close( + rotation.transpose(1, 2) @ rotation, torch.eye(3).repeat(2, 1, 1) + ) + torch.testing.assert_close(torch.linalg.det(rotation), torch.ones(2)) + torch.testing.assert_close( + pose[:, :3, 3], torch.tensor([[0.586, 0.047, 0.815]]).repeat(2, 1) + ) + + +def test_seated_rows_preserve_contact_loading_command_while_other_rows_move(): + service, _, measured = _runtime() + commanded = measured.clone() + measured[3] = torch.tensor([0.585, 0.034, 0.781]) + measured[1] = torch.tensor([0.585, 0.033, 0.785]) + commanded[3] = torch.tensor([0.586, 0.035, 0.775]) + commanded[1] = torch.tensor([0.587, 0.034, 0.780]) + service.robot.get_qpos = lambda *, target=False: commanded if target else measured + service.seated = lambda: torch.tensor([True, False]) + target = replace( + service.cfg.target("lower_channel_3"), position=(None, None, 0.760) + ) + + pose = service.pose(target, "left_arm") + + # The loaded command remains intact for the seated row; the other row + # retains its commanded XY and receives the next requested height. + torch.testing.assert_close( + pose[:, :3, 3], + torch.tensor([[0.586, 0.035, 0.775], [0.587, 0.034, 0.760]]), + ) + service.cfg = replace(service.cfg, targets=(target,)) + lowerer = _CableMoveLowerer(service.cfg) + lowerer.runtime = service + endpoint = SimpleNamespace(runtime_target=SimpleNamespace(control_part="left_arm")) + bound = SimpleNamespace( + binding=SimpleNamespace( + resources={"primary": SimpleNamespace(endpoints={"motion": endpoint})} + ) + ) + result = lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.cable_move", arguments={"target": target.target_id} + ), + context=SimpleNamespace(require_control_dt=lambda: 1 / 60), + bound=bound, + option_template=MoveEndEffectorOptions(), + ) + path = result.goal.xpos + # Every command for the seated row must retain its loaded pose, including + # the first interpolation sample. Returning to the measured pose unloads + # the contact even if the final sample restores the original command. + torch.testing.assert_close(path[0], pose[0].expand_as(path[0])) + torch.testing.assert_close(path[1, -1], pose[1]) + assert bool((path[1, :, 2, 3].diff() < 0).all()) + assert path[1, 0, 2, 3] < measured[1, 2] + assert path[1, 0, 2, 3] > commanded[1, 2] + + def test_cable_tracking_rejects_wrong_orientation_at_same_position(): service, _, _ = _runtime() target = service.tool("left_arm") @@ -216,6 +393,55 @@ def test_cable_tracking_rejects_wrong_orientation_at_same_position(): ) +@pytest.mark.parametrize("tracking_error", ["position", "orientation"]) +def test_release_alignment_above_channel_does_not_bypass_tcp_tracking(tracking_error): + service, _, _ = _runtime() + service.last_targets["left_arm"] = service.tool("left_arm") + # Both rows are aligned for release, but only the first is physically + # inside the channel. Both tools miss the 6 mm or 0.25 rad tracking limit. + if tracking_error == "position": + service.last_targets["left_arm"][:, 0, 3] += 0.007 + else: + service.last_targets["left_arm"][:, :2, :2] = torch.tensor( + [[math.cos(0.3), -math.sin(0.3)], [math.sin(0.3), math.cos(0.3)]] + ) + service.seated = lambda: torch.tensor([True, True]) + service.metrics = lambda: {"strict_port": torch.tensor([True, False])} + lowerer = _CableCheckLowerer(service.cfg) + lowerer.runtime = service + endpoint = SimpleNamespace(runtime_target=SimpleNamespace(control_part="left_arm")) + bound = SimpleNamespace( + binding=SimpleNamespace( + resources={"primary": SimpleNamespace(endpoints={"motion": endpoint})} + ) + ) + call = RegisteredSemanticCall( + call_id="simulation.cable_check", + arguments={"target": "lower_channel_3_tracking"}, + ) + with pytest.raises(ValueError, match=r"failed in envs \[1\]"): + lowerer.lower( + call, + context=SimpleNamespace(env_ids=service.env_ids), + bound=bound, + option_template=MoveJointsOptions(), + ) + # The hovered row can proceed once it meets the original tracking limit; + # the inserted row may retain its contact-loaded command. + if tracking_error == "position": + service.last_targets["left_arm"][1, 0, 3] = 0.005 + else: + service.last_targets["left_arm"][1, :2, :2] = torch.tensor( + [[math.cos(0.2), -math.sin(0.2)], [math.sin(0.2), math.cos(0.2)]] + ) + lowerer.lower( + call, + context=SimpleNamespace(env_ids=service.env_ids), + bound=bound, + option_template=MoveJointsOptions(), + ) + + def test_cable_observation_rejects_invalid_quaternion_per_row(): service, state, _ = _runtime() state[1, :, 3:7] = 0 diff --git a/tests/gym/envs/tasks/test_route_the_cable.py b/tests/gym/envs/tasks/test_route_the_cable.py index 3a47b12bab..a1de6eb6ac 100644 --- a/tests/gym/envs/tasks/test_route_the_cable.py +++ b/tests/gym/envs/tasks/test_route_the_cable.py @@ -36,15 +36,20 @@ pytestmark = pytest.mark.no_sim -def measure_route(state, lengths, hands, origins): +def _route_geometry(): path = ( Path(__file__).resolve().parents[4] / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml" ) - geometry = yaml.safe_load(path.read_text())["runtime_services"]["cable_material"][ + return yaml.safe_load(path.read_text())["runtime_services"]["cable_material"][ "geometry" ] - return _measure_cable_route(state, lengths, hands, origins, geometry, 0.0035) + + +def measure_route(state, lengths, hands, origins): + return _measure_cable_route( + state, lengths, hands, origins, _route_geometry(), 0.0035 + ) @pytest.fixture(autouse=True) @@ -238,6 +243,69 @@ def test_strict_channel_rejects_end_that_only_meets_broad_region(): assert not metrics["strict_port"].item() +def test_release_requires_seating_depth_even_when_tip_is_inside_strict_channel(): + geometry = _route_geometry() + geometry["release_region"] = [[0.585, 0.072, 0.753], [0.587, 0.083, 0.772]] + state = _goal_state().repeat(2, 1, 1) + # The last capsule points along +Y and has a 6 mm half length. Both tips + # are in the final channel. The shallow tip must not stop corrections; + # the deeper tip is seated near a sidewall, outside the hover corridor. + state[:, -1, :3] = torch.tensor([[0.586, 0.060, 0.755], [0.5846, 0.067, 0.757]]) + metrics = _measure_cable_route( + state, + torch.full((50,), 0.012), + torch.ones(2, 2, 3), + torch.zeros(2, 3), + geometry, + 0.0035, + ) + assert metrics["strict_port"].tolist() == [True, True] + assert metrics["release_ready"].tolist() == [False, True] + + +def test_above_channel_release_region_is_independent_of_final_strict_region(): + geometry = _route_geometry() + # An open-top placement may release from above the channel. This is an + # action precondition; the final endpoint must still be at or below .759 m. + geometry["release_region"] = [[0.585, 0.072, 0.753], [0.587, 0.083, 0.772]] + state = _goal_state().repeat(3, 1, 1) + state[:, -1, :3] = torch.tensor( + [[0.586, 0.071, 0.766], [0.586, 0.071, 0.773], [0.5846, 0.071, 0.766]] + ) + metrics = _measure_cable_route( + state, + torch.full((50,), 0.012), + torch.ones(3, 2, 3), + torch.zeros(3, 3), + geometry, + 0.0035, + ) + assert metrics["release_ready"].tolist() == [True, False, False] + assert metrics["strict_port"].tolist() == [False, False, False] + + +@pytest.mark.parametrize("invalid", ["nonfinite", "zero_quaternion"]) +def test_strict_seating_does_not_allow_release_with_invalid_cable_state(invalid): + geometry = _route_geometry() + geometry["release_region"] = [[0.585, 0.072, 0.753], [0.587, 0.083, 0.772]] + state = _goal_state() + state[0, -1, :3] = torch.tensor([0.5846, 0.067, 0.757]) + if invalid == "nonfinite": + state[0, 0, 0] = float("nan") + else: + state[0, 0, 3:7] = 0 + metrics = _measure_cable_route( + state, + torch.full((50,), 0.012), + torch.ones(1, 2, 3), + torch.zeros(1, 3), + geometry, + 0.0035, + ) + assert metrics["strict_port"].item() + assert not metrics["release_ready"].item() + + @pytest.mark.parametrize("invalid", ["motion", "nonfinite", "held", "empty_seat"]) def test_reference_metrics_reject_invalid_or_incomplete_observations(invalid): state = _goal_state() diff --git a/tests/gym/envs/tasks/test_route_the_cable_cuda.py b/tests/gym/envs/tasks/test_route_the_cable_cuda.py index d5f2802e84..286962f8a0 100644 --- a/tests/gym/envs/tasks/test_route_the_cable_cuda.py +++ b/tests/gym/envs/tasks/test_route_the_cable_cuda.py @@ -75,15 +75,17 @@ def test_first_spool_release_preserves_seating_without_gripper_penetration( scene = env.sim.spawn_result handles = scene.handles robot_path = next(path for path in handles if path.endswith("/yam")) - spool_paths = [path for path in handles if "/spool_" in path] + obstacle_paths = [ + path for path in handles if "/spool_" in path or path.endswith("/bench") + ] query = scene.create_contact_query( - [handles[robot_path], *(handles[path] for path in spool_paths)], + [handles[robot_path], *(handles[path] for path in obstacle_paths)], match="all", capacity=4096, device=env.device, ) - spool_ids = torch.tensor( - [actor.actor_id for actor in query.actors if actor.path in spool_paths], + obstacle_ids = torch.tensor( + [actor.actor_id for actor in query.actors if actor.path in obstacle_paths], device=env.device, ) robot_ids = torch.tensor( @@ -99,10 +101,10 @@ def test_first_spool_release_preserves_seating_without_gripper_penetration( contacts = query.fetch_solver_report() assert contacts.dropped_count == 0 pairs = contacts.actor_ids[: contacts.count] - robot_spool = torch.isin(pairs, robot_ids).any(-1) & torch.isin( - pairs, spool_ids + robot_obstacle = torch.isin(pairs, robot_ids).any(-1) & torch.isin( + pairs, obstacle_ids ).any(-1) - distances = contacts.distance[: contacts.count][robot_spool] + distances = contacts.distance[: contacts.count][robot_obstacle] if distances.numel(): distance = float(distances.min()) if distance < worst_distance: @@ -113,6 +115,8 @@ def test_first_spool_release_preserves_seating_without_gripper_penetration( # Finite cable states and seated cable alone previously accepted a jaw # sweep through the rim with about 16 mm reported penetration. + # Include the bench when qualifying the deeper initial pinch. + record_property("first_spool_robot_obstacle_distance_m", worst_distance) assert worst_distance >= -0.001, (worst_segment, worst_distance) max_slip = float(_material_runtime(env).max_slip.max()) record_property("max_grasp_slip_m", max_slip) @@ -196,6 +200,133 @@ def test_third_carry_preserves_grasp_and_clears_spools( return pytest.fail("The expert did not complete the third carry") + def test_full_route_keeps_released_endpoint_in_channel( + self, record_property: Callable[[str, float], None] + ) -> None: + """Require the measured route to survive opening, retreat and settling.""" + env = self.env + assert env.num_envs == 1 + scene = env.sim.spawn_result + handles = scene.handles + robot_path = next(path for path in handles if path.endswith("/yam")) + obstacle_paths = [ + path + for path in handles + if path.endswith("/bench") + or path.rsplit("/", 1)[-1].startswith(("spool_", "port_")) + ] + assert any(path.endswith("/bench") for path in obstacle_paths) + assert any("/spool_" in path for path in obstacle_paths) + assert any("/port_" in path for path in obstacle_paths) + query = scene.create_contact_query( + [handles[robot_path], *(handles[path] for path in obstacle_paths)], + match="all", + capacity=4096, + device=env.device, + ) + robot_ids = torch.tensor( + [actor.actor_id for actor in query.actors if actor.path == robot_path], + device=env.device, + ) + obstacle_ids = torch.tensor( + [actor.actor_id for actor in query.actors if actor.path in obstacle_paths], + device=env.device, + ) + assert robot_ids.numel() and obstacle_ids.numel() + worst_distance = 0.0 + worst_segment = "" + required_boundaries = { + "release_free_end", + "retreat_free_end", + "settle_and_validate", + } + completed_boundaries: set[str] = set() + settling_steps = 0 + last_segment = None + env.reset() + for segment in env.create_demo_segments(): + last_segment = segment.name + segment_distance = 0.0 + for action in segment.actions: + env.step(action) + # The material goal cannot detect a finger penetrating the + # bench or port during the left-hand pinch and insertion. + contacts = query.fetch_solver_report() + assert contacts.dropped_count == 0, ( + segment.name, + contacts.dropped_count, + ) + pairs = contacts.actor_ids[: contacts.count] + robot_obstacle = torch.isin(pairs, robot_ids).any(-1) & torch.isin( + pairs, obstacle_ids + ).any(-1) + distances = contacts.distance[: contacts.count][robot_obstacle] + if distances.numel(): + segment_distance = min(segment_distance, float(distances.min())) + if segment_distance < worst_distance: + worst_distance, worst_segment = segment_distance, segment.name + record_property( + f"robot_obstacle_{segment.name}_distance_m", segment_distance + ) + record_property( + "full_route_robot_obstacle_distance_m", worst_distance + ) + assert worst_distance >= -0.001, (worst_segment, worst_distance) + if segment.name == "settle_and_validate": + settling_steps += 1 + measurements = _material_runtime(env).metrics() + endpoint = measurements["end_position"][0] + # Keep the most recent endpoint even when an earlier stage fails. + # Named boundaries distinguish release-induced escape from drift + # during retreat or the final settling interval. + for axis, value in zip("xyz", endpoint, strict=True): + record_property(f"last_endpoint_{axis}_m", float(value)) + if segment.name in required_boundaries: + record_property(f"{segment.name}_endpoint_{axis}_m", float(value)) + record_property( + "max_grasp_slip_m", float(measurements["max_grasp_slip"].max()) + ) + assert bool(segment.validator().all()), ( + segment.name, + { + key: value.detach().cpu().tolist() + for key, value in measurements.items() + }, + ) + if segment.name in required_boundaries: + completed_boundaries.add(segment.name) + + assert completed_boundaries == required_boundaries + assert last_segment == "settle_and_validate" + record_property("full_route_robot_obstacle_distance_m", worst_distance) + # The deployment's 120 control frames at 60 Hz must include a real + # two-second physical settling interval after release and retreat. + settling_seconds = settling_steps * env.step_dt + record_property("settling_seconds", settling_seconds) + assert settling_seconds >= 2.0 - 1e-9 + + measurements = _material_runtime(env).metrics() + endpoint = measurements["end_position"] + # Independent physical channel bounds from the task acceptance + # contract: loosening the configured evaluator must not pass this case. + lower = endpoint.new_tensor((0.5845, 0.064, 0.753)) + upper = endpoint.new_tensor((0.5875, 0.0896, 0.759)) + assert bool(((endpoint >= lower) & (endpoint <= upper)).all()), endpoint + assert bool((measurements["max_grasp_slip"] <= 0.012).all()) + for metric in ( + "bench_penetration", + "spool_penetration", + "port_penetration", + ): + record_property(metric + "_m", float(measurements[metric].max())) + assert bool((measurements[metric] <= 0.001).all()), ( + metric, + measurements[metric], + ) + success = env.is_task_success() + record_property("task_success", float(success.all())) + assert bool(success.all()), measurements + def _run_spool_case(case_name: str, metrics_path: Path) -> None: """Execute one unchanged acceptance case in its own NoRender world.""" @@ -266,3 +397,14 @@ def test_third_carry_preserves_grasp_and_clears_spools( tmp_path, record_property, ) + + def test_full_route_keeps_released_endpoint_in_channel( + self, + tmp_path: Path, + record_property: Callable[[str, float], None], + ) -> None: + _assert_spool_case_in_subprocess( + "test_full_route_keeps_released_endpoint_in_channel", + tmp_path, + record_property, + ) From 8536d9958624fab7323645337d745c0cc22dc236 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 16 Nov 2026 14:49:33 +0800 Subject: [PATCH 34/43] feat(task): execute the Franka rope wrap as a configured Task Program --- .../topics/atomic-actions/execution.md | 8 +- .../topics/task-programs/configuration.md | 7 + docs/source/api_reference/public_api.rst | 69 ++ docs/source/api_reference/robot_catalogs.rst | 7 + embodichain/lab/evaluation/_recording.py | 12 +- .../lab/gym/envs/managers/observations.py | 22 + embodichain/lab/sim/atomic_skills/core.py | 8 +- .../atomic_skills/primitives/move_joints.py | 19 +- .../task_program/integrations/configured.py | 35 +- examples/README.md | 10 + examples/sim/demo/franka_rope_wrap/README.md | 143 ++++ .../sim/demo/franka_rope_wrap/__main__.py | 412 ++++++++++++ .../sim/demo/franka_rope_wrap/config.json | 1 + .../embodichain_robots/_asset_catalog.py | 4 + .../embodichain_robots/franka/assets.py | 75 ++- .../embodichain_robots/franka/rope_pads.py | 67 ++ .../embodiments/franka_flat_pads.yaml | 66 ++ .../cable/franka_rope_wrap/catalog.yaml | 8 + .../cable/franka_rope_wrap/config.json | 210 ++++++ .../cable/franka_rope_wrap/env.yaml | 228 +++++++ .../cable/franka_rope_wrap/task.franka.yaml | 22 + .../task_program/evaluation.yaml | 102 +++ .../task_program/integration.yaml | 23 + .../task_program/program.yaml | 87 +++ .../embodichain_tasks/__init__.py | 1 + .../manipulation/cable/franka_rope_wrap.py | 617 ++++++++++++++++++ .../manipulation/cable/wrap_evaluation.py | 286 ++++++++ .../manipulation/cable/wrap_integration.py | 161 +++++ .../manipulation/cable/wrap_measurements.py | 318 +++++++++ .../manipulation/cable/wrap_motion.py | 334 ++++++++++ tests/gym/envs/tasks/test_franka_rope_wrap.py | 143 ++++ .../envs/tasks/test_franka_rope_wrap_scene.py | 83 +++ tests/sim/atomic_skills/test_actions.py | 42 ++ 33 files changed, 3607 insertions(+), 23 deletions(-) create mode 100644 examples/sim/demo/franka_rope_wrap/README.md create mode 100644 examples/sim/demo/franka_rope_wrap/__main__.py create mode 120000 examples/sim/demo/franka_rope_wrap/config.json create mode 100644 extensions/embodichain_robots/franka/rope_pads.py create mode 100644 extensions/embodichain_tasks/configs/components/embodiments/franka_flat_pads.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/catalog.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/config.json create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/env.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task.franka.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/evaluation.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/integration.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/program.yaml create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_evaluation.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_integration.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_measurements.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_motion.py create mode 100644 tests/gym/envs/tasks/test_franka_rope_wrap.py create mode 100644 tests/gym/envs/tasks/test_franka_rope_wrap_scene.py diff --git a/agent_context/topics/atomic-actions/execution.md b/agent_context/topics/atomic-actions/execution.md index a1f9c0f8b9..79f251ea92 100644 --- a/agent_context/topics/atomic-actions/execution.md +++ b/agent_context/topics/atomic-actions/execution.md @@ -192,9 +192,11 @@ physically qualified task needs a lower speed than the selected robot's limits. default) or `zero` (explicit position-only control). Configured execution policies decode the same field under `motion`. `AtomicSkill.build_plan` derives missing velocities after composite skills -have assembled their final full-robot positions and timing. The first command, -both ends of every stationary joint interval, and terminal settling commands -use zero velocity; failed rows remain held. +have assembled their final full-robot positions and timing. By default the first +command, both ends of a stationary joint interval and terminal settling commands +use zero velocity. For consecutive explicitly timed trajectories, +`MoveJointsOptions.preserve_velocity_samples` preserves the supplied arrival +velocities across phase boundaries. Failed rows remain held. `SimulationExecutionAdapter` always clears absent velocity references to zero; it never substitutes observed velocities or depends on peer active masks. diff --git a/agent_context/topics/task-programs/configuration.md b/agent_context/topics/task-programs/configuration.md index fab2afa4d6..680beb9754 100644 --- a/agent_context/topics/task-programs/configuration.md +++ b/agent_context/topics/task-programs/configuration.md @@ -230,6 +230,13 @@ selected robot rows to native cable arenas. Evaluation consumes qualified `cables//` channels, including strict endpoint geometry and episode slip. Keep routing acceptance in the deployment's named evaluation rules. +`FrankaRopeWrap-v1` uses the registered `simulation.cable_wrap_phase` call. +Each phase lowers its checked portion of the full Franka arm and finger trajectory +to `MoveJoints`. Scene reset owns pre-roll. The monitor latches its slip reference +after closure and a simultaneous two-finger contact sample. The final +`wrap_success` condition reads all 19 measured checks. Task components live +under `configs/tasks/manipulation/cable/franka_rope_wrap/` in the task extension. + The configured lowerer allowlist also exposes profile-owned Park, AxisAlign, relative Place and coordinated transport. Their declarations in `integrations/_configured_services.py` select exact Atomic Skill descriptors; diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index a4ef743671..f126dd590d 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -4776,6 +4776,24 @@ reference-state resets and terminal snapshots before automatic row resets. .. automodule:: embodichain_tasks.locomotion.tracking.g1_motion :members: +embodichain_tasks.manipulation.cable.franka_rope_wrap +----------------------------------------------------- + +Build the Franka floor-pickup scene. The configuration defines the cable centerline, +fixed first segment, post and jaw geometry. The reset functor settles the cable +before the motion clock starts. + +.. currentmodule:: embodichain_tasks.manipulation.cable.franka_rope_wrap + +.. autosummary:: + + cable_points + load_config + materialize_deployment + prepare_models + settle_franka_rope + task_directory + embodichain_tasks.manipulation.cable.route_the_cable ---------------------------------------------------- @@ -4789,6 +4807,57 @@ penetration and end-insertion acceptance. RouteTheCableEnv measure_route +embodichain_tasks.manipulation.cable.wrap_evaluation +---------------------------------------------------- + +Attach episode-scoped contact sampling after the scene settles. The monitor records +both fingers at the same physical sample and tracks slip of the grasped material point. + +.. currentmodule:: embodichain_tasks.manipulation.cable.wrap_evaluation + +.. autosummary:: + + attach_wrap_monitor + +embodichain_tasks.manipulation.cable.wrap_integration +----------------------------------------------------- + +Bind a named wrap phase to the full-robot joint position and velocity commands +executed by the standard Task Program bridge. + +.. currentmodule:: embodichain_tasks.manipulation.cable.wrap_integration + +.. autosummary:: + + CableWrapPhaseFactory + +embodichain_tasks.manipulation.cable.wrap_measurements +------------------------------------------------------ + +Evaluate recorded control frames against all 19 physical acceptance checks, +including bilateral grasp, line winding, post contact and the final hold. + +.. currentmodule:: embodichain_tasks.manipulation.cable.wrap_measurements + +.. autosummary:: + + evaluate_wrap + +embodichain_tasks.manipulation.cable.wrap_motion +------------------------------------------------ + +Plan the continuous tool path and solve its joint targets in time order. +The phase boundaries partition the trajectory on the configured control grid. +The first sample describes the settled start at zero seconds. + +.. currentmodule:: embodichain_tasks.manipulation.cable.wrap_motion + +.. autosummary:: + + PHASE_END_S + WrapMotion + plan_wrap_motion + embodichain_tasks.manipulation.push_cube ---------------------------------------- diff --git a/docs/source/api_reference/robot_catalogs.rst b/docs/source/api_reference/robot_catalogs.rst index 9179bd9eff..bd9ced2287 100644 --- a/docs/source/api_reference/robot_catalogs.rst +++ b/docs/source/api_reference/robot_catalogs.rst @@ -629,6 +629,13 @@ and cache prefixes while using the shared core download and locking services. .. autoclass:: embodichain_robots.franka.assets.FrankaPandaArm +.. autoclass:: embodichain_robots.franka.assets.FrankaRopePads + + The cable-wrap embodiment resolves the flat-pad model through + ``FrankaRopePads/Panda/Fr3RopePadsV1.urdf``. + +.. autofunction:: embodichain_robots.franka.rope_pads.apply_rope_pads + .. autofunction:: embodichain_robots.robotiq.two_finger.robotiq85_joint_positions diff --git a/embodichain/lab/evaluation/_recording.py b/embodichain/lab/evaluation/_recording.py index 6317a1c118..bb1c860ef1 100644 --- a/embodichain/lab/evaluation/_recording.py +++ b/embodichain/lab/evaluation/_recording.py @@ -371,9 +371,17 @@ def get(name, default=None): _CABLE_METRIC_UNITS, ) - if quantity not in _METRICS: + if quantity.startswith("wrap_"): + from embodichain_tasks.manipulation.cable.wrap_measurements import ( + WRAP_CHECKS, + ) + + if quantity.removeprefix("wrap_") not in WRAP_CHECKS: + return None + contract = ("arena", "scalar", ("1",), (1,)) + elif quantity not in _METRICS: return None - if quantity == "end_position": + elif quantity == "end_position": contract = ("arena", "xyz", ("m",), (3,)) else: contract = ("arena", "scalar", (_CABLE_METRIC_UNITS[quantity],), (1,)) diff --git a/embodichain/lab/gym/envs/managers/observations.py b/embodichain/lab/gym/envs/managers/observations.py index 5d62b2020b..764d77a45d 100644 --- a/embodichain/lab/gym/envs/managers/observations.py +++ b/embodichain/lab/gym/envs/managers/observations.py @@ -241,6 +241,28 @@ def _get_evaluation_state( ) _, uid, quantity = parts + if quantity.startswith("wrap_"): + monitor = getattr(env, "_wrap_monitor", None) + available = ( + monitor is not None and monitor.active and monitor.cable.cfg.uid == uid + ) + report = monitor.terminal_report if available else None + passed = ( + report is not None and report["checks"][quantity.removeprefix("wrap_")] + ) + values = torch.full((env.num_envs, 1), float(passed), device=env.device) + valid = torch.full( + (env.num_envs,), available, dtype=torch.bool, device=env.device + ) + return ObservationSample( + ObservationDescriptor( + uid, quantity, "arena", "scalar", ("1",), (1,), values.dtype + ), + values, + valid, + (None if available else "Wrap episode has not started.",) + * env.num_envs, + ) if quantity not in _METRICS: raise ValueError(f"Unknown cable observation {quantity!r}.") bridge = getattr(env, "_active_task_program_bridge", None) diff --git a/embodichain/lab/sim/atomic_skills/core.py b/embodichain/lab/sim/atomic_skills/core.py index 20587e026b..a4887a3167 100644 --- a/embodichain/lab/sim/atomic_skills/core.py +++ b/embodichain/lab/sim/atomic_skills/core.py @@ -981,6 +981,7 @@ def build_plan( effect_candidates: StateDelta | None = None, effect_verification: EffectVerificationRequirement | None = None, replannable: bool = True, + preserve_velocity_samples: bool = False, diagnostics: PlannerDiagnostics | None = None, segment_lengths: Mapping[str, int] | None = None, scene_dependency_monitor_until: Mapping[str, int] | None = None, @@ -999,6 +1000,9 @@ def build_plan( effect_verification: Optional explicit physical-effect boundary. Use this when verification is required without a symbolic task- state delta. + preserve_velocity_samples: Keep explicitly authored arrival velocities across + consecutive timed segments. Derived velocity targets retain their usual + boundary and stationary treatment. replannable: Whether the execution runtime may replan this action. diagnostics: Optional retained planner diagnostics. segment_lengths: Optional ordered mapping from semantic segment @@ -1053,7 +1057,9 @@ def build_plan( velocities = differentiate_positions(timed.positions, timed.dt) else: velocities = velocities.clone() - if timed.waypoint_count: + if timed.waypoint_count and not ( + preserve_velocity_samples and timed.velocities is not None + ): # Commands are held until the next arrival. A stationary interval # and the final settling window must never retain a moving target. stationary = timed.positions[:, 1:] == timed.positions[:, :-1] diff --git a/embodichain/lab/sim/atomic_skills/primitives/move_joints.py b/embodichain/lab/sim/atomic_skills/primitives/move_joints.py index 13e972060c..53d6751279 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_skills/primitives/move_joints.py @@ -88,7 +88,12 @@ class MoveJointsOptions(SkillOptions): preserve_samples: bool = False """Consume explicitly sampled joint positions at the host control cadence.""" + preserve_velocity_samples: bool = False + """Retain explicit arrival velocities, including the ends of consecutive timed phases.""" + def __post_init__(self) -> None: + if type(self.preserve_velocity_samples) is not bool: + raise TypeError("preserve_velocity_samples must be a bool.") if type(self.preserve_samples) is not bool: raise TypeError("preserve_samples must be a bool.") @@ -120,6 +125,13 @@ def _plan( motion_target = motion.require_target(JointPositionTarget) control_part = motion_target.control_part joint_ids = list(motion_target.joint_ids) + if request.skill_options.preserve_velocity_samples and ( + not isinstance(goal.target, TimedTrajectory) + or goal.target.velocities is None + ): + raise ValueError( + "Preserving velocity samples requires an explicitly timed velocity trajectory." + ) if isinstance(goal.target, TimedTrajectory): trajectory = goal.target if sorted(joint_ids) != list(range(context.robot.robot_dof)): @@ -147,7 +159,12 @@ def _plan( "Each timed replay command must occupy one control step." ) return self.build_plan( - request, context, success=True, trajectory=trajectory, replannable=False + request, + context, + success=True, + trajectory=trajectory, + replannable=False, + preserve_velocity_samples=request.skill_options.preserve_velocity_samples, ) joint_dof = len(motion_target.joint_ids) diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index 45afe72b15..65c3707e4a 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -143,6 +143,9 @@ ) if TYPE_CHECKING: + from embodichain_tasks.manipulation.cable.wrap_integration import ( + CableWrapPhaseFactory, + ) from .environment import TaskProgramEnvironmentAdapter __all__: list[str] = [] @@ -1035,6 +1038,7 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: { "hand_interp_steps", "preserve_samples", + "preserve_velocity_samples", "preserve_cartesian_samples", "hold_steps", "release", @@ -1290,10 +1294,13 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: value, path=path, required=frozenset({"kind"}), - optional=frozenset({"preserve_samples"}), + optional=frozenset({"preserve_samples", "preserve_velocity_samples"}), ) return MoveJointsOptions( - preserve_samples=_boolean(config.get("preserve_samples", False), path=path) + preserve_samples=_boolean(config.get("preserve_samples", False), path=path), + preserve_velocity_samples=_boolean( + config.get("preserve_velocity_samples", False), path=path + ), ) if kind == "move_end_effector": config = _mapping( @@ -2577,8 +2584,16 @@ def _decode_registered_lowerer( | _PushObjectLowererFactory | _RelativePlaceLowererFactory | _KinematicReplayFactory + | CableWrapPhaseFactory ): """Decode one allowlisted registered semantic lowerer factory.""" + if isinstance(value, Mapping) and value.get("kind") == "cable_wrap_phase": + from embodichain_tasks.manipulation.cable.wrap_integration import ( + CableWrapPhaseFactory, + ) + + _mapping(value, path=path, required=frozenset({"kind"})) + return CableWrapPhaseFactory() if isinstance(value, Mapping) and value.get("kind") == "kinematic_joint_replay": return _decode_kinematic_replay(value, path=path) if isinstance(value, Mapping) and value.get("kind") in { @@ -3049,7 +3064,8 @@ class _DecodedRuntimeServices: | _PourLowererFactory | _PushObjectLowererFactory | _RelativePlaceLowererFactory - | _KinematicReplayFactory, + | _KinematicReplayFactory + | CableWrapPhaseFactory, ..., ] = () control_part_evidence: _JointPositionConstraintEvidenceProviderFactory | None = None @@ -3465,6 +3481,7 @@ def _decode_evaluation( robot_links: Mapping[str, str] | None = None, registry: PredicateRegistry | None = None, cable_material: object | None = None, + cable_wrap: bool = False, transport_targets: Mapping[str, _MoveHeldObjectLowererFactory] | None = None, ) -> tuple[dict[str, CompiledEvaluation], tuple[str, ...], tuple[str, ...]]: if value is None: @@ -3488,6 +3505,14 @@ def _decode_evaluation( channels[f"cables/{cable_material.entity_id}/{quantity}"] = ( f"cables/{cable_material.simulation_uid}/{quantity}" ) + if cable_wrap: + from embodichain_tasks.manipulation.cable.wrap_measurements import WRAP_CHECKS + + for binding in scene.cables: + for name in WRAP_CHECKS: + channels[f"cables/{binding.entity_id}/wrap_{name}"] = ( + f"cables/{binding.simulation_uid}/wrap_{name}" + ) articulations = { binding.entity_id: binding.simulation_uid for binding in scene.articulations } @@ -3686,6 +3711,10 @@ def _decode_configured_task_program_integration( robot_links=evaluation_robot_links, registry=evaluation_registry, cable_material=services.cable_material, + cable_wrap=any( + factory.call_id == "simulation.cable_wrap_phase" + for factory in services.registered_semantic_lowerers + ), transport_targets={ factory.target_id: factory for factory in services.registered_semantic_lowerers diff --git a/examples/README.md b/examples/README.md index 5480107f3b..cd40224396 100644 --- a/examples/README.md +++ b/examples/README.md @@ -141,3 +141,13 @@ capsules in the browser: ```bash python -m embodichain run-example sim/demo/cable_viser --num-envs 2 --port 8080 --duration 12 ``` + +## Franka rope wrapping + +The [Franka rope wrap Task Program](sim/demo/franka_rope_wrap/README.md) picks up a +cable from the floor, wraps it around a post and measures the grasp and winding. +Its compatibility entry point exports recorded frames and all 19 physical checks: + +```bash +python -m embodichain run-example sim/demo/franka_rope_wrap --output /tmp/franka-rope-wrap-run +``` diff --git a/examples/sim/demo/franka_rope_wrap/README.md b/examples/sim/demo/franka_rope_wrap/README.md new file mode 100644 index 0000000000..63c472dfdd --- /dev/null +++ b/examples/sim/demo/franka_rope_wrap/README.md @@ -0,0 +1,143 @@ +# Franka floor pickup and rope wrapping + +A single Franka arm picks up a rope end directly from the floor, lifts it, +wraps it around one post, tightens it and holds it for two seconds. The default +run simulates 36 seconds with 96 cable segments in one CUDA environment. + +The joint targets are planned before execution. During physics, the arm uses +position/velocity drives and the fingers hold the rope through contact. There +is no gripper attachment and no per-frame rope state assignment. The final +hold keeps the gripper closed; release and self-locking are not demonstrated. +Finger geometry, friction and material values are demonstration settings, not +hardware calibration. The configured Task Program executes ten named phases through the Gym +controller. Each phase uses the same continuously seeded IK plan and position/velocity +drive samples that pass the offline motion checks. + +## Requirements and assets + +Use a cable-capable DexSim build with +`SpawnedCable`, cable state access and DexUni. See the parent +[Newton cable example](../../../README.md#newton-cable-demo) for SDK installation. +A version string alone does not establish compatibility; the example records +the actual SDK build commit. Full physics execution requires a CUDA GPU. + +The original robot is resolved through +`get_data_path("Franka/Panda/PandaWithHand.urdf")`. The existing `Franka` registry +uses `FrankaV4.zip` with MD5 `0c613d884798e8b084604381641cc8e1`. Although the +asset path contains `Panda`, this demo uses the model's `fr3_*` joint/link names. +Do not substitute an arbitrary Panda URDF without adapting the configuration. +Pre-download it for an offline run: + +```bash +python -m embodichain data download --name Franka +``` + +Set `EMBODICHAIN_DATA_ROOT` before starting Python to select an existing asset +cache. A complete replacement configuration may instead set `robot_urdf` to an +absolute local URDF path. Missing meshes or other referenced model files fail +before simulation. + +The example prepares a robot URDF with flat finger pads, preserving the source +joint definitions and inertias. It copies referenced meshes and material +resources into the run directory using relative paths. The original asset cache +is unchanged. `generated_assets/provenance.json` records the asset reference, +source and generated model hashes, resource hashes and finger modifications. +Original model notices remain part of their resources. + +The floor slab, fixed rope clamp and post are primitive shapes. The rope is +created from configuration; these objects require no additional model bundle. +The default run uses `no-render` and does not load native rendering resources. +Native rendering helpers, if used separately, also need the existing +`SimResources` asset (`python -m embodichain data download --name SimResources`). +Video generation, browser visualization and report publishing are outside this +example's command-line interface. + +## Run + +Use the configured deployment to execute the Task Program or record a standard trajectory: + +```bash +python -m embodichain run-task \ + --gym_config extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task.franka.yaml \ + --headless --max_episodes 1 --record_trajectory --trajectory_save_dir /tmp/franka-wrap-episodes +``` + +The example below runs the same program and exports the per-frame measurements, +robot motion and 19 individual acceptance results. + +```bash +python -m embodichain run-example sim/demo/franka_rope_wrap \ + --output /tmp/franka-rope-wrap-run +``` + +The output directory must not already exist. The bundled `config.json` is the +default regardless of the working directory. To replace it with a complete +configuration: + +```bash +python -m embodichain run-example sim/demo/franka_rope_wrap \ + --config /path/to/config.json --output /tmp/franka-rope-wrap-custom +``` + +Generate assets and the joint plan without starting physics: + +```bash +python -m embodichain run-example sim/demo/franka_rope_wrap \ + --plan-only --output /tmp/franka-rope-wrap-plan +``` + +`--seconds 2` truncates execution for a smoke check. It records `partial`, keeps +`passed=false` and exits with code 1; it does not qualify the full task. A full +run exits with code 0 only when every physical acceptance check passes. +`--plan-only` exits with code 0 after a valid plan and records `planned`. + +## Results and acceptance + +The run directory contains: + +- `config.json`, `source_snapshot.py` and `manifest.json`: exact inputs, source + hash, optional repository commit, package/build versions and device metadata. +- `generated_assets/`: the modified URDF, copied resources and provenance. +- `robot_motion.npz` and `motion_validation.json`: planned targets and motion + checks before physics. +- `states.npz`, `metrics.json` and `settled_state.json`: recorded states and + measurements from physical execution. +- `validation.json`: the 19 acceptance checks, thresholds and measured summary. + Exceptions are recorded in `failure.txt` with a failed manifest. + +The acceptance checks cover completion, finite state, joint limits, direct floor +pickup, retained grasp, material-point slip, bilateral finger contact, winding, +contact arc, hold stability, rope height, post penetration, observed robot/post +clearance, anchor direction and exit bend, fixed anchor, segment gaps, dropped +contacts and floor penetration. They use actual simulated states and valid +contact geometry. DexUni contact geometry does not imply that normal or friction +impulses are available; consumers must inspect query capabilities. + +### Same-engine migration check + +The original example at `c911dc21` and the configured Task Program both passed +19/19 in a 36-second GPU episode on DexSim **0.6.0rc2** (build +`dbaca660471d5aaa7e750093e7fe1c207465c4af`), Newton 1.7.0 and an RTX 5090 D v2. +The program executed ten segments and 2,160 controls. Both runs used the same +97 cable points, derived robot model and control targets. + +| Measured quantity | Original example | Task Program | +| --- | ---: | ---: | +| Final rope winding | 524.8542° | 524.8543° | +| Maximum material slip | 0.1174 mm | 0.1176 mm | +| Bilateral contact after closing | 100% | 100% | +| Minimum contact arc over final hold | 360° | 360° | +| Rope/post penetration | 0 mm | 0 mm | +| Free-end drift over final hold | 0.0512 mm | 0.0429 mm | + +On 2026-11-08 the default 36-second NoRender run passed all 19 checks with +DexSim `0.6.0.dev11491` (dev commit +`505ee1b5600ca0b3d0a2c8235930e7684283d049`), Newton 1.7.0, MuJoCo/MuJoCo-Warp +3.15.0, Python 3.12 and an RTX 5090 D v2. Final winding was 524.86 degrees, +maximum material grasp slip was 0.123 mm, and hold winding drift was +0.00949 degrees. No post penetration or dropped contacts were reported. + +The included checks do not constitute an independent robot self-collision or +continuous swept-volume certification. One successful run also does not measure +multi-seed reliability, batched execution or reset stability. Recorded states +can support later replay, but the CLI does not generate a video. diff --git a/examples/sim/demo/franka_rope_wrap/__main__.py b/examples/sim/demo/franka_rope_wrap/__main__.py new file mode 100644 index 0000000000..12b5e5188e --- /dev/null +++ b/examples/sim/demo/franka_rope_wrap/__main__.py @@ -0,0 +1,412 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Dynamic Franka floor pickup and single-post rope wrapping. + +Only robot joint drive targets move during physics. The rope's first segment +is fixed at the visible floor clamp; its free end is grasped by finger contact. +No robot/rope attachment or rope state assignment is used during simulation. +Material and fingertip parameters are demonstration settings, not calibration. +""" + +from __future__ import annotations + +import argparse +from datetime import datetime, timezone +from decimal import Decimal +import hashlib +from importlib import metadata +import json +import os +from pathlib import Path +import platform +import shutil +import subprocess +import sys +from tempfile import TemporaryDirectory +import traceback +from typing import Any + +import numpy as np +import torch + +from embodichain_tasks.manipulation.cable.franka_rope_wrap import ( + cable_points, + load_config, + materialize_deployment, + prepare_models, + task_directory, +) +from embodichain_tasks.manipulation.cable.wrap_motion import plan_wrap_motion +from embodichain_tasks.manipulation.cable.wrap_evaluation import attach_wrap_monitor + +__all__ = ["main", "create_replay_scene", "restore_replay_frame", "replay_camera_views"] + + +def _json(path: Path, data: Any) -> None: + path.write_text(json.dumps(data, indent=2, ensure_ascii=False) + "\n") + + +def _hash(path: Path, algorithm: str = "sha256") -> str: + digest = hashlib.new(algorithm) + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _runtime_versions() -> dict[str, Any]: + """Collect reproducibility metadata without constructing a simulation world.""" + packages = {} + metadata_errors = {} + for name in ( + "embodichain", + "dexsim-engine", + "newton", + "warp-lang", + "torch", + "scipy", + "pytorch-kinematics", + ): + try: + packages[name] = metadata.version(name) + except Exception as error: + packages[name] = None + metadata_errors[name] = str(error) + result: dict[str, Any] = { + "python": platform.python_version(), + "python_executable": sys.executable, + "platform": platform.platform(), + "packages": packages, + "metadata_errors": metadata_errors, + "torch_cuda": torch.version.cuda, + "cuda_visible_devices": os.environ.get("CUDA_VISIBLE_DEVICES"), + "gpus": [], + } + try: + import dexsim + + result["dexsim_commit"] = getattr(dexsim, "__commit_id__", None) + except Exception as error: + result["dexsim_metadata_error"] = str(error) + try: + query = subprocess.run( + [ + "nvidia-smi", + "--query-gpu=index,name,uuid,driver_version", + "--format=csv,noheader", + ], + capture_output=True, + text=True, + check=False, + timeout=5, + ) + if query.returncode == 0: + for row in query.stdout.splitlines(): + values = [item.strip() for item in row.split(",")] + if len(values) == 4: + result["gpus"].append( + dict(zip(("index", "name", "uuid", "driver_version"), values)) + ) + except (OSError, subprocess.TimeoutExpired): + pass + return result + + +def _source_revision() -> str | None: + """Report the source checkout revision when available, including from any cwd.""" + source = Path(__file__).resolve() + for parent in source.parents: + if not (parent / "pyproject.toml").is_file(): + continue + if source.relative_to(parent).parts[0] != "examples": + continue + try: + return subprocess.check_output( + ["git", "-C", str(parent), "rev-parse", "HEAD"], + text=True, + stderr=subprocess.DEVNULL, + timeout=5, + ).strip() + except (OSError, subprocess.CalledProcessError, subprocess.TimeoutExpired): + return None + return None + + +def _deployment_config(deployment: Path) -> tuple: + """Resolve the same deployment and hooks used by the standard run-task CLI.""" + from embodichain.lab.scripts.run_task import _create_parser + from embodichain.lab.gym.utils.gym_utils import build_env_cfg_from_args + from embodichain.lab.gym.utils.registration import ( + discover_task_packages, + execute_init_hooks, + ) + + discover_task_packages() + execute_init_hooks() + args = _create_parser().parse_args(["--gym_config", str(deployment), "--headless"]) + return build_env_cfg_from_args(args) + + +def _run(deployment: Path, config: dict, out: Path, seconds: float | None) -> dict: + import gymnasium + from embodichain.lab.gym.envs.demo import execute_demo_episode + + cfg, gym_config, actions = _deployment_config(deployment) + env = gymnasium.make(gym_config["id"], cfg=cfg, **actions) + target = env.unwrapped + steps = 0 + limit = None + if seconds is not None and seconds < config["duration_s"]: + limit = int(Decimal(str(seconds)) * config["control_hz"]) + + def observed_step(*_) -> None: + nonlocal steps + steps += 1 + + try: + env.reset(options={"save_data": False}) + result = execute_demo_episode( + env, + should_stop=lambda: limit is not None and steps >= limit, + step_observer=observed_step, + ) + _json(out / "task_program_episode.json", result.to_metadata()) + monitor = getattr(target, "_wrap_monitor", None) + if monitor is None and limit == 0: + # The collector stops before constructing its first segment. + path = Path(target.robot.cfg.fpath) + monitor = attach_wrap_monitor( + target, config, plan_wrap_motion(config, path), path + ) + if monitor is None: + raise RuntimeError( + f"Task Program stopped before the first phase: {result.terminal_reason}" + ) + monitor.motion.write(out) + report = monitor.write(out) + report["passed"] = bool( + report["passed"] + and result.completed + and result.all_success + and steps == len(monitor.motion.times) - 1 + and limit is None + ) + _json(out / "validation.json", report) + return report + finally: + target.close(exit_process=False) + + +def create_replay_scene( + config: dict[str, Any], run_dir: Path, output_dir: Path +) -> dict[str, Any]: + """Load the same physical scene configuration and the recorded robot asset for rendering.""" + from embodichain.lab.sim import SimulationManager + from embodichain.lab.sim.cfg import DenoisingCfg, LightCfg, RenderCfg + from embodichain.lab.sim.sensors import CameraCfg + + with TemporaryDirectory() as scratch: + deployment = materialize_deployment( + config, run_dir / "generated_assets/franka/franka_rope.urdf", Path(scratch) + ) + cfg, _, _ = _deployment_config(deployment) + cfg.sim_cfg.render_cfg = RenderCfg( + renderer=config["render"].get("renderer", "hybrid"), + spp=config["render"]["spp"], + denoising=DenoisingCfg(window="off", offscreen="optix"), + tone_mapping_enabled=config["render"].get("tone_mapping_enabled", False), + tone_mapping_exposure=config["render"].get("exposure", 1.0), + min_bounces=config["render"].get("min_bounces", 4), + max_bounces=config["render"].get("max_bounces", 8), + ) + sim = SimulationManager(cfg.sim_cfg) + cameras = [] + try: + sim.set_ground_plane_visibility(False) + for rigid_cfg in (*cfg.background, *cfg.rigid_object): + sim.add_rigid_object(rigid_cfg) + robot = sim.add_robot(cfg.robot) + cable = sim.add_cable_object(cfg.cable_object[0]) + for name, view, fov in ( + ("overview", config["camera"], 48), + ("detail", config["detail_camera"], 42), + ): + width, height = (config["render"]["width"], config["render"]["height"]) + focal = height / (2 * np.tan(np.deg2rad(fov / 2))) + cameras.append( + sim.add_sensor( + CameraCfg( + uid=name, + width=width, + height=height, + near=0.01, + far=4, + intrinsics=(focal, focal, width / 2, height / 2), + enable_color=True, + enable_depth=False, + enable_mask=False, + extrinsics=CameraCfg.ExtrinsicsCfg( + eye=tuple(view["eye_m"]), target=tuple(view["target_m"]) + ), + ) + ) + ) + for uid, position, power in ( + ("key", (0.4, -0.6, 1.6), 14), + ("fill", (0.1, 0.7, 1.1), 9), + ): + sim.add_light( + LightCfg( + uid=uid, init_pos=position, light_type="point", intensity=power + ) + ) + sim.prepare() + return dict( + sim=sim, + robot=robot, + cable=cable, + contact=sim.add_sensor(cfg.sensor[0]), + cameras=cameras, + ) + except BaseException: + sim.destroy(exit_process=False) + raise + + +def restore_replay_frame( + scene: dict[str, Any], states: dict[str, np.ndarray], index: int +) -> None: + """Restore an exact recorded state; this function never steps physics.""" + device = scene["sim"].device + scene["robot"].set_qpos( + torch.as_tensor( + states["robot_qpos"][index : index + 1], dtype=torch.float32, device=device + ), + target=False, + ) + scene["cable"].write_segment_state_to_sim( + torch.as_tensor( + states["cable_states"][index : index + 1], + dtype=torch.float32, + device=device, + ).contiguous() + ) + + +def replay_camera_views( + t: float, config: dict[str, Any], states: dict[str, np.ndarray], index: int +) -> dict[str, tuple[np.ndarray, np.ndarray]]: + """Use stable overview and high oblique views to expose the actual winding.""" + return { + name: (np.asarray(view["eye_m"]), np.asarray(view["target_m"])) + for name, view in ( + ("overview", config["camera"]), + ("detail", config["detail_camera"]), + ) + } + + +def main() -> int: + """Run a reproducible contact-driven floor pickup and rope wrapping demo.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--config", + type=Path, + default=task_directory() / "config.json", + help="Scene configuration; defaults to the config shipped beside this example", + ) + parser.add_argument( + "--output", type=Path, required=True, help="New output directory" + ) + parser.add_argument( + "--seconds", + type=float, + help="Stop early for diagnostics; an incomplete run does not pass acceptance", + ) + parser.add_argument( + "--plan-only", + action="store_true", + help="Prepare assets and validate the motion without stepping physics", + ) + args = parser.parse_args() + if args.seconds is not None and ( + not np.isfinite(args.seconds) or args.seconds <= 0 + ): + parser.error("--seconds must be a finite positive number") + args.config = args.config.expanduser().resolve() + try: + config = load_config(args.config) + except (OSError, ValueError, TypeError, KeyError) as error: + parser.error(f"Invalid configuration: {error}") + args.output = args.output.resolve() + args.output.mkdir(parents=True, exist_ok=False) + shutil.copyfile(args.config, args.output / "config.json") + shutil.copyfile(__file__, args.output / "source_snapshot.py") + manifest = { + "run_id": args.output.name, + "status": "running", + "created_at": datetime.now(timezone.utc).isoformat(), + "framework": "EmbodiChain", + "robot": "Franka FR3 / Panda hand with flat rope pads", + "physics": "Newton / DexUni", + "rendered": False, + "control": "Dynamic position/velocity joint drives; direct finger contact on the rope; no rope state writes or gripper attachment during physics", + "floor": { + "default_ground_hidden": True, + "surface_height_m": config["floor_height_m"], + "pickup_stand": False, + }, + "source_sha256": _hash(Path(__file__)), + "config_sha256": _hash(args.config), + "git_head": _source_revision(), + "runtime": _runtime_versions(), + } + _json(args.output / "manifest.json", manifest) + try: + paths = prepare_models(config, args.output) + points = cable_points(config) + np.savez_compressed( + args.output / "scene_inputs.npz", + points=points, + anchor=np.asarray(config["cable_waypoints_m"][0]), + post_center=config["post_center_xy_m"], + ) + if args.plan_only: + plan_wrap_motion(config, paths["robot"]).write(args.output) + manifest["status"] = "planned" + return 0 + deployment = materialize_deployment(config, paths["robot"], args.output) + report = _run(deployment, config, args.output, args.seconds) + print( + ("DEMO_COMPLETE " if report["checks"]["completed"] else "DEMO_PARTIAL ") + + json.dumps(report), + flush=True, + ) + manifest.update( + status="completed" if report["checks"]["completed"] else "partial", + passed=report["passed"], + ) + return 0 if report["passed"] else 1 + except Exception: + manifest["status"] = "failed" + (args.output / "failure.txt").write_text(traceback.format_exc()) + raise + finally: + _json(args.output / "manifest.json", manifest) + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/examples/sim/demo/franka_rope_wrap/config.json b/examples/sim/demo/franka_rope_wrap/config.json new file mode 120000 index 0000000000..8686481a96 --- /dev/null +++ b/examples/sim/demo/franka_rope_wrap/config.json @@ -0,0 +1 @@ +../../../../extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/config.json \ No newline at end of file diff --git a/extensions/embodichain_robots/_asset_catalog.py b/extensions/embodichain_robots/_asset_catalog.py index 6ea9d7bcd9..57eef7a3f5 100644 --- a/extensions/embodichain_robots/_asset_catalog.py +++ b/extensions/embodichain_robots/_asset_catalog.py @@ -83,6 +83,10 @@ "class_path": "embodichain_robots.franka.assets:FrankaPandaArm", "category": "robot", }, + "FrankaRopePads": { + "class_path": "embodichain_robots.franka.assets:FrankaRopePads", + "category": "robot", + }, "Franka": { "class_path": "embodichain_robots.franka.assets:Franka", "category": "robot", diff --git a/extensions/embodichain_robots/franka/assets.py b/extensions/embodichain_robots/franka/assets.py index 974cf61b52..2566ac4419 100644 --- a/extensions/embodichain_robots/franka/assets.py +++ b/extensions/embodichain_robots/franka/assets.py @@ -33,17 +33,26 @@ EMBODICHAIN_DEFAULT_DATA_ROOT, ) -__all__ = ["Franka", "FrankaPandaArm"] +__all__ = ["Franka", "FrankaPandaArm", "FrankaRopePads"] robot_assets = "robot_assets" +_FR3_WITH_HAND_SHA256 = ( + "a4a2d3265dadd69c4c7d8ed9238ae44494c217db6f66f586fcc40e8754ce2315" +) + + +def _franka_v4_archive() -> o3d.data.DataDescriptor: + return o3d.data.DataDescriptor( + os.path.join(EMBODICHAIN_DOWNLOAD_PREFIX, robot_assets, "FrankaV4.zip"), + "0c613d884798e8b084604381641cc8e1", + ) def _materialize_hand_component(extract_dir: str, data_root: str) -> None: """Split the authored hand subtree without modifying its source URDF.""" source = Path(extract_dir) / "Panda/PandaWithHand.urdf" content = source.read_bytes() - expected = "a4a2d3265dadd69c4c7d8ed9238ae44494c217db6f66f586fcc40e8754ce2315" - if hashlib.sha256(content).hexdigest() != expected: + if hashlib.sha256(content).hexdigest() != _FR3_WITH_HAND_SHA256: raise ValueError("pinned FR3/hand source changed in the asset cache") original = ElementTree.fromstring(content) links = ("fr3_hand", "fr3_hand_tcp", "fr3_leftfinger", "fr3_rightfinger") @@ -59,7 +68,7 @@ def _materialize_hand_component(extract_dir: str, data_root: str) -> None: report = json.dumps( dict( schema="fr3-hand-component/v1", - source_sha256=expected, + source_sha256=_FR3_WITH_HAND_SHA256, derived_sha256=hashlib.sha256(output).hexdigest(), links=links, joints=joints, @@ -67,15 +76,22 @@ def _materialize_hand_component(extract_dir: str, data_root: str) -> None: sort_keys=True, indent=2, ).encode() + _write_derivatives( + source, + data_root, + {"Fr3HandV1.urdf": output, "Fr3HandV1.derivation.json": report}, + ) + + +def _write_derivatives(source: Path, data_root: str, files: dict[str, bytes]) -> None: with _dataset_download_lock(data_root, "Franka"): - for name, value in ( - ("Fr3HandV1.urdf", output), - ("Fr3HandV1.derivation.json", report), - ): + for name, value in files.items(): target = source.parent / name if target.exists(): if target.read_bytes() != value: - raise ValueError("derived FR3 hand differs from its pinned source") + raise ValueError( + f"derived FR3 asset {name} differs from its pinned source" + ) continue temporary = None try: @@ -114,10 +130,7 @@ class Franka(EmbodiChainDataset): """ def __init__(self, data_root: str = None): - data_descriptor = o3d.data.DataDescriptor( - os.path.join(EMBODICHAIN_DOWNLOAD_PREFIX, robot_assets, "FrankaV4.zip"), - "0c613d884798e8b084604381641cc8e1", - ) + data_descriptor = _franka_v4_archive() prefix = type(self).__name__ path = EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root @@ -125,6 +138,42 @@ def __init__(self, data_root: str = None): _materialize_hand_component(self.extract_dir, path) +class FrankaRopePads(EmbodiChainDataset): + """FR3 with flat rope-grasping pads, derived on demand in the shared V4 cache.""" + + def __init__(self, data_root: str | None = None) -> None: + from .rope_pads import apply_rope_pads + + root = EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root + super().__init__("Franka", _franka_v4_archive(), root) + source = Path(self.extract_dir) / "Panda/PandaWithHand.urdf" + content = source.read_bytes() + if hashlib.sha256(content).hexdigest() != _FR3_WITH_HAND_SHA256: + raise ValueError("pinned FR3/hand source changed in the asset cache") + padded = ElementTree.fromstring(content) + apply_rope_pads(padded) + model = ElementTree.tostring(padded, encoding="utf-8", xml_declaration=True) + report = json.dumps( + { + "schema": "fr3-rope-pads/v1", + "source": source.name, + "source_sha256": _FR3_WITH_HAND_SHA256, + "derived_sha256": hashlib.sha256(model).hexdigest(), + "modified_links": ["fr3_leftfinger", "fr3_rightfinger"], + }, + sort_keys=True, + indent=2, + ).encode() + _write_derivatives( + source, + root, + { + "Fr3RopePadsV1.urdf": model, + "Fr3RopePadsV1.derivation.json": report, + }, + ) + + class FrankaPandaArm(EmbodiChainDataset): """Pinned original Panda arm with Panda limits, distinct from FR3 assets. diff --git a/extensions/embodichain_robots/franka/rope_pads.py b/extensions/embodichain_robots/franka/rope_pads.py new file mode 100644 index 0000000000..59c846cf55 --- /dev/null +++ b/extensions/embodichain_robots/franka/rope_pads.py @@ -0,0 +1,67 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Construct flat FR3 fingertip pads for direct floor cable pickup.""" + +from __future__ import annotations +import xml.etree.ElementTree as ET +from typing import Sequence + +__all__ = ["apply_rope_pads"] + + +def apply_rope_pads( + root: ET.Element, + *, + center: Sequence[float] = (0, 0.002, 0.04175), + size: Sequence[float] = (0.028, 0.004, 0.0135), +) -> None: + """Replace finger geometry, preserving the original inertias and joint frames.""" + for name in ("fr3_leftfinger", "fr3_rightfinger"): + link = root.find(f"link[@name='{name}']") + if link is None: + raise ValueError(f"FR3 model is missing its finger link {name!r}") + for child in list(link): + if child.tag in ("visual", "collision"): + link.remove(child) + _box( + link, + name + "_stem", + (0, 0.004, 0.02), + (0.018, 0.008, 0.04), + ".08 .09 .11 1", + ) + _box(link, name + "_tip", center, size, ".16 .18 .21 1") + + +def _box( + link: ET.Element, + name: str, + xyz: Sequence[float], + size: Sequence[float], + color: str, +) -> None: + for kind in ("visual", "collision"): + element = ET.SubElement(link, kind, name=name + "_" + kind) + ET.SubElement(element, "origin", xyz=" ".join(map(str, xyz)), rpy="0 0 0") + ET.SubElement( + ET.SubElement(element, "geometry"), "box", size=" ".join(map(str, size)) + ) + if kind == "visual": + ET.SubElement( + ET.SubElement(element, "material", name=name + "_material"), + "color", + rgba=color, + ) diff --git a/extensions/embodichain_tasks/configs/components/embodiments/franka_flat_pads.yaml b/extensions/embodichain_tasks/configs/components/embodiments/franka_flat_pads.yaml new file mode 100644 index 0000000000..df2f6a0274 --- /dev/null +++ b/extensions/embodichain_tasks/configs/components/embodiments/franka_flat_pads.yaml @@ -0,0 +1,66 @@ +embodiment_id: franka_flat_pads +simulation: + uid: franka + fpath: FrankaRopePads/Panda/Fr3RopePadsV1.urdf + init_pos: [0, 0, 0.002] + init_qpos: [0.0, -0.569, 0.0, -2.81, 0.0, 3.037, 0.741, 0.0395, 0.0395] + control_parts: + arm: ["fr3_joint[1-7]"] + eef: ["fr3_finger_joint[1-2]"] + arm_and_hand: ["fr3_joint[1-7]", "fr3_finger_joint[1-2]"] + solver_cfg: + arm: + class_type: PytorchSolver + root_link_name: fr3_link0 + end_link_name: fr3_hand_tcp + joint_drive_props: + # Preserve the source URDF joint armature and friction as in the calibrated scene. + drive_type: null + armature: null + friction: null + stiffness: {arm: 6000.0, eef: 6000.0} + damping: {arm: 150.0, eef: 35.0} + max_effort: {arm: 87.0, eef: 20.0} + target_mode: position_velocity + root_props: + newton_gravity_compensation: 1.0 + attrs: + collision_props: {backend: newton, margin: 0.0, gap: 0.00005} + material_props: {backend: newton, dynamic_friction: 1.2, ke: 20000.0, kd: 1.0} +sensor: +- sensor_type: ContactSensor + uid: contacts + rigid_uid_list: [post] + articulation_cfg_list: + - articulation_uid: franka + link_name_list: [] + filter_need_both_actor: false + max_contacts_per_env: 4096 +skill_profile: + contract_id: single_arm_parallel_gripper + profile_id: franka_rope_wrap + evaluation_links: + first_grasp_finger: fr3_leftfinger + second_grasp_finger: fr3_rightfinger + resources: + - resource_id: primary_manipulator + endpoints: + - endpoint_id: motion + control_part: arm + capabilities: [kinematics.batch_inverse, motion.cartesian_pose, kinematics.forward, motion.joint_position] + - endpoint_id: grasp + control_part: eef + capabilities: [interaction.grasp, motion.joint_position] + command_preset: flat_pads + - resource_id: arm_and_hand + endpoints: + - endpoint_id: motion + control_part: arm_and_hand + capabilities: [motion.joint_position] + members: [primary_manipulator] + command_presets: + - preset_id: flat_pads + control_part: eef + commands: + open: [0.0395, 0.0395] + grasp: [0.0018, 0.0018] diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/catalog.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/catalog.yaml new file mode 100644 index 0000000000..71eaa05ac4 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/catalog.yaml @@ -0,0 +1,8 @@ +task_key: franka_rope_wrap +title: Franka Rope Wrap +summary: Pick up a rope from the floor, wrap it around a post and keep hold of the free end. +tags: [cable, deformable, manipulation, task_program_expert] +default_deployment: franka +deployments: + franka: + config: task.franka.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/config.json b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/config.json new file mode 100644 index 0000000000..a2cf72ea2f --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/config.json @@ -0,0 +1,210 @@ +{ + "schema_version": 1, + "robot_urdf": "Franka/Panda/PandaWithHand.urdf", + "home_qpos": [ + 0.0, + -0.569, + 0.0, + -2.81, + 0.0, + 3.037, + 0.741 + ], + "floor_height_m": 0.002, + "post_center_xy_m": [ + 0.48, + 0 + ], + "post_radius_m": 0.045, + "post_height_m": 0.12, + "cable_waypoints_m": [ + [ + 0.21, + -0.2, + 0.006 + ], + [ + 0.22607114386689492, + -0.18809544898748526, + 0.006 + ], + [ + 0.24214228773378982, + -0.1761908979749705, + 0.006 + ], + [ + 0.2582134316006847, + -0.16428634696245575, + 0.006 + ], + [ + 0.32, + -0.17, + 0.006 + ], + [ + 0.34, + -0.25, + 0.006 + ], + [ + 0.28, + -0.37626441961695295, + 0.006 + ], + [ + 0.32, + -0.44626441961695296, + 0.006 + ], + [ + 0.42, + -0.44626441961695296, + 0.006 + ], + [ + 0.45, + -0.37626441961695295, + 0.006 + ], + [ + 0.42, + -0.3, + 0.006 + ], + [ + 0.42, + -0.25, + 0.006 + ], + [ + 0.44, + -0.25, + 0.006 + ], + [ + 0.46, + -0.25, + 0.006 + ], + [ + 0.48, + -0.25, + 0.006 + ], + [ + 0.5, + -0.25, + 0.006 + ], + [ + 0.52, + -0.25, + 0.006 + ] + ], + "cable_segments": 96, + "cable_radius_m": 0.004, + "cable_density_kg_m3": 800.0, + "bend_stiffness_Nm_rad": 0.0001, + "twist_stiffness_Nm_rad": 0.0001, + "bend_damping_Nms_rad": 0.0001, + "linear_stiffness_N_m": 100000.0, + "control_hz": 60, + "substeps": 32, + "iterations": 48, + "pre_roll_s": 2.0, + "duration_s": 36.0, + "grasp_complete_s": 5.0, + "lift_complete_s": 7.0, + "finger_pad_width_m": 0.028, + "grip_half_gap_m": 0.0018, + "grasp_inset_m": 0.025, + "post_friction": 0.3, + "rope_friction": 0.8, + "floor_friction": 0.2, + "contact_ke_N_m": 20000.0, + "contact_kd_Ns_m": 1.0, + "contact_gap_m": 5e-05, + "contact_match_distance_m": 5e-05, + "rigid_contact_hard": false, + "rigid_contact_history": false, + "friction_epsilon": 1e-05, + "camera": { + "eye_m": [ + 1.24, + -1.16, + 0.93 + ], + "target_m": [ + 0.35, + -0.05, + 0.3 + ] + }, + "detail_camera": { + "eye_m": [ + 0.55, + -0.24, + 1.05 + ], + "target_m": [ + 0.46, + -0.04, + 0.05 + ] + }, + "render": { + "width": 3840, + "height": 2160, + "fps": 30, + "spp": 16, + "renderer": "rt", + "tone_mapping_enabled": true, + "exposure": 0.8, + "min_bounces": 4, + "max_bounces": 8, + "crf": 15, + "preset": "slow" + }, + "validation": { + "tail_tcp_max_m": 0.045, + "winding_min_deg": 360.0, + "contact_arc_min_deg": 300.0, + "contact_band_m": 0.003, + "hold_winding_drift_max_deg": 3.0, + "hold_end_drift_max_m": 0.003, + "post_penetration_max_m": 0.0008, + "robot_post_penetration_max_m": 0.0001, + "joint_gap_max_m": 0.0008, + "floor_penetration_max_m": 0.0008, + "grasp_slip_max_m": 0.008, + "anchor_exit_bend_max_deg": 20.0 + }, + "grasp_offset_world_m": [ + -0.025, + 0, + 0 + ], + "orbit_radius_m": 0.16, + "orbit_degrees": 465, + "orbit_start_z_m": 0.065, + "orbit_end_z_m": 0.085, + "orbit_start_deg": -90.0, + "orbit_direction": 1, + "grasp_yaw_deg": 0, + "tighten_radial_m": 0.018, + "tighten_tangent_m": 0.02, + "open_half_gap_m": 0.0395, + "finger_pad_center_m": [ + 0, + 0.002, + 0.04175 + ], + "finger_pad_size_m": [ + 0.028, + 0.004, + 0.0135 + ] +} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/env.yaml new file mode 100644 index 0000000000..6dc70cda8b --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/env.yaml @@ -0,0 +1,228 @@ +environment_id: franka_rope_wrap +physics: newton +physics_config: + physics_dt: 0.0005208333333333333 + num_substeps: 1 + collision_cfg: null + sync_to_renderer: false + use_cuda_graph: true + solver_cfg: + solver_type: dexuni + joint_mode: dynamic + coupling: two_way + mujoco_articulation_paths: + - arena_0/franka + vbd_options: + iterations: 48 + rigid_contact_history: false + rigid_contact_hard: false + rigid_joint_linear_ke: 100000.0 + rigid_joint_angular_ke: 10.0 + friction_epsilon: 1.0e-05 + collision_options: + contact_matching: sticky + contact_matching_pos_threshold: 5.0e-05 + rigid_contact_max: 4096 +max_episode_steps: 2161 +num_envs: 1 +simulation: + background: + - uid: table + body_type: static + init_pos: [0.35, 0, -0.023] + shape: + shape_type: Cube + size: [1.2, 1.05, 0.05] + visual_material: + uid: table_mat + base_color: [0.33, 0.37, 0.41, 1] + roughness: 0.65 + attrs: + collision_props: + backend: newton + margin: 0.0 + gap: 5.0e-05 + material_props: + backend: newton + dynamic_friction: 0.2 + ke: 20000.0 + kd: 1.0 + - uid: anchor_clamp + body_type: static + init_pos: [0.20517865683993153, -0.20357136530375444, 0.002] + shape: + shape_type: Cube + size: [0.028, 0.018, 0.008] + visual_material: + uid: anchor_clamp_mat + base_color: [0.08, 0.11, 0.15, 1] + roughness: 0.65 + attrs: + collision_props: + backend: newton + margin: 0.0 + gap: 5.0e-05 + material_props: + backend: newton + dynamic_friction: 0.2 + ke: 20000.0 + kd: 1.0 + init_rot: [0, 0, 36.52885536698517] + - uid: post + body_type: static + init_pos: [0.48, 0, 0.062] + shape: + shape_type: Cylinder + radius: 0.045 + height: 0.12 + visual_material: + uid: post_mat + base_color: [0.025, 0.45, 0.39, 1] + roughness: 0.38 + attrs: + collision_props: + backend: newton + margin: 0.0 + gap: 5.0e-05 + material_props: + backend: newton + dynamic_friction: 0.3 + ke: 20000.0 + kd: 1.0 + cable_object: + - uid: cable + points: + - [0.20999999344348907, -0.20000000298023224, 0.006000000052154064] + - [0.21718823909759521, -0.19467538595199585, 0.006000000052154064] + - [0.22437062859535217, -0.18934296071529388, 0.006000000052154064] + - [0.23159274458885193, -0.18406438827514648, 0.006000000052154064] + - [0.2387981414794922, -0.17876334488391876, 0.006000000052154064] + - [0.24584205448627472, -0.17324984073638916, 0.006000000052154064] + - [0.2528063952922821, -0.16763833165168762, 0.006000000052154064] + - [0.2605273425579071, -0.16314896941184998, 0.006000000052154064] + - [0.26897111535072327, -0.1602364033460617, 0.006000000052154064] + - [0.27780279517173767, -0.15888118743896484, 0.006000000052154064] + - [0.2867411673069, -0.1588876098394394, 0.006000000052154064] + - [0.29561394453048706, -0.1599913239479065, 0.006000000052154064] + - [0.30429837107658386, -0.162111297249794, 0.006000000052154064] + - [0.3125905692577362, -0.16544237732887268, 0.006000000052154064] + - [0.320187509059906, -0.170141339302063, 0.006000000052154064] + - [0.3267393708229065, -0.17621032893657684, 0.006000000052154064] + - [0.33198633790016174, -0.18343883752822876, 0.006000000052154064] + - [0.33587074279785156, -0.19148574769496918, 0.006000000052154064] + - [0.33851319551467896, -0.2000250220298767, 0.006000000052154064] + - [0.34014323353767395, -0.20881757140159607, 0.006000000052154064] + - [0.34118005633354187, -0.21770146489143372, 0.006000000052154064] + - [0.34166350960731506, -0.22663235664367676, 0.006000000052154064] + - [0.34154966473579407, -0.2355753779411316, 0.006000000052154064] + - [0.34080052375793457, -0.2444874793291092, 0.006000000052154064] + - [0.33938780426979065, -0.25331854820251465, 0.006000000052154064] + - [0.337296724319458, -0.26201388239860535, 0.006000000052154064] + - [0.33452850580215454, -0.27051788568496704, 0.006000000052154064] + - [0.33110126852989197, -0.2787783741950989, 0.006000000052154064] + - [0.3270482122898102, -0.2867506146430969, 0.006000000052154064] + - [0.32241466641426086, -0.29440030455589294, 0.006000000052154064] + - [0.31725358963012695, -0.3017047047615051, 0.006000000052154064] + - [0.31162112951278687, -0.30865243077278137, 0.006000000052154064] + - [0.3055749833583832, -0.315243661403656, 0.006000000052154064] + - [0.29962629079818726, -0.32192203402519226, 0.006000000052154064] + - [0.2942173480987549, -0.3290437161922455, 0.006000000052154064] + - [0.28947392106056213, -0.3366239368915558, 0.006000000052154064] + - [0.28553885221481323, -0.3446524441242218, 0.006000000052154064] + - [0.28255751729011536, -0.35308074951171875, 0.006000000052154064] + - [0.28065264225006104, -0.36181479692459106, 0.006000000052154064] + - [0.27989357709884644, -0.3707217276096344, 0.006000000052154064] + - [0.2802771329879761, -0.3796531856060028, 0.006000000052154064] + - [0.2817305326461792, -0.38847485184669495, 0.006000000052154064] + - [0.2841351330280304, -0.39708706736564636, 0.006000000052154064] + - [0.28735488653182983, -0.4054298400878906, 0.006000000052154064] + - [0.29125821590423584, -0.4134763479232788, 0.006000000052154064] + - [0.29576927423477173, -0.4211982190608978, 0.006000000052154064] + - [0.30094438791275024, -0.42849084734916687, 0.006000000052154064] + - [0.30681082606315613, -0.43523889780044556, 0.006000000052154064] + - [0.3133632242679596, -0.4413222670555115, 0.006000000052154064] + - [0.32055678963661194, -0.446631520986557, 0.006000000052154064] + - [0.3283084034919739, -0.45108672976493835, 0.006000000052154064] + - [0.3365079164505005, -0.45465198159217834, 0.006000000052154064] + - [0.3450368046760559, -0.45733752846717834, 0.006000000052154064] + - [0.35378497838974, -0.45919135212898254, 0.006000000052154064] + - [0.3626609742641449, -0.4602845311164856, 0.006000000052154064] + - [0.37159502506256104, -0.46069765090942383, 0.006000000052154064] + - [0.38053399324417114, -0.4604310095310211, 0.006000000052154064] + - [0.3894113004207611, -0.4593579173088074, 0.006000000052154064] + - [0.39812397956848145, -0.4573517441749573, 0.006000000052154064] + - [0.4065268039703369, -0.4543015658855438, 0.006000000052154064] + - [0.4144398272037506, -0.45014530420303345, 0.006000000052154064] + - [0.421678364276886, -0.44490253925323486, 0.006000000052154064] + - [0.42809784412384033, -0.43868303298950195, 0.006000000052154064] + - [0.4336283504962921, -0.4316597282886505, 0.006000000052154064] + - [0.4382772445678711, -0.4240225851535797, 0.006000000052154064] + - [0.4421062767505646, -0.4159418046474457, 0.006000000052154064] + - [0.44523176550865173, -0.4075622856616974, 0.006000000052154064] + - [0.4476798474788666, -0.3989610970020294, 0.006000000052154064] + - [0.44932815432548523, -0.39017242193222046, 0.006000000052154064] + - [0.45004716515541077, -0.3812604546546936, 0.006000000052154064] + - [0.449721097946167, -0.37232646346092224, 0.006000000052154064] + - [0.44827505946159363, -0.36350488662719727, 0.006000000052154064] + - [0.4456997215747833, -0.3549446165561676, 0.006000000052154064] + - [0.442058801651001, -0.34677985310554504, 0.006000000052154064] + - [0.4374801516532898, -0.33910006284713745, 0.006000000052154064] + - [0.4328865706920624, -0.3314262628555298, 0.006000000052154064] + - [0.42882800102233887, -0.32345619797706604, 0.006000000052154064] + - [0.4252707362174988, -0.31524980068206787, 0.006000000052154064] + - [0.42217734456062317, -0.30685725808143616, 0.006000000052154064] + - [0.41950976848602295, -0.29831960797309875, 0.006000000052154064] + - [0.41723108291625977, -0.2896699011325836, 0.006000000052154064] + - [0.41530680656433105, -0.2809343636035919, 0.006000000052154064] + - [0.41370517015457153, -0.27213385701179504, 0.006000000052154064] + - [0.41278955340385437, -0.2632472515106201, 0.006000000052154064] + - [0.4146764874458313, -0.25459831953048706, 0.006000000052154064] + - [0.4216809570789337, -0.24947531521320343, 0.006000000052154064] + - [0.4305758774280548, -0.24930177628993988, 0.006000000052154064] + - [0.4394950568675995, -0.24997548758983612, 0.006000000052154064] + - [0.44843724370002747, -0.25016841292381287, 0.006000000052154064] + - [0.45738160610198975, -0.2500264346599579, 0.006000000052154064] + - [0.46632689237594604, -0.24997052550315857, 0.006000000052154064] + - [0.47527238726615906, -0.24999088048934937, 0.006000000052154064] + - [0.48421791195869446, -0.2500043213367462, 0.006000000052154064] + - [0.49316343665122986, -0.2500026524066925, 0.006000000052154064] + - [0.5021089315414429, -0.24999944865703583, 0.006000000052154064] + - [0.5110544562339783, -0.2499985694885254, 0.006000000052154064] + - [0.5199999809265137, -0.25, 0.006000000052154064] + radius: 0.004 + fixed_segments: [0] + collision_filter_span: 2 + attrs: + density: 800.0 + stretch_stiffness: 100000.0 + shear_stiffness: 100000.0 + bend_stiffness: 0.0001 + twist_stiffness: 0.0001 + stretch_damping: 0.03 + shear_damping: 0.03 + bend_damping: 0.0001 + twist_damping: 0.0001 + collision_props: + backend: newton + margin: 0.0 + gap: 5.0e-05 + material_props: + backend: newton + dynamic_friction: 0.8 + ke: 20000.0 + kd: 1.0 + visual_material: + uid: white_rope + base_color: [0.93, 0.94, 0.92, 1] + roughness: 0.6 +env: + sim_steps_per_control: 32 + ignore_terminations: true + expert_trajectory: + joint_command_mode: position_velocity + task_instruction: Pick up the rope, wrap it around the post, tighten and hold it. + events: + settle_rope: + func: settle_franka_rope + mode: reset + params: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task.franka.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task.franka.yaml new file mode 100644 index 0000000000..e98ade67f3 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task.franka.yaml @@ -0,0 +1,22 @@ +id: FrankaRopeWrap-v1 +environment: + component: env.yaml +embodiment: + component: ../../../../components/embodiments/franka_flat_pads.yaml +headless: true +render_cfg: + renderer: no-render +ground_plane_visible: false +enable_entity_gizmo: false +robot_ik_gizmo: null +visualization: + allow_commands: false +collection: + target_episodes: 1 + max_attempts: 1 +task_program: + program: task_program/program.yaml + integration: task_program/integration.yaml + execution_policy: ../../../../components/execution_policies/trajectory_open_loop.yaml +evaluation: + component: task_program/evaluation.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/evaluation.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/evaluation.yaml new file mode 100644 index 0000000000..74a73699de --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/evaluation.yaml @@ -0,0 +1,102 @@ +graph: + roots: [wrap_success] + rules: + completed: + kind: scalar_range + bindings: {value: cables/cable/wrap_completed} + params: {minimum: 1, maximum: 1} + finite: + kind: scalar_range + bindings: {value: cables/cable/wrap_finite} + params: {minimum: 1, maximum: 1} + joint_limits: + kind: scalar_range + bindings: {value: cables/cable/wrap_joint_limits} + params: {minimum: 1, maximum: 1} + direct_floor_pickup: + kind: scalar_range + bindings: {value: cables/cable/wrap_direct_floor_pickup} + params: {minimum: 1, maximum: 1} + grasp_retained: + kind: scalar_range + bindings: {value: cables/cable/wrap_grasp_retained} + params: {minimum: 1, maximum: 1} + material_grasp_slip: + kind: scalar_range + bindings: {value: cables/cable/wrap_material_grasp_slip} + params: {minimum: 1, maximum: 1} + bilateral_finger_contact: + kind: scalar_range + bindings: {value: cables/cable/wrap_bilateral_finger_contact} + params: {minimum: 1, maximum: 1} + actual_winding: + kind: scalar_range + bindings: {value: cables/cable/wrap_actual_winding} + params: {minimum: 1, maximum: 1} + post_contact_arc: + kind: scalar_range + bindings: {value: cables/cable/wrap_post_contact_arc} + params: {minimum: 1, maximum: 1} + hold_stable: + kind: scalar_range + bindings: {value: cables/cable/wrap_hold_stable} + params: {minimum: 1, maximum: 1} + rope_below_post_top: + kind: scalar_range + bindings: {value: cables/cable/wrap_rope_below_post_top} + params: {minimum: 1, maximum: 1} + post_nonpenetration: + kind: scalar_range + bindings: {value: cables/cable/wrap_post_nonpenetration} + params: {minimum: 1, maximum: 1} + robot_post_clearance: + kind: scalar_range + bindings: {value: cables/cable/wrap_robot_post_clearance} + params: {minimum: 1, maximum: 1} + anchor_points_toward_post: + kind: scalar_range + bindings: {value: cables/cable/wrap_anchor_points_toward_post} + params: {minimum: 1, maximum: 1} + smooth_anchor_exit: + kind: scalar_range + bindings: {value: cables/cable/wrap_smooth_anchor_exit} + params: {minimum: 1, maximum: 1} + anchor_fixed: + kind: scalar_range + bindings: {value: cables/cable/wrap_anchor_fixed} + params: {minimum: 1, maximum: 1} + length_preserved: + kind: scalar_range + bindings: {value: cables/cable/wrap_length_preserved} + params: {minimum: 1, maximum: 1} + contacts_not_dropped: + kind: scalar_range + bindings: {value: cables/cable/wrap_contacts_not_dropped} + params: {minimum: 1, maximum: 1} + floor_nonpenetration: + kind: scalar_range + bindings: {value: cables/cable/wrap_floor_nonpenetration} + params: {minimum: 1, maximum: 1} + wrap_success: + kind: all + refs: + - completed + - finite + - joint_limits + - direct_floor_pickup + - grasp_retained + - material_grasp_slip + - bilateral_finger_contact + - actual_winding + - post_contact_arc + - hold_stable + - rope_below_post_top + - post_nonpenetration + - robot_post_clearance + - anchor_points_toward_post + - smooth_anchor_exit + - anchor_fixed + - length_preserved + - contacts_not_dropped + - floor_nonpenetration +final_acceptance: [wrap_success] diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/integration.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/integration.yaml new file mode 100644 index 0000000000..93fc48cd5c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/integration.yaml @@ -0,0 +1,23 @@ +integration_id: franka_rope_wrap_v1 +program_id: franka_rope_wrap +requires: + scene_contract: cable_post_v1 + embodiment_contract: single_arm_parallel_gripper +scene_binding: + contract_id: cable_post_v1 + registry_id: cable_post + cables: + - entity_id: cable + simulation_uid: cable +profile: + defaults: + move_joints: + primary: arm_and_hand + skill_options: + simulation.cable_wrap_phase: + kind: move_joints + preserve_velocity_samples: true + effect_monitors: {} +runtime_services: + registered_semantic_lowerers: + - kind: cable_wrap_phase diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/program.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/program.yaml new file mode 100644 index 0000000000..2afa4e7ee0 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/program.yaml @@ -0,0 +1,87 @@ +program_id: franka_rope_wrap +instruction: Grasp the free end of the floor cable, wrap it around the post and hold the tightened wrap. +targets: {} +program: +- segment: home + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: home +- segment: approach + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: approach +- segment: descend + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: descend +- segment: close + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: close +- segment: lift + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: lift +- segment: approach_post + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: approach_post +- segment: orbit + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: orbit +- segment: tighten + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: tighten +- segment: settle + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: settle +- segment: hold_closed + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: hold_closed + validators: + - kind: condition + ref: wrap_success diff --git a/extensions/embodichain_tasks/embodichain_tasks/__init__.py b/extensions/embodichain_tasks/embodichain_tasks/__init__.py index 395f943fe0..fad82a9567 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/__init__.py +++ b/extensions/embodichain_tasks/embodichain_tasks/__init__.py @@ -34,6 +34,7 @@ register_manager_modules( [ + "embodichain_tasks.manipulation.cable.franka_rope_wrap", "embodichain_tasks.locomotion.managers.observations", "embodichain_tasks.locomotion.managers.rewards", ] diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py new file mode 100644 index 0000000000..28f18d8199 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py @@ -0,0 +1,617 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Scene, asset and reset handling for the Franka rope-wrapping task.""" + +from __future__ import annotations + +from importlib.resources import files +from pathlib import Path +from typing import TYPE_CHECKING, Any +from urllib.parse import unquote, urlsplit +import hashlib +import json +import shlex +import shutil +import xml.etree.ElementTree as ET +import numpy as np +from scipy.interpolate import make_interp_spline + +if TYPE_CHECKING: + import torch + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + +__all__ = [ + "cable_points", + "load_config", + "materialize_deployment", + "prepare_models", + "settle_franka_rope", + "task_directory", +] + + +def task_directory() -> Path: + """Locate the co-installed physical scene and task components.""" + local = Path(__file__).resolve().parents[3] / "configs" + root = local if local.is_dir() else Path(str(files("embodichain_tasks.configs"))) + return root / "tasks/manipulation/cable/franka_rope_wrap" + + +def load_config(path: Path | None = None) -> dict[str, Any]: + """Load scene settings and derive evaluation windows from the motion clock.""" + config = json.loads( + ( + Path(path) if path is not None else task_directory() / "config.json" + ).read_text() + ) + config.setdefault("open_half_gap_m", 0.04) + config.setdefault("grip_half_gap_m", config["cable_radius_m"] - 0.0005) + config.setdefault("floor_friction", 0.2) + scale = config["duration_s"] / 36.0 + config["grasp_complete_s"] = 5.0 * scale + config["lift_complete_s"] = 7.0 * scale + _validate_config(config) + return config + + +def _json(path: Path, data: Any) -> None: + path.write_text(json.dumps(data, indent=2, ensure_ascii=False) + "\n") + + +def _hash(path: Path, algorithm: str = "sha256") -> str: + digest = hashlib.new(algorithm) + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _texture_reference(tokens: list[str]) -> str: + """Remove standard MTL map options before resolving the texture filename.""" + one_value = { + "-blendu", + "-blendv", + "-cc", + "-clamp", + "-imfchan", + "-texres", + "-type", + "-bm", + "-boost", + } + index = 1 + while index < len(tokens) and tokens[index].startswith("-"): + option = tokens[index] + index += 1 + if option in one_value: + index += 1 + elif option == "-mm": + index += 2 + elif option in {"-o", "-s", "-t"}: + count = 0 + while index < len(tokens) and count < 3: + try: + float(tokens[index]) + except ValueError: + break + index += 1 + count += 1 + else: + raise ValueError(f"Unsupported MTL texture option: {option}") + if index >= len(tokens): + raise ValueError("MTL texture declaration has no filename") + return " ".join(tokens[index:]) + + +def _mesh_paths(root: ET.Element, source: Path) -> list[Path]: + """Resolve the generated model's local mesh and material dependency closure. + + The generated model preserves the source directory layout. References must + therefore stay beneath the source URDF directory; package and remote URIs + are not supported by this standalone example. + """ + directory = source.parent.resolve() + pending = [(directory, mesh.get("filename", "")) for mesh in root.iter("mesh")] + dependencies: set[Path] = set() + while pending: + parent, reference = pending.pop() + uri = urlsplit(reference) + if uri.scheme or uri.netloc or not uri.path: + raise ValueError( + f"Asset references must be local relative paths: {reference!r}" + ) + relative = Path(unquote(uri.path)) + if relative.is_absolute(): + raise ValueError(f"Asset references must be relative: {reference!r}") + path = (parent / relative).resolve() + if not path.is_relative_to(directory): + raise ValueError( + f"Asset dependency escapes the source directory: {reference!r}" + ) + if not path.is_file(): + raise FileNotFoundError(path) + if path in dependencies: + continue + dependencies.add(path) + if path.suffix.lower() in {".obj", ".mtl"}: + for line in path.read_text().splitlines(): + tokens = shlex.split(line, comments=True) + if not tokens: + continue + if path.suffix.lower() == ".obj" and tokens[0] == "mtllib": + pending.extend((path.parent, name) for name in tokens[1:]) + elif path.suffix.lower() == ".mtl" and ( + tokens[0].startswith("map_") + or tokens[0].lower() in {"bump", "disp", "decal", "norm", "refl"} + ): + pending.append((path.parent, _texture_reference(tokens))) + elif path.suffix.lower() == ".dae": + for image in ET.parse(path).findall(".//{*}image"): + for value in image.iter(): + if value.tag.rsplit("}", 1)[-1] in {"init_from", "ref"}: + reference = (value.text or "").strip() + if reference and not reference.startswith("#"): + pending.append((path.parent, reference)) + return sorted(dependencies) + + +def _asset_archives(reference: str) -> list[dict[str, str]]: + """Record available registry archive identities without initiating downloads.""" + if Path(reference).is_absolute(): + return [] + from embodichain.data.constants import EMBODICHAIN_DEFAULT_DATA_ROOT + + prefix = reference.split("/", 1)[0] + directory = Path(EMBODICHAIN_DEFAULT_DATA_ROOT) / "download" / prefix + archives = [] + try: + paths = sorted(directory.glob("*.zip")) + except OSError as error: + return [{"metadata_error": str(error)}] + for path in paths: + try: + if path.is_file(): + archives.append( + { + "filename": path.name, + "md5": _hash(path, "md5"), + "sha256": _hash(path), + } + ) + except OSError as error: + archives.append({"filename": path.name, "metadata_error": str(error)}) + return archives + + +def _validate_config(config: dict[str, Any]) -> None: + """Reject inputs that break the standalone control and geometry contracts.""" + + def finite_values(value: Any, name: str) -> None: + if isinstance(value, dict): + for key, item in value.items(): + finite_values(item, f"{name}.{key}") + elif isinstance(value, list): + for index, item in enumerate(value): + finite_values(item, f"{name}[{index}]") + elif isinstance(value, float) and not np.isfinite(value): + raise ValueError(f"{name} must be finite") + + finite_values(config, "config") + for name, minimum in ( + ("control_hz", 1), + ("substeps", 4), + ("iterations", 1), + ("cable_segments", 8), + ): + value = config[name] + if isinstance(value, bool) or not isinstance(value, int) or value < minimum: + raise ValueError(f"{name} must be an integer >= {minimum}") + if config["substeps"] % 4: + raise ValueError("substeps must be divisible by four for contact sampling") + for name in ( + "cable_radius_m", + "cable_density_kg_m3", + "post_radius_m", + "post_height_m", + "finger_pad_width_m", + "duration_s", + ): + value = config[name] + if isinstance(value, bool) or not isinstance(value, (int, float)) or value <= 0: + raise ValueError(f"{name} must be a positive number") + for name in ( + "pre_roll_s", + "linear_stiffness_N_m", + "bend_stiffness_Nm_rad", + "twist_stiffness_Nm_rad", + "bend_damping_Nms_rad", + "contact_ke_N_m", + "contact_kd_Ns_m", + "contact_gap_m", + "contact_match_distance_m", + ): + value = config[name] + if isinstance(value, bool) or not isinstance(value, (int, float)) or value < 0: + raise ValueError(f"{name} must be a nonnegative number") + if config["duration_s"] < 36: + raise ValueError("duration_s must include the full 36-second motion and hold") + if not np.isclose( + config["duration_s"] * config["control_hz"], + round(config["duration_s"] * config["control_hz"]), + atol=1e-9, + rtol=0, + ): + raise ValueError("duration_s must align to the control grid") + if ( + not 0 + <= config["grasp_complete_s"] + < config["lift_complete_s"] + <= config["duration_s"] - 2 + ): + raise ValueError("Grasp and lift checks must precede the final two-second hold") + for name, shape in ( + ("home_qpos", (7,)), + ("post_center_xy_m", (2,)), + ("finger_pad_center_m", (3,)), + ("finger_pad_size_m", (3,)), + ): + vector = np.asarray(config[name], dtype=float) + if vector.shape != shape: + raise ValueError(f"{name} must have shape {shape}") + if not np.isfinite(vector).all(): + raise ValueError(f"{name} must contain finite values") + if np.any(np.asarray(config["finger_pad_size_m"], dtype=float) <= 0): + raise ValueError("finger_pad_size_m must contain positive dimensions") + points = np.asarray(config["cable_waypoints_m"], dtype=float) + if points.ndim != 2 or points.shape[1] != 3 or len(points) < 3: + raise ValueError("cable_waypoints_m must contain at least three XYZ waypoints") + if not np.isfinite(points).all(): + raise ValueError("cable_waypoints_m must contain finite values") + if np.any(np.linalg.norm(np.diff(points, axis=0), axis=1) <= 0): + raise ValueError("Consecutive cable waypoints must be distinct") + if ( + np.linalg.norm( + points[0, :2] - np.asarray(config["post_center_xy_m"], dtype=float) + ) + <= 0 + ): + raise ValueError( + "The cable anchor and post center must have distinct XY coordinates" + ) + total_length = float( + np.linalg.norm(np.diff(cable_points(config), axis=0), axis=1).sum() + ) + inset = config["grasp_inset_m"] + if ( + isinstance(inset, bool) + or not isinstance(inset, (int, float)) + or not 0 < inset < total_length + ): + raise ValueError( + "grasp_inset_m must lie strictly inside the sampled cable length" + ) + thresholds = config["validation"] + if not isinstance(thresholds, dict): + raise ValueError("validation must be a mapping of acceptance thresholds") + for name, value in thresholds.items(): + if ( + isinstance(value, bool) + or not isinstance(value, (int, float)) + or not np.isfinite(value) + or value < 0 + ): + raise ValueError(f"validation.{name} must be a finite nonnegative number") + if not isinstance(config["robot_urdf"], str) or not config["robot_urdf"].strip(): + raise ValueError("robot_urdf must be an asset reference or absolute URDF path") + + +def prepare_models(config: dict[str, Any], out: Path) -> dict[str, Path]: + """Build a standalone robot and record the source and dependency hashes.""" + from embodichain.data import get_data_path + + generated = out / "generated_assets" + generated.mkdir() + robot_directory = generated / "franka" + robot_directory.mkdir() + reference = config["robot_urdf"] + source = Path(get_data_path(reference)).resolve() + if not source.is_file(): + raise FileNotFoundError(f"Robot URDF does not exist: {source}") + tree = ET.parse(source) + root = tree.getroot() + from embodichain_robots.franka.rope_pads import apply_rope_pads + + apply_rope_pads( + root, center=config["finger_pad_center_m"], size=config["finger_pad_size_m"] + ) + dependencies = [] + for path in _mesh_paths(root, source): + relative = path.relative_to(source.parent) + target = robot_directory / relative + target.parent.mkdir(parents=True, exist_ok=True) + shutil.copy2(path, target) + dependencies.append( + {"path": target.relative_to(out).as_posix(), "sha256": _hash(target)} + ) + paths = {"robot": robot_directory / "franka_rope.urdf"} + tree.write(paths["robot"], encoding="utf-8", xml_declaration=True) + _json( + generated / "provenance.json", + { + "asset_reference": reference, + "cached_registry_archives": _asset_archives(reference), + "robot_source": str(source.resolve()), + "robot_source_sha256": _hash(source), + "generated_robot": paths["robot"].relative_to(out).as_posix(), + "generated_robot_sha256": _hash(paths["robot"]), + "dependencies": dependencies, + "modifications": "Flat finger pads for direct floor rope grasp; original joint names, origins, axes, limits and masses retained. No moving rope attachment.", + "finger_pad_width_m": config["finger_pad_width_m"], + "finger_pad_center_m": config["finger_pad_center_m"], + "finger_pad_size_m": config["finger_pad_size_m"], + }, + ) + return paths + + +def cable_points(config: dict[str, Any]) -> np.ndarray: + """Sample the anchored quadratic centerline at equal arc-length intervals.""" + points = np.asarray(config["cable_waypoints_m"], float) + distances = np.r_[0.0, np.cumsum(np.linalg.norm(np.diff(points, axis=0), axis=1))] + curve = make_interp_spline(distances, points, k=2)( + np.linspace(0, distances[-1], 4000) + ) + curve[:, 2] = np.maximum( + curve[:, 2], config["floor_height_m"] + config["cable_radius_m"] + ) + arc = np.r_[0.0, np.cumsum(np.linalg.norm(np.diff(curve, axis=0), axis=1))] + result = np.column_stack( + [ + np.interp( + np.linspace(0, arc[-1], config["cable_segments"] + 1), arc, curve[:, i] + ) + for i in range(3) + ] + ) + # The fixed root capsule exits its clamp toward the post. Preserve the + # sampled first-segment length while removing spline endpoint tangent error. + direction = np.r_[np.asarray(config["post_center_xy_m"]) - result[0, :2], 0.0] + direction /= np.linalg.norm(direction) + result[1] = result[0] + direction * np.linalg.norm(result[1] - result[0]) + return result.astype(np.float32) + + +def settle_franka_rope( + env: EmbodiedEnv, env_ids: torch.Tensor | None, scenario_path: str | None = None +) -> None: + """Settle the free cable against the slab before observing task time zero.""" + import torch + + if env_ids is not None and len(env_ids) != env.num_envs: + raise ValueError( + "Franka rope pre-roll advances the world and requires a full reset" + ) + from .wrap_motion import _JOINT_NAMES + + if tuple(env.robot.joint_names) != _JOINT_NAMES: + raise ValueError(f"Unexpected Franka joint order {env.robot.joint_names!r}.") + config = load_config(Path(scenario_path) if scenario_path is not None else None) + env.franka_rope_config = config + arm_home = [*config["home_qpos"], *([config["open_half_gap_m"]] * 2)] + qpos = torch.tensor(arm_home, dtype=torch.float32, device=env.device).expand( + env.num_envs, -1 + ) + env.robot.set_qpos(qpos, target=True) + steps = round(config["pre_roll_s"] * config["control_hz"] * config["substeps"]) + for _ in range(steps): + env.sim.update(step=1, render_final_step=False) + + +def _contact(config: dict[str, Any], friction: float) -> dict[str, Any]: + return { + "collision_props": { + "backend": "newton", + "margin": 0.0, + "gap": config["contact_gap_m"], + }, + "material_props": { + "backend": "newton", + "dynamic_friction": friction, + "ke": config["contact_ke_N_m"], + "kd": config["contact_kd_Ns_m"], + }, + } + + +def _environment( + config: dict[str, Any], scenario_path: Path | None = None +) -> dict[str, Any]: + floor = config["floor_height_m"] + center = config["post_center_xy_m"] + anchor = np.asarray(config["cable_waypoints_m"][0], dtype=float) + direction = np.r_[np.array(center) - anchor[:2], 0.0] + direction /= np.linalg.norm(direction) + + def rigid( + uid: str, + shape: dict, + position: list, + friction: float, + color: list[float], + roughness: float, + ) -> dict: + return { + "uid": uid, + "body_type": "static", + "init_pos": position, + "shape": { + **shape, + "visual_material": { + "uid": uid + "_mat", + "base_color": color, + "roughness": roughness, + }, + }, + "attrs": _contact(config, friction), + } + + slab = rigid( + "table", + {"shape_type": "Cube", "size": [1.2, 1.05, 0.05]}, + [0.35, 0, floor - 0.025], + config["floor_friction"], + [0.33, 0.37, 0.41, 1], + 0.65, + ) + clamp = rigid( + "anchor_clamp", + {"shape_type": "Cube", "size": [0.028, 0.018, 0.008]}, + (anchor - 0.006 * direction - [0, 0, 0.004]).tolist(), + config["floor_friction"], + [0.08, 0.11, 0.15, 1], + 0.65, + ) + clamp["init_rot"] = [ + 0, + 0, + float(np.rad2deg(np.arctan2(direction[1], direction[0]))), + ] + post = rigid( + "post", + { + "shape_type": "Cylinder", + "radius": config["post_radius_m"], + "height": config["post_height_m"], + }, + [*center, floor + config["post_height_m"] / 2], + config["post_friction"], + [0.025, 0.45, 0.39, 1], + 0.38, + ) + cable = { + "uid": "cable", + "points": cable_points(config).tolist(), + "radius": config["cable_radius_m"], + "fixed_segments": [0], + "collision_filter_span": 2, + "attrs": { + "density": config["cable_density_kg_m3"], + "stretch_stiffness": config["linear_stiffness_N_m"], + "shear_stiffness": config["linear_stiffness_N_m"], + "bend_stiffness": config["bend_stiffness_Nm_rad"], + "twist_stiffness": config["twist_stiffness_Nm_rad"], + "stretch_damping": 0.03, + "shear_damping": 0.03, + "bend_damping": config["bend_damping_Nms_rad"], + "twist_damping": config["bend_damping_Nms_rad"], + **_contact(config, config["rope_friction"]), + }, + "visual_material": { + "uid": "white_rope", + "base_color": [0.93, 0.94, 0.92, 1], + "roughness": 0.6, + }, + } + return { + "environment_id": "franka_rope_wrap", + "physics": "newton", + "physics_config": { + "physics_dt": 1.0 / (config["control_hz"] * config["substeps"]), + "num_substeps": 1, + "collision_cfg": None, + "sync_to_renderer": False, + "use_cuda_graph": True, + "solver_cfg": { + "solver_type": "dexuni", + "joint_mode": "dynamic", + "coupling": "two_way", + "mujoco_articulation_paths": ["arena_0/franka"], + "vbd_options": { + "iterations": config["iterations"], + "rigid_contact_history": config.get("rigid_contact_history", True), + "rigid_contact_hard": config.get("rigid_contact_hard", False), + "rigid_joint_linear_ke": config.get( + "rigid_joint_linear_ke", 100000.0 + ), + "rigid_joint_angular_ke": config.get( + "rigid_joint_angular_ke", 10.0 + ), + "friction_epsilon": config.get("friction_epsilon", 1e-5), + }, + "collision_options": { + "contact_matching": "sticky", + "contact_matching_pos_threshold": config[ + "contact_match_distance_m" + ], + "rigid_contact_max": 4096, + }, + }, + }, + "max_episode_steps": round(config["duration_s"] * config["control_hz"]) + 1, + "num_envs": 1, + "simulation": {"background": [slab, clamp, post], "cable_object": [cable]}, + "env": { + "sim_steps_per_control": config["substeps"], + "ignore_terminations": True, + "expert_trajectory": {"joint_command_mode": "position_velocity"}, + "task_instruction": "Pick up the rope, wrap it around the post, tighten and hold it.", + "events": { + "settle_rope": { + "func": "settle_franka_rope", + "mode": "reset", + "params": ( + {"scenario_path": str(scenario_path)} if scenario_path else {} + ), + } + }, + }, + } + + +def materialize_deployment(config: dict[str, Any], robot_path: Path, out: Path) -> Path: + """Write an equivalent configured Gym deployment for a user-supplied scene.""" + import yaml + + _validate_config(config) + folder = out.resolve() / "deployment" + folder.mkdir() + reference = task_directory() + deployment = yaml.safe_load((reference / "task.franka.yaml").read_text()) + embodiment_path = reference / deployment["embodiment"]["component"] + embodiment = yaml.safe_load(embodiment_path.read_text()) + robot = embodiment["simulation"] + robot["fpath"] = str(robot_path.resolve()) + robot["init_pos"] = [0, 0, config["floor_height_m"]] + robot["init_qpos"] = [*config["home_qpos"], *([config["open_half_gap_m"]] * 2)] + robot["attrs"] = _contact(config, 1.2) + commands = embodiment["skill_profile"]["command_presets"][0]["commands"] + commands.update( + open=[config["open_half_gap_m"]] * 2, grasp=[config["grip_half_gap_m"]] * 2 + ) + scenario_path = folder / "config.json" + _json(scenario_path, config) + environment = _environment(config, scenario_path) + for section in ("task_program", "evaluation"): + for key, path in deployment[section].items(): + deployment[section][key] = str((reference / path).resolve()) + deployment["embodiment"]["component"] = "embodiment.yaml" + for name, values in [ + ("env.yaml", environment), + ("embodiment.yaml", embodiment), + ("task.franka.yaml", deployment), + ]: + (folder / name).write_text(yaml.safe_dump(values, sort_keys=False)) + return folder / "task.franka.yaml" diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_evaluation.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_evaluation.py new file mode 100644 index 0000000000..4635c57825 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_evaluation.py @@ -0,0 +1,286 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Observe the Franka wrap using the Gym-owned contact sampling clock.""" + +from __future__ import annotations + +from collections.abc import Sequence +import json +from pathlib import Path +from typing import Any, TYPE_CHECKING +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.spatial.transform import Rotation + +from .wrap_measurements import _capsule_pad_gap, _rope_geometry, evaluate_wrap + +if TYPE_CHECKING: + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + from .wrap_motion import WrapMotion + +__all__ = ["attach_wrap_monitor"] + + +class _WrapMonitor: + def __init__( + self, + env: EmbodiedEnv, + config: dict[str, Any], + motion: WrapMotion, + robot_path: Path, + ) -> None: + if env.num_envs != 1: + raise ValueError("The Franka rope wrap deployment requires one arena.") + self.config = config + self.robot = env.robot + self.cable = env.sim.get_cable_object("cable") + self.sensor = env.sim.get_sensor("contacts") + self.motion = motion + self.lengths = np.linalg.norm(np.diff(self.cable.cfg.points, axis=0), axis=1) + material_arc = self.lengths.sum() - config["grasp_inset_m"] + self.material_index = int( + np.searchsorted(np.cumsum(self.lengths), material_arc) + ) + self.material_offset = ( + material_arc + - self.lengths[: self.material_index].sum() + - self.lengths[self.material_index] / 2 + ) + tree = ET.parse(robot_path) + limits = [ + tree.find(f".//joint[@name='{name}']/limit") + for name in self.robot.joint_names + ] + self.lo = np.array([float(limit.get("lower")) for limit in limits]) + self.hi = np.array([float(limit.get("upper")) for limit in limits]) + self.actor_paths = {} + self.active = True + self.rows, self.states, self.joint_states, self.tcp_states = [], [], [], [] + self.baseline = self.cable.data.segment_state_w[0, 0, :3].cpu().numpy().copy() + self.material_baseline = None + self.terminal_report = None + self.begin_control_step() + self._capture() + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + if env_ids is None or 0 in env_ids: + self.active = False + + def begin_control_step(self) -> None: + self.substep = 0 + self.finger_contacts = [False, False] + self.bilateral_contact = False + self.robot_post_peak = self.penetration = 0.0 + self.dropped = 0 + self.worst = None + + def before_sample(self) -> None: + return None + + def after_sample(self, dt: float, token: object) -> None: + if not self.active or self.terminal_report is not None: + return + self.substep += 1 + if self.substep % (self.config["substeps"] // 4) == 0: + self._contacts() + if self.substep == self.config["substeps"]: + self._capture() + + def _contacts(self) -> None: + contact = self.sensor + robot, cable = self.robot, self.cable + config, lengths = self.config, self.lengths + actor_paths = self.actor_paths + data = contact.get_data() + valid = data["is_valid"] + candidate_positions = [[], []] + for ids, separation, position in zip( + data["user_ids"][valid].cpu().tolist(), + data["distance"][valid].cpu().tolist(), + data["position"][valid].cpu().tolist(), + ): + pair = [] + for actor_id in ids: + if actor_id < 0: + continue + if actor_id not in actor_paths: + info = contact.get_actor_info(actor_id) + actor_paths[actor_id] = f"{info.path}/{info.link_name or ''}" + pair.append(actor_paths[actor_id]) + # DexSim currently registers rigid/articulation actors, + # leaving cable counterparts anonymous. Unknown identity + # is only a candidate: match the live cable capsules and + # finger gap below before accepting contact evidence. + is_rope = any(actor_id < 0 for actor_id in ids) or any( + "/cable/" in p for p in pair + ) + for k, name in enumerate(("fr3_leftfinger", "fr3_rightfinger")): + if is_rope and any(name in p for p in pair) and separation <= 1e-5: + candidate_positions[k].append(position) + if any("/post/" in p for p in pair) and any("/franka/" in p for p in pair): + self.robot_post_peak = max(self.robot_post_peak, -separation) + if -separation > self.penetration: + self.penetration = -separation + self.worst = pair + matched = [False, False] + if any(candidate_positions): + sampled_state = cable.data.segment_state_w[0, -8:].cpu().numpy() + axes = Rotation.from_quat(sampled_state[:, 3:7]).as_matrix()[:, :, 2] + starts = sampled_state[:, :3] - axes * lengths[-8:, None] / 2 + delta = axes * lengths[-8:, None] + for k, name in enumerate(("fr3_leftfinger", "fr3_rightfinger")): + if not candidate_positions[k]: + continue + contact_points = np.asarray(candidate_positions[k]) + fraction = np.clip( + np.sum( + (contact_points[:, None] - starts) * delta, + axis=-1, + ) + / np.sum(delta * delta, axis=-1), + 0.0, + 1.0, + ) + closest = starts + fraction[..., None] * delta + point_gap = ( + np.linalg.norm(contact_points[:, None] - closest, axis=-1).min() + - config["cable_radius_m"] + ) + pose = robot.get_link_pose(name, to_matrix=True)[0].cpu().numpy() + pad_gap = _capsule_pad_gap(sampled_state, lengths[-8:], pose, config) + matched[k] = bool(point_gap <= 0.0003 and pad_gap <= 0.0002) + self.finger_contacts[k] |= matched[k] + self.bilateral_contact |= all(matched) + self.dropped += contact.latest_sample_dropped_contacts + + def _capture(self) -> None: + config, lengths = self.config, self.lengths + robot, cable = self.robot, self.cable + frame = len(self.rows) + t = frame / config["control_hz"] + targets, labels = self.motion.poses, self.motion.labels + material_index, material_offset = self.material_index, self.material_offset + lo, hi, baseline = self.lo, self.hi, self.baseline + finger_contacts = self.finger_contacts + state = cable.data.segment_state_w[0].cpu().numpy() + qpos = robot.get_qpos()[0].cpu().numpy() + tcp = robot.get_link_pose("fr3_hand_tcp", to_matrix=True)[0].cpu().numpy() + pad_gaps = [] + for name in ("fr3_leftfinger", "fr3_rightfinger"): + pose = robot.get_link_pose(name, to_matrix=True)[0].cpu().numpy() + pad_gaps.append(_capsule_pad_gap(state[-8:], lengths[-8:], pose, config)) + finger_contacts = [ + present and gap <= 0.0002 for present, gap in zip(finger_contacts, pad_gaps) + ] + material_axis = Rotation.from_quat(state[material_index, 3:7]).as_matrix()[:, 2] + material_world = state[material_index, :3] + material_offset * material_axis + material_local = tcp[:3, :3].T @ (material_world - tcp[:3, 3]) + if ( + self.material_baseline is None + and t >= config["grasp_complete_s"] + and self.bilateral_contact + ): + self.material_baseline = material_local.copy() + material_slip = ( + float(np.linalg.norm(material_local - self.material_baseline)) + if self.material_baseline is not None + else 0.0 + ) + geometry = _rope_geometry(state, lengths, config) + nodes = geometry.pop("nodes") + row = { + "time_s": t, + "phase": labels[frame], + **geometry, + "actual_tcp_pose": tcp.tolist(), + "command_tcp_pose": targets[frame].tolist(), + "tcp_error_m": float(np.linalg.norm(tcp[:3, 3] - targets[frame, :3, 3])), + "tail_tcp_distance_m": float(np.linalg.norm(nodes[-1] - tcp[:3, 3])), + "grasp_region_tcp_distance_m": float( + np.linalg.norm(state[-6:, :3] - tcp[:3, 3], axis=1).min() + ), + "grasp_confirmed": self.material_baseline is not None, + "bilateral_rope_contact": self.bilateral_contact, + "material_point_tcp_m": material_local.tolist(), + "material_slip_m": material_slip, + "finger_pad_gaps_m": pad_gaps, + "left_rope_contact": finger_contacts[0], + "right_rope_contact": finger_contacts[1], + "finger_gap_m": float(qpos[7] + qpos[8]), + "anchor_error_m": float(np.linalg.norm(state[0, :3] - baseline)), + "arm_joint_limit_violation_rad": float( + max(0.0, (lo[:7] - qpos[:7]).max(), (qpos[:7] - hi[:7]).max()) + ), + "finger_joint_limit_violation_m": float( + max(0.0, (lo[7:] - qpos[7:]).max(), (qpos[7:] - hi[7:]).max()) + ), + "robot_post_penetration_m": self.robot_post_peak, + "penetration_m": self.penetration, + "worst_contact": self.worst, + "dropped_contacts": self.dropped, + "finite": bool(np.isfinite(state).all() and np.isfinite(qpos).all()), + } + self.rows.append(row) + self.states.append(state.copy()) + self.joint_states.append(qpos.copy()) + self.tcp_states.append(tcp.copy()) + if not row["finite"]: + raise RuntimeError(f"Nonfinite physical state at {t}") + + if len(self.rows) == len(self.motion.times): + self.terminal_report = evaluate_wrap(self.rows, self.config) + + def write(self, out: Path) -> dict[str, Any]: + """Export measured frames for the existing offline renderer and report.""" + report = self.terminal_report or evaluate_wrap(self.rows, self.config) + for name, value in { + "validation": report, + "settled_state": { + "robot_qpos": self.joint_states[0].tolist(), + "cable_state": self.states[0].tolist(), + }, + "metrics": { + "rows": self.rows, + "actor_paths": self.actor_paths, + "contact_sample_hz": self.config["control_hz"] * 4, + }, + }.items(): + (out / f"{name}.json").write_text(json.dumps(value, indent=2) + "\n") + np.savez_compressed( + out / "states.npz", + times=[row["time_s"] for row in self.rows], + cable_states=self.states, + robot_qpos=self.joint_states, + tcp_poses=self.tcp_states, + joint_names=self.robot.joint_names, + ) + return report + + +def attach_wrap_monitor( + env: EmbodiedEnv, config: dict[str, Any], motion: WrapMotion, robot_path: Path +) -> _WrapMonitor: + """Start the episode at the settled state before its first command.""" + previous = getattr(env, "_wrap_monitor", None) + if previous is not None: + if previous.active: + return previous + previous.sensor.remove_sampling_observer("franka_rope_wrap", expected=previous) + monitor = _WrapMonitor(env, config, motion, robot_path) + monitor.sensor.add_sampling_observer("franka_rope_wrap", monitor) + env._wrap_monitor = monitor + return monitor diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_integration.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_integration.py new file mode 100644 index 0000000000..4688a3c9fd --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_integration.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. +# ---------------------------------------------------------------------------- +"""Bind a checked, single-post cable wrap plan to the standard MoveJoints skill.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from pathlib import Path +from typing import ClassVar, TYPE_CHECKING +from weakref import proxy + +import torch + +from embodichain.lab.sim.atomic_skills import ( + AtomicSkillEngine, + JointPositionGoal, + MoveJoints, + PlanningContext, + SkillDescriptor, + SkillOptions, + TimedTrajectory, +) +from embodichain.lab.task_program.compiler.lowering import ( + RegisteredSemanticLowerer, + SemanticLowering, +) +from embodichain.lab.task_program.language.semantic_contracts import ( + SemanticCallContract, +) +from embodichain.lab.task_program.semantics import ( + BoundSemanticCall, + RegisteredSemanticCall, + SceneRegistry, +) +from .wrap_motion import PHASE_END_S, WrapMotion, plan_wrap_motion + +if TYPE_CHECKING: + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + from embodichain.lab.sim.objects.robot import Robot + +__all__ = ["CableWrapPhaseFactory"] + + +@dataclass(frozen=True, slots=True) +class CableWrapPhaseFactory: + """Create a phase lowerer with the physical scene's robot model and control grid.""" + + call_id: ClassVar[str] = "simulation.cable_wrap_phase" + revision: ClassVar[str] = "1" + target_descriptor: ClassVar[SkillDescriptor] = MoveJoints.descriptor() + + @property + def argument_contract(self) -> SemanticCallContract: + """Expose the supported semantic phases during static preflight.""" + return SemanticCallContract(required={"phase": tuple(PHASE_END_S)}, optional={}) + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicSkillEngine, + ) -> RegisteredSemanticLowerer: + raise ValueError( + "Cable wrap planning requires its environment's scene configuration." + ) + + def create_for_environment( + self, + *, + environment: EmbodiedEnv, + simulation: object, + robot: Robot, + scene_registry: SceneRegistry, + engine: AtomicSkillEngine, + ) -> RegisteredSemanticLowerer: + from .wrap_evaluation import attach_wrap_monitor + + if ( + engine.robot is not robot + or environment.robot is not robot + or environment.num_envs != 1 + ): + raise ValueError( + "The Franka wrap deployment requires its bound, single-arena robot." + ) + config = environment.franka_rope_config + monitor = getattr(environment, "_wrap_monitor", None) + if monitor is None or not monitor.active: + robot_path = Path(robot.cfg.fpath) + motion = plan_wrap_motion(config, robot_path) + monitor = attach_wrap_monitor(environment, config, motion, robot_path) + motion = monitor.motion + # The registration retains its lowerer identities. The episode monitor owns + # the full trajectory so expired episodes can release their command arrays. + return _CableWrapPhaseLowerer(proxy(motion), int(config["control_hz"])) + + +class _CableWrapPhaseLowerer(RegisteredSemanticLowerer): + call_id: ClassVar[str] = CableWrapPhaseFactory.call_id + target_descriptor: ClassVar[SkillDescriptor] = MoveJoints.descriptor() + preserves_symbolic_state: ClassVar[bool] = True + + def __init__(self, motion: WrapMotion, hz: int) -> None: + self._motion = motion + self._hz = hz + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: SkillOptions, + ) -> SemanticLowering: + if set(call.arguments) != {"phase"} or not isinstance( + call.arguments["phase"], str + ): + raise ValueError("A cable wrap call requires one named phase.") + dt = context.require_control_dt() + if context.batch_size != 1 or not math.isclose( + dt, 1.0 / self._hz, rel_tol=1e-8 + ): + raise ValueError( + "Franka wrap commands must run at the scene's configured control rate." + ) + phase = self._motion.phase_slice(call.arguments["phase"]) + # Include the last command of the preceding phase so its incoming + # finite difference is preserved at every segment boundary. + samples = self._motion.positions[phase.start - 1 : phase.stop] + positions = torch.as_tensor( + samples.copy(), device=context.robot.qpos.device + ).unsqueeze(0) + trajectory = TimedTrajectory( + positions=positions[:, 1:], + velocities=positions.diff(dim=1) * self._hz, + accelerations=None, + dt=torch.full( + (1, positions.shape[1] - 1), + dt, + dtype=torch.float64, + device=positions.device, + ), + env_ids=context.env_ids, + ) + return SemanticLowering(goal=JointPositionGoal(trajectory)) diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_measurements.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_measurements.py new file mode 100644 index 0000000000..ad6af641f5 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_measurements.py @@ -0,0 +1,318 @@ +# ---------------------------------------------------------------------------- +# 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 measurements and episode acceptance for wrapping a cable around a post.""" + +from __future__ import annotations + +from typing import Any +import numpy as np +from scipy.spatial.transform import Rotation + +__all__ = ["evaluate_wrap"] + +WRAP_CHECKS = ( + "completed", + "finite", + "joint_limits", + "direct_floor_pickup", + "grasp_retained", + "material_grasp_slip", + "bilateral_finger_contact", + "actual_winding", + "post_contact_arc", + "hold_stable", + "rope_below_post_top", + "post_nonpenetration", + "robot_post_clearance", + "anchor_points_toward_post", + "smooth_anchor_exit", + "anchor_fixed", + "length_preserved", + "contacts_not_dropped", + "floor_nonpenetration", +) + + +def _capsule_pad_gap( + state: np.ndarray, + lengths: np.ndarray, + finger_pose: np.ndarray, + config: dict[str, Any], +) -> float: + """Exact capsule-axis / finger-box distance, less the physical rope radius.""" + axes = Rotation.from_quat(state[:, 3:7]).as_matrix()[:, :, 2] + starts = state[:, :3] - axes * lengths[:, None] / 2 + ends = state[:, :3] + axes * lengths[:, None] / 2 + half = np.asarray(config["finger_pad_size_m"]) / 2 + a = (starts - finger_pose[:3, 3]) @ finger_pose[:3, :3] - config[ + "finger_pad_center_m" + ] + b = (ends - finger_pose[:3, 3]) @ finger_pose[:3, :3] - config[ + "finger_pad_center_m" + ] + best = np.inf + for origin, delta in zip(a, b - a): + breaks = [0.0, 1.0] + for k in range(3): + if abs(delta[k]) > 1e-12: + breaks.extend( + float(v) + for v in ( + (-half[k] - origin[k]) / delta[k], + (half[k] - origin[k]) / delta[k], + ) + if 0 < v < 1 + ) + breaks = sorted(breaks) + for left, right in zip(breaks[:-1], breaks[1:]): + midpoint = origin + delta * ((left + right) / 2) + active = np.abs(midpoint) > half + offset = origin[active] - np.sign(midpoint[active]) * half[active] + velocity = delta[active] + denominator = float(velocity @ velocity) + t = ( + float(np.clip(-(offset @ velocity) / denominator, left, right)) + if denominator > 1e-20 + else left + ) + distance = np.linalg.norm(np.maximum(np.abs(origin + t * delta) - half, 0)) + best = min(best, float(distance)) + return best - config["cable_radius_m"] + + +def _rope_geometry( + state: np.ndarray, lengths: np.ndarray, config: dict[str, Any] +) -> dict[str, Any]: + """Measure actual capsule centerline geometry, independently of commanded orbit.""" + axis = Rotation.from_quat(state[:, 3:7]).as_matrix()[:, :, 2] + starts = state[:, :3] - axis * lengths[:, None] / 2 + ends = state[:, :3] + axis * lengths[:, None] / 2 + nodes = np.vstack([starts[:1], (ends[:-1] + starts[1:]) / 2, ends[-1:]]) + center = np.asarray(config["post_center_xy_m"]) + theta = np.unwrap(np.arctan2(nodes[:, 1] - center[1], nodes[:, 0] - center[0])) + samples = ( + starts[:, None] + + (ends - starts)[:, None] * np.linspace(0, 1, 11)[None, :, None] + ) + radial = np.linalg.norm(samples[:, :, :2] - center, axis=-1) + gaps = radial - config["post_radius_m"] - config["cable_radius_m"] + in_height = ( + samples[:, :, 2] > config["floor_height_m"] - config["cable_radius_m"] + ) & ( + samples[:, :, 2] + < config["floor_height_m"] + config["post_height_m"] + config["cable_radius_m"] + ) + valid_gaps = gaps[in_height] + near = ( + np.min(np.where(in_height, gaps, np.inf), axis=1) + <= config["validation"]["contact_band_m"] + ) + # Union on the circle: revisiting one sector must not inflate coverage. + intervals = [] + for low, high, include in zip( + np.minimum(theta[:-1], theta[1:]), np.maximum(theta[:-1], theta[1:]), near + ): + if not include: + continue + width = high - low + start = low % (2 * np.pi) + end = start + width + intervals.append((start, min(end, 2 * np.pi))) + if end > 2 * np.pi: + intervals.append((0.0, end - 2 * np.pi)) + coverage, right = 0.0, 0.0 + for low, high in sorted(intervals): + coverage += max(0.0, high - max(low, right)) + right = max(right, high) + joint_gap = np.linalg.norm(ends[:-1] - starts[1:], axis=1) + anchor_direction = np.r_[center - starts[0, :2], 0.0] + anchor_direction /= np.linalg.norm(anchor_direction) + anchor_angle = np.rad2deg(np.arccos(np.clip(axis[0] @ anchor_direction, -1.0, 1.0))) + anchor_bends = np.rad2deg( + np.arccos(np.clip(np.sum(axis[:4] * axis[1:5], axis=-1), -1.0, 1.0)) + ) + return { + "nodes": nodes, + "anchor_direction_error_deg": float(anchor_angle), + "anchor_exit_bend_max_deg": float(anchor_bends.max()), + "winding_deg": float(np.rad2deg(theta[-1] - theta[0])), + "contact_arc_deg": float(np.rad2deg(coverage)), + "post_contact_segments": int(near.sum()), + "post_penetration_m": ( + float(max(0.0, -valid_gaps.min())) if valid_gaps.size else 0.0 + ), + "joint_gap_max_m": float(joint_gap.max()), + "centerline_length_m": float( + np.linalg.norm(np.diff(nodes, axis=0), axis=1).sum() + ), + "free_end_m": ends[-1].tolist(), + "rope_min_z_m": float(samples[:, :, 2].min() - config["cable_radius_m"]), + "rope_max_z_m": float(samples[:, :, 2].max() + config["cable_radius_m"]), + } + + +def evaluate_wrap(rows: list[dict[str, Any]], config: dict[str, Any]) -> dict[str, Any]: + thresholds = config["validation"] + hold = [r for r in rows if r["time_s"] >= config["duration_s"] - 2 - 1e-8] + grasped = [r for r in rows if r["time_s"] >= config["grasp_complete_s"]] + lifted = [ + r + for r in rows + if config["lift_complete_s"] <= r["time_s"] <= config["lift_complete_s"] + 0.5 + ] + checks = { + "completed": bool( + rows + and abs(rows[-1]["time_s"] - config["duration_s"]) < 1e-8 + and len(hold) >= round(2 * config["control_hz"]) + 1 + ), + "finite": bool(rows and all(r["finite"] for r in rows)), + "joint_limits": bool( + rows + and max(r["arm_joint_limit_violation_rad"] for r in rows) < 1e-4 + and max(r["finger_joint_limit_violation_m"] for r in rows) < 1e-4 + ), + "direct_floor_pickup": bool( + lifted + and rows[0]["free_end_m"][2] + <= config["floor_height_m"] + 2 * config["cable_radius_m"] + and min(r["free_end_m"][2] for r in lifted) + > config["floor_height_m"] + 0.035 + ), + "grasp_retained": bool( + grasped + and all(r["grasp_confirmed"] for r in grasped) + and max(r["tail_tcp_distance_m"] for r in grasped) + <= thresholds["tail_tcp_max_m"] + ), + "material_grasp_slip": bool( + grasped + and max(r["material_slip_m"] for r in grasped) + <= thresholds.get("grasp_slip_max_m", 0.008) + ), + "bilateral_finger_contact": bool( + grasped and np.mean([r["bilateral_rope_contact"] for r in grasped]) >= 0.95 + ), + "actual_winding": bool( + hold + and min(abs(r["winding_deg"]) for r in hold) + >= thresholds["winding_min_deg"] + ), + "post_contact_arc": bool( + hold + and min(r["contact_arc_deg"] for r in hold) + >= thresholds["contact_arc_min_deg"] + ), + "hold_stable": bool( + hold + and np.ptp([r["winding_deg"] for r in hold]) + <= thresholds["hold_winding_drift_max_deg"] + and np.max( + np.linalg.norm( + np.asarray([r["free_end_m"] for r in hold]) - hold[0]["free_end_m"], + axis=1, + ) + ) + <= thresholds["hold_end_drift_max_m"] + ), + "rope_below_post_top": bool( + rows + and max(r["rope_max_z_m"] for r in rows) + < config["floor_height_m"] + config["post_height_m"] + ), + "post_nonpenetration": bool( + rows + and max(r["post_penetration_m"] for r in rows) + <= thresholds["post_penetration_max_m"] + ), + "robot_post_clearance": bool( + rows + and max(r["robot_post_penetration_m"] for r in rows) + <= thresholds["robot_post_penetration_max_m"] + ), + "anchor_points_toward_post": bool( + rows and max(r["anchor_direction_error_deg"] for r in rows) < 0.1 + ), + "smooth_anchor_exit": bool( + hold + and max(r["anchor_exit_bend_max_deg"] for r in hold) + <= thresholds.get("anchor_exit_bend_max_deg", 20.0) + ), + "anchor_fixed": bool(rows and max(r["anchor_error_m"] for r in rows) < 1e-5), + "length_preserved": bool( + rows + and max(r["joint_gap_max_m"] for r in rows) <= thresholds["joint_gap_max_m"] + ), + "contacts_not_dropped": bool( + rows and max(r["dropped_contacts"] for r in rows) == 0 + ), + "floor_nonpenetration": bool( + rows + and min(r["rope_min_z_m"] for r in rows) + >= config["floor_height_m"] - thresholds["floor_penetration_max_m"] + ), + } + return { + "passed": all(checks.values()), + "checks": checks, + "summary": { + "hold_anchor_exit_bend_max_deg": max( + [r["anchor_exit_bend_max_deg"] for r in hold], default=None + ), + "material_grasp_slip_max_m": max( + [r["material_slip_m"] for r in grasped], default=None + ), + "final_winding_deg": rows[-1]["winding_deg"] if rows else None, + "hold_winding_min_deg": min( + [abs(r["winding_deg"]) for r in hold], default=None + ), + "hold_contact_arc_min_deg": min( + [r["contact_arc_deg"] for r in hold], default=None + ), + "tail_tcp_max_m": max( + [r["tail_tcp_distance_m"] for r in grasped], default=None + ), + "post_penetration_max_m": max( + [r["post_penetration_m"] for r in rows], default=None + ), + "robot_post_penetration_max_m": max( + [r["robot_post_penetration_m"] for r in rows], default=None + ), + "hold_winding_drift_deg": ( + float(np.ptp([r["winding_deg"] for r in hold])) if hold else None + ), + "hold_end_drift_m": ( + float( + np.max( + np.linalg.norm( + np.asarray([r["free_end_m"] for r in hold]) + - hold[0]["free_end_m"], + axis=1, + ) + ) + ) + if hold + else None + ), + "joint_gap_max_m": max([r["joint_gap_max_m"] for r in rows], default=None), + "bilateral_contact_fraction": ( + float(np.mean([r["bilateral_rope_contact"] for r in grasped])) + if grasped + else None + ), + }, + "thresholds": thresholds, + } diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_motion.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_motion.py new file mode 100644 index 0000000000..6408d6f556 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_motion.py @@ -0,0 +1,334 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Plan a floor pickup and a continuous, fixed-wrist cable wrap around a post.""" + +from __future__ import annotations + +from dataclasses import dataclass +import json +from pathlib import Path +from typing import Any +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.optimize import least_squares +from scipy.spatial.transform import Rotation, Slerp +import torch + +__all__ = ["PHASE_END_S", "WrapMotion", "plan_wrap_motion"] + +_JOINT_NAMES = tuple(f"fr3_joint{i}" for i in range(1, 8)) + ( + "fr3_finger_joint1", + "fr3_finger_joint2", +) + +PHASE_END_S = { + "home": 1.0, + "approach": 3.0, + "descend": 4.0, + "close": 5.0, + "lift": 7.0, + "approach_post": 10.0, + "orbit": 28.0, + "tighten": 32.0, + "settle": 34.0, + "hold_closed": 36.0, +} + + +@dataclass(frozen=True) +class WrapMotion: + """Time samples including the home reference at t=0, before the first action.""" + + positions: np.ndarray + poses: np.ndarray + labels: list[str] + times: np.ndarray + report: dict[str, Any] + joint_names: tuple[str, ...] + + def phase_slice(self, phase: str) -> slice: + """Select commands (start, stop] exactly once on the global control grid.""" + scale = self.report["duration_s"] / PHASE_END_S["hold_closed"] + previous_end = 0 + for name, nominal_end in PHASE_END_S.items(): + end = int( + np.searchsorted(self.times, nominal_end * scale + 1e-9, side="right") + ) + if phase == name: + return slice(max(1, previous_end), end) + previous_end = end + raise ValueError(f"Unknown wrap phase {phase!r}.") + + def write(self, out: Path) -> None: + """Export the checked plan for inspection or comparison against a recording.""" + out.mkdir(parents=True, exist_ok=True) + (out / "motion_validation.json").write_text( + json.dumps(self.report, indent=2) + "\n" + ) + np.savez_compressed( + out / "robot_motion.npz", + times=self.times, + joint_positions=self.positions, + joint_names=self.joint_names, + tcp_poses=self.poses, + phases=self.labels, + ) + + +def plan_wrap_motion(config: dict[str, Any], robot_path: Path) -> WrapMotion: + """Sample the complete TCP trajectory, solve seeded IK and verify FK and joint limits.""" + import pytorch_kinematics as pk + + arm_names = list(_JOINT_NAMES[:7]) + floor = float(config.get("floor_height_m", 0.002)) + fps = int(config.get("control_hz", 60)) + duration = float(config.get("duration_s", 36.0)) + if duration < 36.0: + raise ValueError( + "At least 36 seconds are required, including a two-second hold" + ) + scale = duration / PHASE_END_S["hold_closed"] + radius = float(config.get("orbit_radius_m", 0.12)) + wrap_degrees = float(config.get("orbit_degrees", 450.0)) + direction = int(config.get("orbit_direction", 1)) + if direction not in (-1, 1): + raise ValueError("orbit_direction must be +1 (CCW) or -1 (CW)") + theta_start = np.deg2rad(float(config.get("orbit_start_deg", -90.0))) + theta_end = theta_start + direction * np.deg2rad(wrap_degrees) + center = np.asarray(config["post_center_xy_m"], dtype=float) + pickup_end = np.asarray(config["cable_waypoints_m"][-1], dtype=float) + grasp_offset = np.asarray( + config.get("grasp_offset_world_m", [-0.020, 0, 0]), dtype=float + ) + z_start = float(config.get("orbit_start_z_m", floor + 0.055)) + z_end = float(config.get("orbit_end_z_m", floor + 0.075)) + lift_height = float(config.get("pickup_lift_m", 0.09)) + radial_pull = float(config.get("tighten_radial_m", 0.018)) + tangent_pull = float(config.get("tighten_tangent_m", 0.020)) + open_gap = float(config.get("open_half_gap_m", 0.04)) + closed_gap = float(config.get("grip_half_gap_m", config["cable_radius_m"] - 0.0005)) + if not 0 < closed_gap < open_gap <= 0.04: + raise ValueError("Invalid Franka gripper half gaps") + hand_r = Rotation.from_euler( + "z", float(config.get("grasp_yaw_deg", 0.0)), degrees=True + ).as_matrix() @ np.diag([1.0, -1.0, -1.0]) + chain = pk.build_serial_chain_from_urdf(robot_path.read_bytes(), "fr3_hand_tcp").to( + dtype=torch.float64, device="cpu" + ) + if chain.get_joint_parameter_names() != arm_names: + raise ValueError("Unexpected Franka arm joint order") + limits = np.asarray(chain.get_joint_limits(), dtype=float).T + lo, hi = limits[:, 0] + 1e-5, limits[:, 1] - 1e-5 + home = np.asarray(config["home_qpos"], dtype=float) + + def pose(p: np.ndarray, rotation: np.ndarray = hand_r) -> np.ndarray: + result = np.eye(4) + result[:3, :3], result[:3, 3] = rotation, p + return result + + def fk(q: np.ndarray) -> np.ndarray: + result = ( + chain.forward_kinematics(torch.tensor(q, dtype=torch.float64)) + .get_matrix()[0] + .numpy() + ) + result[2, 3] += floor + return result + + def ik(target: np.ndarray, seed: np.ndarray) -> np.ndarray: + q = seed.copy() + for _ in range(45): + current = fk(q) + dp = target[:3, 3] - current[:3, 3] + dr = Rotation.from_matrix(target[:3, :3] @ current[:3, :3].T).as_rotvec() + if np.linalg.norm(dp) < 2e-7 and np.linalg.norm(dr) < 2e-6: + return q + jac = chain.jacobian(torch.tensor(q, dtype=torch.float64))[0].numpy() + jac[3:] *= 0.15 + delta = jac.T @ np.linalg.solve( + jac @ jac.T + np.eye(6) * 1e-8, np.r_[dp, 0.15 * dr] + ) + q = np.clip( + q + delta * min(1.0, 0.1 / max(np.max(np.abs(delta)), 1e-9)), lo, hi + ) + + def residual(candidate: np.ndarray) -> np.ndarray: + current = fk(candidate) + return np.r_[ + current[:3, 3] - target[:3, 3], + 0.15 + * Rotation.from_matrix(target[:3, :3] @ current[:3, :3].T).as_rotvec(), + ] + + solved = least_squares( + residual, + q, + bounds=(lo, hi), + ftol=1e-12, + xtol=1e-12, + gtol=1e-12, + max_nfev=160, + ) + if np.linalg.norm(residual(solved.x)) > 1e-5: + raise RuntimeError( + f"Unreachable wrap TCP {target[:3, 3]}: {residual(solved.x)}" + ) + return solved.x + + def smooth(u: float) -> float: + u = float(np.clip(u, 0, 1)) + return u**3 * (10 - 15 * u + 6 * u * u) + + def end_at(theta: float, height: float) -> np.ndarray: + return np.r_[center + radius * np.array([np.cos(theta), np.sin(theta)]), height] + + home_pose = fk(home) + pickup = pose(pickup_end + grasp_offset) + above = pickup.copy() + above[2, 3] += lift_height + orbit_start = pose(end_at(theta_start, z_start) + grasp_offset) + orbit_end = pose(end_at(theta_end, z_end) + grasp_offset) + radial = np.array([np.cos(theta_end), np.sin(theta_end), 0.0]) + tangent = direction * np.array([-np.sin(theta_end), np.cos(theta_end), 0.0]) + tightened = orbit_end.copy() + tightened[:3, 3] += radial_pull * radial + tangent_pull * tangent + pickup_keys = [ + (0.0, home_pose, open_gap, "HOME"), + (PHASE_END_S["home"], home_pose, open_gap, "HOME"), + (PHASE_END_S["approach"], above, open_gap, "APPROACH FLOOR END"), + (PHASE_END_S["descend"], pickup, open_gap, "LOWER TO ROPE"), + (PHASE_END_S["close"], pickup, closed_gap, "GRASP ROPE"), + (PHASE_END_S["lift"], above, closed_gap, "LIFT AND ORGANIZE SLACK"), + (PHASE_END_S["approach_post"], orbit_start, closed_gap, "TANGENTIAL APPROACH"), + ] + times = np.arange(round(duration * fps) + 1) / fps + targets, labels, gaps = [], [], [] + orbit_start_clock = PHASE_END_S["approach_post"] + orbit_end_clock = PHASE_END_S["orbit"] + for t in times: + clock = t / scale + if clock <= PHASE_END_S["approach_post"]: + index = min( + max( + int( + np.searchsorted( + [k[0] for k in pickup_keys], clock, side="right" + ) + ) + - 1, + 0, + ), + len(pickup_keys) - 2, + ) + a, b = pickup_keys[index : index + 2] + u = smooth((clock - a[0]) / (b[0] - a[0])) + target = pose( + (1 - u) * a[1][:3, 3] + u * b[1][:3, 3], + Slerp([0, 1], Rotation.from_matrix([a[1][:3, :3], b[1][:3, :3]]))( + [u] + ).as_matrix()[0], + ) + gap, label = (1 - u) * a[2] + u * b[2], b[3] + elif clock <= PHASE_END_S["orbit"]: + u = smooth( + (clock - orbit_start_clock) / (orbit_end_clock - orbit_start_clock) + ) + theta = theta_start + (theta_end - theta_start) * u + target = pose(end_at(theta, (1 - u) * z_start + u * z_end) + grasp_offset) + gap, label = closed_gap, "WRAP ROPE AROUND PILLAR" + elif clock <= PHASE_END_S["tighten"]: + u = smooth( + (clock - orbit_end_clock) / (PHASE_END_S["tighten"] - orbit_end_clock) + ) + target = pose((1 - u) * orbit_end[:3, 3] + u * tightened[:3, 3]) + gap, label = closed_gap, "GENTLE TANGENTIAL TIGHTEN" + elif clock < PHASE_END_S["settle"]: + target, gap, label = tightened.copy(), closed_gap, "SETTLE WRAP" + else: + target, gap, label = tightened.copy(), closed_gap, "HOLD COMPLETED WRAP" + targets.append(target) + gaps.append(gap) + labels.append(label) + targets = np.asarray(targets) + qpath = np.zeros((len(times), 9), dtype=np.float32) + q = home.copy() + for i, target in enumerate(targets): + q = ik(target, q) + qpath[i, :7], qpath[i, 7:] = q, gaps[i] + actual = ( + chain.forward_kinematics(torch.tensor(qpath[:, :7], dtype=torch.float64)) + .get_matrix() + .numpy() + ) + actual[:, 2, 3] += floor + pe = np.linalg.norm(actual[:, :3, 3] - targets[:, :3, 3], axis=1) + re = Rotation.from_matrix( + targets[:, :3, :3] @ actual[:, :3, :3].transpose(0, 2, 1) + ).magnitude() + vmax = np.max(np.abs(np.diff(qpath[:, :7], axis=0) * fps), axis=0) + root = ET.parse(robot_path).getroot() + velocity_limits = np.array( + [ + float(root.find(f"joint[@name='{name}']/limit").get("velocity")) + for name in arm_names + ] + ) + joint_limits_ok = bool( + np.all(qpath[:, :7] >= lo - 1e-6) and np.all(qpath[:, :7] <= hi + 1e-6) + ) + passed = bool( + pe.max() < 1e-5 + and re.max() < 1e-4 + and np.all(vmax <= velocity_limits) + and joint_limits_ok + ) + report = { + "passed": passed, + "ik_position_max_m": float(pe.max()), + "ik_rotation_max_rad": float(re.max()), + "joint_velocity_max_rad_s": vmax.tolist(), + "joint_velocity_limits_rad_s": velocity_limits.tolist(), + "joint_limits_ok": joint_limits_ok, + "minimum_joint_limit_margin_rad": float( + np.min(np.minimum(qpath[:, :7] - lo, hi - qpath[:, :7])) + ), + "duration_s": duration, + "hold_start_s": PHASE_END_S["settle"] * scale, + "hold_duration_s": (PHASE_END_S["hold_closed"] - PHASE_END_S["settle"]) * scale, + "orbit_start_s": orbit_start_clock * scale, + "orbit_end_s": orbit_end_clock * scale, + "orbit_degrees": wrap_degrees, + "orbit_direction": direction, + "orbit_radius_m": radius, + "orbit_start_z_m": z_start, + "orbit_end_z_m": z_end, + "grasp_offset_world_m": grasp_offset.tolist(), + "grasp_yaw_deg": float(config.get("grasp_yaw_deg", 0.0)), + "grip_half_gap_m": closed_gap, + "final_end_target_m": (tightened[:3, 3] - grasp_offset).tolist(), + "physical_success_requires_actual_centerline_and_contacts": True, + } + if not passed: + raise RuntimeError(f"Wrap motion validation failed: {report}") + return WrapMotion( + qpath, + targets, + labels, + times, + report, + _JOINT_NAMES, + ) diff --git a/tests/gym/envs/tasks/test_franka_rope_wrap.py b/tests/gym/envs/tasks/test_franka_rope_wrap.py new file mode 100644 index 0000000000..64bf397a01 --- /dev/null +++ b/tests/gym/envs/tasks/test_franka_rope_wrap.py @@ -0,0 +1,143 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""CPU regressions for the Franka task's timed acceptance and contact evidence.""" + +from __future__ import annotations + +import json +from types import SimpleNamespace +from unittest.mock import Mock + +import numpy as np +import pytest +import torch + +from embodichain_tasks.manipulation.cable.franka_rope_wrap import load_config +from embodichain_tasks.manipulation.cable.wrap_evaluation import attach_wrap_monitor +from embodichain_tasks.manipulation.cable.wrap_measurements import evaluate_wrap + +pytestmark = pytest.mark.no_sim + + +def test_scaled_pickup_and_final_hold_are_evaluated_at_their_measured_times(tmp_path): + path = tmp_path / "72s.json" + path.write_text(json.dumps({**load_config(), "duration_s": 72.0})) + config = load_config(path) + # At 2x duration the grasp finishes at 10 s, the lift at 14 s, and + # the final two seconds of the measured hold start at 70 s. + rows = [] + for frame in range(72 * 60 + 1): + t = frame / 60 + rows.append( + dict( + time_s=t, + finite=True, + arm_joint_limit_violation_rad=0, + finger_joint_limit_violation_m=0, + free_end_m=[0, 0, 0.06 if t >= 14 else 0.006], + grasp_confirmed=t >= 10, + bilateral_rope_contact=t >= 10, + tail_tcp_distance_m=0.025, + material_slip_m=0, + winding_deg=420 if t >= 70 else 0, + contact_arc_deg=330 if t >= 70 else 0, + rope_max_z_m=0.08, + rope_min_z_m=0.002, + post_penetration_m=0, + robot_post_penetration_m=0, + anchor_direction_error_deg=0, + anchor_exit_bend_max_deg=0, + anchor_error_m=0, + joint_gap_max_m=0, + dropped_contacts=0, + ) + ) + + report = evaluate_wrap(rows, config) + assert len(report["checks"]) == 19 and report["passed"] + partial = evaluate_wrap(rows[:-1], config) + assert not partial["checks"]["completed"] and not partial["passed"] + + # A bad measurement at the first hold frame cannot be hidden by recovery. + rows[70 * 60]["contact_arc_deg"] = 295 + failed = evaluate_wrap(rows, config) + assert {name for name, ok in failed["checks"].items() if not ok} == { + "post_contact_arc" + } + + +def test_grasp_requires_both_fingers_on_rope_in_the_same_sample(tmp_path): + config = {**load_config(), "grasp_complete_s": 0, "finger_pad_center_m": [0, 0, 0]} + env = Mock(num_envs=1, _wrap_monitor=None) + cable = env.sim.get_cable_object.return_value + points = np.zeros((9, 3)) + points[:, 2] = np.arange(9) * 0.01 + cable.cfg.points = points + segments = torch.zeros(1, 8, 13, dtype=torch.float64) + segments[0, :, :3] = torch.from_numpy((points[1:] + points[:-1]) / 2) + segments[0, :, 6] = 1 # identity quaternion; capsule axes point up + cable.data.segment_state_w = segments + tcp = torch.eye(4, dtype=torch.float64)[None] + tcp[0, :3, 3] = segments[0, -1, :3] + env.robot.get_link_pose.return_value = tcp + env.robot.get_qpos.return_value = torch.zeros(1, 9) + env.robot.joint_names = [f"j{i}" for i in range(9)] + urdf = tmp_path / "limits.urdf" + urdf.write_text( + "" + + "".join( + f'' + for name in env.robot.joint_names + ) + + "" + ) + motion = SimpleNamespace( + poses=np.tile(np.eye(4), (4, 1, 1)), + labels=["grasp"] * 4, + times=np.arange(4) / 60, + ) + sensor = env.sim.get_sensor.return_value + sensor.latest_sample_dropped_contacts = 0 + names = ["fr3_leftfinger", "fr3_rightfinger"] + sensor.get_actor_info.side_effect = lambda i: SimpleNamespace( + path="/franka", link_name=names[i] + ) + monitor = attach_wrap_monitor(env, config, motion, urdf) + + def advance(contacts): + monitor.begin_control_step() + for substep in range(1, 33): + fingers = contacts.get(substep, []) + sensor.get_data.return_value = { + "user_ids": torch.tensor( + [[i, -1] for i in fingers], dtype=torch.long + ).reshape(-1, 2), + "distance": torch.zeros(len(fingers)), + "position": torch.tensor([[0.004, 0.0, 0.075]]).repeat(len(fingers), 1), + "is_valid": torch.ones(len(fingers), dtype=torch.bool), + } + monitor.after_sample(1 / 1920, None) + return monitor.rows[-1] + + # Between samples both touch, but on the 240 Hz sample grid their + # contacts alternate. The fingers must not jointly confirm this grasp. + split = advance({3: [0, 1], 8: [0], 16: [1]}) + assert split["left_rope_contact"] and split["right_rope_contact"] + assert not split["bilateral_rope_contact"] and not split["grasp_confirmed"] + together = advance({24: [0, 1]}) + assert together["bilateral_rope_contact"] and together["grasp_confirmed"] + assert sensor.get_data.call_count == 8 diff --git a/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py b/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py new file mode 100644 index 0000000000..4da3d29b82 --- /dev/null +++ b/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py @@ -0,0 +1,83 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Check that the packaged scene and the custom-scene deployment share one geometry.""" + +from __future__ import annotations + +import json +from pathlib import Path + +import numpy as np +import yaml + +from embodichain_tasks.manipulation.cable.franka_rope_wrap import ( + load_config, + materialize_deployment, + task_directory, +) + + +def test_default_deployment_matches_materialized_scene(tmp_path: Path) -> None: + root = task_directory() + defaults = load_config() + urdf = tmp_path / "derived.urdf" + alternate = materialize_deployment(defaults, urdf, tmp_path) + stored = yaml.safe_load((root / "env.yaml").read_text()) + compiled = yaml.safe_load((alternate.parent / "env.yaml").read_text()) + event = compiled["env"]["events"]["settle_rope"] + assert load_config(Path(event["params"].pop("scenario_path"))) == defaults + assert compiled == stored + assert compiled["env"]["sim_steps_per_control"] == 32 + assert compiled["physics_config"]["num_substeps"] == 1 + np.testing.assert_allclose(compiled["physics_config"]["physics_dt"] * 32, 1 / 60) + cable = compiled["simulation"]["cable_object"][0] + points = np.asarray(cable["points"]) + assert points.shape == (97, 3) + np.testing.assert_allclose( + points[[0, -1]], np.asarray(defaults["cable_waypoints_m"])[[0, -1]], atol=1e-7 + ) + delta = np.r_[defaults["post_center_xy_m"], points[0, 2]] - points[0] + np.testing.assert_allclose( + (points[1] - points[0]) / np.linalg.norm(points[1] - points[0]), + delta / np.linalg.norm(delta), + atol=1e-6, + ) + assert cable["fixed_segments"] == [0] + assert cable["collision_filter_span"] == 2 + + +def test_custom_scene_rescales_windows_and_uses_requested_assets( + tmp_path: Path, +) -> None: + scenario = load_config() + scenario.update(duration_s=72.0, floor_height_m=0.013, cable_segments=64) + custom = tmp_path / "custom.json" + custom.write_text(json.dumps(scenario)) + effective = load_config(custom) + assert (effective["grasp_complete_s"], effective["lift_complete_s"]) == (10.0, 14.0) + urdf = tmp_path / "padded_franka.urdf" + deployment = materialize_deployment(effective, urdf, tmp_path) + env = yaml.safe_load(deployment.with_name("env.yaml").read_text()) + cylinder = next( + body for body in env["simulation"]["background"] if body["uid"] == "post" + ) + assert cylinder["init_pos"] == [0.48, 0.0, 0.073] + rope = env["simulation"]["cable_object"][0] + assert len(rope["points"]) == 65 + assert min(p[2] for p in rope["points"]) >= 0.017 - 1e-8 + embodiment = yaml.safe_load(deployment.with_name("embodiment.yaml").read_text()) + assert embodiment["simulation"]["fpath"] == str(urdf) + assert embodiment["simulation"]["init_pos"] == [0, 0, 0.013] diff --git a/tests/sim/atomic_skills/test_actions.py b/tests/sim/atomic_skills/test_actions.py index b9d967723e..910ada00b6 100644 --- a/tests/sim/atomic_skills/test_actions.py +++ b/tests/sim/atomic_skills/test_actions.py @@ -9969,3 +9969,45 @@ def test_presampled_joint_commands_cannot_bypass_motion_gen_backend(): ) with pytest.raises(ValueError, match="requires strategy"): _plan_action(action, invocation, _context()) + + +@pytest.mark.parametrize("preserve", [False, True]) +def test_timed_joint_trajectory_can_preserve_explicit_phase_boundary_velocities( + preserve: bool, +): + from embodichain.lab.sim.atomic_skills.policies import DynamicCollisionMode + + generator = _motion_generator() + generator.robot.control_parts["whole_robot"] = object() + action = _bind_action(generator, MoveJoints()) + context = _context() + position = ( + torch.tensor([0.003, 0.006, 0.009]) + .view(1, 3, 1) + .expand(NUM_ENVS, -1, ROBOT_DOF) + ) + velocity = torch.full_like(position, 0.18) + goal = JointPositionGoal( + TimedTrajectory( + positions=position, + velocities=velocity, + accelerations=None, + dt=torch.full((NUM_ENVS, 3), CONTROL_DT), + env_ids=context.env_ids, + ) + ) + plan = _plan_action( + action, + SkillInvocation( + skill_id="move_joints", + binding=_binding(action, motion="whole_robot"), + goal=goal, + motion_policy=MotionPolicy(dynamic_collision_mode=DynamicCollisionMode.OFF), + skill_options=MoveJointsOptions(preserve_velocity_samples=preserve), + ), + context, + ) + expected = velocity.clone() + if not preserve: + expected[:, 0] = expected[:, -1] = 0 + torch.testing.assert_close(_joint_trajectory(plan).velocities, expected) From 48d803c610fa402432a49226e3c90ef897c3b2a2 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 16 Nov 2026 15:00:09 +0800 Subject: [PATCH 35/43] fix(task): honor wrap phase durations and diagnostic step limits --- embodichain/lab/gym/envs/demo.py | 70 ++++++++++++------- .../lab/gym/envs/task_program/bridge.py | 9 ++- examples/sim/demo/franka_rope_wrap/README.md | 13 +++- .../sim/demo/franka_rope_wrap/__main__.py | 13 +--- .../manipulation/cable/franka_rope_wrap.py | 15 ++++ tests/gym/envs/task_program/test_bridge.py | 23 ++++++ .../envs/tasks/test_franka_rope_wrap_scene.py | 5 ++ tests/gym/envs/test_demo.py | 11 +++ 8 files changed, 120 insertions(+), 39 deletions(-) diff --git a/embodichain/lab/gym/envs/demo.py b/embodichain/lab/gym/envs/demo.py index d898c20fa7..e09247d48b 100644 --- a/embodichain/lab/gym/envs/demo.py +++ b/embodichain/lab/gym/envs/demo.py @@ -387,7 +387,7 @@ def _segment_outcome_kind( return "not_attempted" if success: return "succeeded" - if failure_reason == "truncated": + if failure_reason in {"truncated", DEMO_STEP_LIMIT}: return "truncated" if failure_reason in {"interrupted", "batch_aborted", "empty_segment"}: return "cancelled" @@ -805,6 +805,7 @@ def to_metadata(self) -> dict[str, Any]: ProgressWrapper = Callable[[Iterable[Any], str], Iterable[Any]] +DEMO_STEP_LIMIT = "simulation_step_limit" StopPredicate = Callable[[], bool] @@ -1083,6 +1084,7 @@ def execute_demo_episode( attempt_id: int = 0, instruction: str | None = None, should_stop: StopPredicate | None = None, + max_steps: int | None = None, progress: ProgressWrapper | None = None, segments: Iterable[DemoSegment] | None = None, active_env_ids: tuple[int, ...] | None = None, @@ -1106,6 +1108,12 @@ def execute_demo_episode( attempt_id: Zero-based identifier for this collection attempt. instruction: Optional overall language override bound to this episode. should_stop: Optional callback checked before every action. + max_steps: Optional hard limit on simulation steps for a partial episode. + An episode that completes exactly at the limit retains its result. + Requesting another physical step truncates the active rows. Reset + or close the environment after this truncation. + Ordinary cancellation keeps its physical safe-stop actions while + simulation steps are available. progress: Optional wrapper such as ``tqdm`` for action iterables. segments: Optional trusted segment iterable. When provided, bypasses task planner selection while retaining ordinary segment validation, @@ -1128,6 +1136,8 @@ def execute_demo_episode( execution_cfg = DemoExecutionCfg() elif not isinstance(execution_cfg, DemoExecutionCfg): raise TypeError("execution_cfg must be a DemoExecutionCfg or None.") + if max_steps is not None and (type(max_steps) is not int or max_steps < 0): + raise ValueError("max_steps must be a non-negative integer or None.") if attempt_id < 0: raise ValueError("attempt_id must be non-negative.") @@ -1193,6 +1203,32 @@ def publish_active_mask() -> None: recording_started = False outcome_published = False + physical_steps = 0 + + def budget_exhausted() -> bool: + return max_steps is not None and physical_steps >= max_steps + + def stop_requested( + failures: list[str | None] | None = None, *, check_step_limit: bool + ) -> bool: + nonlocal fatal_reason + if check_step_limit and budget_exhausted(): + fatal_reason = DEMO_STEP_LIMIT + elif should_stop is not None and should_stop(): + fatal_reason = "interrupted" + else: + return False + for env_id, is_active in enumerate(active): + if is_active: + terminal_reasons[env_id] = fatal_reason + if failures is not None: + failures[env_id] = fatal_reason + if fatal_reason == DEMO_STEP_LIMIT: + truncated[env_id] = True + active[env_id] = False + publish_active_mask() + return True + def capture_frozen_rows(before: tuple[bool, ...]) -> None: if capture_terminal_physical is not None and ( "task_outcome" in last_info or "physical_objective" in last_info @@ -1250,13 +1286,7 @@ def episode_result( else _validated_demo_segments(env, segments, **plan_kwargs) ) while any(active): - if should_stop is not None and should_stop(): - fatal_reason = "interrupted" - for env_id, is_active in enumerate(active): - if is_active: - terminal_reasons[env_id] = fatal_reason - active[env_id] = False - publish_active_mask() + if stop_requested(check_step_limit=max_steps == 0): break try: segment = next(segment_iterator) @@ -1307,16 +1337,9 @@ def episode_result( segment_reason = "action_generation_failed" break last_action_consumed = False - if should_stop is not None and should_stop(): + if stop_requested(segment_failure_reasons, check_step_limit=True): actions_exhausted = False - fatal_reason = "interrupted" segment_reason = fatal_reason - for env_id, is_active in enumerate(active): - if is_active: - terminal_reasons[env_id] = fatal_reason - segment_failure_reasons[env_id] = fatal_reason - active[env_id] = False - publish_active_mask() break try: @@ -1341,6 +1364,7 @@ def episode_result( observation, _, terminated_value, truncated_value, info = env.step( action ) + physical_steps += 1 last_action_consumed = True if step_observer is not None: step_observer(active_before_step, action, observation) @@ -1430,16 +1454,11 @@ def episode_result( # and do not request another lazy segment. actions_exhausted = False break - if should_stop is not None and should_stop(): + # Allow the lazy action iterator and final validators to finish at + # the exact cap. A subsequent action is checked before env.step. + if stop_requested(segment_failure_reasons, check_step_limit=False): actions_exhausted = False - fatal_reason = "interrupted" segment_reason = fatal_reason - for env_id, is_active in enumerate(active): - if is_active: - terminal_reasons[env_id] = fatal_reason - segment_failure_reasons[env_id] = fatal_reason - active[env_id] = False - publish_active_mask() break if not actions_exhausted: @@ -1461,6 +1480,8 @@ def episode_result( emergency_iterator = iter(emergency_actions) try: for emergency_action in emergency_iterator: + if budget_exhausted(): + break if normalize_action is not None: emergency_action = normalize_action( emergency_action @@ -1469,6 +1490,7 @@ def episode_result( emergency_observation, _, _, _, emergency_info = ( env.step(emergency_action) ) + physical_steps += 1 if step_observer is not None: step_observer( tuple(participants), diff --git a/embodichain/lab/gym/envs/task_program/bridge.py b/embodichain/lab/gym/envs/task_program/bridge.py index 04e160c9f0..e7228184d7 100644 --- a/embodichain/lab/gym/envs/task_program/bridge.py +++ b/embodichain/lab/gym/envs/task_program/bridge.py @@ -36,7 +36,7 @@ from tensordict import TensorDict from embodichain.lab.gym.envs._json import json_safe_copy as _json_safe_copy -from embodichain.lab.gym.envs.demo import DemoSegment +from embodichain.lab.gym.envs.demo import DEMO_STEP_LIMIT, DemoSegment from embodichain.lab.gym.envs.expert_trajectory import JointCommandMode from embodichain.lab.gym.envs.types import ControllerAction from ._segment_contract import _check_outcomes, _check_prerequisites, _unrun_outcomes @@ -2151,6 +2151,13 @@ def _abort_segment( result = _validate_runtime_result(runtime.cancel(reason)) self._record_runtime_result(lifecycle, result) + if reason == DEMO_STEP_LIMIT: + # The partial episode ends at its requested physical frame. Cancel + # the controller without emitting or acknowledging an unstepped hold. + self._sink.discard_pending() + lifecycle.abort_complete = True + return + pending_kind = ( None if pending is None else pending.metadata.get("bridge_action_kind") ) diff --git a/examples/sim/demo/franka_rope_wrap/README.md b/examples/sim/demo/franka_rope_wrap/README.md index 63c472dfdd..7557e0170d 100644 --- a/examples/sim/demo/franka_rope_wrap/README.md +++ b/examples/sim/demo/franka_rope_wrap/README.md @@ -86,9 +86,16 @@ python -m embodichain run-example sim/demo/franka_rope_wrap \ --plan-only --output /tmp/franka-rope-wrap-plan ``` -`--seconds 2` truncates execution for a smoke check. It records `partial`, keeps -`passed=false` and exits with code 1; it does not qualify the full task. A full -run exits with code 0 only when every physical acceptance check passes. +`--seconds` limits how many complete control intervals the diagnostic can execute. +At 60 Hz, `--seconds 35.999` allows 2,159 commands and records 2,160 states +including the settled initial frame. A truncated run writes `partial` and +`passed=false` and exits with code 1. A complete run exits with code 0 when +the Task Program and all physical checks pass. + +Changing `duration_s` rescales the motion stages and their observation +windows. For example, at 72 seconds the fingers close by 10 s, the lift ends +at 14 s, and the 465° orbit runs from 20 s through 56 s. The stability +measurement still uses the last two seconds of recorded states. `--plan-only` exits with code 0 after a valid plan and records `planned`. ## Results and acceptance diff --git a/examples/sim/demo/franka_rope_wrap/__main__.py b/examples/sim/demo/franka_rope_wrap/__main__.py index 12b5e5188e..5652f667ee 100644 --- a/examples/sim/demo/franka_rope_wrap/__main__.py +++ b/examples/sim/demo/franka_rope_wrap/__main__.py @@ -167,22 +167,13 @@ def _run(deployment: Path, config: dict, out: Path, seconds: float | None) -> di cfg, gym_config, actions = _deployment_config(deployment) env = gymnasium.make(gym_config["id"], cfg=cfg, **actions) target = env.unwrapped - steps = 0 limit = None if seconds is not None and seconds < config["duration_s"]: limit = int(Decimal(str(seconds)) * config["control_hz"]) - def observed_step(*_) -> None: - nonlocal steps - steps += 1 - try: env.reset(options={"save_data": False}) - result = execute_demo_episode( - env, - should_stop=lambda: limit is not None and steps >= limit, - step_observer=observed_step, - ) + result = execute_demo_episode(env, max_steps=limit) _json(out / "task_program_episode.json", result.to_metadata()) monitor = getattr(target, "_wrap_monitor", None) if monitor is None and limit == 0: @@ -201,7 +192,7 @@ def observed_step(*_) -> None: report["passed"] and result.completed and result.all_success - and steps == len(monitor.motion.times) - 1 + and result.length == len(monitor.motion.times) - 1 and limit is None ) _json(out / "validation.json", report) diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py index 28f18d8199..93410bc5b9 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py @@ -607,6 +607,21 @@ def materialize_deployment(config: dict[str, Any], robot_path: Path, out: Path) for section in ("task_program", "evaluation"): for key, path in deployment[section].items(): deployment[section][key] = str((reference / path).resolve()) + # The orbit is one atomic action. Its timeout uses simulation seconds and + # must cover all its samples, including the boundary acknowledgement. + from embodichain.lab.sim.atomic_skills.policies import RecoveryPolicy + from .wrap_motion import PHASE_END_S + + ends = np.asarray([0.0, *PHASE_END_S.values()]) + longest_phase_s = float(np.diff(ends).max()) * config["duration_s"] / ends[-1] + required_timeout = float(longest_phase_s + 2 / config["control_hz"]) + policy_path = deployment["task_program"]["execution_policy"] + recovery = yaml.safe_load(Path(policy_path).read_text())["recovery"] + if required_timeout > recovery.get("skill_timeout", RecoveryPolicy().skill_timeout): + deployment["task_program"]["execution_policy"] = { + "component": policy_path, + "overrides": {"recovery": {"skill_timeout": required_timeout}}, + } deployment["embodiment"]["component"] = "embodiment.yaml" for name, values in [ ("env.yaml", environment), diff --git a/tests/gym/envs/task_program/test_bridge.py b/tests/gym/envs/task_program/test_bridge.py index 3043729f58..dbf1db412f 100644 --- a/tests/gym/envs/task_program/test_bridge.py +++ b/tests/gym/envs/task_program/test_bridge.py @@ -2027,6 +2027,29 @@ def test_demo_executor_pre_step_stop_consumes_only_abort_hold() -> None: assert runtime.sink.pending_count == 0 +@pytest.mark.parametrize("max_steps", [0, 1]) +def test_simulation_step_limit_physically_truncates_and_does_not_advance_the_clock( + max_steps: int, +) -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + env = _BridgeExecutorEnv(bridge) + + result = execute_demo_episode(env, max_steps=max_steps) + + assert result.terminal_reason == "simulation_step_limit" + assert result.truncated == (True, True) + assert not result.completed + assert result.length == clock.step_index == max_steps + assert [action.metadata["bridge_action_kind"] for action in env.steps] == [ + "runtime_command" + ] * max_steps + assert runtime.cancel_count == (1 if max_steps else 0) + assert runtime.sink.pending_count == 0 + if max_steps: + assert result.segments[-1].metadata["runtime"]["status"] == "cancelled" + assert bridge.execution_finished + + def test_demo_executor_post_step_failure_acknowledges_then_safe_stops() -> None: bridge, runtime, clock = _bridge(duration=2 * STEP_DT) env = _BridgeExecutorEnv( diff --git a/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py b/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py index 4da3d29b82..48ef27ebb5 100644 --- a/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py +++ b/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py @@ -70,6 +70,11 @@ def test_custom_scene_rescales_windows_and_uses_requested_assets( assert (effective["grasp_complete_s"], effective["lift_complete_s"]) == (10.0, 14.0) urdf = tmp_path / "padded_franka.urdf" deployment = materialize_deployment(effective, urdf, tmp_path) + policy = yaml.safe_load(deployment.read_text())["task_program"]["execution_policy"] + # 20–56 s is a single continuous orbit, plus two control ticks to acknowledge it. + np.testing.assert_allclose( + policy["overrides"]["recovery"]["skill_timeout"], 36 + 2 / 60 + ) env = yaml.safe_load(deployment.with_name("env.yaml").read_text()) cylinder = next( body for body in env["simulation"]["background"] if body["uid"] == "post" diff --git a/tests/gym/envs/test_demo.py b/tests/gym/envs/test_demo.py index 9e31a9e5a9..e071b2a99d 100644 --- a/tests/gym/envs/test_demo.py +++ b/tests/gym/envs/test_demo.py @@ -578,6 +578,17 @@ def _end_demo_segment_recording(self, result) -> None: self.segment_results.append(result) +def test_exact_step_limit_keeps_naturally_completed_episode() -> None: + env = _SegmentedEnv() + + result = execute_demo_episode(env, max_steps=3) + + assert env.actions == [1, 2, 3] + assert result.completed and result.all_success + assert result.truncated == (False,) + assert result.terminal_reason == "success" + + def test_execute_demo_episode_runs_lazy_segments_as_one_episode() -> None: """A task can plan the second object after the first segment executes.""" env = _SegmentedEnv() From b6ab656b88434e43d4cad357eb7e63d70f629abf Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 16 Nov 2026 16:24:23 +0800 Subject: [PATCH 36/43] Bind task cables from USD scene inventory and ship their assets --- embodichain/lab/gym/utils/_component_composition.py | 1 + setup.py | 1 + 2 files changed, 2 insertions(+) diff --git a/embodichain/lab/gym/utils/_component_composition.py b/embodichain/lab/gym/utils/_component_composition.py index b104c0555c..e6c7f95d31 100644 --- a/embodichain/lab/gym/utils/_component_composition.py +++ b/embodichain/lab/gym/utils/_component_composition.py @@ -455,6 +455,7 @@ def _validate_scene_binding_targets( "USD scene binding validation requires its preflight inventory." ) for binding_field, kind in ( + ("cables", "cable"), ("rigid_objects", "rigid_object"), ("articulations", "articulation"), ("rigidized_articulations", "articulation"), diff --git a/setup.py b/setup.py index d2b5b24d76..b543867a1f 100644 --- a/setup.py +++ b/setup.py @@ -172,6 +172,7 @@ def get_package_data() -> dict[str, list[str]]: **{name: list(extension_patterns) for name in _extension_package_dirs()}, "embodichain_tasks.configs": [ "tasks/manipulation/cloth_fold/task_program/trajectory.npz", + "**/*.usda", "**/*.json", "**/*.yaml", "**/*.yml", From f2be8ea7d0eff79f54d4ebcb18f3fbb71e129517 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 16 Nov 2026 16:24:30 +0800 Subject: [PATCH 37/43] feat(sim): support sparse physics overrides on USD cables --- .../topics/simulation-system/deformables.md | 5 +- embodichain/lab/sim/cfg/usd.py | 5 +- embodichain/lab/sim/spawn/descriptors.py | 30 ++-- embodichain/lab/sim/spawn/usd_import.py | 128 +++++++++++++----- tests/sim/spawn/test_usd_scene_overrides.py | 68 ++++++++++ 5 files changed, 185 insertions(+), 51 deletions(-) diff --git a/agent_context/topics/simulation-system/deformables.md b/agent_context/topics/simulation-system/deformables.md index f59bbc9339..db52a5268f 100644 --- a/agent_context/topics/simulation-system/deformables.md +++ b/agent_context/topics/simulation-system/deformables.md @@ -111,7 +111,10 @@ deformables. `SimulationManager.add_cable_object()` lowers through and binds the same facade in place during `prepare()`. `add_usd(UsdImportCfg)` also returns a `CableObject` per imported cable under its source resource key, with the same UID lookup, segment state and reset as a procedural declaration. -The parsed `CableDesc` remains the source of cable physics and visuals. +Cable geometry, placement and visuals come from the parsed `CableDesc`. +A sparse `asset_cfgs` entry keyed by the simulation curve prim can override +the UID, `fixed_segments`, `collision_filter_span` and individual `attrs` physics and +contact fields before declaration. Omitted fields retain the parsed values. Cable declarations require DexUni on CUDA with gradient mode disabled. With this backend, an arena containing a cable cannot also contain fluid, cloth or soft-body particles: cable episode reset does not support those diff --git a/embodichain/lab/sim/cfg/usd.py b/embodichain/lab/sim/cfg/usd.py index 72a9b6e7fd..41ee08a679 100644 --- a/embodichain/lab/sim/cfg/usd.py +++ b/embodichain/lab/sim/cfg/usd.py @@ -74,7 +74,10 @@ class UsdImportCfg: In sparse mappings with ``auto`` placement, explicit ``init_pos`` or ``init_rot`` keys select the configured pose, including zero values. - Omitted pose fields retain source placement. + Omitted pose fields retain source placement. For cables, sparse mappings + accept ``uid``, ``fixed_segments``, ``collision_filter_span`` and fields of + ``CablePhysicsCfg`` under ``attrs``. Overrides apply to all curves on + the targeted prim, preserving its authored geometry and placement. """ robot_cfgs: dict[str, RobotCfg] = field(default_factory=dict) diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index 6e78c429d9..30b3a410fd 100644 --- a/embodichain/lab/sim/spawn/descriptors.py +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -809,6 +809,22 @@ def _explicit_tet_mesh_from_cfg( } +def _cable_contact_spec(attrs: CablePhysicsCfg) -> _RigidPhysicsSpec: + if not isinstance(attrs, CablePhysicsCfg): + raise TypeError("Cable attrs must be CablePhysicsCfg.") + if isinstance(attrs.collision_props, DefaultCollisionPropertiesCfg): + raise NotImplementedError( + "Cable contact properties require common or Newton configuration." + ) + return _resolve_rigid_physics( + RigidBodyPhysicsCfg( + collision_props=attrs.collision_props, + material_props=attrs.material_props, + ), + newton_solver_type="dexuni", + ) + + def cable_desc_from_cfg( cfg: CableObjectCfg, *, @@ -835,19 +851,7 @@ def cable_desc_from_cfg( ) from exc cfg.validate() uid = _required_uid(cfg.uid, "Cable object") - if not isinstance(cfg.attrs, CablePhysicsCfg): - raise TypeError("Cable attrs must be CablePhysicsCfg.") - if isinstance(cfg.attrs.collision_props, DefaultCollisionPropertiesCfg): - raise NotImplementedError( - "Cable contact properties require common or Newton configuration." - ) - contact = _resolve_rigid_physics( - RigidBodyPhysicsCfg( - collision_props=cfg.attrs.collision_props, - material_props=cfg.attrs.material_props, - ), - newton_solver_type="dexuni", - ) + contact = _cable_contact_spec(cfg.attrs) collision = _compile_newton_collision( contact, newton_solver_type="dexuni", author_shape_defaults=True ) diff --git a/embodichain/lab/sim/spawn/usd_import.py b/embodichain/lab/sim/spawn/usd_import.py index 24761bd7e4..6731eb18e8 100644 --- a/embodichain/lab/sim/spawn/usd_import.py +++ b/embodichain/lab/sim/spawn/usd_import.py @@ -19,8 +19,9 @@ from __future__ import annotations from collections import Counter +from collections.abc import Mapping from copy import deepcopy -from dataclasses import dataclass, field, fields +from dataclasses import dataclass, field, fields, replace from typing import TYPE_CHECKING, Any import numpy as np @@ -59,10 +60,17 @@ ) from embodichain.lab.sim.sensors import Camera, CameraCfg, StereoCamera, StereoCameraCfg from embodichain.lab.sim.sensors.registry import _resolve_sensor_factory -from .descriptors import _pose_from_cfg, configure_articulation_desc +from .descriptors import ( + _cable_contact_spec, + _compile_newton_collision, + _pose_from_cfg, + _required_uid, + configure_articulation_desc, +) from .usd import ( _configure_articulation_usd, _configure_rigid_usd, + _overlay_optional_properties, _parse_usd_source, _source_pose, _set_resolved_usd_pose, @@ -96,6 +104,67 @@ def _source_path(descriptor: Any) -> str: return usd.prim_path if usd is not None and usd.prim_path else descriptor.name +def _usd_cable_cfg( + descriptor: CableDesc, overrides: Mapping[str, Any] | None +) -> CableObjectCfg: + """Apply sparse cable properties before declaring the descriptor and its facade.""" + if overrides is None: + overrides = {} + if not isinstance(overrides, Mapping): + raise TypeError("USD cable overrides must be a sparse mapping.") + unknown = overrides.keys() - { + "uid", + "attrs", + "fixed_segments", + "collision_filter_span", + } + if unknown: + raise ValueError(f"Unknown USD cable override fields: {sorted(unknown)}.") + attrs = overrides.get("attrs", {}) + if not isinstance(attrs, Mapping): + raise TypeError("USD cable attrs must be a sparse mapping.") + parsed_physics = { + item.name: getattr(descriptor.physics, item.name) + for item in fields(descriptor.physics) + } + physics_cfg = CablePhysicsCfg(**{**parsed_physics, **attrs}) + if overrides: + if "uid" in overrides: + descriptor.name = _required_uid(overrides["uid"], "Cable object") + # Preserve the difference between an omitted coefficient and an explicit zero/None. + descriptor.physics = replace( + descriptor.physics, + **{key: attrs[key] for key in attrs.keys() & parsed_physics.keys()}, + ) + contact = _cable_contact_spec(physics_cfg) + descriptor.collision = _overlay_optional_properties( + descriptor.collision, + _compile_newton_collision(contact, newton_solver_type="dexuni"), + ) + if contact.collision_enabled is not None: + descriptor.enable_collision = contact.collision_enabled + for name in ("fixed_segments", "collision_filter_span"): + if name in overrides: + setattr(descriptor, name, overrides[name]) + # Revalidate indices, attachments and thickness against the source + # geometry before publishing any part of this freshly parsed scene. + descriptor.__post_init__() + + frames = descriptor.segment_quaternions + return CableObjectCfg( + uid=descriptor.name, + points=descriptor.points, + radius=descriptor.radius, + init_local_pose=descriptor.pose, + segment_quaternions=None if frames is None else frames[:, [1, 2, 3, 0]], + closed=descriptor.closed, + fixed_segments=descriptor.fixed_segments, + attrs=physics_cfg, + collision_filter_span=descriptor.collision_filter_span, + render_mode=descriptor.render_mode, + ) + + def _compile_usd_import( manager: SimulationManager | None, recipe: UsdImportCfg, @@ -170,7 +239,12 @@ def _compile_usd_import( if item.usd is not None and item.usd.prim_path else item.name ) - for item in (*source.mesh_objects, *source.articulations, *source.particle_sets) + for item in ( + *source.mesh_objects, + *source.articulations, + *source.particle_sets, + *source.cables, + ) } overlap = robot_cfgs.keys() & asset_cfgs.keys() missing = (robot_cfgs.keys() | asset_cfgs.keys()) - source_paths @@ -258,6 +332,21 @@ def _compile_usd_import( and descriptor.physics is None and not descriptor.collisions ) + if isinstance(descriptor, CableDesc): + if root_pose is not None: + descriptor.pose = root_pose @ descriptor.pose + cfg = _usd_cable_cfg(descriptor, configured) + if descriptor.name in occupied: + raise ValueError(f"USD asset UID already exists: {descriptor.name!r}.") + occupied.add(descriptor.name) + renamed[original_name] = descriptor.name + facade = ( + None if manager is None else CableObject(cfg, device=manager.device) + ) + declarations.append( + (resource_keys[id(descriptor)], "cable_object", descriptor, facade) + ) + continue if configured is not None and not isinstance( descriptor, (MeshObjectDesc, ArticulationDesc, SoftBodyDesc, ClothDesc) ): @@ -390,39 +479,6 @@ def _compile_usd_import( cfg.body_type = None cfg.body_scale = None pose_mode = "preserve" - elif isinstance(descriptor, CableDesc): - if original_name in occupied: - raise ValueError(f"USD asset UID already exists: {original_name!r}.") - occupied.add(original_name) - if root_pose is not None: - descriptor.pose = root_pose @ descriptor.pose - # Keep the parsed descriptor as the source of physics and rendering. - # The facade config exposes the resolved segment and placement data. - frames = descriptor.segment_quaternions - cfg = CableObjectCfg( - uid=original_name, - points=descriptor.points, - radius=descriptor.radius, - init_local_pose=descriptor.pose, - segment_quaternions=None if frames is None else frames[:, [1, 2, 3, 0]], - closed=descriptor.closed, - fixed_segments=descriptor.fixed_segments, - attrs=CablePhysicsCfg( - **{ - item.name: getattr(descriptor.physics, item.name) - for item in fields(descriptor.physics) - } - ), - collision_filter_span=descriptor.collision_filter_span, - render_mode=descriptor.render_mode, - ) - facade = ( - None if manager is None else CableObject(cfg, device=manager.device) - ) - declarations.append( - (resource_keys[id(descriptor)], "cable_object", descriptor, facade) - ) - continue else: if descriptor.name in occupied: raise ValueError(f"USD asset UID already exists: {descriptor.name!r}.") diff --git a/tests/sim/spawn/test_usd_scene_overrides.py b/tests/sim/spawn/test_usd_scene_overrides.py index 9a3097824f..91b3610182 100644 --- a/tests/sim/spawn/test_usd_scene_overrides.py +++ b/tests/sim/spawn/test_usd_scene_overrides.py @@ -18,6 +18,7 @@ from __future__ import annotations +from copy import deepcopy from types import SimpleNamespace from unittest.mock import MagicMock @@ -27,6 +28,9 @@ from pxr import Sdf, Usd, UsdGeom, UsdPhysics from dexsim.spawn import ( + CableDesc, + CablePhysicsDesc, + NewtonCollisionDesc, MeshObjectDesc, ConstraintDesc, RigidBodyPhysicsDesc, @@ -931,6 +935,70 @@ def test_curve_prim_resources_keep_all_uids_and_stable_source_keys( result.resource_source_paths[expected_keys[0]] = "/Other" +def test_usd_cable_sparse_overrides_preserve_source_geometry_and_material(monkeypatch): + import dexsim.kit.usd + + source = _source() + key = "/World/Wire" + points = np.array([[0, 0, 1], [0.2, 0, 1], [0.55, 0, 1]], dtype=np.float32) + source.cables.append( + CableDesc( + name="wire", + points=points, + radius=0.015, + usd=UsdSourceDesc(prim_path=key), + physics=CablePhysicsDesc(density=321, twist_stiffness=7, bend_damping=2), + collision=NewtonCollisionDesc(mu=0.77), + ) + ) + monkeypatch.setattr(dexsim.kit.usd, "parse_usd", lambda *a, **k: deepcopy(source)) + manager = _manager() + manager.device = torch.device("cuda") + manager.physics = SimpleNamespace( + name="newton", solver_type="dexuni", supports_cables=True + ) + manager.sim_config.physics_cfg = SimpleNamespace(requires_grad=False) + recipe = UsdImportCfg( + name="scene", + source=UsdSourceCfg(file_path="scene.usda"), + asset_cfgs={key: {"fixed_segments": [2]}}, + ) + with pytest.raises(ValueError, match="fixed_segments"): + manager.add_usd(recipe) + manager._spawn_scene.builder.add_scene.assert_not_called() + + recipe.asset_cfgs[key] = { + "uid": "cable", + "fixed_segments": [0], + "collision_filter_span": 2, + "attrs": { + "stretch_stiffness": 1e5, + "bend_damping": 0, + "collision_props": {"contact_offset": 0.01, "rest_offset": 0.0}, + "material_props": {"backend": "newton", "ke": 10000, "kd": 0.1}, + }, + } + imported = manager.add_usd(recipe) + native = manager._spawn_scene.builder.add_scene.call_args.args[0].cables[0] + assert native.name == imported.resource_uids[key] == "cable" + facade = manager.get_cable_object("cable") + assert facade is imported[key] + np.testing.assert_array_equal(native.points, points) + assert native.radius == pytest.approx(0.015) + assert native.fixed_segments == facade.cfg.fixed_segments == (0,) + assert native.collision_filter_span == facade.cfg.collision_filter_span == 2 + for physics in (native.physics, facade.cfg.attrs): + assert physics.stretch_stiffness == 1e5 + assert physics.bend_damping == 0 + assert physics.density == 321 + assert physics.twist_stiffness == 7 + assert native.collision.mu == 0.77 + assert native.collision.margin == 0 + assert native.collision.gap == 0.01 + assert native.collision.ke == 10000 + assert native.collision.kd == 0.1 + + def test_portable_sparse_articulation_pose_matrix_is_decoded(monkeypatch): import json import dexsim.kit.usd From 1beeba08b7721330e1f9c8f652fd1cd767ee4660 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 16 Nov 2026 16:24:53 +0800 Subject: [PATCH 38/43] feat(task): import the Franka wrapping cable from USD --- examples/sim/demo/franka_rope_wrap/README.md | 12 +- .../sim/demo/franka_rope_wrap/__main__.py | 26 ++- .../cable/franka_rope_wrap/cable.usda | 133 +++++++++++++++ .../cable/franka_rope_wrap/env.yaml | 156 ++++-------------- .../manipulation/cable/franka_rope_wrap.py | 47 +++++- .../envs/tasks/test_franka_rope_wrap_scene.py | 23 ++- 6 files changed, 243 insertions(+), 154 deletions(-) create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/cable.usda diff --git a/examples/sim/demo/franka_rope_wrap/README.md b/examples/sim/demo/franka_rope_wrap/README.md index 7557e0170d..374bcd02e9 100644 --- a/examples/sim/demo/franka_rope_wrap/README.md +++ b/examples/sim/demo/franka_rope_wrap/README.md @@ -15,8 +15,8 @@ drive samples that pass the offline motion checks. ## Requirements and assets -Use a cable-capable DexSim build with -`SpawnedCable`, cable state access and DexUni. See the parent +Use DexSim `0.6.0rc2` with USD cable import, `SpawnedCable` state access +and DexUni. See the parent [Newton cable example](../../../README.md#newton-cable-demo) for SDK installation. A version string alone does not establish compatibility; the example records the actual SDK build commit. Full physics execution requires a CUDA GPU. @@ -44,8 +44,12 @@ is unchanged. `generated_assets/provenance.json` records the asset reference, source and generated model hashes, resource hashes and finger modifications. Original model notices remain part of their resources. -The floor slab, fixed rope clamp and post are primitive shapes. The rope is -created from configuration; these objects require no additional model bundle. +The floor slab, fixed rope clamp and post are primitive shapes. The packaged +`cable.usda` supplies the 97 original centerline points, 8 mm diameter and +white material. The deployment imports `/World/cable` and sets the fixed first +segment, collision filter span, rod coefficients and contact parameters in +`env.yaml`. A custom geometry configuration writes a matching USD into the run's +`deployment` directory. The default run uses `no-render` and does not load native rendering resources. Native rendering helpers, if used separately, also need the existing `SimResources` asset (`python -m embodichain data download --name SimResources`). diff --git a/examples/sim/demo/franka_rope_wrap/__main__.py b/examples/sim/demo/franka_rope_wrap/__main__.py index 5652f667ee..fdc9e59b07 100644 --- a/examples/sim/demo/franka_rope_wrap/__main__.py +++ b/examples/sim/demo/franka_rope_wrap/__main__.py @@ -43,7 +43,6 @@ import torch from embodichain_tasks.manipulation.cable.franka_rope_wrap import ( - cable_points, load_config, materialize_deployment, prepare_models, @@ -205,15 +204,20 @@ def create_replay_scene( config: dict[str, Any], run_dir: Path, output_dir: Path ) -> dict[str, Any]: """Load the same physical scene configuration and the recorded robot asset for rendering.""" - from embodichain.lab.sim import SimulationManager - from embodichain.lab.sim.cfg import DenoisingCfg, LightCfg, RenderCfg - from embodichain.lab.sim.sensors import CameraCfg - with TemporaryDirectory() as scratch: deployment = materialize_deployment( config, run_dir / "generated_assets/franka/franka_rope.urdf", Path(scratch) ) cfg, _, _ = _deployment_config(deployment) + return _prepare_replay_scene(cfg, config) + + +def _prepare_replay_scene(cfg, config: dict[str, Any]) -> dict[str, Any]: + """Prepare replay resources while the deployment USD is available.""" + from embodichain.lab.sim import SimulationManager + from embodichain.lab.sim.cfg import DenoisingCfg, LightCfg, RenderCfg + from embodichain.lab.sim.sensors import CameraCfg + cfg.sim_cfg.render_cfg = RenderCfg( renderer=config["render"].get("renderer", "hybrid"), spp=config["render"]["spp"], @@ -230,7 +234,7 @@ def create_replay_scene( for rigid_cfg in (*cfg.background, *cfg.rigid_object): sim.add_rigid_object(rigid_cfg) robot = sim.add_robot(cfg.robot) - cable = sim.add_cable_object(cfg.cable_object[0]) + cable = sim.add_usd(cfg.usd_scene)["/World/cable"] for name, view, fov in ( ("overview", config["camera"], 48), ("detail", config["detail_camera"], 42), @@ -367,11 +371,16 @@ def main() -> int: } _json(args.output / "manifest.json", manifest) try: + from dexsim.kit.usd import parse_usd + paths = prepare_models(config, args.output) - points = cable_points(config) + deployment = materialize_deployment(config, paths["robot"], args.output) + asset = deployment.with_name("cable.usda") + (cable,) = parse_usd(str(asset)).cables + manifest["cable_usd_sha256"] = _hash(asset) np.savez_compressed( args.output / "scene_inputs.npz", - points=points, + points=np.asarray(cable.points, dtype=np.float32), anchor=np.asarray(config["cable_waypoints_m"][0]), post_center=config["post_center_xy_m"], ) @@ -379,7 +388,6 @@ def main() -> int: plan_wrap_motion(config, paths["robot"]).write(args.output) manifest["status"] = "planned" return 0 - deployment = materialize_deployment(config, paths["robot"], args.output) report = _run(deployment, config, args.output, args.seconds) print( ("DEMO_COMPLETE " if report["checks"]["completed"] else "DEMO_PARTIAL ") diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/cable.usda b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/cable.usda new file mode 100644 index 0000000000..c913466ce6 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/cable.usda @@ -0,0 +1,133 @@ +#usda 1.0 +( + defaultPrim = "World" + metersPerUnit = 1 + kilogramsPerUnit = 1 + upAxis = "Z" +) + +def Xform "World" +{ + def BasisCurves "cable" ( + prepend apiSchemas = ["PhysicsCurvesDeformableSimAPI", "PhysicsCollisionAPI", "MaterialBindingAPI"] + ) + { + uniform token type = "linear" + uniform token wrap = "nonperiodic" + int[] curveVertexCounts = [97] + point3f[] points = [ + (0.209999993, -0.200000003, 0.00600000005), + (0.217188239, -0.194675386, 0.00600000005), + (0.224370629, -0.189342961, 0.00600000005), + (0.231592745, -0.184064388, 0.00600000005), + (0.238798141, -0.178763345, 0.00600000005), + (0.245842054, -0.173249841, 0.00600000005), + (0.252806395, -0.167638332, 0.00600000005), + (0.260527343, -0.163148969, 0.00600000005), + (0.268971115, -0.160236403, 0.00600000005), + (0.277802795, -0.158881187, 0.00600000005), + (0.286741167, -0.15888761, 0.00600000005), + (0.295613945, -0.159991324, 0.00600000005), + (0.304298371, -0.162111297, 0.00600000005), + (0.312590569, -0.165442377, 0.00600000005), + (0.320187509, -0.170141339, 0.00600000005), + (0.326739371, -0.176210329, 0.00600000005), + (0.331986338, -0.183438838, 0.00600000005), + (0.335870743, -0.191485748, 0.00600000005), + (0.338513196, -0.200025022, 0.00600000005), + (0.340143234, -0.208817571, 0.00600000005), + (0.341180056, -0.217701465, 0.00600000005), + (0.34166351, -0.226632357, 0.00600000005), + (0.341549665, -0.235575378, 0.00600000005), + (0.340800524, -0.244487479, 0.00600000005), + (0.339387804, -0.253318548, 0.00600000005), + (0.337296724, -0.262013882, 0.00600000005), + (0.334528506, -0.270517886, 0.00600000005), + (0.331101269, -0.278778374, 0.00600000005), + (0.327048212, -0.286750615, 0.00600000005), + (0.322414666, -0.294400305, 0.00600000005), + (0.31725359, -0.301704705, 0.00600000005), + (0.31162113, -0.308652431, 0.00600000005), + (0.305574983, -0.315243661, 0.00600000005), + (0.299626291, -0.321922034, 0.00600000005), + (0.294217348, -0.329043716, 0.00600000005), + (0.289473921, -0.336623937, 0.00600000005), + (0.285538852, -0.344652444, 0.00600000005), + (0.282557517, -0.35308075, 0.00600000005), + (0.280652642, -0.361814797, 0.00600000005), + (0.279893577, -0.370721728, 0.00600000005), + (0.280277133, -0.379653186, 0.00600000005), + (0.281730533, -0.388474852, 0.00600000005), + (0.284135133, -0.397087067, 0.00600000005), + (0.287354887, -0.40542984, 0.00600000005), + (0.291258216, -0.413476348, 0.00600000005), + (0.295769274, -0.421198219, 0.00600000005), + (0.300944388, -0.428490847, 0.00600000005), + (0.306810826, -0.435238898, 0.00600000005), + (0.313363224, -0.441322267, 0.00600000005), + (0.32055679, -0.446631521, 0.00600000005), + (0.328308403, -0.45108673, 0.00600000005), + (0.336507916, -0.454651982, 0.00600000005), + (0.345036805, -0.457337528, 0.00600000005), + (0.353784978, -0.459191352, 0.00600000005), + (0.362660974, -0.460284531, 0.00600000005), + (0.371595025, -0.460697651, 0.00600000005), + (0.380533993, -0.46043101, 0.00600000005), + (0.3894113, -0.459357917, 0.00600000005), + (0.39812398, -0.457351744, 0.00600000005), + (0.406526804, -0.454301566, 0.00600000005), + (0.414439827, -0.450145304, 0.00600000005), + (0.421678364, -0.444902539, 0.00600000005), + (0.428097844, -0.438683033, 0.00600000005), + (0.43362835, -0.431659728, 0.00600000005), + (0.438277245, -0.424022585, 0.00600000005), + (0.442106277, -0.415941805, 0.00600000005), + (0.445231766, -0.407562286, 0.00600000005), + (0.447679847, -0.398961097, 0.00600000005), + (0.449328154, -0.390172422, 0.00600000005), + (0.450047165, -0.381260455, 0.00600000005), + (0.449721098, -0.372326463, 0.00600000005), + (0.448275059, -0.363504887, 0.00600000005), + (0.445699722, -0.354944617, 0.00600000005), + (0.442058802, -0.346779853, 0.00600000005), + (0.437480152, -0.339100063, 0.00600000005), + (0.432886571, -0.331426263, 0.00600000005), + (0.428828001, -0.323456198, 0.00600000005), + (0.425270736, -0.315249801, 0.00600000005), + (0.422177345, -0.306857258, 0.00600000005), + (0.419509768, -0.298319608, 0.00600000005), + (0.417231083, -0.289669901, 0.00600000005), + (0.415306807, -0.280934364, 0.00600000005), + (0.41370517, -0.272133857, 0.00600000005), + (0.412789553, -0.263247252, 0.00600000005), + (0.414676487, -0.25459832, 0.00600000005), + (0.421680957, -0.249475315, 0.00600000005), + (0.430575877, -0.249301776, 0.00600000005), + (0.439495057, -0.249975488, 0.00600000005), + (0.448437244, -0.250168413, 0.00600000005), + (0.457381606, -0.250026435, 0.00600000005), + (0.466326892, -0.249970526, 0.00600000005), + (0.475272387, -0.24999088, 0.00600000005), + (0.484217912, -0.250004321, 0.00600000005), + (0.493163437, -0.250002652, 0.00600000005), + (0.502108932, -0.249999449, 0.00600000005), + (0.511054456, -0.249998569, 0.00600000005), + (0.519999981, -0.25, 0.00600000005) + ] + float[] physics:thicknesses = [0.008] + token physics:thicknesses:elementType = "constant" + rel material:binding = + } + + def Material "white_rope" + { + token outputs:surface.connect = + def Shader "Surface" + { + uniform token info:id = "UsdPreviewSurface" + color3f inputs:diffuseColor = (0.93, 0.94, 0.92) + float inputs:roughness = 0.6 + token outputs:surface + } + } +} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/env.yaml index 6dc70cda8b..116035f1ab 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/env.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/env.yaml @@ -74,7 +74,7 @@ simulation: shape: shape_type: Cylinder radius: 0.045 - height: 0.12 + half_height: 0.06 visual_material: uid: post_mat base_color: [0.025, 0.45, 0.39, 1] @@ -89,132 +89,34 @@ simulation: dynamic_friction: 0.3 ke: 20000.0 kd: 1.0 - cable_object: - - uid: cable - points: - - [0.20999999344348907, -0.20000000298023224, 0.006000000052154064] - - [0.21718823909759521, -0.19467538595199585, 0.006000000052154064] - - [0.22437062859535217, -0.18934296071529388, 0.006000000052154064] - - [0.23159274458885193, -0.18406438827514648, 0.006000000052154064] - - [0.2387981414794922, -0.17876334488391876, 0.006000000052154064] - - [0.24584205448627472, -0.17324984073638916, 0.006000000052154064] - - [0.2528063952922821, -0.16763833165168762, 0.006000000052154064] - - [0.2605273425579071, -0.16314896941184998, 0.006000000052154064] - - [0.26897111535072327, -0.1602364033460617, 0.006000000052154064] - - [0.27780279517173767, -0.15888118743896484, 0.006000000052154064] - - [0.2867411673069, -0.1588876098394394, 0.006000000052154064] - - [0.29561394453048706, -0.1599913239479065, 0.006000000052154064] - - [0.30429837107658386, -0.162111297249794, 0.006000000052154064] - - [0.3125905692577362, -0.16544237732887268, 0.006000000052154064] - - [0.320187509059906, -0.170141339302063, 0.006000000052154064] - - [0.3267393708229065, -0.17621032893657684, 0.006000000052154064] - - [0.33198633790016174, -0.18343883752822876, 0.006000000052154064] - - [0.33587074279785156, -0.19148574769496918, 0.006000000052154064] - - [0.33851319551467896, -0.2000250220298767, 0.006000000052154064] - - [0.34014323353767395, -0.20881757140159607, 0.006000000052154064] - - [0.34118005633354187, -0.21770146489143372, 0.006000000052154064] - - [0.34166350960731506, -0.22663235664367676, 0.006000000052154064] - - [0.34154966473579407, -0.2355753779411316, 0.006000000052154064] - - [0.34080052375793457, -0.2444874793291092, 0.006000000052154064] - - [0.33938780426979065, -0.25331854820251465, 0.006000000052154064] - - [0.337296724319458, -0.26201388239860535, 0.006000000052154064] - - [0.33452850580215454, -0.27051788568496704, 0.006000000052154064] - - [0.33110126852989197, -0.2787783741950989, 0.006000000052154064] - - [0.3270482122898102, -0.2867506146430969, 0.006000000052154064] - - [0.32241466641426086, -0.29440030455589294, 0.006000000052154064] - - [0.31725358963012695, -0.3017047047615051, 0.006000000052154064] - - [0.31162112951278687, -0.30865243077278137, 0.006000000052154064] - - [0.3055749833583832, -0.315243661403656, 0.006000000052154064] - - [0.29962629079818726, -0.32192203402519226, 0.006000000052154064] - - [0.2942173480987549, -0.3290437161922455, 0.006000000052154064] - - [0.28947392106056213, -0.3366239368915558, 0.006000000052154064] - - [0.28553885221481323, -0.3446524441242218, 0.006000000052154064] - - [0.28255751729011536, -0.35308074951171875, 0.006000000052154064] - - [0.28065264225006104, -0.36181479692459106, 0.006000000052154064] - - [0.27989357709884644, -0.3707217276096344, 0.006000000052154064] - - [0.2802771329879761, -0.3796531856060028, 0.006000000052154064] - - [0.2817305326461792, -0.38847485184669495, 0.006000000052154064] - - [0.2841351330280304, -0.39708706736564636, 0.006000000052154064] - - [0.28735488653182983, -0.4054298400878906, 0.006000000052154064] - - [0.29125821590423584, -0.4134763479232788, 0.006000000052154064] - - [0.29576927423477173, -0.4211982190608978, 0.006000000052154064] - - [0.30094438791275024, -0.42849084734916687, 0.006000000052154064] - - [0.30681082606315613, -0.43523889780044556, 0.006000000052154064] - - [0.3133632242679596, -0.4413222670555115, 0.006000000052154064] - - [0.32055678963661194, -0.446631520986557, 0.006000000052154064] - - [0.3283084034919739, -0.45108672976493835, 0.006000000052154064] - - [0.3365079164505005, -0.45465198159217834, 0.006000000052154064] - - [0.3450368046760559, -0.45733752846717834, 0.006000000052154064] - - [0.35378497838974, -0.45919135212898254, 0.006000000052154064] - - [0.3626609742641449, -0.4602845311164856, 0.006000000052154064] - - [0.37159502506256104, -0.46069765090942383, 0.006000000052154064] - - [0.38053399324417114, -0.4604310095310211, 0.006000000052154064] - - [0.3894113004207611, -0.4593579173088074, 0.006000000052154064] - - [0.39812397956848145, -0.4573517441749573, 0.006000000052154064] - - [0.4065268039703369, -0.4543015658855438, 0.006000000052154064] - - [0.4144398272037506, -0.45014530420303345, 0.006000000052154064] - - [0.421678364276886, -0.44490253925323486, 0.006000000052154064] - - [0.42809784412384033, -0.43868303298950195, 0.006000000052154064] - - [0.4336283504962921, -0.4316597282886505, 0.006000000052154064] - - [0.4382772445678711, -0.4240225851535797, 0.006000000052154064] - - [0.4421062767505646, -0.4159418046474457, 0.006000000052154064] - - [0.44523176550865173, -0.4075622856616974, 0.006000000052154064] - - [0.4476798474788666, -0.3989610970020294, 0.006000000052154064] - - [0.44932815432548523, -0.39017242193222046, 0.006000000052154064] - - [0.45004716515541077, -0.3812604546546936, 0.006000000052154064] - - [0.449721097946167, -0.37232646346092224, 0.006000000052154064] - - [0.44827505946159363, -0.36350488662719727, 0.006000000052154064] - - [0.4456997215747833, -0.3549446165561676, 0.006000000052154064] - - [0.442058801651001, -0.34677985310554504, 0.006000000052154064] - - [0.4374801516532898, -0.33910006284713745, 0.006000000052154064] - - [0.4328865706920624, -0.3314262628555298, 0.006000000052154064] - - [0.42882800102233887, -0.32345619797706604, 0.006000000052154064] - - [0.4252707362174988, -0.31524980068206787, 0.006000000052154064] - - [0.42217734456062317, -0.30685725808143616, 0.006000000052154064] - - [0.41950976848602295, -0.29831960797309875, 0.006000000052154064] - - [0.41723108291625977, -0.2896699011325836, 0.006000000052154064] - - [0.41530680656433105, -0.2809343636035919, 0.006000000052154064] - - [0.41370517015457153, -0.27213385701179504, 0.006000000052154064] - - [0.41278955340385437, -0.2632472515106201, 0.006000000052154064] - - [0.4146764874458313, -0.25459831953048706, 0.006000000052154064] - - [0.4216809570789337, -0.24947531521320343, 0.006000000052154064] - - [0.4305758774280548, -0.24930177628993988, 0.006000000052154064] - - [0.4394950568675995, -0.24997548758983612, 0.006000000052154064] - - [0.44843724370002747, -0.25016841292381287, 0.006000000052154064] - - [0.45738160610198975, -0.2500264346599579, 0.006000000052154064] - - [0.46632689237594604, -0.24997052550315857, 0.006000000052154064] - - [0.47527238726615906, -0.24999088048934937, 0.006000000052154064] - - [0.48421791195869446, -0.2500043213367462, 0.006000000052154064] - - [0.49316343665122986, -0.2500026524066925, 0.006000000052154064] - - [0.5021089315414429, -0.24999944865703583, 0.006000000052154064] - - [0.5110544562339783, -0.2499985694885254, 0.006000000052154064] - - [0.5199999809265137, -0.25, 0.006000000052154064] - radius: 0.004 - fixed_segments: [0] - collision_filter_span: 2 - attrs: - density: 800.0 - stretch_stiffness: 100000.0 - shear_stiffness: 100000.0 - bend_stiffness: 0.0001 - twist_stiffness: 0.0001 - stretch_damping: 0.03 - shear_damping: 0.03 - bend_damping: 0.0001 - twist_damping: 0.0001 - collision_props: - backend: newton - margin: 0.0 - gap: 5.0e-05 - material_props: - backend: newton - dynamic_friction: 0.8 - ke: 20000.0 - kd: 1.0 - visual_material: - uid: white_rope - base_color: [0.93, 0.94, 0.92, 1] - roughness: 0.6 + usd_scene: + name: cable_scene + source: + file_path: ./cable.usda + asset_cfgs: + /World/cable: + uid: cable + fixed_segments: [0] + collision_filter_span: 2 + attrs: + density: 800.0 + stretch_stiffness: 100000.0 + shear_stiffness: 100000.0 + bend_stiffness: 0.0001 + twist_stiffness: 0.0001 + stretch_damping: 0.03 + shear_damping: 0.03 + bend_damping: 0.0001 + twist_damping: 0.0001 + collision_props: + backend: newton + margin: 0.0 + gap: 5.0e-05 + material_props: + backend: newton + dynamic_friction: 0.8 + ke: 20000.0 + kd: 1.0 env: sim_steps_per_control: 32 ignore_terminations: true diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py index 93410bc5b9..2a8894e2bc 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py @@ -394,6 +394,34 @@ def cable_points(config: dict[str, Any]) -> np.ndarray: return result.astype(np.float32) +def _write_cable_asset(config: dict[str, Any], path: Path) -> None: + """Copy the packaged rope, updating its geometry for a custom scenario.""" + source = task_directory() / "cable.usda" + defaults = json.loads((task_directory() / "config.json").read_text()) + geometry_keys = ( + "cable_waypoints_m", + "post_center_xy_m", + "floor_height_m", + "cable_radius_m", + "cable_segments", + ) + if all(config[key] == defaults[key] for key in geometry_keys): + shutil.copyfile(source, path) + return + + from pxr import Sdf, Vt + + layer = Sdf.Layer.OpenAsAnonymous(str(source)) + curve = layer.GetPrimAtPath("/World/cable") + points = cable_points(config) + curve.attributes["points"].default = Vt.Vec3fArray.FromNumpy(points) + curve.attributes["curveVertexCounts"].default = Vt.IntArray([len(points)]) + curve.attributes["physics:thicknesses"].default = Vt.FloatArray( + [2.0 * config["cable_radius_m"]] + ) + layer.Export(str(path)) + + def settle_franka_rope( env: EmbodiedEnv, env_ids: torch.Tensor | None, scenario_path: str | None = None ) -> None: @@ -494,7 +522,7 @@ def rigid( { "shape_type": "Cylinder", "radius": config["post_radius_m"], - "height": config["post_height_m"], + "half_height": config["post_height_m"] / 2, }, [*center, floor + config["post_height_m"] / 2], config["post_friction"], @@ -503,8 +531,6 @@ def rigid( ) cable = { "uid": "cable", - "points": cable_points(config).tolist(), - "radius": config["cable_radius_m"], "fixed_segments": [0], "collision_filter_span": 2, "attrs": { @@ -519,11 +545,6 @@ def rigid( "twist_damping": config["bend_damping_Nms_rad"], **_contact(config, config["rope_friction"]), }, - "visual_material": { - "uid": "white_rope", - "base_color": [0.93, 0.94, 0.92, 1], - "roughness": 0.6, - }, } return { "environment_id": "franka_rope_wrap", @@ -562,7 +583,14 @@ def rigid( }, "max_episode_steps": round(config["duration_s"] * config["control_hz"]) + 1, "num_envs": 1, - "simulation": {"background": [slab, clamp, post], "cable_object": [cable]}, + "simulation": { + "background": [slab, clamp, post], + "usd_scene": { + "name": "cable_scene", + "source": {"file_path": "./cable.usda"}, + "asset_cfgs": {"/World/cable": cable}, + }, + }, "env": { "sim_steps_per_control": config["substeps"], "ignore_terminations": True, @@ -588,6 +616,7 @@ def materialize_deployment(config: dict[str, Any], robot_path: Path, out: Path) _validate_config(config) folder = out.resolve() / "deployment" folder.mkdir() + _write_cable_asset(config, folder / "cable.usda") reference = task_directory() deployment = yaml.safe_load((reference / "task.franka.yaml").read_text()) embodiment_path = reference / deployment["embodiment"]["component"] diff --git a/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py b/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py index 48ef27ebb5..d43a4eced8 100644 --- a/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py +++ b/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py @@ -22,8 +22,10 @@ import numpy as np import yaml +from dexsim.kit.usd import parse_usd from embodichain_tasks.manipulation.cable.franka_rope_wrap import ( + cable_points, load_config, materialize_deployment, task_directory, @@ -43,8 +45,17 @@ def test_default_deployment_matches_materialized_scene(tmp_path: Path) -> None: assert compiled["env"]["sim_steps_per_control"] == 32 assert compiled["physics_config"]["num_substeps"] == 1 np.testing.assert_allclose(compiled["physics_config"]["physics_dt"] * 32, 1 / 60) - cable = compiled["simulation"]["cable_object"][0] - points = np.asarray(cable["points"]) + scene = compiled["simulation"]["usd_scene"] + cable = scene["asset_cfgs"]["/World/cable"] + asset = alternate.parent / scene["source"]["file_path"] + assert asset.read_bytes() == (root / "cable.usda").read_bytes() + (desc,) = parse_usd(str(asset)).cables + assert desc.usd.prim_path == "/World/cable" + assert desc.enable_collision + assert desc.render_material is not None + np.testing.assert_allclose(desc.radius, defaults["cable_radius_m"]) + points = np.asarray(desc.points) + np.testing.assert_array_equal(points, cable_points(defaults)) assert points.shape == (97, 3) np.testing.assert_allclose( points[[0, -1]], np.asarray(defaults["cable_waypoints_m"])[[0, -1]], atol=1e-7 @@ -80,9 +91,11 @@ def test_custom_scene_rescales_windows_and_uses_requested_assets( body for body in env["simulation"]["background"] if body["uid"] == "post" ) assert cylinder["init_pos"] == [0.48, 0.0, 0.073] - rope = env["simulation"]["cable_object"][0] - assert len(rope["points"]) == 65 - assert min(p[2] for p in rope["points"]) >= 0.017 - 1e-8 + scene = env["simulation"]["usd_scene"] + (desc,) = parse_usd(str(deployment.parent / scene["source"]["file_path"])).cables + np.testing.assert_array_equal(desc.points, cable_points(effective)) + assert len(desc.points) == 65 + assert np.min(desc.points[:, 2]) >= 0.017 - 1e-8 embodiment = yaml.safe_load(deployment.with_name("embodiment.yaml").read_text()) assert embodiment["simulation"]["fpath"] == str(urdf) assert embodiment["simulation"]["init_pos"] == [0, 0, 0.013] From cfc184e72e81fa17bd768662f3eb013af80a67c0 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 16 Nov 2026 16:28:18 +0800 Subject: [PATCH 39/43] fix(task): preserve the Franka rope material in scene-linear USD --- .../tasks/manipulation/cable/franka_rope_wrap/cable.usda | 3 ++- tests/gym/envs/tasks/test_franka_rope_wrap_scene.py | 8 +++++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/cable.usda b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/cable.usda index c913466ce6..bd910fcf36 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/cable.usda +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/cable.usda @@ -125,7 +125,8 @@ def Xform "World" def Shader "Surface" { uniform token info:id = "UsdPreviewSurface" - color3f inputs:diffuseColor = (0.93, 0.94, 0.92) + # Scene-linear equivalent of the original (0.93, 0.94, 0.92) sRGB. + color3f inputs:diffuseColor = (0.84808826, 0.86889939, 0.82757083) float inputs:roughness = 0.6 token outputs:surface } diff --git a/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py b/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py index d43a4eced8..5f27f2da69 100644 --- a/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py +++ b/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py @@ -52,7 +52,13 @@ def test_default_deployment_matches_materialized_scene(tmp_path: Path) -> None: (desc,) = parse_usd(str(asset)).cables assert desc.usd.prim_path == "/World/cable" assert desc.enable_collision - assert desc.render_material is not None + material = desc.render_material + assert material.color_space == "linear" + original_srgb = np.array([0.93, 0.94, 0.92]) + np.testing.assert_allclose( + material.base_color[:3], ((original_srgb + 0.055) / 1.055) ** 2.4 + ) + np.testing.assert_allclose(material.roughness, 0.6) np.testing.assert_allclose(desc.radius, defaults["cable_radius_m"]) points = np.asarray(desc.points) np.testing.assert_array_equal(points, cable_points(defaults)) From d05a790a94e2aacf6856c02c1505fa7c03a67808 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 16 Nov 2026 16:29:38 +0800 Subject: [PATCH 40/43] Load the routing task cable from a packaged USD curve --- .../cable/route_the_cable/README.md | 18 +- .../cable/route_the_cable/cable.usda | 36 +++ .../cable/route_the_cable/env.yaml | 209 +++--------------- tests/gym/envs/tasks/test_route_the_cable.py | 37 ++-- 4 files changed, 93 insertions(+), 207 deletions(-) create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/cable.usda diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md index 7e8235a769..eded3688ef 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md @@ -11,17 +11,13 @@ in-episode state writes. ## Requirements and entry point -Use CUDA and a DexSim build exposing `CableDesc`, `Scene.create_cable_batch`, -`CableBatch.fetch_default_state/apply_state` and `Scene.reset_arenas`. See the -[cable example installation guide](../../../../../../../examples/README.md#newton-cable-demo) -for the development-build dependency and solver migration. The current -revalidation runtime uses DexSim `0.6.0.dev12034`, build -`b040478aa5cb4902b26a19a611eff768d229548b`, Newton 1.7.0 and -Torch 2.11.0+cu130. Full-episode revalidation of the revised path is still in -progress; results from earlier engine builds do not qualify this configuration. -A wheel lacking the required cable APIs cannot run this deployment. The -environment file owns Newton/DexUni, dynamic MuJoCo joints and two-way VBD cable -contact; CLI `--physics` does not switch it. +Use CUDA and the pinned DexSim `0.6.0rc2` wheel. The packaged +[`cable.usda`](cable.usda) defines the original 51 centerline points, the 7 mm +diameter and the initial pose. The `usd_scene.asset_cfgs` entry in +[`env.yaml`](env.yaml) binds the curve as `cable` and supplies the fixed first +segment, the four per-joint stiffness and damping pairs, density and contact +parameters. The environment config also selects Newton/DexUni, dynamic MuJoCo +joints and two-way VBD cable contact. From the EmbodiChain repository: diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/cable.usda b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/cable.usda new file mode 100644 index 0000000000..711b15d23d --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/cable.usda @@ -0,0 +1,36 @@ +#usda 1.0 +( + defaultPrim = "World" + metersPerUnit = 0.001 + kilogramsPerUnit = 1 + upAxis = "Z" +) + +def Xform "World" +{ + def BasisCurves "cable" ( + prepend apiSchemas = ["PhysicsCurvesDeformableSimAPI", "PhysicsCollisionAPI"] + ) + { + uniform token type = "linear" + uniform token wrap = "nonperiodic" + int[] curveVertexCounts = [51] + point3f[] points = [ + (0, 0, 0), (0, 12, 0), (0, 24, 0), (0, 36, 0), (0, 48, 0), + (0, 60, 0), (0, 72, 0), (0, 84, 0), (0, 96, 0), (0, 108, 0), + (0, 120, 0), (0, 132, 0), (0, 144, 0), (0, 156, 0), (0, 168, 0), + (0, 180, 0), (0, 192, 0), (0, 204, 0), (0, 216, 0), (0, 228, 0), + (0, 240, 0), (0, 252, 0), (0, 264, 0), (0, 276, 0), (0, 288, 0), + (0, 300, 0), (0, 312, 0), (0, 324, 0), (0, 336, 0), (0, 348, 0), + (0, 360, 0), (0, 372, 0), (0, 384, 0), (0, 396, 0), (0, 408, 0), + (0, 420, 0), (0, 432, 0), (0, 444, 0), (0, 456, 0), (0, 468, 0), + (0, 480, 0), (0, 492, 0), (0, 504, 0), (0, 516, 0), (0, 528, 0), + (0, 540, 0), (0, 552, 0), (0, 564, 0), (0, 576, 0), (0, 588, 0), + (0, 600, 0) + ] + float[] physics:thicknesses = [7] + token physics:thicknesses:elementType = "constant" + double3 xformOp:translate = (420, -360, 753.5) + uniform token[] xformOpOrder = ["xformOp:translate"] + } +} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml index 29e7c681d5..d4b80fd063 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml @@ -1,5 +1,6 @@ # Material coefficients follow the pinned upstream RodSpec and its scene-friction pass. environment_id: route_the_cable +enable_ground_plane: true physics: newton physics_config: physics_dt: 0.004166666666666667 @@ -243,187 +244,33 @@ simulation: dynamic_friction: 2.0 ke: 10000.0 kd: 0.1 - cable_object: - - uid: cable - points: - - - 0 - - 0.0 - - 0 - - - 0 - - 0.012 - - 0 - - - 0 - - 0.024 - - 0 - - - 0 - - 0.036000000000000004 - - 0 - - - 0 - - 0.048 - - 0 - - - 0 - - 0.06 - - 0 - - - 0 - - 0.07200000000000001 - - 0 - - - 0 - - 0.084 - - 0 - - - 0 - - 0.096 - - 0 - - - 0 - - 0.108 - - 0 - - - 0 - - 0.12 - - 0 - - - 0 - - 0.132 - - 0 - - - 0 - - 0.14400000000000002 - - 0 - - - 0 - - 0.156 - - 0 - - - 0 - - 0.168 - - 0 - - - 0 - - 0.18 - - 0 - - - 0 - - 0.192 - - 0 - - - 0 - - 0.20400000000000001 - - 0 - - - 0 - - 0.216 - - 0 - - - 0 - - 0.228 - - 0 - - - 0 - - 0.24 - - 0 - - - 0 - - 0.252 - - 0 - - - 0 - - 0.264 - - 0 - - - 0 - - 0.276 - - 0 - - - 0 - - 0.28800000000000003 - - 0 - - - 0 - - 0.3 - - 0 - - - 0 - - 0.312 - - 0 - - - 0 - - 0.324 - - 0 - - - 0 - - 0.336 - - 0 - - - 0 - - 0.34800000000000003 - - 0 - - - 0 - - 0.36 - - 0 - - - 0 - - 0.372 - - 0 - - - 0 - - 0.384 - - 0 - - - 0 - - 0.396 - - 0 - - - 0 - - 0.40800000000000003 - - 0 - - - 0 - - 0.42 - - 0 - - - 0 - - 0.432 - - 0 - - - 0 - - 0.444 - - 0 - - - 0 - - 0.456 - - 0 - - - 0 - - 0.468 - - 0 - - - 0 - - 0.48 - - 0 - - - 0 - - 0.492 - - 0 - - - 0 - - 0.504 - - 0 - - - 0 - - 0.516 - - 0 - - - 0 - - 0.528 - - 0 - - - 0 - - 0.54 - - 0 - - - 0 - - 0.552 - - 0 - - - 0 - - 0.5640000000000001 - - 0 - - - 0 - - 0.5760000000000001 - - 0 - - - 0 - - 0.588 - - 0 - - - 0 - - 0.6 - - 0 - radius: 0.0035 - init_pos: - - 0.42 - - -0.36 - - 0.7535 - fixed_segments: - - 0 - attrs: - density: 350.0 - bend_stiffness: 0.00111572809234342 - bend_damping: 0.00167359213851513 - twist_stiffness: 0.00111572809234342 - twist_damping: 0.00167359213851513 - collision_props: - contact_offset: 0.01 - rest_offset: 0.0 - material_props: - backend: newton - dynamic_friction: 60.0 - ke: 10000.0 - kd: 0.1 - stretch_stiffness: 200000.0 - stretch_damping: 1.0 - shear_stiffness: 200000.0 - shear_damping: 1.0 + usd_scene: + name: cable_scene + source: + file_path: ./cable.usda + asset_cfgs: + /World/cable: + uid: cable + fixed_segments: + - 0 + attrs: + density: 350.0 + bend_stiffness: 0.00111572809234342 + bend_damping: 0.00167359213851513 + twist_stiffness: 0.00111572809234342 + twist_damping: 0.00167359213851513 + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 60.0 + ke: 10000.0 + kd: 0.1 + stretch_stiffness: 200000.0 + stretch_damping: 1.0 + shear_stiffness: 200000.0 + shear_damping: 1.0 env: sim_steps_per_control: 4 ignore_terminations: true diff --git a/tests/gym/envs/tasks/test_route_the_cable.py b/tests/gym/envs/tasks/test_route_the_cable.py index 947b052a80..773b5f6459 100644 --- a/tests/gym/envs/tasks/test_route_the_cable.py +++ b/tests/gym/envs/tasks/test_route_the_cable.py @@ -21,6 +21,7 @@ import yaml import pytest import torch +from dexsim.kit.usd import parse_usd from embodichain.lab.gym.utils.gym_utils import ( add_env_launcher_args_to_parser, build_env_cfg_from_args, @@ -99,11 +100,15 @@ def test_route_deployment_composes_registered_robot_cable_and_board(): ) cfg = config_to_cfg(yaml.safe_load(p.read_text()), source_path=p) assert cfg.robot.root_props.newton_gravity_compensation == 1.0 - assert len(cfg.cable_object) == 1 and len(cfg.cable_object[0].points) == 51 - assert cfg.cable_object[0].fixed_segments == [0] - points = torch.tensor(cfg.cable_object[0].points) + torch.tensor( - cfg.cable_object[0].init_pos - ) + (curve,) = parse_usd(cfg.usd_scene.source.file_path).cables + override = cfg.usd_scene.asset_cfgs[curve.usd.prim_path] + assert not cfg.cable_object + assert cfg.sim_cfg.enable_ground_plane is True + assert curve.usd.prim_path == "/World/cable" + assert override["uid"] == "cable" and override["fixed_segments"] == [0] + assert curve.radius == pytest.approx(0.0035) + assert curve.enable_collision + points = torch.from_numpy(curve.points) + torch.from_numpy(curve.pose[:3, 3]) torch.testing.assert_close(points[:, 2], torch.full((51,), 0.7535)) assert bool( ( @@ -127,10 +132,11 @@ def test_route_deployment_composes_registered_robot_cable_and_board(): assert lengths.max() <= 0.0121 assert lengths.min() > 0.0115 assert len(cfg.background) == 11 - assert cfg.cable_object[0].attrs.density == 350.0 - assert cfg.cable_object[0].attrs.stretch_stiffness == 2e5 - assert cfg.cable_object[0].attrs.material_props.kd == 0.1 - assert cfg.cable_object[0].attrs.material_props.dynamic_friction == 60.0 + attrs = override["attrs"] + assert attrs["density"] == 350.0 + assert attrs["stretch_stiffness"] == 2e5 + assert attrs["material_props"]["kd"] == 0.1 + assert attrs["material_props"]["dynamic_friction"] == 60.0 assert cfg.robot.link_attrs["pads"].attrs.material_props.dynamic_friction == 60.0 assert all( obj.attrs.material_props.dynamic_friction == 2.0 @@ -226,8 +232,8 @@ def test_cable_deployment_rejects_duplicate_component_ownership(): / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml" ) data = yaml.safe_load(p.read_text()) - data["cable_object"] = [] - with pytest.raises(ValueError, match="cable_object"): + data["usd_scene"] = {} + with pytest.raises(ValueError, match="usd_scene"): config_to_cfg(data, source_path=p) @@ -331,11 +337,10 @@ def test_contact_penetration_uses_the_configured_native_back_wall(): cfg = config_to_cfg(yaml.safe_load(p.read_text()), source_path=p) wall = next(obj for obj in cfg.background if obj.uid == "port_back") inner_y = wall.init_pos[1] - wall.shape.size[1] / 2 + (curve,) = parse_usd(cfg.usd_scene.source.file_path).cables state = torch.zeros(1, 1, 13) state[..., 6] = 1 - state[..., :3] = torch.tensor( - [0.586, inner_y - cfg.cable_object[0].radius + 0.002, 0.755] - ) + state[..., :3] = torch.tensor([0.586, inner_y - curve.radius + 0.002, 0.755]) metrics = measure_route( state, torch.tensor([0.001]), torch.ones(1, 2, 3), torch.zeros(1, 3) ) @@ -349,7 +354,9 @@ def test_unknown_cable_fields_are_rejected_by_production_loader(tmp_path): ) data = yaml.safe_load(p.read_text()) physical = yaml.safe_load((p.parent / "env.yaml").read_text()) - physical["simulation"]["cable_object"][0]["unknown_cable_option"] = True + scene = physical["simulation"]["usd_scene"] + scene["source"]["file_path"] = str(p.parent / "cable.usda") + scene["asset_cfgs"]["/World/cable"]["unknown_cable_option"] = True component = tmp_path / "env.yaml" component.write_text(yaml.safe_dump(physical)) data["environment"]["component"] = str(component) From 85f25b4d869159e91154a9f2afe114a79767be72 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 16 Nov 2026 16:30:12 +0800 Subject: [PATCH 41/43] test(gym): exercise USD cable overrides through scene preflight and replay --- tests/gym/envs/test_cable_recording_cuda.py | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/tests/gym/envs/test_cable_recording_cuda.py b/tests/gym/envs/test_cable_recording_cuda.py index 9edbfed986..41fd6750a0 100644 --- a/tests/gym/envs/test_cable_recording_cuda.py +++ b/tests/gym/envs/test_cable_recording_cuda.py @@ -94,16 +94,9 @@ def _run_recording(urdf: str, usd: str) -> None: UsdSourceCfg, ) - class CableRecordingEnv(EmbodiedEnv): - def _prepare_scene(self, **kwargs) -> None: - super()._prepare_scene(**kwargs) - result = self.sim.add_usd( - UsdImportCfg(name="scene", source=UsdSourceCfg(file_path=usd)) - ) - assert result["/World/wire"] is self.sim.get_cable_object("wire") - cfg = EmbodiedEnvCfg( num_envs=2, + enable_ground_plane=True, max_episode_steps=4, sim_steps_per_control=1, ignore_terminations=True, @@ -124,6 +117,11 @@ def _prepare_scene(self, **kwargs) -> None: solver_cfg={"solver_type": "dexuni"}, ), ), + usd_scene=UsdImportCfg( + name="scene", + source=UsdSourceCfg(file_path=usd), + asset_cfgs={"/World/wire": {"uid": "wire", "collision_filter_span": 2}}, + ), robot=RobotCfg( uid="slider", fpath=urdf, @@ -153,7 +151,10 @@ def _prepare_scene(self, **kwargs) -> None: ) env = None try: - env = CableRecordingEnv(cfg) + env = EmbodiedEnv(cfg) + cable = env.usd_scene_import["/World/wire"] + assert cable is env.sim.get_cable_object("wire") + assert cable.cfg.collision_filter_span == 2 assert not env.sim.has_native_renderer obs, _ = env.reset(seed=73) manager = env.observation_manager @@ -277,7 +278,7 @@ def _prepare_scene(self, **kwargs) -> None: cfg.num_envs = 1 cfg.sim_cfg.num_envs = 1 cfg.record_trajectory = False - env = CableRecordingEnv(cfg) + env = EmbodiedEnv(cfg) replay = ReplayWrapper(env, saved) replay.reset() for _ in range(saved["meta"]["lengths"][0]): From 27652ab93ad72e2154bebff23c548bd9f7b0205c Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 16 Nov 2026 16:31:03 +0800 Subject: [PATCH 42/43] fix(task): retain the ground plane and use the authored default centerline --- .../cable/franka_rope_wrap/env.yaml | 1 + .../manipulation/cable/franka_rope_wrap.py | 39 +++++++++++++------ .../envs/tasks/test_franka_rope_wrap_scene.py | 1 + 3 files changed, 30 insertions(+), 11 deletions(-) diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/env.yaml index 116035f1ab..5ea9153d2d 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/env.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/env.yaml @@ -1,4 +1,5 @@ environment_id: franka_rope_wrap +enable_ground_plane: true physics: newton physics_config: physics_dt: 0.0005208333333333333 diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py index 2a8894e2bc..ad18dc057c 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py @@ -367,8 +367,32 @@ def prepare_models(config: dict[str, Any], out: Path) -> dict[str, Path]: return paths +def _uses_packaged_geometry(config: dict[str, Any]) -> bool: + defaults = json.loads((task_directory() / "config.json").read_text()) + return all( + config[key] == defaults[key] + for key in ( + "cable_waypoints_m", + "post_center_xy_m", + "floor_height_m", + "cable_radius_m", + "cable_segments", + ) + ) + + def cable_points(config: dict[str, Any]) -> np.ndarray: - """Sample the anchored quadratic centerline at equal arc-length intervals.""" + """Read the packaged centerline or sample the user's custom geometry.""" + if _uses_packaged_geometry(config): + from pxr import Sdf + + layer = Sdf.Layer.FindOrOpen(str(task_directory() / "cable.usda")) + curve = layer.GetPrimAtPath("/World/cable") + return np.array(curve.attributes["points"].default, dtype=np.float32) + return _sample_custom_centerline(config) + + +def _sample_custom_centerline(config: dict[str, Any]) -> np.ndarray: points = np.asarray(config["cable_waypoints_m"], float) distances = np.r_[0.0, np.cumsum(np.linalg.norm(np.diff(points, axis=0), axis=1))] curve = make_interp_spline(distances, points, k=2)( @@ -397,15 +421,7 @@ def cable_points(config: dict[str, Any]) -> np.ndarray: def _write_cable_asset(config: dict[str, Any], path: Path) -> None: """Copy the packaged rope, updating its geometry for a custom scenario.""" source = task_directory() / "cable.usda" - defaults = json.loads((task_directory() / "config.json").read_text()) - geometry_keys = ( - "cable_waypoints_m", - "post_center_xy_m", - "floor_height_m", - "cable_radius_m", - "cable_segments", - ) - if all(config[key] == defaults[key] for key in geometry_keys): + if _uses_packaged_geometry(config): shutil.copyfile(source, path) return @@ -413,7 +429,7 @@ def _write_cable_asset(config: dict[str, Any], path: Path) -> None: layer = Sdf.Layer.OpenAsAnonymous(str(source)) curve = layer.GetPrimAtPath("/World/cable") - points = cable_points(config) + points = _sample_custom_centerline(config) curve.attributes["points"].default = Vt.Vec3fArray.FromNumpy(points) curve.attributes["curveVertexCounts"].default = Vt.IntArray([len(points)]) curve.attributes["physics:thicknesses"].default = Vt.FloatArray( @@ -548,6 +564,7 @@ def rigid( } return { "environment_id": "franka_rope_wrap", + "enable_ground_plane": True, "physics": "newton", "physics_config": { "physics_dt": 1.0 / (config["control_hz"] * config["substeps"]), diff --git a/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py b/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py index 5f27f2da69..dd264f1e9d 100644 --- a/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py +++ b/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py @@ -42,6 +42,7 @@ def test_default_deployment_matches_materialized_scene(tmp_path: Path) -> None: event = compiled["env"]["events"]["settle_rope"] assert load_config(Path(event["params"].pop("scenario_path"))) == defaults assert compiled == stored + assert compiled["enable_ground_plane"] is True assert compiled["env"]["sim_steps_per_control"] == 32 assert compiled["physics_config"]["num_substeps"] == 1 np.testing.assert_allclose(compiled["physics_config"]["physics_dt"] * 32, 1 / 60) From 367fc0381f3e84b8116f2c6a903ad4b88b225af5 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 16 Nov 2026 16:43:23 +0800 Subject: [PATCH 43/43] Document the paired rc2 route acceptance results --- .../manipulation/cable/route_the_cable/README.md | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md index eded3688ef..90a2922c07 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md @@ -233,8 +233,17 @@ spool: python -m pytest tests/gym/envs/tasks/test_route_the_cable_cuda.py --run-gpu -m gpu -q ``` -The three NoRender CUDA regressions pass on the development build listed above, -using seed 73 and a fresh process per case. The full-route case naturally +The standard headless collector completed one episode with the original point +list and one with the packaged USD on DexSim `0.6.0rc2` (build `dbaca660`), +using the same RTX 5090 D v2 and seed 73. Both passed all 51 segments and final +physical acceptance after two seconds of settling. The original and USD runs +took 5,981 and 5,994 control steps, respectively. The final tip positions differed +by 1.184 mm, both inside the strict channel. Maximum grasp slip was 0.316 mm +and 0.307 mm, respectively, and both runs reported zero final sampled cable +penetration against the bench, spools and port. + +The earlier three NoRender CUDA regressions ran on DexSim development build +`b040478a`, using seed 73 and a fresh process per case. The full-route case naturally exhausts all Task Program segments, validates every stage, then requires `task_success=True` after release, retreat and two seconds of physical settling.