From 92dd7cd9e8bf7cb449cf88bb7c489ccb68b8acf3 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Thu, 19 Nov 2026 14:02:27 +0800 Subject: [PATCH 01/11] Integrate YAM cable routing and USD cable support Carry the net cable stack changes from ac38b4dae relative to the already integrated main snapshot. Retain Task Program routing, contact grasping, USD cable lifecycle and override support, demos and regression coverage. --- NOTICE | 5 + agent_context/MAP.yaml | 2 +- .../topics/atomic-actions/atomic-actions.md | 12 + .../topics/simulation-system/deformables.md | 10 +- .../topics/task-programs/configuration.md | 19 + docs/source/api_reference/public_api.rst | 42 + .../sim/atomic_skills/builtin_skills.md | 28 +- embodichain/compute/geometry/_cable_route.py | 180 +++ embodichain/compute/geometry/cable.py | 105 ++ embodichain/lab/evaluation/_predicates.py | 35 + embodichain/lab/evaluation/_recording.py | 16 +- embodichain/lab/evaluation/observations.py | 2 + .../gym/envs/managers/observations/_common.py | 50 + .../lab/gym/envs/task_program/bridge.py | 13 + .../lab/gym/utils/_component_composition.py | 2 + embodichain/lab/gym/utils/trajectory_state.py | 2 +- .../lab/sim/atomic_actions/__init__.py | 1 + embodichain/lab/sim/atomic_skills/__init__.py | 2 + embodichain/lab/sim/atomic_skills/policies.py | 16 +- .../sim/atomic_skills/primitives/__init__.py | 3 +- .../primitives/move_end_effector.py | 17 + .../atomic_skills/primitives/move_joints.py | 43 +- .../sim/atomic_skills/primitives/pick_up.py | 613 ++++++--- embodichain/lab/sim/cfg/usd.py | 5 +- embodichain/lab/sim/sim_manager.py | 48 +- embodichain/lab/sim/spawn/descriptors.py | 30 +- embodichain/lab/sim/spawn/usd_import.py | 121 +- embodichain/lab/sim/spawn/usd_result.py | 26 +- .../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 | 898 +++++++++++++ .../integrations/simulation/environment.py | 51 +- .../integrations/simulation/policies.py | 18 + .../lab/task_program/runtime/executor.py | 11 + examples/sim/demo/cable_two_posts.py | 416 ++++++ examples/sim/demo/cable_wall_franka.py | 966 ++++++++++++++ .../components/embodiments/yam_i2rt.yaml | 104 ++ .../cable/route_the_cable/README.md | 290 +++++ .../cable/route_the_cable/cable.usda | 36 + .../cable/route_the_cable/catalog.yaml | 13 + .../cable/route_the_cable/env.yaml | 278 ++++ .../cable/route_the_cable/task.yam.yaml | 20 + .../task_program/evaluation.yaml | 104 ++ .../task_program/execution_policy.yaml | 24 + .../task_program/integration.yaml | 765 +++++++++++ .../route_the_cable/task_program/program.yaml | 1142 +++++++++++++++++ .../manipulation/cable/__init__.py | 19 + scripts/validation/accept_usd_import.py | 11 +- setup.py | 1 + tests/compute/geometry/test_cable.py | 101 ++ tests/gym/envs/task_program/test_bridge.py | 10 + .../envs/task_program/test_cable_material.py | 485 +++++++ .../test_configured_integration.py | 10 +- tests/gym/envs/tasks/test_route_the_cable.py | 364 ++++++ .../envs/tasks/test_route_the_cable_cuda.py | 410 ++++++ tests/gym/envs/test_cable_recording_cuda.py | 58 +- tests/lab/evaluation/test_rule_graph.py | 24 + tests/sim/atomic_skills/test_actions.py | 403 +++++- 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 | 83 +- 63 files changed, 8503 insertions(+), 293 deletions(-) create mode 100644 embodichain/compute/geometry/_cable_route.py create mode 100644 embodichain/compute/geometry/cable.py create mode 100644 embodichain/lab/task_program/integrations/simulation/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_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/cable.usda 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/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 create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/__init__.py create mode 100644 tests/compute/geometry/test_cable.py create mode 100644 tests/gym/envs/task_program/test_cable_material.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 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/agent_context/MAP.yaml b/agent_context/MAP.yaml index ab128e81b4..6d02465f2b 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -443,7 +443,7 @@ topics: - id: atomic-actions title: Atomic Skills aliases: [atomic skill, atomic skills, atomic action, 原子技能, 原子动作] - keywords: [PlanningBudget, PlanningRecovery, ExecutionPlanLifecycle, 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 f8927e9e9b..ac5475e91f 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -24,6 +24,18 @@ Hosts acquire versioned measurements; verification is the boundary that commits or invalidates observed effects. Preserve required stop/hold boundaries on the executed control grid instead of relying only on native planner timing. +`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`. + ## Package map | Concern | Source of truth | diff --git a/agent_context/topics/simulation-system/deformables.md b/agent_context/topics/simulation-system/deformables.md index 165ad49dfe..db52a5268f 100644 --- a/agent_context/topics/simulation-system/deformables.md +++ b/agent_context/topics/simulation-system/deformables.md @@ -108,8 +108,14 @@ Focused coverage: `tests/sim/test_deformable_cfg.py`, `CableObjectCfg` and `CableObject(BatchEntity)` are independent of particle-set deformables. `SimulationManager.add_cable_object()` lowers through `spawn/descriptors.py` into `CableDesc`, declares in the existing `SpawnScene`, -and binds the same facade in place during `prepare()`. Backend capabilities -reject Default, CPU, other Newton solvers and gradient mode; use DexUni on CUDA. +and binds the same facade in place during `prepare()`. `add_usd(UsdImportCfg)` +also returns a `CableObject` per imported cable under its source resource key, +with the same UID lookup, segment state and reset as a procedural declaration. +Cable geometry, placement and visuals come from the parsed `CableDesc`. +A sparse `asset_cfgs` entry keyed by the simulation curve prim can override +the UID, `fixed_segments`, `collision_filter_span` and individual `attrs` physics and +contact fields before declaration. Omitted fields retain the parsed values. +Cable declarations require DexUni on CUDA with gradient mode disabled. With this backend, an arena containing a cable cannot also contain fluid, cloth or soft-body particles: cable episode reset does not support those particles. Spawn checks the native cable and particle descriptors for direct diff --git a/agent_context/topics/task-programs/configuration.md b/agent_context/topics/task-programs/configuration.md index 941c80e510..f0fe6c6577 100644 --- a/agent_context/topics/task-programs/configuration.md +++ b/agent_context/topics/task-programs/configuration.md @@ -254,6 +254,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. @@ -268,6 +275,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/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 527e63d5a7..ebe3772b1f 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -43,6 +43,19 @@ Chunked planar distances preserve tensor batch axes, dtype and device. .. autofunction:: signed_distance_to_polygon +embodichain.compute.geometry.cable +---------------------------------- + +Pure tensor geometry for ordered cable endpoints, material-coordinate sampling +and slip of the same material point in a moving tool frame. + +.. currentmodule:: embodichain.compute.geometry.cable + +.. autosummary:: + + sample_cable_material + cable_endpoints + cable_grasp_slip embodichain.compute.geometry.particles -------------------------------------- @@ -3272,6 +3285,22 @@ embodichain.lab.sim.atomic_skills.primitives BUILTIN_SKILL_TYPES +embodichain.lab.sim.atomic_skills.primitives.pick_up +-------------------------------------------------- + +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:: + + GraspGoal + ContactGraspGoal + PickUp + PickUpOptions + embodichain.lab.sim.atomic_skills.requirements ----------------------------------------------- @@ -4879,6 +4908,19 @@ reference-state resets and terminal snapshots before automatic row resets. .. automodule:: embodichain_tasks.locomotion.tracking.g1_motion :members: +embodichain_tasks.manipulation.cable.route_the_cable +---------------------------------------------------- + +Two-spool routing with observed material geometry and measured grasp-slip, +penetration and end-insertion acceptance. + +.. currentmodule:: embodichain_tasks.manipulation.cable.route_the_cable + +.. autosummary:: + + RouteTheCableEnv + measure_route + embodichain_tasks.manipulation.push_cube ---------------------------------------- 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/compute/geometry/_cable_route.py b/embodichain/compute/geometry/_cable_route.py new file mode 100644 index 0000000000..b05c6ae098 --- /dev/null +++ b/embodichain/compute/geometry/_cable_route.py @@ -0,0 +1,180 @@ +# ---------------------------------------------------------------------------- +# 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) + 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) + & (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/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/evaluation/_predicates.py b/embodichain/lab/evaluation/_predicates.py index e63a006b8f..c3712a3a92 100644 --- a/embodichain/lab/evaluation/_predicates.py +++ b/embodichain/lab/evaluation/_predicates.py @@ -488,10 +488,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 f7183e4df0..6317a1c118 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("deformables/"): from ._deformable import _centroid_selection @@ -363,7 +365,19 @@ def get(name, default=None): expected = expected_names[field] if expected is None or tuple(names) != tuple(expected): return None - if quantity == "centroid" and field.startswith("deformables/"): + 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 == "centroid" and field.startswith("deformables/"): contract = ("world", "xyz", ("m",), (3,)) elif quantity in ("pose", "segment_start_pose"): contract = ("arena", "xyz_xyzw", ("m", "unitless"), (7,)) diff --git a/embodichain/lab/evaluation/observations.py b/embodichain/lab/evaluation/observations.py index ac46d07fdd..111fd3f683 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 == "deformables": from ._deformable import _centroid_selection diff --git a/embodichain/lab/gym/envs/managers/observations/_common.py b/embodichain/lab/gym/envs/managers/observations/_common.py index fa4ef217f2..ffd84ec67f 100644 --- a/embodichain/lab/gym/envs/managers/observations/_common.py +++ b/embodichain/lab/gym/envs/managers/observations/_common.py @@ -234,6 +234,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 9de1c60d4d..04e160c9f0 100644 --- a/embodichain/lab/gym/envs/task_program/bridge.py +++ b/embodichain/lab/gym/envs/task_program/bridge.py @@ -1784,6 +1784,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 201b5b972f..9938c1aa79 100644 --- a/embodichain/lab/gym/utils/_component_composition.py +++ b/embodichain/lab/gym/utils/_component_composition.py @@ -672,6 +672,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"), @@ -691,6 +692,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/embodichain/lab/gym/utils/trajectory_state.py b/embodichain/lab/gym/utils/trajectory_state.py index b010e19320..ab77613290 100644 --- a/embodichain/lab/gym/utils/trajectory_state.py +++ b/embodichain/lab/gym/utils/trajectory_state.py @@ -305,7 +305,7 @@ def _evaluation_observation_cfgs( params={"field": name}, consumers=("recording",), ) - elif name.startswith(("links/", "robot/", "robot_links/")): + elif 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_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index cefb9aa904..f17d002d33 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -132,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 6c7a7dbd99..238d58ef43 100644 --- a/embodichain/lab/sim/atomic_skills/__init__.py +++ b/embodichain/lab/sim/atomic_skills/__init__.py @@ -200,6 +200,7 @@ CoordinatedPlacementGoal, CoordinatedPlacementOptions, EndEffectorPoseGoal, + ContactGraspGoal, GraspGoal, HandOver, HandOverGoal, @@ -387,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 fef056f58a..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, @@ -103,6 +103,7 @@ "CoordinatedPlacementGoal", "CoordinatedPlacementOptions", "EndEffectorPoseGoal", + "ContactGraspGoal", "GraspGoal", "HandOver", "HandOverGoal", 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 d1d28cc130..13e972060c 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_skills/primitives/move_joints.py @@ -85,6 +85,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.""" @@ -156,14 +163,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/sim/atomic_skills/primitives/pick_up.py b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py index 3213f9eb22..a0ec0312e5 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, @@ -66,6 +67,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, @@ -85,6 +87,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, @@ -130,6 +133,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) @@ -324,11 +348,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=( @@ -351,10 +375,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 @@ -381,6 +407,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] @@ -388,114 +417,233 @@ 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": + + 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] - 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 - waypoints = [pre_grasp_xpos, grasp_xpos, lift_xpos] - if options.initial_lift_height > 0.0: - start_xpos = self.robot.compute_fk( - qpos=start_arm_qpos, - name=manipulator.control_part, - to_matrix=True, - **( - {"env_ids": self.planning_services.env_ids} - if self.planning_services.env_ids is not None - else {} - ), + approach_start = last_qpos + prefix = None + prefix_valid = torch.ones( + self.num_envs, dtype=torch.bool, device=self.device + ) + if options.initial_lift_height > 0.0: + start_xpos = self.robot.compute_fk( + qpos=start_arm_qpos, + name=manipulator.control_part, + to_matrix=True, + **( + {"env_ids": self.planning_services.env_ids} + if self.planning_services.env_ids is not None + else {} + ), + ) + initial_lift = translate_pose_world( + start_xpos, + start_xpos.new_tensor([0.0, 0.0, options.initial_lift_height]), + ) + prefix_valid, prefix = sampled_move( + approach_start, initial_lift, motion_policy.sample_count + ) + approach_start = last_qpos.clone() + approach_start[:, list(manipulator.joint_ids)] = prefix[:, -1] + if options.pre_grasp_distance > 0: + pre_grasp_valid, pre_grasp = sampled_move( + approach_start, pre_grasp_xpos, motion_policy.sample_count + ) + prefix_valid &= pre_grasp_valid + prefix = ( + pre_grasp + if prefix is None + else torch.cat((prefix, pre_grasp), dim=1) + ) + 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 ) - initial_lift = translate_pose_world( - start_xpos, - start_xpos.new_tensor([0.0, 0.0, options.initial_lift_height]), + 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], ) - waypoints.insert(0, initial_lift) - - if options.cartesian_lift: - if motion_policy.strategy != "ik_interp": - raise ValueError("cartesian_lift requires strategy='ik_interp'.") - if motion_policy.interpolation_mode == "adaptive": - n_approach = max(n_approach, len(waypoints)) - approach_success, approach_arm = plan_named_arm_trajectory( - self.motion_generator, - manipulator.control_part, - start_arm_qpos, - torch.stack(waypoints[:-1], dim=1), - n_approach, - motion_policy, - interpolation_dt, + 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", ) - if n_lift < 2: - raise ValueError("cartesian_lift requires at least two lift samples.") - lift_targets = axis_translation_keyframes( + + lift_xpos = translate_pose_world( grasp_xpos, - lift_xpos, - grasp_xpos.new_tensor([0.0, 0.0, 1.0]), - n_waypoints=n_lift - 1, + torch.tensor([0, 0, 1], device=self.device) * options.lift_height, ) - if bool(approach_success.any()): - lift_success, lift_arm = plan_named_arm_trajectory( + 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 + waypoints = [pre_grasp_xpos, grasp_xpos, lift_xpos] + if options.initial_lift_height > 0.0: + start_xpos = self.robot.compute_fk( + qpos=start_arm_qpos, + name=manipulator.control_part, + to_matrix=True, + **( + {"env_ids": self.planning_services.env_ids} + if self.planning_services.env_ids is not None + else {} + ), + ) + initial_lift = translate_pose_world( + start_xpos, + start_xpos.new_tensor([0.0, 0.0, options.initial_lift_height]), + ) + waypoints.insert(0, initial_lift) + + if options.cartesian_lift: + if motion_policy.strategy != "ik_interp": + raise ValueError("cartesian_lift requires strategy='ik_interp'.") + if motion_policy.interpolation_mode == "adaptive": + n_approach = max(n_approach, len(waypoints)) + approach_success, approach_arm = plan_named_arm_trajectory( self.motion_generator, manipulator.control_part, - approach_arm[:, -1], - lift_targets, - n_lift, + start_arm_qpos, + torch.stack(waypoints[:-1], dim=1), + n_approach, motion_policy, interpolation_dt, - active_mask=approach_success, - cartesian_linear=True, ) + if n_lift < 2: + raise ValueError( + "cartesian_lift requires at least two lift samples." + ) + lift_targets = axis_translation_keyframes( + grasp_xpos, + lift_xpos, + grasp_xpos.new_tensor([0.0, 0.0, 1.0]), + n_waypoints=n_lift - 1, + ) + if bool(approach_success.any()): + lift_success, lift_arm = plan_named_arm_trajectory( + self.motion_generator, + manipulator.control_part, + approach_arm[:, -1], + lift_targets, + n_lift, + motion_policy, + interpolation_dt, + active_mask=approach_success, + cartesian_linear=True, + ) + else: + lift_arm = approach_arm[:, -1:].expand(-1, n_lift, -1).clone() + lift_success = approach_success.clone() + is_success = approach_success & lift_success else: - lift_arm = approach_arm[:, -1:].expand(-1, n_lift, -1).clone() - lift_success = approach_success.clone() - is_success = approach_success & lift_success - else: - motion_result = self.motion_generator.generate( - build_pose_plan_states(torch.stack(waypoints, dim=1)), - options=motion_options, + motion_result = self.motion_generator.generate( + build_pose_plan_states(torch.stack(waypoints, 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=len(waypoints) - 2, + success=motion_result.success, + env_ids=self.planning_services.env_ids, + ) + is_success = motion_result.success + n_approach = approach_arm.shape[1] + n_lift = lift_arm.shape[1] + grasp_arm_qpos = approach_arm[:, -1, :] + + 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 ) - 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=len(waypoints) - 2, - success=motion_result.success, - env_ids=self.planning_services.env_ids, + else: + hand_close_path = interpolate_hand_qpos( + hand_open_qpos, hand_grasp_qpos, n_waypoints=n_close ) - is_success = motion_result.success - n_approach = approach_arm.shape[1] - n_lift = lift_arm.shape[1] - grasp_arm_qpos = approach_arm[:, -1, :] - - 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 @@ -518,7 +666,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: @@ -542,11 +692,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( @@ -594,76 +746,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, @@ -688,6 +849,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, @@ -712,14 +875,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") ) @@ -772,27 +970,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 ): @@ -825,13 +1032,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, @@ -845,31 +1052,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 @@ -1191,4 +1412,4 @@ def _upright_adjusted_grasp_poses( return adjusted_grasp_xpos -__all__ = ["GraspGoal", "PickUp", "PickUpOptions"] +__all__ = ["GraspGoal", "ContactGraspGoal", "PickUp", "PickUpOptions"] 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/sim_manager.py b/embodichain/lab/sim/sim_manager.py index b211809a1a..25ba945b8c 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -3319,7 +3319,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. @@ -3348,7 +3354,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 @@ -3400,6 +3407,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 @@ -3898,22 +3906,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": @@ -3930,6 +3924,24 @@ def add_cable_object(self, cfg: CableObjectCfg) -> CableObject: raise NotImplementedError( "Declare cable objects before scene finalization." ) + + def add_cable_object(self, cfg: CableObjectCfg) -> CableObject: + """Declare a cable before ``prepare()`` using DexSim's public Spawn API. + + Args: + cfg: Typed cable geometry, four-mode material and visual configuration. + + Returns: + A declared facade whose data becomes available after ``prepare()``. + + Raises: + NotImplementedError: Backend/device/solver/gradient mode is unsupported, + or the physical scene has already been finalized. + ValueError: Required configuration or the asset UID is invalid. + """ + if not isinstance(cfg, CableObjectCfg): + raise TypeError("Cable objects require CableObjectCfg.") + self._require_cable_declaration() descriptor, materials = cable_desc_from_cfg(cfg, per_env=True) if descriptor.name in self.asset_uids or descriptor.name in self._spawn_scene: raise ValueError( diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index cd397b8e86..0553f12583 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 239ac45c9d..86e5bc23fa 100644 --- a/embodichain/lab/sim/spawn/usd_import.py +++ b/embodichain/lab/sim/spawn/usd_import.py @@ -19,14 +19,16 @@ from __future__ import annotations from collections import Counter +from collections.abc import Mapping from copy import deepcopy -from dataclasses import dataclass, field, fields +from dataclasses import dataclass, field, fields, replace from typing import TYPE_CHECKING, Any import numpy as np from dexsim.spawn import ( ArticulationDesc, + CableDesc, ConstraintDesc, MeshObjectDesc, SoftBodyDesc, @@ -36,6 +38,9 @@ from embodichain.lab.sim.cfg import ( ArticulationCfg, ArticulationRootPropertiesCfg, + CableAttachmentCfg, + CableObjectCfg, + CablePhysicsCfg, RigidObjectCfg, RobotCfg, VisualObjectCfg, @@ -46,6 +51,7 @@ ) from embodichain.lab.sim.objects import ( Articulation, + CableObject, RigidObject, Robot, VisualObject, @@ -54,10 +60,17 @@ ) from embodichain.lab.sim.sensors import Camera, CameraCfg, StereoCamera, StereoCameraCfg from embodichain.lab.sim.sensors.registry import _resolve_sensor_factory -from .descriptors import _pose_from_cfg, configure_articulation_desc +from .descriptors import ( + _cable_contact_spec, + _compile_newton_collision, + _pose_from_cfg, + _required_uid, + configure_articulation_desc, +) from .usd import ( _configure_articulation_usd, _configure_rigid_usd, + _overlay_optional_properties, _parse_usd_source, _source_pose, _set_resolved_usd_pose, @@ -91,6 +104,67 @@ def _source_path(descriptor: Any) -> str: return usd.prim_path if usd is not None and usd.prim_path else descriptor.name +def _usd_cable_cfg( + descriptor: CableDesc, overrides: Mapping[str, Any] | None +) -> CableObjectCfg: + """Apply sparse cable properties before declaring the descriptor and its facade.""" + if overrides is None: + overrides = {} + if not isinstance(overrides, Mapping): + raise TypeError("USD cable overrides must be a sparse mapping.") + unknown = overrides.keys() - { + "uid", + "attrs", + "fixed_segments", + "collision_filter_span", + } + if unknown: + raise ValueError(f"Unknown USD cable override fields: {sorted(unknown)}.") + attrs = overrides.get("attrs", {}) + if not isinstance(attrs, Mapping): + raise TypeError("USD cable attrs must be a sparse mapping.") + parsed_physics = { + item.name: getattr(descriptor.physics, item.name) + for item in fields(descriptor.physics) + } + physics_cfg = CablePhysicsCfg(**{**parsed_physics, **attrs}) + if overrides: + if "uid" in overrides: + descriptor.name = _required_uid(overrides["uid"], "Cable object") + # Preserve the difference between an omitted coefficient and an explicit zero/None. + descriptor.physics = replace( + descriptor.physics, + **{key: attrs[key] for key in attrs.keys() & parsed_physics.keys()}, + ) + contact = _cable_contact_spec(physics_cfg) + descriptor.collision = _overlay_optional_properties( + descriptor.collision, + _compile_newton_collision(contact, newton_solver_type="dexuni"), + ) + if contact.collision_enabled is not None: + descriptor.enable_collision = contact.collision_enabled + for name in ("fixed_segments", "collision_filter_span"): + if name in overrides: + setattr(descriptor, name, overrides[name]) + # Revalidate indices, attachments and thickness against the source + # geometry before publishing any part of this freshly parsed scene. + descriptor.__post_init__() + + frames = descriptor.segment_quaternions + return CableObjectCfg( + uid=descriptor.name, + points=descriptor.points, + radius=descriptor.radius, + init_local_pose=descriptor.pose, + segment_quaternions=None if frames is None else frames[:, [1, 2, 3, 0]], + closed=descriptor.closed, + fixed_segments=descriptor.fixed_segments, + attrs=physics_cfg, + collision_filter_span=descriptor.collision_filter_span, + render_mode=descriptor.render_mode, + ) + + def _compile_usd_import( manager: SimulationManager | None, recipe: UsdImportCfg, @@ -143,6 +217,8 @@ def _compile_usd_import( manager._spawn_scene._require_compatible_particles( cables=bool(source.cables), particles=bool(source.particle_sets) ) + if source.cables: + manager._require_cable_declaration() resources = list(source.iter_entities()) source_paths_by_id = {id(item): _source_path(item) for item in resources} counts = Counter(source_paths_by_id.values()) @@ -163,7 +239,12 @@ def _compile_usd_import( if item.usd is not None and item.usd.prim_path else item.name ) - for item in (*source.mesh_objects, *source.articulations, *source.particle_sets) + for item in ( + *source.mesh_objects, + *source.articulations, + *source.particle_sets, + *source.cables, + ) } overlap = robot_cfgs.keys() & asset_cfgs.keys() missing = (robot_cfgs.keys() | asset_cfgs.keys()) - source_paths @@ -251,6 +332,21 @@ def _compile_usd_import( and descriptor.physics is None and not descriptor.collisions ) + if isinstance(descriptor, CableDesc): + if root_pose is not None: + descriptor.pose = root_pose @ descriptor.pose + cfg = _usd_cable_cfg(descriptor, configured) + if descriptor.name in occupied: + raise ValueError(f"USD asset UID already exists: {descriptor.name!r}.") + occupied.add(descriptor.name) + renamed[original_name] = descriptor.name + facade = ( + None if manager is None else CableObject(cfg, device=manager.device) + ) + declarations.append( + (resource_keys[id(descriptor)], "cable_object", descriptor, facade) + ) + continue if configured is not None and not isinstance( descriptor, (MeshObjectDesc, ArticulationDesc, SoftBodyDesc, ClothDesc) ): @@ -543,6 +639,23 @@ def renamed_endpoint(endpoint: str) -> str: owner, separator, suffix = endpoint.partition("/") return renamed.get(owner, owner) + separator + suffix + for _, kind, descriptor, facade in declarations: + if kind == "cable_object": + for name in ("start_attachment", "end_attachment"): + anchor = getattr(descriptor, name) + if anchor is not None: + anchor.target = renamed.get(anchor.target, anchor.target) + if facade is not None: + setattr( + facade.cfg, + name, + CableAttachmentCfg( + target=anchor.target, + position=anchor.position.copy(), + link_name=anchor.link_name, + ), + ) + for constraint in source.constraints: if constraint.body0 is not None: constraint.body0 = renamed_endpoint(constraint.body0) @@ -573,6 +686,7 @@ def renamed_endpoint(endpoint: str) -> str: public_kinds = { "soft_object": "volume_deformable", "cloth_object": "surface_deformable", + "cable_object": "cable", } resource_types = { path: "robot" if path in robot_paths else public_kinds.get(kind, kind) @@ -582,7 +696,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/embodichain/lab/task_program/integrations/_configured_composition.py b/embodichain/lab/task_program/integrations/_configured_composition.py index 55647ff708..a44b98083a 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( @@ -266,6 +267,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"], @@ -789,6 +796,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 aa27214dbe..cfdde0f401 100644 --- a/embodichain/lab/task_program/integrations/catalog.py +++ b/embodichain/lab/task_program/integrations/catalog.py @@ -238,6 +238,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") if isinstance(value, PickUpOptions) and value.initial_lift_height == 0.0: diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index f65c2691b3..7c7928fb93 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -885,6 +885,7 @@ def _decode_scene(value: object) -> SimulationSceneBinding: optional=frozenset( { "rigid_objects", + "cables", "rigidized_articulations", "articulations", "links", @@ -893,6 +894,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] = [] @@ -941,6 +959,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, @@ -1075,6 +1094,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", @@ -1340,15 +1361,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, @@ -1791,6 +1824,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", @@ -1858,7 +1892,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 @@ -3142,6 +3180,7 @@ class _DecodedRuntimeServices: ] = () control_part_evidence: _JointPositionConstraintEvidenceProviderFactory | None = None nominal_motion_sources: tuple[_ConfiguredPrototypeSource, ...] = () + cable_material: object | None = None settle_presets: ( tuple[ tuple[ @@ -3258,6 +3297,7 @@ def _decode_runtime_services( "registered_semantic_lowerers", "control_part_evidence", "nominal_motion_sources", + "cable_material", "settle_presets", } ), @@ -3308,6 +3348,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, @@ -3320,6 +3373,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( @@ -3475,6 +3538,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=( @@ -3525,6 +3589,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, transport_targets: Mapping[str, _MoveHeldObjectLowererFactory] | None = None, ) -> tuple[dict[str, CompiledEvaluation], tuple[str, ...], tuple[str, ...]]: if value is None: @@ -3541,6 +3606,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 } @@ -3708,6 +3780,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 @@ -3728,6 +3810,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, transport_targets={ factory.target_id: factory for factory in services.registered_semantic_lowerers diff --git a/embodichain/lab/task_program/integrations/simulation/bindings.py b/embodichain/lab/task_program/integrations/simulation/bindings.py index e09c521ab2..fa9db7642b 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.""" @@ -816,6 +828,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, ... @@ -833,6 +846,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, @@ -1059,6 +1073,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..117be006a3 --- /dev/null +++ b/embodichain/lab/task_program/integrations/simulation/cable.py @@ -0,0 +1,898 @@ +# ---------------------------------------------------------------------------- +# 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 + retract_distance: float = 0.0 + 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) + 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 + 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 == "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): + 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: + # 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 + + +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) + 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]) + .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: + # 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(), + } + 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"}, + "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, + "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", + "material_lift", + "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", "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"}: + 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 dfee0f64d9..5918bc3dd1 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 @@ -916,7 +916,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: @@ -939,7 +945,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: @@ -1115,7 +1124,8 @@ 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( environment=( self._environment_ref() if self._environment_ref is not None else None ), @@ -1125,6 +1135,30 @@ def create_registered_semantic_lowerers( 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() @@ -1210,6 +1244,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( @@ -1220,7 +1259,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 66ec1937b7..f2cc1c2c70 100644 --- a/embodichain/lab/task_program/integrations/simulation/policies.py +++ b/embodichain/lab/task_program/integrations/simulation/policies.py @@ -458,6 +458,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. @@ -468,6 +485,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/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_tasks/configs/components/embodiments/yam_i2rt.yaml b/extensions/embodichain_tasks/configs/components/embodiments/yam_i2rt.yaml new file mode 100644 index 0000000000..ca423d557a --- /dev/null +++ b/extensions/embodichain_tasks/configs/components/embodiments/yam_i2rt.yaml @@ -0,0 +1,104 @@ +embodiment_id: yam_i2rt +simulation: + class_type: YamI2rt + uid: yam + 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: [] +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 new file mode 100644 index 0000000000..eded3688ef --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md @@ -0,0 +1,290 @@ +# Route the Cable (YAM) + +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 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. + +## Requirements and entry point + +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: + +```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. +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 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.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. + +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. + +- `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 + 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. + `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 + 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 +`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. + +## 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 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 +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 +``` + +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 +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 +physical configuration. New recordings retain source Arena origins so a selected +row can replay in a fresh single-environment scene. Older files without origin +metadata require matching Arena placement. `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 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. + +## 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. 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/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/catalog.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/catalog.yaml new file mode 100644 index 0000000000..4711589548 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/catalog.yaml @@ -0,0 +1,13 @@ +task_key: route_the_cable +title: Route the Cable +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: + 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..d4b80fd063 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml @@ -0,0 +1,278 @@ +# 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 + 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 + # 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: latest +max_episode_steps: 12000 +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 + half_height: 0.00125 + 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 + half_height: 0.0085895 + 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 + half_height: 0.0019105000000000007 + 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 + half_height: 0.00125 + 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 + half_height: 0.0085895 + 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 + half_height: 0.0019105000000000007 + 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 + 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 + 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..34dad4c17c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml @@ -0,0 +1,20 @@ +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 +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..452524ba31 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml @@ -0,0 +1,765 @@ +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.0 + 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.055 + - 0.805 + 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.5845 + - 0.072 + - 0.753 + - - 0.5875 + - 0.083 + - 0.758 + 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.7605 + firm: true + port_screen: false + right_lifted_0.162: + kind: check + condition: lift + raise_0: + kind: material_lift + position: + - null + - null + - 0.774 + material_distance: 0.162 + retract_distance: 0.0015 + 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.7605 + firm: false + port_screen: false + right_lifted_0.282: + kind: check + condition: lift + raise_1: + kind: material_lift + position: + - null + - null + - 0.78 + material_distance: 0.282 + retract_distance: 0.003 + 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.7605 + firm: true + port_screen: false + right_lifted_0.438: + kind: check + condition: lift + raise_2: + kind: material_lift + position: + - null + - null + - 0.8 + material_distance: 0.438 + retract_distance: 0.008 + 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.7605 + firm: true + port_screen: true + left_lifted_0.57: + kind: check + condition: lift + stage_free_end: + kind: port_stage + position: + - 0.586 + - 0.055 + - 0.805 + stage_free_end_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.795 + 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.05 + 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.05 + 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.7875 + 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.05 + 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.05 + 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.78 + 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.05 + 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.05 + 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.7655 + 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.05 + minimum_z: 0.7655 + 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.05 + minimum_z: 0.7655 + 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 + finger_position: 0.008 + 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 + 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 new file mode 100644 index 0000000000..b1cd0dea7e --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.yaml @@ -0,0 +1,1142 @@ +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_0.162 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pick + arguments: + target: right_grasp_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 + - 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 + 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_0.282 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pick + arguments: + target: right_grasp_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 + - 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 + 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_0.438 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pick + arguments: + target: right_grasp_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 + - 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 + 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_0.57 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pick + arguments: + 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 + 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_over_port_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_over_port_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_over_port_0_tracking + resources: + primary: left_manipulator +- segment: align_over_port_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_over_port_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_over_port_1_tracking + resources: + primary: left_manipulator +- segment: align_over_port_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_over_port_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: align_over_port_2_tracking + resources: + primary: left_manipulator +- segment: align_over_port_3 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_over_port_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: align_over_port_3_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: 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 + 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/__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/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/setup.py b/setup.py index b18acb621a..da2e680274 100644 --- a/setup.py +++ b/setup.py @@ -177,6 +177,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", 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/gym/envs/task_program/test_bridge.py b/tests/gym/envs/task_program/test_bridge.py index 1746c21e81..3043729f58 100644 --- a/tests/gym/envs/task_program/test_bridge.py +++ b/tests/gym/envs/task_program/test_bridge.py @@ -981,6 +981,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), @@ -996,8 +1004,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..07e8a5f305 --- /dev/null +++ b/tests/gym/envs/task_program/test_cable_material.py @@ -0,0 +1,485 @@ +# ---------------------------------------------------------------------------- +# 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 dataclasses import replace +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, + _CableMoveLowerer, + _CablePickFactory, +) +from embodichain.lab.task_program.semantics import RegisteredSemanticCall +from embodichain.lab.sim.atomic_skills import MoveEndEffectorOptions, 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 == 51 + 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) + + +@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, + ) + + 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, + ) + + +@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")) + 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_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") + 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(), + ) + + +@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 + 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 dc237d6fbb..e68f1efa95 100644 --- a/tests/gym/envs/task_program/test_configured_integration.py +++ b/tests/gym/envs/task_program/test_configured_integration.py @@ -4660,7 +4660,7 @@ 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 path = _config_path("hand_over") @@ -4686,15 +4686,15 @@ def test_legacy_task_program_identities_keep_pre_evaluation_values(): 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 new file mode 100644 index 0000000000..773b5f6459 --- /dev/null +++ b/tests/gym/envs/tasks/test_route_the_cable.py @@ -0,0 +1,364 @@ +# ---------------------------------------------------------------------------- +# 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 xml.etree.ElementTree as ET +import math +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, + config_to_cfg, +) +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.shapes import CylinderCfg + +pytestmark = pytest.mark.no_sim + + +def _route_geometry(): + path = ( + Path(__file__).resolve().parents[4] + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml" + ) + return yaml.safe_load(path.read_text())["runtime_services"]["cable_material"][ + "geometry" + ] + + +def measure_route(state, lengths, hands, origins): + return _measure_cable_route( + state, lengths, hands, origins, _route_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.""" + 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 + (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( + ( + (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 + 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 + 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, half_height=0.00125), + 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["usd_scene"] = {} + with pytest.raises(ValueError, match="usd_scene"): + 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_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() + 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 + (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 - curve.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()) + 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) + with pytest.raises((TypeError, ValueError), match="unknown_cable_option"): + config_to_cfg(data, source_path=p) 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..286962f8a0 --- /dev/null +++ b/tests/gym/envs/tasks/test_route_the_cable_cuda.py @@ -0,0 +1,410 @@ +# ---------------------------------------------------------------------------- +# 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 +import gymnasium as gym +import embodichain_tasks + +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"), +] + + +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.""" + + 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 = gym.make("RouteTheCable-YAM-v1", cfg=cfg).unwrapped + + 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")) + 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 obstacle_paths)], + match="all", + capacity=4096, + device=env.device, + ) + 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( + [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(): + 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_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(): + distance = float(distances.min()) + 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. + # 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) + assert max_slip <= 0.012 + 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()) + + 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(_material_runtime(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 _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.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((_material_runtime(env).metrics()["seat_1"] >= 2).all()) + 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.""" + 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, + ) + + 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, + ) diff --git a/tests/gym/envs/test_cable_recording_cuda.py b/tests/gym/envs/test_cable_recording_cuda.py index 81c39b5eae..41fd6750a0 100644 --- a/tests/gym/envs/test_cable_recording_cuda.py +++ b/tests/gym/envs/test_cable_recording_cuda.py @@ -30,7 +30,7 @@ pytestmark = [pytest.mark.subprocess_sim, pytest.mark.gpu] -def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( +def test_usd_and_procedural_cables_record_and_replay_across_arenas( tmp_path: Path, ) -> None: # A fresh process preserves NoRender without the shared test engine's @@ -44,8 +44,32 @@ def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( """, encoding="utf-8", ) + usd = tmp_path / "wire.usda" + usd.write_text( + """#usda 1.0 +( + defaultPrim = "World" + metersPerUnit = 1 + upAxis = "Z" +) + +def Xform "World" +{ + def BasisCurves "wire" ( + prepend apiSchemas = ["PhysicsCurvesDeformableSimAPI"] + ) + { + int[] curveVertexCounts = [3] + point3f[] points = [(0, 0, 1), (0.1, 0, 1), (0.2, 0, 1)] + uniform token type = "linear" + uniform token wrap = "nonperiodic" + } +} +""", + encoding="utf-8", + ) result = subprocess.run( - [sys.executable, str(Path(__file__).resolve()), str(urdf)], + [sys.executable, str(Path(__file__).resolve()), str(urdf), str(usd)], env=os.environ.copy(), capture_output=True, text=True, @@ -54,7 +78,7 @@ def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( assert result.returncode == 0, result.stdout + result.stderr -def _run_recording(urdf: str) -> None: +def _run_recording(urdf: str, usd: str) -> None: import torch from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg @@ -66,10 +90,13 @@ def _run_recording(urdf: str) -> None: NewtonPhysicsCfg, RenderCfg, RobotCfg, + UsdImportCfg, + UsdSourceCfg, ) cfg = EmbodiedEnvCfg( num_envs=2, + enable_ground_plane=True, max_episode_steps=4, sim_steps_per_control=1, ignore_terminations=True, @@ -90,6 +117,11 @@ def _run_recording(urdf: str) -> None: solver_cfg={"solver_type": "dexuni"}, ), ), + usd_scene=UsdImportCfg( + name="scene", + source=UsdSourceCfg(file_path=usd), + asset_cfgs={"/World/wire": {"uid": "wire", "collision_filter_span": 2}}, + ), robot=RobotCfg( uid="slider", fpath=urdf, @@ -98,12 +130,11 @@ def _run_recording(urdf: str) -> None: ), cable_object=[ CableObjectCfg( - uid=uid, - points=[[0.1 * index, 0, 0] for index in range(segments + 1)], + uid="long_wire", + points=[[0.1 * index, 0, 0] for index in range(4)], radius=0.005, - init_pos=(0.0, y, 1.0), + init_pos=(0.0, 0.5, 1.0), ) - for uid, segments, y in (("wire", 2, 0.0), ("long_wire", 3, 0.5)) ], observations={ "wire": ObservationCfg( @@ -121,11 +152,16 @@ def _run_recording(urdf: str) -> None: env = None try: env = EmbodiedEnv(cfg) + cable = env.usd_scene_import["/World/wire"] + assert cable is env.sim.get_cable_object("wire") + assert cable.cfg.collision_filter_span == 2 assert not env.sim.has_native_renderer obs, _ = env.reset(seed=73) manager = env.observation_manager assert manager is not None and manager.has_history cables = {uid: env.sim.get_cable_object(uid) for uid in cfg.trajectory_uids} + assert cables["wire"].data.num_segments == 2 + assert cables["long_wire"].data.num_segments == 3 fields = {uid: f"cables/{uid}/segment_state_w" for uid in cables} episodes = env.episode_context.episode_ids expected = { @@ -225,6 +261,12 @@ def _run_recording(urdf: str) -> None: assert saved["meta"]["arena_origins_w"] == [source_origin.tolist()] for uid, field in fields.items(): assert saved["raw_observations"]["fields"][field]["values"].shape[1] == 4 + torch.testing.assert_close( + saved["terminal_states"]["cables", uid, "segment_state_w"].cpu(), + latest.values[field][[1]], + rtol=0, + atol=0, + ) env.close(exit_process=False) env = None @@ -258,4 +300,4 @@ def _run_recording(urdf: str) -> None: if __name__ == "__main__": faulthandler.enable() faulthandler.dump_traceback_later(120) - _run_recording(sys.argv[1]) + _run_recording(sys.argv[1], sys.argv[2]) diff --git a/tests/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 c2d3391ad9..02001c2656 100644 --- a/tests/sim/atomic_skills/test_actions.py +++ b/tests/sim/atomic_skills/test_actions.py @@ -946,7 +946,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) @@ -2388,7 +2390,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() @@ -2406,9 +2413,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( @@ -2425,6 +2436,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_pick_failed_candidate_does_not_delay_successful_grasp() -> None: positions = ( @@ -5448,7 +5463,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", @@ -10001,3 +10015,380 @@ def fk(qpos, **kwargs): return commands[0] torch.testing.assert_close(run(True), run(False), rtol=0, atol=0) + + +@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() + 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( + skill, + SkillInvocation( + 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] == ["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") + 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), + ) + 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("pre_grasp_distance", [0.0, 0.03]) +def test_contact_pickup_initial_lift_precedes_approach( + pre_grasp_distance: float, +) -> None: + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + + generator = _dual_motion_generator() + generator.generate = Mock(wraps=generator.generate) + skill = _bind_action(generator, PickUp()) + plan = _plan_action( + skill, + SkillInvocation( + 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, + initial_lift_height=0.08, + pre_grasp_distance=pre_grasp_distance, + lift_height=0.0185, + ), + ), + _dual_context(), + ) + + assert plan.plan_success.all() + calls = generator.generate.call_args_list + assert len(calls) == (4 if pre_grasp_distance else 3) + initial_lift_target = calls[0].args[0][-1].xpos + torch.testing.assert_close( + initial_lift_target[:, :3, 3], + torch.tensor([0.0, 0.0, 0.08]).expand(NUM_ENVS, -1), + ) + torch.testing.assert_close( + calls[-2].args[0][-1].xpos, torch.eye(4).repeat(NUM_ENVS, 1, 1) + ) + assert plan.requires_effect_verification + generator.robot.set_qpos.assert_not_called() + + +@pytest.mark.parametrize("occupied", ["held", "coordinated"]) +def test_contact_pickup_rejects_occupied_task_resource(occupied): + from embodichain.lab.sim.atomic_skills import ( + ContactGraspGoal, + CoordinatedHeldObjectState, + ) + + 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( + "bad_pose", [torch.zeros(3, 3), torch.full((4, 4), float("nan"))] +) +def test_contact_pickup_rejects_invalid_goal_pose(bad_pose): + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + + with pytest.raises(ValueError): + 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() + + +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()) 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 723db485a5..3296d28f09 100644 --- a/tests/sim/spawn/test_usd_scene_overrides.py +++ b/tests/sim/spawn/test_usd_scene_overrides.py @@ -18,6 +18,7 @@ from __future__ import annotations +from copy import deepcopy from types import SimpleNamespace from unittest.mock import MagicMock @@ -27,6 +28,9 @@ from pxr import Sdf, Usd, UsdGeom, UsdPhysics from dexsim.spawn import ( + CableDesc, + CablePhysicsDesc, + NewtonCollisionDesc, MeshObjectDesc, ConstraintDesc, RigidBodyPhysicsDesc, @@ -43,6 +47,7 @@ from embodichain.lab.sim.sim_manager import SimulationManager from embodichain.lab.sim.spawn.scene import SpawnScene from embodichain.lab.sim.cfg import UsdImportCfg, UsdSourceCfg +from embodichain.lab.sim.objects import CableObject pytestmark = pytest.mark.no_sim @@ -61,6 +66,7 @@ def _manager(): "_rigid_object_groups", "_deformable_objects", "_fluid_objects", + "_cable_objects", "_articulations", ): setattr(manager, name, {}) @@ -928,6 +934,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", @@ -943,11 +954,81 @@ def test_curve_prim_resources_keep_all_uids_and_stable_source_keys( assert dict(result.resource_source_paths) == { key: "/World/Cable" for key in expected_keys } - assert len(result) == 0 + assert list(result) == expected_keys + for key in expected_keys: + cable = result[key] + uid = result.resource_uids[key] + assert isinstance(cable, CableObject) + assert cable is manager.get_cable_object(uid) is manager.get_asset(uid) + assert cable.is_declared and cable.data is None with pytest.raises(TypeError): result.resource_source_paths[expected_keys[0]] = "/Other" +def test_usd_cable_sparse_overrides_preserve_source_geometry_and_material(monkeypatch): + import dexsim.kit.usd + + source = _source() + key = "/World/Wire" + points = np.array([[0, 0, 1], [0.2, 0, 1], [0.55, 0, 1]], dtype=np.float32) + source.cables.append( + CableDesc( + name="wire", + points=points, + radius=0.015, + usd=UsdSourceDesc(prim_path=key), + physics=CablePhysicsDesc(density=321, twist_stiffness=7, bend_damping=2), + collision=NewtonCollisionDesc(mu=0.77), + ) + ) + monkeypatch.setattr(dexsim.kit.usd, "parse_usd", lambda *a, **k: deepcopy(source)) + manager = _manager() + manager.device = torch.device("cuda") + manager.physics = SimpleNamespace( + name="newton", solver_type="dexuni", supports_cables=True + ) + manager.sim_config.physics_cfg = SimpleNamespace(requires_grad=False) + recipe = UsdImportCfg( + name="scene", + source=UsdSourceCfg(file_path="scene.usda"), + asset_cfgs={key: {"fixed_segments": [2]}}, + ) + with pytest.raises(ValueError, match="fixed_segments"): + manager.add_usd(recipe) + manager._spawn_scene.builder.add_scene.assert_not_called() + + recipe.asset_cfgs[key] = { + "uid": "cable", + "fixed_segments": [0], + "collision_filter_span": 2, + "attrs": { + "stretch_stiffness": 1e5, + "bend_damping": 0, + "collision_props": {"contact_offset": 0.01, "rest_offset": 0.0}, + "material_props": {"backend": "newton", "ke": 10000, "kd": 0.1}, + }, + } + imported = manager.add_usd(recipe) + native = manager._spawn_scene.builder.add_scene.call_args.args[0].cables[0] + assert native.name == imported.resource_uids[key] == "cable" + facade = manager.get_cable_object("cable") + assert facade is imported[key] + np.testing.assert_array_equal(native.points, points) + assert native.radius == pytest.approx(0.015) + assert native.fixed_segments == facade.cfg.fixed_segments == (0,) + assert native.collision_filter_span == facade.cfg.collision_filter_span == 2 + for physics in (native.physics, facade.cfg.attrs): + assert physics.stretch_stiffness == 1e5 + assert physics.bend_damping == 0 + assert physics.density == 321 + assert physics.twist_stiffness == 7 + assert native.collision.mu == 0.77 + assert native.collision.margin == 0 + assert native.collision.gap == 0.01 + assert native.collision.ke == 10000 + assert native.collision.kd == 0.1 + + def test_portable_sparse_articulation_pose_matrix_is_decoded(monkeypatch): import json import dexsim.kit.usd From 1eab45975adcff3a6c8f64d4ff531bd6a9a23686 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 16 Nov 2026 16:43:23 +0800 Subject: [PATCH 02/11] 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. From d82f409a594bdb9349f988b1149f940a51a04462 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 16 Nov 2026 17:13:30 +0800 Subject: [PATCH 03/11] Remove the unqualified Franka wall-wrapping demo --- examples/sim/demo/cable_wall_franka.py | 966 ------------------------- 1 file changed, 966 deletions(-) delete mode 100644 examples/sim/demo/cable_wall_franka.py diff --git a/examples/sim/demo/cable_wall_franka.py b/examples/sim/demo/cable_wall_franka.py deleted file mode 100644 index ed5dee6d9c..0000000000 --- a/examples/sim/demo/cable_wall_franka.py +++ /dev/null @@ -1,966 +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. -# ---------------------------------------------------------------------------- -"""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) From abb846f11cc8d7a96b9a10de6e8678c186253f7f Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 16 Nov 2026 17:28:30 +0800 Subject: [PATCH 04/11] Start the YAM cable route with both open grippers above the board --- .../manipulation/cable/route_the_cable/README.md | 11 +++++++++++ .../manipulation/cable/route_the_cable/task.yam.yaml | 8 ++++++++ tests/gym/envs/tasks/test_route_the_cable_cuda.py | 11 +++++++++++ 3 files changed, 30 insertions(+) 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 90a2922c07..4bf27bb883 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 @@ -96,6 +96,11 @@ once the release condition is reached. They do not run an unbounded Python loop. The program contains 51 segments and 129 Semantic Call invocations. +The deployment starts with both grippers open, 180 mm above the bench. +In the arena frame, the right TCP starts at approximately `(0.45, -0.26, 0.93)` m +and the left at `(0.45, 0.26, 0.93)` m. Both tool axes point down and tilt +25° toward +X. Each arm returns to its own starting pose after releasing the cable. + 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.57 m, leaving a 30 mm free span. All four @@ -242,6 +247,12 @@ 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. +With the two-arm ready pose, the same USD deployment passed all 51 segments +in 4,802 control steps at seed 73. The first approaches took 181 steps for +the right arm and 301 for the left, down from 638 and 518. The separate +full-route contact regression reported zero robot 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 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 34dad4c17c..b357fa33e5 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 @@ -3,6 +3,14 @@ environment: component: env.yaml embodiment: component: ../../../../components/embodiments/yam_i2rt.yaml + overrides: + # Park the open grippers above their own sides of the bench, facing the cable. + # Arm joints are paired left/right (J1 through J6), followed by the four fingers. + init_qpos: [ + -0.414, 0.359, 0.945, 0.943, 1.021, 1.019, + -1.243, -1.236, -0.171, 0.149, 1.192, -1.243, + 0.0475, 0.0475, 0.0475, 0.0475, + ] headless: true render_cfg: renderer: no-render 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 286962f8a0..696ec3739f 100644 --- a/tests/gym/envs/tasks/test_route_the_cable_cuda.py +++ b/tests/gym/envs/tasks/test_route_the_cable_cuda.py @@ -244,6 +244,17 @@ def test_full_route_keeps_released_endpoint_in_channel( settling_steps = 0 last_segment = None env.reset() + qpos = env.robot.get_qpos() + for side, ready_y in (("right", -0.26), ("left", 0.26)): + arm = f"{side}_arm" + tcp = env.robot.compute_fk( + qpos[:, env.robot.get_joint_ids(arm)], name=arm, to_matrix=True + )[0] + torch.testing.assert_close( + tcp[:3, 3], tcp.new_tensor([0.45, ready_y, 0.93]), atol=0.01, rtol=0 + ) + assert tcp[2, 2] < -0.85 # Tool +Z points down toward the work surface. + assert bool((qpos[:, env.robot.get_joint_ids(f"{side}_eef")] > 0.045).all()) for segment in env.create_demo_segments(): last_segment = segment.name segment_distance = 0.0 From f0d3dbbc6c56a5c902ec2adaa552c7bc23900b26 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Mon, 16 Nov 2026 17:54:04 +0800 Subject: [PATCH 05/11] Qualify cable task motion and isolate per-environment check failures Check authored joint speeds from the observed pose through the last sample. Carry measured cable check failures through grounding and planning by physical environment, and verify that healthy rows can finish the following stage. Reconcile the drawer declaration fingerprints and enable Viser updates in the two-post cable preview, with and without video. --- .../topics/task-programs/configuration.md | 10 +- .../primitives/coordinated_pickment.py | 27 +--- .../atomic_skills/primitives/move_joints.py | 13 +- .../lab/sim/motion/motion_generator.py | 23 +++ .../lab/task_program/compiler/lowering.py | 32 ++++ .../integrations/simulation/cable.py | 14 +- .../lab/task_program/runtime/executor.py | 29 +++- examples/sim/demo/cable_two_posts.py | 22 ++- .../envs/task_program/test_cable_material.py | 150 +++++++++++++----- .../task_program/test_task_vertical_slices.py | 7 +- tests/sim/atomic_skills/test_actions.py | 31 ++++ 11 files changed, 273 insertions(+), 85 deletions(-) diff --git a/agent_context/topics/task-programs/configuration.md b/agent_context/topics/task-programs/configuration.md index f0fe6c6577..e96a854a11 100644 --- a/agent_context/topics/task-programs/configuration.md +++ b/agent_context/topics/task-programs/configuration.md @@ -257,9 +257,11 @@ 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. +sampled joint commands at the host cadence using `ik_interp`. The joint speed +check includes the step from the observed posture to the first command and every +subsequent step. Each row must satisfy the effective per-joint limits. +Registration fingerprints include shared skill goal types and declared motion +policies, even when a deployment uses their defaults. Configured motion also accepts explicit `interpolation_mode` and `ik_seed_mode`. `fixed_grid` requires `acceleration_limit: null` and no scalar speed override; @@ -282,6 +284,8 @@ 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. +Measured cable conditions reject individual eligible rows. Healthy rows continue +to the next stage. 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 diff --git a/embodichain/lab/sim/atomic_skills/primitives/coordinated_pickment.py b/embodichain/lab/sim/atomic_skills/primitives/coordinated_pickment.py index e80eccaec1..ba095aa610 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/coordinated_pickment.py +++ b/embodichain/lab/sim/atomic_skills/primitives/coordinated_pickment.py @@ -1483,14 +1483,14 @@ def _plan_synchronized_object_motion( ) left_valid &= torch.isfinite(left_projected).all(dim=-1) right_valid &= torch.isfinite(right_projected).all(dim=-1) - left_step_valid = self._joint_steps_within_velocity_limits( + left_step_valid = self.motion_generator.joint_steps_within_velocity_limits( left_start_qpos, left_projected, resources.left_arm.control_part, velocity_limit=velocity_limit, step_dt=step_dt, ) - right_step_valid = self._joint_steps_within_velocity_limits( + right_step_valid = self.motion_generator.joint_steps_within_velocity_limits( right_start_qpos, right_projected, resources.right_arm.control_part, @@ -1527,29 +1527,6 @@ def _plan_synchronized_object_motion( ) return success_mask, left_projected, right_projected - def _joint_steps_within_velocity_limits( - self, - start_qpos: torch.Tensor, - trajectory: torch.Tensor, - control_part: str, - *, - velocity_limit: float | None, - step_dt: float, - ) -> torch.Tensor: - """Reject dense IK paths that jump farther than one control interval.""" - limits = self.motion_generator._resolve_velocity_limits( - control_part=control_part, - batch_size=self.num_envs, - dof=start_qpos.shape[-1], - requested=velocity_limit, - reference=start_qpos, - ) - positions = torch.cat((start_qpos[:, None], trajectory), dim=1) - max_step = limits.view(1, 1, -1) * step_dt - return torch.isfinite(positions).all(dim=(1, 2)) & ( - positions.diff(dim=1).abs() <= max_step + 1.0e-6 - ).all(dim=(1, 2)) - def _plan( self, request: ResolvedSkillRequest[CoordinatedPickGoal, CoordinatedPickmentOptions], diff --git a/embodichain/lab/sim/atomic_skills/primitives/move_joints.py b/embodichain/lab/sim/atomic_skills/primitives/move_joints.py index 13e972060c..40cd00277d 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_skills/primitives/move_joints.py @@ -86,7 +86,7 @@ class MoveJointsOptions(SkillOptions): """Per-invocation behavior for :class:`MoveJoints`.""" preserve_samples: bool = False - """Consume explicitly sampled joint positions at the host control cadence.""" + """Keep authored joint samples at host cadence, rejecting overspeed rows.""" def __post_init__(self) -> None: if type(self.preserve_samples) is not bool: @@ -170,15 +170,22 @@ def _plan( if target_qpos.ndim != 3: raise ValueError("preserve_samples requires batched joint samples.") + step_dt = context.require_control_dt() dt = torch.full( target_qpos.shape[:2], - context.require_control_dt(), + step_dt, dtype=torch.float64, device=target_qpos.device, ) dt[:, 0] = 0 result = PlanResult( - success=torch.isfinite(target_qpos).all(dim=(1, 2)), + success=self.motion_generator.joint_steps_within_velocity_limits( + start_qpos, + target_qpos, + control_part, + velocity_limit=request.motion_policy.velocity_limit, + step_dt=step_dt, + ), positions=target_qpos, dt=dt, ) diff --git a/embodichain/lab/sim/motion/motion_generator.py b/embodichain/lab/sim/motion/motion_generator.py index 88bae56c6a..6129f2417c 100644 --- a/embodichain/lab/sim/motion/motion_generator.py +++ b/embodichain/lab/sim/motion/motion_generator.py @@ -695,6 +695,29 @@ def _resolve_velocity_limits( requested_limits, ) + def joint_steps_within_velocity_limits( + self, + start_qpos: torch.Tensor, + trajectory: torch.Tensor, + control_part: str, + *, + velocity_limit: float | None, + step_dt: float, + ) -> torch.Tensor: + """Check each row of a sampled joint path, including its first command.""" + limits = self._resolve_velocity_limits( + control_part=control_part, + batch_size=start_qpos.shape[0], + dof=start_qpos.shape[-1], + requested=velocity_limit, + reference=start_qpos, + ) + positions = torch.cat((start_qpos[:, None], trajectory), dim=1) + max_step = limits.view(1, 1, -1) * step_dt + return torch.isfinite(positions).all(dim=(1, 2)) & ( + positions.diff(dim=1).abs() <= max_step + 1.0e-6 + ).all(dim=(1, 2)) + @classmethod def register_planner_type(cls, name: str, planner_class, planner_cfg_class) -> None: """ diff --git a/embodichain/lab/task_program/compiler/lowering.py b/embodichain/lab/task_program/compiler/lowering.py index c2d01e751f..36236d355f 100644 --- a/embodichain/lab/task_program/compiler/lowering.py +++ b/embodichain/lab/task_program/compiler/lowering.py @@ -609,7 +609,20 @@ class SemanticLowering: ) registered_effect: RegisteredSemanticEffect | None = None + failed_preconditions: Mapping[int, str] = field(default_factory=dict) + """Measured grounding failures by physical environment ID.""" + def __post_init__(self) -> None: + failures = dict(self.failed_preconditions) + if any( + type(env_id) is not int + or env_id < 0 + or not isinstance(reason, str) + or not reason + for env_id, reason in failures.items() + ): + raise ValueError("Grounding failures require physical env IDs and reasons.") + object.__setattr__(self, "failed_preconditions", MappingProxyType(failures)) if self.skill_options is not None and not isinstance( self.skill_options, SkillOptions ): @@ -875,6 +888,7 @@ class GroundedSemanticCall: effect_monitor: EffectMonitor | None = field(repr=False, compare=False) effect_guards: tuple[GroundedHeldObjectGuard, ...] effect_gates: tuple[GroundedPhaseEffectGate, ...] + failed_preconditions: Mapping[int, str] _eligible_mask: torch.Tensor = field(repr=False, compare=False) def __init__(self, *args: object, **kwargs: object) -> None: @@ -895,6 +909,7 @@ def _create( effect_monitor: EffectMonitor | None, effect_guards: tuple[GroundedHeldObjectGuard, ...], effect_gates: tuple[GroundedPhaseEffectGate, ...], + failed_preconditions: Mapping[int, str], eligible_mask: torch.Tensor, ) -> GroundedSemanticCall: """Create one compiler-owned grounded result.""" @@ -905,6 +920,11 @@ def _create( object.__setattr__(instance, "effect_monitor", effect_monitor) object.__setattr__(instance, "effect_guards", tuple(effect_guards)) object.__setattr__(instance, "effect_gates", tuple(effect_gates)) + object.__setattr__( + instance, + "failed_preconditions", + MappingProxyType(dict(failed_preconditions)), + ) object.__setattr__(instance, "_eligible_mask", eligible_mask.clone()) instance.__post_init__() return instance @@ -1750,6 +1770,17 @@ def ground( invocation, phase_effect_gates=tuple(value.requirement for value in effect_gates), ) + failures = lowering.failed_preconditions + if failures: + if not failures.keys() <= set(context.env_ids.tolist()): + raise ValueError( + "Grounding failures must refer to selected environment IDs." + ) + failures = { + i: failures[i] + for i in context.env_ids[eligible].tolist() + if i in failures + } return GroundedSemanticCall._create( analyzed=analyzed, invocation=invocation, @@ -1757,6 +1788,7 @@ def ground( effect_monitor=effect_monitor, effect_guards=effect_guards, effect_gates=effect_gates, + failed_preconditions=failures, eligible_mask=eligible, ) diff --git a/embodichain/lab/task_program/integrations/simulation/cable.py b/embodichain/lab/task_program/integrations/simulation/cable.py index 117be006a3..d5034149f7 100644 --- a/embodichain/lab/task_program/integrations/simulation/cable.py +++ b/embodichain/lab/task_program/integrations/simulation/cable.py @@ -603,15 +603,21 @@ def lower( 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()}" + failures = {} + for row in (~passed).nonzero().flatten().tolist(): + measured = {name: values[row] for name, values in details.items()} + measured["max_slip_m"] = service.max_slip[row].item() + failures[int(context.env_ids[row])] = ( + f"Cable condition {target.target_id!r} failed: {measured}" ) 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))) + return SemanticLowering( + goal=JointPositionGoal(qpos[:, None].repeat(1, 2, 1)), + failed_preconditions=failures, + ) @dataclass(frozen=True, slots=True) diff --git a/embodichain/lab/task_program/runtime/executor.py b/embodichain/lab/task_program/runtime/executor.py index e6460e15f8..2352e4eb40 100644 --- a/embodichain/lab/task_program/runtime/executor.py +++ b/embodichain/lab/task_program/runtime/executor.py @@ -30,7 +30,7 @@ from embodichain.lab.sim.atomic_skills.bindings import EndpointBinding from embodichain.lab.sim.atomic_skills.engine import AtomicSkillEngine from embodichain.lab.sim.atomic_skills.invocation import ResolvedSkillRequest -from embodichain.lab.sim.atomic_skills.plans import SkillPlan +from embodichain.lab.sim.atomic_skills.plans import PlanningFailure, SkillPlan from embodichain.lab.sim.atomic_skills.effects import StateDelta from embodichain.lab.sim.atomic_skills.execution import ( ExecutionEvent, @@ -1214,7 +1214,13 @@ def _prepare_grounded_call( nominal_source = self._nominal_source_factory.create_nominal_source( plan_request, engine=self._engine ) - if effect_guards: + failures = getattr(grounded, "failed_preconditions", {}) + if failures or effect_guards: + passes = ( + ~torch.isin(context.env_ids, context.env_ids.new_tensor(list(failures))) + if failures + else None + ) external_plan_transform = plan_transform def validate_guarded_plan( @@ -1226,6 +1232,25 @@ def validate_guarded_plan( plan = external_plan_transform(request, planning_context, plan) if not isinstance(plan, SkillPlan): raise TypeError("Plan transform must return an SkillPlan.") + if passes is not None: + plan = replace( + plan, + plan_success=plan.plan_success & passes, + diagnostics=replace( + plan.diagnostics, + failure=plan.diagnostics.failure + or PlanningFailure( + "semantic_precondition_failed", retryable=False + ), + messages=( + *plan.diagnostics.messages, + *( + f"env {i}: {reason}" + for i, reason in failures.items() + ), + ), + ), + ) if plan.plan_success.any(): names = {segment.name for segment in plan.segments} for guard in effect_guards: diff --git a/examples/sim/demo/cable_two_posts.py b/examples/sim/demo/cable_two_posts.py index c10843a667..ab006df761 100644 --- a/examples/sim/demo/cable_two_posts.py +++ b/examples/sim/demo/cable_two_posts.py @@ -32,6 +32,11 @@ import numpy as np from scipy.interpolate import make_interp_spline +from embodichain.lab.visualization import ( + add_viser_args_to_parser, + visualization_cfg_from_args, +) + FPS = 60 DURATION = 14.0 RADIUS = 0.008 @@ -166,7 +171,7 @@ def run(args: argparse.Namespace) -> None: NewtonPhysicsCfg, NewtonRigidBodyMaterialCfg, RenderCfg, - DLSSCfg, + DenoisingCfg, RigidBodyPhysicsCfg, ) from embodichain.lab.sim.sensors import CameraCfg @@ -187,8 +192,11 @@ def run(args: argparse.Namespace) -> None: headless=True, startup_summary="compact", robot_ik_gizmo=None, + visualization=visualization_cfg_from_args(args), render_cfg=RenderCfg( - renderer="hybrid", spp=4, dlss=DLSSCfg(dlss_enabled=False) + renderer="no-render" if args.no_video else "hybrid", + spp=4, + denoising=DenoisingCfg(window="off", offscreen="off"), ), physics_cfg=NewtonPhysicsCfg( physics_dt=1 / FPS, @@ -291,8 +299,11 @@ def run(args: argparse.Namespace) -> 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): + write_video_frame = camera is not None and frame % 2 == 0 if frame: - sim.update(step=1, render_final_step=False) + sim.update( + step=1, render_final_step=args.viser and not write_video_frame + ) state = cable.data.segment_state_w[0].detach().cpu() p = xyz_quat_to_4x4_matrix(state[:, :7]).numpy() v = state[:, 7:].numpy() @@ -310,8 +321,8 @@ def run(args: argparse.Namespace) -> None: flush=True, ) phase = current - if camera is not None and frame % 2 == 0: - with sim.render_frame(capture_camera_images=False): + if write_video_frame: + with sim.render_frame(capture_camera_images=args.viser): sim.sync_render_state() camera.update() rgb = camera.get_data()["color"][0, ..., :3].cpu().numpy() @@ -407,6 +418,7 @@ def run(args: argparse.Namespace) -> None: if __name__ == "__main__": parser = argparse.ArgumentParser(description=__doc__) + add_viser_args_to_parser(parser) parser.add_argument("--output", type=Path, required=True) parser.add_argument("--segments", type=int, default=64) parser.add_argument("--duration", type=float, default=DURATION) diff --git a/tests/gym/envs/task_program/test_cable_material.py b/tests/gym/envs/task_program/test_cable_material.py index 07e8a5f305..021a6fdb4e 100644 --- a/tests/gym/envs/task_program/test_cable_material.py +++ b/tests/gym/envs/task_program/test_cable_material.py @@ -54,7 +54,7 @@ def _declaration(): ]["cable_material"] -def test_cable_deployment_preflights_all_calls_and_measured_final_acceptance(): +def _deployment(): path = _TASK / "task.yam.yaml" raw = yaml.safe_load(path.read_text()) physical = resolve_gym_components(raw, base_dir=path.parent) @@ -65,6 +65,11 @@ def test_cable_deployment_preflights_all_calls_and_measured_final_acceptance(): base_dir=path.parent, ) physical.validate_scene_bindings(deployment.scene_binding) + return deployment + + +def test_cable_deployment_preflights_all_calls_and_measured_final_acceptance(): + deployment = _deployment() registration = deployment.integration.registration compiled = registration.catalog.preflight(deployment.load_program()) assert compiled.segment_count == 51 @@ -367,11 +372,7 @@ def test_seated_rows_preserve_contact_loading_command_while_other_rows_move(): 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") - target[:, :2, :2] *= -1 - service.last_targets["left_arm"] = target +def _lower_check(service, target): lowerer = _CableCheckLowerer(service.cfg) lowerer.runtime = service endpoint = SimpleNamespace(runtime_target=SimpleNamespace(control_part="left_arm")) @@ -380,17 +381,24 @@ def test_cable_tracking_rejects_wrong_orientation_at_same_position(): resources={"primary": SimpleNamespace(endpoints={"motion": endpoint})} ) ) - call = RegisteredSemanticCall( - call_id="simulation.cable_check", - arguments={"target": "stage_free_end_tracking"}, + return lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.cable_check", arguments={"target": target} + ), + context=SimpleNamespace(env_ids=service.env_ids), + bound=bound, + option_template=MoveJointsOptions(), ) - 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_tracking_rejects_wrong_orientation_at_same_position(): + service, _, _ = _runtime() + target = service.tool("left_arm") + target[:, :2, :2] *= -1 + service.last_targets["left_arm"] = target + failures = _lower_check(service, "stage_free_end_tracking").failed_preconditions + assert set(failures) == {3, 1} + assert "orientation_error_rad" in failures[3] @pytest.mark.parametrize("tracking_error", ["position", "orientation"]) @@ -407,25 +415,8 @@ def test_release_alignment_above_channel_does_not_bypass_tcp_tracking(tracking_e ) 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(), - ) + failures = _lower_check(service, "lower_channel_3_tracking").failed_preconditions + assert set(failures) == {1} # 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": @@ -434,12 +425,91 @@ def test_release_alignment_above_channel_does_not_bypass_tcp_tracking(tracking_e 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(), + assert not _lower_check(service, "lower_channel_3_tracking").failed_preconditions + + +@pytest.mark.parametrize("prior_slip", [False, True]) +def test_cable_check_allows_surviving_row_to_execute_next_segment(prior_slip): + from test_simulation_environment import _DualRobot, _motion_generator + from embodichain.lab.task_program.integrations import SimulationTaskProgramFactory + + # Run the deployed YAM check and two short holds through the actual semantic + # executor, command sink and Gym bridge, with selected physical rows 3, 1. + deployment = _deployment() + full = deployment.load_program() + first = full.program.items[0] + _, hold, check = first.steps.items + segments = tuple( + replace(first, name=name, steps=node) + for name, node in (("before", hold), ("check", check), ("after", hold)) + ) + program = replace(full, program=replace(full.program, items=segments)) + service, _, _ = _runtime() + robot = _DualRobot() + robot._joint_ids = { + "left_arm": (0,), + "right_arm": (1,), + "left_eef": (2, 3), + "right_eef": (4, 5), + } + robot.control_parts = { + part: tuple(f"joint_{i}" for i in ids) for part, ids in robot._joint_ids.items() + } + robot.dof = 6 + robot.qpos = torch.zeros(4, robot.dof) + robot.body_data = service.robot.body_data + simulation = service.simulation + simulation.get_robot = lambda uid: robot if uid == robot.uid else None + generator = _motion_generator(robot) + generator._env_ids = (3, 1) + generator.planner.cfg.planner_type = "trapezoidal" + generator.joint_steps_within_velocity_limits.return_value = torch.ones( + 2, dtype=torch.bool + ) + factory = SimulationTaskProgramFactory( + simulation, + robot, + deployment.integration.registration, + step_dt=1 / 60, + motion_generator_factory=lambda: generator, ) + adapter = factory.for_env_ids((3, 1)).create_adapter() + bridge = adapter.create_bridge(adapter.compile(program)) + cable = bridge.post_policy_port._cable_runtime + cable.last_targets["right_arm"] = cable.tool("right_arm") + if prior_slip: + cable.max_slip[0] = 0.013 + else: + cable.last_targets["right_arm"][0, 0, 3] += 0.1 + + accepted = [] + rejected_ids = [] + for index, segment in enumerate(bridge.iter_segments()): + commands = list(segment.actions) + assert any( + a.metadata["bridge_action_kind"] == "runtime_command" for a in commands + ) + rejected_ids.extend( + i for a in commands for i in a.metadata.get("rejected_env_ids", ()) + ) + accepted.append(segment.validator().tolist()) + call = segment.metadata["runtime"]["calls"][0] + if index == 1: + diagnostics = call["plan_attempts"][-1]["planner_diagnostics"] + if prior_slip: + assert call["masks"]["entered"] == [False, True] + assert not diagnostics["messages"] + else: + assert call["masks"]["failed"] == [True, False] + assert diagnostics["failure"]["code"] == "semantic_precondition_failed" + assert diagnostics["messages"][0].startswith("env 3: Cable condition") + if index == 2: + assert call["masks"]["entered"] == [False, True] + assert call["masks"]["completed"] == [False, True] + assert accepted == [[not prior_slip, True], [False, True], [False, True]] + # The earlier slip went through the bridge's physical guard. A new check + # failure is reported directly by the semantic executor during planning. + assert rejected_ids == ([3] if prior_slip else []) def test_cable_observation_rejects_invalid_quaternion_per_row(): 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 4996eb2186..deb3a1a833 100644 --- a/tests/gym/envs/task_program/test_task_vertical_slices.py +++ b/tests/gym/envs/task_program/test_task_vertical_slices.py @@ -531,14 +531,15 @@ def test_open_drawer_config_owns_registered_lowerer_factory() -> None: integration = _drawer_integration() registration = integration.registration - # The joint-target extension must preserve the shipped handle-only deployment. + # The handle-only deployment also fingerprints the shared PickUp goal types + # and the interpolation step declared by its motion policies. assert ( registration.fingerprint - == "c8767354951735e47897e046ba35bc26b8e8f47433915174655d4c5247ec2a88" + == "d4a935e630c9c912eeedd1e6c443617dfeae9a725cec15778a14bb383c7e2366" ) assert ( integration.integration_fingerprint - == "aede0eed53368155b3d58bf288188079f9278fd883e342197ee38c101123b7da" + == "7b3223ed6d865f6863d3596a8b4350308d13a55182217e096ab5090b698a1a75" ) assert tuple(registration.catalog.registered_semantic_lowerer_declarations) == ( _OPEN_DRAWER_CALL_ID, diff --git a/tests/sim/atomic_skills/test_actions.py b/tests/sim/atomic_skills/test_actions.py index 02001c2656..929e442a9e 100644 --- a/tests/sim/atomic_skills/test_actions.py +++ b/tests/sim/atomic_skills/test_actions.py @@ -10362,6 +10362,37 @@ def test_move_joints_preserves_authored_samples_and_cadence() -> None: ) +@pytest.mark.parametrize( + ("first_bad_sample", "velocity_limit", "displacement"), + [ + (0, None, 1.0), # Jump from the observed position, then hold. + (1, 2.0, 1.0), # A loose request cannot raise the Robot's 1 rad/s cap. + (1, 0.3, 0.01), # Within the Robot cap but above the requested speed. + ], +) +def test_move_joints_rejects_overspeed_authored_samples_per_row( + first_bad_sample: int, velocity_limit: float | None, displacement: float +) -> None: + action = _bind_action(_motion_generator(), MoveJoints()) + context = _context() + context.robot.qpos.fill_(0.5) + samples = context.robot.qpos[:, None, :ARM_DOF].repeat(1, 2, 1) + samples[0, 1, 0] += 0.002 + samples[1, first_bad_sample:, 0] += displacement + invocation = replace( + _invocation(action, JointPositionGoal(samples)), + motion_policy=MotionPolicy(velocity_limit=velocity_limit), + skill_options=MoveJointsOptions(preserve_samples=True), + ) + plan = _plan_action(action, invocation, context) + assert plan.plan_success.tolist() == [True, False] + commands = _joint_command_positions(plan, "arm") + torch.testing.assert_close(commands[0], samples[0]) + torch.testing.assert_close( + commands[1], context.robot.qpos[1, :ARM_DOF].expand_as(commands[1]) + ) + + def test_move_end_effector_preserves_authored_cartesian_samples() -> None: generator = _motion_generator() calls = [] From 7ac6c57d76d5851669abd884742b8f6cf5dd408e Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Tue, 17 Nov 2026 23:08:07 +0800 Subject: [PATCH 06/11] fix: stabilize YAM cable insertion and release Preserve correction travel, lower the held distal span before release, and withdraw vertically while retaining the existing physical acceptance gates. Validate pinned rc2 with four dual-environment and three 16-environment batches, the original full-route and third-carry CUDA regressions, and 104 focused CPU tests. --- .../integrations/simulation/cable.py | 216 ++++++++- .../cable/route_the_cable/README.md | 132 ++++-- .../task_program/integration.yaml | 108 ++++- .../route_the_cable/task_program/program.yaml | 164 +++++++ .../envs/task_program/test_cable_material.py | 441 +++++++++++++++++- tests/sim/objects/test_visual_object.py | 1 + tests/test_task_program_package_data.py | 2 + 7 files changed, 978 insertions(+), 86 deletions(-) diff --git a/embodichain/lab/task_program/integrations/simulation/cable.py b/embodichain/lab/task_program/integrations/simulation/cable.py index d5034149f7..62dadb4ca5 100644 --- a/embodichain/lab/task_program/integrations/simulation/cable.py +++ b/embodichain/lab/task_program/integrations/simulation/cable.py @@ -88,6 +88,9 @@ class _CableTarget: maximum_z_correction: float = 0.003 minimum_z: float | None = None maximum_y: float | None = None + pitch: float = 0.0 + maximum_rotation: float = math.pi / 18 + minimum_tool_pitch: float = -math.pi / 9 samples: int = 2 finger_position: float = 0.0475 condition: str | None = None @@ -323,6 +326,76 @@ def _material_pose(self, target: _CableTarget, part: str) -> torch.Tensor: self.port_yaw[part] = torch.where(choose, -math.pi / 2, math.pi / 2) return torch.where(choose[:, None, None], second[2], first[2]) + def _pitch_delta( + self, target: _CableTarget, part: str, pose: torch.Tensor + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + 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 + # Both selected wrist branches have a material-forward local X axis; + # its sign follows the branch's staging yaw. + forward = pose[:, :3, 0] * torch.sin(self.port_yaw[part]).sign()[:, None] + valid = ( + torch.isfinite(direction).all(-1) + & (direction[:, 1] > 1e-6) + & (forward[:, 1] > 1e-6) + ) + ray_pitch = torch.atan2(direction[:, 2], direction[:, 1]) + tool_pitch = torch.atan2(forward[:, 2], forward[:, 1]) + angle = (target.pitch - ray_pitch).clamp(-target.maximum_rotation, 0) + angle = torch.maximum( + angle, (target.minimum_tool_pitch - tool_pitch).clamp(max=0) + ) + return point, torch.where(valid, angle, torch.zeros_like(angle)), valid + + def _lay_down_delta( + self, target: _CableTarget, part: str, pose: torch.Tensor + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + if part not in self.grasps: + raise ValueError( + "Cable lay-down requires an active grasp for its motion part." + ) + assert target.minimum_z is not None + point = cable_endpoints(self.state(), self.lengths)[1][:, -1] - self.origins + offset = pose[:, :3, 3] - point + forward = pose[:, :3, 0] * torch.sin(self.port_yaw[part]).sign()[:, None] + valid = ( + self.seated() + & torch.isfinite(pose).all(dim=(-2, -1)) + & torch.isfinite(point).all(-1) + & torch.isfinite(forward).all(-1) + & (forward[:, 1] > 1e-6) + & (offset[:, 1] < -1e-6) + & (offset[:, 2] >= 0) + # Allow 0.1 mm of measured tracking error only for holding the + # loaded command at the floor, never for further downward motion. + & (pose[:, 2, 3] >= target.minimum_z - 0.0001) + ) + angle = (-torch.atan2(forward[:, 2], forward[:, 1])).clamp( + min=0, max=target.maximum_rotation + ) + # With TCP behind and above the tip, positive world-X rotation lowers + # TCP monotonically: z(theta) = tip_z + radius * cos(theta + phase). + # Limit the angle at the floor instead of moving the fixed tip pivot. + radius = offset[:, 1:3].norm(dim=-1) + phase = torch.atan2(-offset[:, 1], offset[:, 2]) + floor_angle = ( + torch.acos(((target.minimum_z - point[:, 2]) / radius).clamp(-1, 1)) - phase + ) + angle = torch.minimum(angle, floor_angle.clamp(min=0)) + angle = torch.where( + pose[:, 2, 3] <= target.minimum_z, torch.zeros_like(angle), angle + ) + return point, torch.where(valid, angle, torch.zeros_like(angle)), valid + + def _pivot_delta( + self, target: _CableTarget, part: str, pose: torch.Tensor + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + if target.kind == "lay_down": + return self._lay_down_delta(target, part, pose) + return self._pitch_delta(target, part, pose) + def pose(self, target: _CableTarget, part: str) -> torch.Tensor: if target.kind == "material": return self._material_pose(target, part) @@ -338,7 +411,24 @@ def pose(self, target: _CableTarget, part: str) -> torch.Tensor: 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"}: + if target.kind in {"pitch_align", "lay_down"}: + point, angle, valid = self._pivot_delta(target, part, pose) + c, s = torch.cos(angle), torch.sin(angle) + rotation = torch.eye(3, device=pose.device).repeat(len(pose), 1, 1) + rotation[:, 1, 1], rotation[:, 1, 2] = c, -s + rotation[:, 2, 1], rotation[:, 2, 2] = s, c + aligned = pose.clone() + aligned[:, :3, 3] = point - ( + rotation @ (point - pose[:, :3, 3]).unsqueeze(-1) + ).squeeze(-1) + aligned[:, :3, :3] = rotation @ pose[:, :3, :3] + pose = torch.where(valid[:, None, None], aligned, pose) + if target.kind == "lay_down": + holding = valid & (angle == 0) + pose = torch.where( + holding[:, None, None], self.tool(part, commanded=True), pose + ) + elif 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 @@ -489,13 +579,30 @@ def lower( service, target = self._target(call) part = self._part(bound) start, end = service.tool(part), service.pose(target, part) + pivot_geometry = ( + service._pivot_delta(target, part, start) + if target.kind in {"pitch_align", "lay_down"} + else None + ) 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() + if target.kind == "lay_down": + assert pivot_geometry is not None + holding = pivot_geometry[2] & (pivot_geometry[1] == 0) + start = torch.where(holding[:, None, None], end, start) + path_start, path_end = start, end + if pivot_geometry is not None: + valid = pivot_geometry[2][:, None, None] + identity = torch.eye(4, dtype=start.dtype, device=start.device) + # Invalid rows are rejected independently, including nonfinite + # observations; they must not poison the shared sample count. + path_start = torch.where(valid, start, identity) + path_end = torch.where(valid, end, identity) + distance = (path_start[:, :3, 3] - path_end[:, :3, 3]).norm(dim=-1).max().item() trace = ( - (start[:, :3, :3].transpose(1, 2) @ end[:, :3, :3]) + (path_start[:, :3, :3].transpose(1, 2) @ path_end[:, :3, :3]) .diagonal(dim1=-2, dim2=-1) .sum(-1) ) @@ -508,8 +615,37 @@ def lower( + 1, ) service.last_targets[part] = end.clone() + path = interpolate_xpos_batched(path_start, path_end, count)[:, 1:] + failures = {} + if pivot_geometry is not None: + point, _, valid = pivot_geometry + local_point = start[:, :3, :3].transpose(1, 2) @ ( + point - start[:, :3, 3] + ).unsqueeze(-1) + pivot_path = point[:, None] - ( + path[:, :, :3, :3] @ local_point[:, None] + ).squeeze(-1) + path[:, :, :3, 3] = torch.where( + valid[:, None, None], pivot_path, path[:, :, :3, 3] + ) + frozen = ~valid + if target.kind == "lay_down": + frozen |= holding + if target.stop_when_seated: + frozen |= service.seated() + path = torch.where(frozen[:, None, None, None], start[:, None], path) + reason = ( + "Cable lay-down requires a seated tip and a finite, forward, " + "monotonically descending pivot arc above its TCP floor." + if target.kind == "lay_down" + else "The distal cable span or tool points away from the channel." + ) + failures = { + int(context.env_ids[row]): reason + for row in (~valid).nonzero().flatten().tolist() + } return SemanticLowering( - goal=EndEffectorPoseGoal(interpolate_xpos_batched(start, end, count)[:, 1:]) + goal=EndEffectorPoseGoal(path), failed_preconditions=failures ) @@ -532,16 +668,25 @@ def lower( :, 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 + if target.kind in {"open", "grip_adjust"}: + 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 + elif service.hand_to_motion.get(part) not in service.grasps: + raise ValueError( + "Adjusting a grip requires an active grasp for the declared hand." + ) + # Adjustment retains the original grasp reference and slip history; + # only the finger command changes while physical contact settles. path = torch.lerp( start[:, None], torch.full_like(start[:, None], target.finger_position), @@ -550,7 +695,9 @@ def lower( ], ) elif target.kind != "hold": - raise ValueError("Cable joint targets must be open or hold declarations.") + raise ValueError( + "Cable joint targets must be open, grip_adjust or hold declarations." + ) return SemanticLowering(goal=JointPositionGoal(path)) @@ -643,10 +790,10 @@ 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"} + return target.kind in {"open", "grip_adjust", "hold"} if self.call_id == "simulation.cable_check": return target.kind == "check" - return target.kind not in {"open", "hold", "check"} + return target.kind not in {"open", "grip_adjust", "hold", "check"} def create( self, @@ -737,6 +884,15 @@ def _decode_cable_service(value: object) -> _CableService: "port_stage": motion_fields | {"position"}, "yaw_align": motion_fields | {"material_distance"}, "ray_align": motion_fields | {"material_distance"}, + "pitch_align": motion_fields + | { + "material_distance", + "pitch", + "maximum_rotation", + "minimum_tool_pitch", + "stop_when_seated", + }, + "lay_down": motion_fields | {"minimum_z", "maximum_rotation"}, "tip_align": motion_fields | { "position", @@ -748,6 +904,7 @@ def _decode_cable_service(value: object) -> _CableService: }, "hold": {"samples"}, "open": {"samples", "finger_position", "release_part"}, + "grip_adjust": {"samples", "finger_position"}, "check": { "condition", "tolerance", @@ -766,9 +923,12 @@ def _decode_cable_service(value: object) -> _CableService: "port_stage", "yaw_align", "ray_align", + "pitch_align", + "lay_down", "tip_align", "hold", "open", + "grip_adjust", "check", }: raise ValueError(f"Unsupported cable target kind {kind!r}") @@ -799,10 +959,26 @@ def _decode_cable_service(value: object) -> _CableService: or target.maximum_correction <= 0 ): raise ValueError("Cable motion limits must be positive.") - if kind in {"material", "material_lift", "yaw_align", "ray_align"} and ( - target.material_distance is None or target.material_distance < 0 - ): + if kind in { + "material", + "material_lift", + "yaw_align", + "ray_align", + "pitch_align", + } and (target.material_distance is None or target.material_distance < 0): raise ValueError("A cable material coordinate must be nonnegative.") + if kind == "pitch_align" and not ( + -math.pi / 2 < target.pitch <= 0 + and -math.pi / 2 < target.minimum_tool_pitch <= 0 + and 0 < target.maximum_rotation <= math.pi / 2 + ): + raise ValueError("Cable pitch alignment requires bounded downward angles.") + if kind == "lay_down" and ( + target.minimum_z is None or not 0 < target.maximum_rotation <= math.pi / 18 + ): + raise ValueError( + "Cable lay-down requires minimum_z and a rotation limit in (0, 10 degrees]." + ) if kind == "material_lift": if target.retract_distance < 0: raise ValueError("Cable retraction distance must be nonnegative.") 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 4bf27bb883..88ffd012d7 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 @@ -89,12 +89,13 @@ 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 retain the commanded pose -once the release condition is reached. They do not run an unbounded Python loop. +geometry when grounded. Early correction slots retain the commanded pose once +the release condition is reached; the final two slots continue toward an +interior endpoint target. They do not run an unbounded Python loop. ## Stages and reusable boundaries -The program contains 51 segments and 129 Semantic Call invocations. +The program contains 57 segments and 148 Semantic Call invocations. The deployment starts with both grippers open, 180 mm above the bench. In the arena frame, the right TCP starts at approximately `(0.45, -0.26, 0.93)` m @@ -116,18 +117,54 @@ post-closure slip reference. | 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 | +| Left, 0.570 m | 8 mm | 0.780 m | + +The final right-arm carry places the held 0.438 m material point at TCP +`[0.555, -0.010, 0.800]` m before lowering and releasing it. This leaves more +longitudinal space for the cable between the second spool and the channel, +while the leftward offset leaves room for rightward relaxation after release. 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 +`[0.586, 0.055, 0.795]` m above the open channel. The left lift retracts 8 mm +proximally to supply slack, and the staging height limits the subsequent upward travel +to reduce loading on the routed cable around the second spool. +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 +0.7685 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. +mouth while lowering. Before the last descent, two bounded pitch corrections +aim the observed distal span 15° downward. Each can turn downward by at most +10°, with a 20° downward limit on the measured tool axis. An already downward +span is never rotated upward. The complete interpolated motion pivots around +the observed grasped material point, and a seated row retains its loaded command. +After the last nominal descent, `soften_free_end` adjusts each finger toward a +4 mm joint target over 60 control samples, then holds for 30 samples before the +last two endpoint corrections. This reduces the commanded pinch load while +retaining the original grasp reference and accumulated slip measurements. +The adjustment neither releases the grasp reference nor resets the 12 mm slip +guard; subsequent motion still checks displacement from the acknowledged close. +Final vertical corrections are limited to 3 mm and retain +a 0.7655 m TCP height floor, leaving 3 mm of downward command travel after the +last nominal height stage. The final two slots continue toward endpoint +`[0.586, 0.077, 0.7535]` m even if the tip has just entered the release region. +Its Z coordinate places the cable centre one radius above the bench. +This avoids cancelling downward correction at the nominal stage height or +stopping immediately beside the release boundary. Corrections accumulate on +the commanded pose to compensate for contact-load bias. + +Before the release check, two bounded `lay_down` moves rotate the wrist back +toward horizontal around the observed cable endpoint. Each move turns at most +10° and limits its angle so the complete pivot arc stays at or above a 0.7625 m +TCP height. The rotation lowers the held cable section before opening the hand; +it does not clamp translation after constructing the rotation. The endpoint +must already be seated, and the original grasp reference and slip guard remain +active. The floor can stop the return before the wrist reaches horizontal. +At the floor, measured tracking error up to 0.1 mm only permits holding the +existing command; it never permits further downward motion. A row with invalid +geometry fails independently. The original release +check runs again after these moves, using the resulting physical cable state. Release requires the observed endpoint to be inside the strict channel with `y >= 0.072` m. The configured release region is @@ -136,17 +173,19 @@ 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. +penetration no greater than 1 mm. These release and final acceptance bounds are +unchanged. Ordinary measured TCP tracking remains required until the endpoint +is seated inside the strict channel. + +The left hand releases toward an 8 mm joint target per finger over 60 control +samples, then holds the arm stationary for 30 samples. It withdraws vertically +to TCP `[same X, same Y, 0.80]` m before a separate retreat moves the TCP to +`[same X, 0.015, 0.82]` m. `open_clear_of_port` opens the hand fully after +that raised 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 @@ -238,7 +277,8 @@ spool: python -m pytest tests/gym/envs/tasks/test_route_the_cable_cuda.py --run-gpu -m gpu -q ``` -The standard headless collector completed one episode with the original point +Before the terminal insertion and retreat changes described above, 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 @@ -247,36 +287,26 @@ 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. -With the two-arm ready pose, the same USD deployment passed all 51 segments -in 4,802 control steps at seed 73. The first approaches took 181 steps for -the right arm and 301 for the left, down from 638 and 518. The separate -full-route contact regression reported zero robot 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. - -| 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. +The 57-segment insertion/release trajectory was qualified on 2026-11-17 with +DexSim `0.6.0rc2` build `dbaca660471d5aaa7e750093e7fe1c207465c4af`, +Newton 1.7.0, seed 73, 20 VBD iterations and an RTX 5090 D v2. Four fresh +2-environment batches passed 8/8 tasks, and three fresh 16-environment batches +passed 48/48. All batches used the same source and initial state for each batch +size. The unchanged full-route and third-carry CUDA cases also passed in +separate fresh processes. The complete-route regression checks raw robot/bench, +robot/spool and robot/port contacts after every control step; the repeated +batches retain the original observable first-release and third-carry milestones. +Final endpoint Z across the 56 multi-environment tasks was 753.595–757.615 mm, +within the unchanged 753–759 mm range. The original displacement, penetration, +spool seating, clearance, speed and two-second settling requirements remain active. + +The deployment inspector reports 57 segments / 148 calls. The 104 focused CPU +cases comprise 70 cable runtime/material tests, two packaged-resource tests and +32 VisualObject tests. They cover bounded pivot motion, both wrist branches, +floor limits, per-environment failures and grip adjustment preserving the +original slip reference. Validation jobs ran sequentially on a shared GPU with +read-only state observers. These fixed-seed runs do not establish deterministic +physics, 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 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 452524ba31..109faf9ca4 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 @@ -50,7 +50,7 @@ runtime_services: port_stage: - 0.586 - 0.055 - - 0.805 + - 0.795 port_mouth: - 0.586 - 0.064 @@ -347,8 +347,8 @@ runtime_services: carry_2: kind: command position: - - 0.565 - - 0.008 + - 0.555 + - -0.01 - null carry_2_tracking: kind: check @@ -431,7 +431,7 @@ runtime_services: position: - 0.586 - 0.055 - - 0.805 + - 0.795 stage_free_end_tracking: kind: check condition: pose @@ -591,12 +591,40 @@ runtime_services: tolerance: 0.006 orientation_tolerance: 0.25 stop_when_seated: true + pitch_free_end_0: + kind: pitch_align + material_distance: 0.57 + pitch: -0.2617993877991494 + maximum_rotation: 0.17453292519943295 + minimum_tool_pitch: -0.3490658503988659 + tolerance: 0.006 + stop_when_seated: true + pitch_free_end_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + pitch_free_end_1: + kind: pitch_align + material_distance: 0.57 + pitch: -0.2617993877991494 + maximum_rotation: 0.17453292519943295 + minimum_tool_pitch: -0.3490658503988659 + tolerance: 0.006 + stop_when_seated: true + pitch_free_end_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.7655 + - 0.7685 speed: 0.025 tolerance: 0.006 stop_when_seated: true @@ -606,18 +634,31 @@ runtime_services: tolerance: 0.006 orientation_tolerance: 0.25 stop_when_seated: true + soften_free_end: + kind: grip_adjust + samples: 60 + finger_position: 0.004 + settle_soft_grasp: + kind: hold + samples: 30 + soften_free_end_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 + - 0.7535 maximum_correction: 0.005 maximum_y: 0.05 minimum_z: 0.7655 speed: 0.02 tolerance: 0.006 - stop_when_seated: true + stop_when_seated: false maximum_z_correction: 0.003 align_tip_3_0_tracking: kind: check @@ -630,13 +671,13 @@ runtime_services: position: - 0.586 - 0.077 - - 0.754 + - 0.7535 maximum_correction: 0.005 maximum_y: 0.05 minimum_z: 0.7655 speed: 0.02 tolerance: 0.006 - stop_when_seated: true + stop_when_seated: false maximum_z_correction: 0.003 align_tip_3_1_tracking: kind: check @@ -644,27 +685,66 @@ runtime_services: tolerance: 0.006 orientation_tolerance: 0.25 stop_when_seated: true + lay_down_free_end_0: + kind: lay_down + maximum_rotation: 0.17453292519943295 + minimum_z: 0.7625 + speed: 0.015 + tolerance: 0.006 + lay_down_free_end_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: false + lay_down_free_end_1: + kind: lay_down + maximum_rotation: 0.17453292519943295 + minimum_z: 0.7625 + speed: 0.015 + tolerance: 0.006 + lay_down_free_end_1_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: false release_alignment: kind: check condition: seated release_free_end: kind: open - samples: 36 + samples: 60 release_part: left_arm finger_position: 0.008 + settle_released_end: + kind: hold + samples: 30 retreat_free_end: kind: command position: - null - - 0.03 - - 0.82 - speed: 0.08 + - null + - 0.80 + speed: 0.025 retreat_free_end_tracking: kind: check condition: pose tolerance: 0.02 orientation_tolerance: 0.25 stop_when_seated: false + raise_clear_of_port: + kind: command + position: + - null + - 0.015 + - 0.82 + speed: 0.08 + raise_clear_of_port_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 home_left: kind: home speed: 0.15 @@ -748,7 +828,7 @@ runtime_services: raise_free_end: kind: material_lift material_distance: 0.57 - retract_distance: 0.003 + retract_distance: 0.008 position: - null - null 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 b1cd0dea7e..fcb7c4285d 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 @@ -967,6 +967,58 @@ program: target: align_tip_2_1_tracking resources: primary: left_manipulator +- segment: pitch_free_end_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: pitch_free_end_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: pitch_free_end_0_tracking + resources: + primary: left_manipulator +- segment: pitch_free_end_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: pitch_free_end_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: pitch_free_end_1_tracking + resources: + primary: left_manipulator - segment: lower_channel_3 steps: - kind: invoke @@ -993,6 +1045,32 @@ program: target: lower_channel_3_tracking resources: primary: left_manipulator +- segment: soften_free_end + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: soften_free_end + resources: + primary: left_hand + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_soft_grasp + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: soften_free_end_tracking + resources: + primary: left_manipulator - segment: align_tip_3_0 steps: - kind: invoke @@ -1045,6 +1123,58 @@ program: target: align_tip_3_1_tracking resources: primary: left_manipulator +- segment: lay_down_free_end_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lay_down_free_end_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: lay_down_free_end_0_tracking + resources: + primary: left_manipulator +- segment: lay_down_free_end_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lay_down_free_end_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: lay_down_free_end_1_tracking + resources: + primary: left_manipulator - segment: verify_release_alignment steps: - kind: invoke @@ -1065,6 +1195,14 @@ program: target: release_free_end resources: primary: left_hand + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_released_end + resources: + primary: left_manipulator - segment: retreat_free_end steps: - kind: invoke @@ -1091,6 +1229,32 @@ program: target: retreat_free_end_tracking resources: primary: left_manipulator +- segment: raise_clear_of_port + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: raise_clear_of_port + 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_clear_of_port_tracking + resources: + primary: left_manipulator - segment: open_clear_of_port 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 021a6fdb4e..25a58a6990 100644 --- a/tests/gym/envs/task_program/test_cable_material.py +++ b/tests/gym/envs/task_program/test_cable_material.py @@ -26,6 +26,7 @@ import torch import yaml +from embodichain.compute.geometry.cable import cable_endpoints from embodichain.lab.gym.utils.composition import resolve_gym_components from embodichain.lab.task_program.integrations import ( load_configured_task_program_deployment, @@ -34,6 +35,9 @@ _decode_cable_service, _CableRuntime, _CableCheckLowerer, + _CableJointLowerer, + _CableJointFactory, + _CableMoveFactory, _CableMoveLowerer, _CablePickFactory, ) @@ -72,7 +76,7 @@ def test_cable_deployment_preflights_all_calls_and_measured_final_acceptance(): deployment = _deployment() registration = deployment.integration.registration compiled = registration.catalog.preflight(deployment.load_program()) - assert compiled.segment_count == 51 + assert compiled.segment_count == 57 assert registration.evaluation_final_refs == ("route_success",) assert registration.scene_binding.declare().entries == () factories = registration.registered_semantic_lowerer_factories @@ -131,6 +135,28 @@ def test_material_lift_rejects_invalid_geometry_and_inapplicable_fields(changes) _decode_cable_service(value) +@pytest.mark.parametrize( + "changes", + [ + {"material_distance": -1}, + {"pitch": 0.1}, + {"pitch": -math.pi / 2}, + {"minimum_tool_pitch": float("nan")}, + {"maximum_rotation": float("inf")}, + {"maximum_rotation": 0}, + {"maximum_rotation": math.pi}, + {"kind": "yaw_align", "pitch": -0.1}, + ], +) +def test_pitch_alignment_rejects_invalid_angles_and_inapplicable_fields(changes): + value = _declaration() + target = {"kind": "pitch_align", "material_distance": 0.162} + target.update(changes) + value["targets"] = {"pitch": 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, @@ -372,6 +398,419 @@ def test_seated_rows_preserve_contact_loading_command_while_other_rows_move(): assert path[1, 0, 2, 3] > commanded[1, 2] +@pytest.mark.parametrize("target_id", ["align_tip_3_0", "align_tip_3_1"]) +def test_terminal_tip_correction_has_downward_travel_and_does_not_stop_at_gate( + target_id, +): + service, state, measured = _runtime() + commanded = measured.clone() + measured[[3, 1]] = torch.tensor([0.586, 0.05, 0.7706]) + commanded[[3, 1]] = torch.tensor([0.586, 0.05, 0.7685]) + service.robot.get_qpos = lambda *, target=False: commanded if target else measured + # Reproduce a rejected 760.756 mm endpoint and an accepted endpoint only + # 0.2 mm below the gate. Both still need to move toward the interior target. + tip_heights = (0.760756, 0.7588) + for row, height in zip((3, 1), tip_heights): + # The fixture's last capsule points along +X and is 12 mm long. + state[row, -1, :3] = torch.tensor([0.580, 0.077, height]) + state[row, -1, :3] += service.origins[0 if row == 3 else 1] + service.seated = lambda: torch.tensor([False, True]) + + pose = service.pose(service.cfg.target(target_id), "left_arm") + + # Keep 3 mm of downward travel below the last nominal height stage. + # Reaching the release gate must not freeze the second row at the boundary. + torch.testing.assert_close( + pose[:, 2, 3] - commanded[[3, 1], 2], + torch.tensor([-0.003, -0.003]), + atol=1e-7, + rtol=0, + ) + torch.testing.assert_close(pose[:, :2, 3], commanded[[3, 1], :2], atol=4e-6, rtol=0) + + +def _pitch_runtime(tool_pitch, ray_pitch): + service, state, measured = _runtime() + value = _declaration() + value["targets"] = { + "pitch": { + "kind": "pitch_align", + "material_distance": 0.162, + "pitch": -math.pi / 12, + } + } + service.cfg = _decode_cable_service(value) + service.port_yaw["left_arm"] = torch.tensor([math.pi / 2, -math.pi / 2]) + branch_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]], + ] + ) + c, s = math.cos(tool_pitch), math.sin(tool_pitch) + rotation = torch.tensor([[1.0, 0.0, 0.0], [0.0, c, -s], [0.0, s, c]]) + initial_rotation = rotation @ branch_rotation + original_fk = service.robot.compute_fk + + def fk(qpos, **kwargs): + pose = original_fk(qpos, **kwargs) + pose[:, :3, :3] = initial_rotation + return pose + + service.robot.compute_fk = fk + measured[[3, 1]] = torch.tensor([0.586, 0.04, 0.795]) + point = torch.tensor([0.586, 0.05, 0.785]) + direction = torch.tensor( + [0.0, 0.04 * math.cos(ray_pitch), 0.04 * math.sin(ray_pitch)] + ) + for selected_row, native_row in enumerate((3, 1)): + state[native_row, 13, :3] = point + service.origins[selected_row] + state[native_row, -1, :3] = ( + point + + direction + - torch.tensor([0.006, 0.0, 0.0]) + + service.origins[selected_row] + ) + return service, state + + +def _lower_pitch(service, target="pitch"): + 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})} + ) + ) + return lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.cable_move", arguments={"target": target} + ), + context=SimpleNamespace( + env_ids=service.env_ids, require_control_dt=lambda: 1 / 60 + ), + bound=bound, + option_template=MoveEndEffectorOptions(), + ) + + +@pytest.mark.parametrize( + "tool_degrees,ray_degrees,expected_degrees", + [(0, 30, -10), (-15, 30, -20), (-25, 30, -25), (0, -30, 0)], +) +def test_pitch_alignment_bounds_both_wrist_branches_and_keeps_material_pivot( + tool_degrees, ray_degrees, expected_degrees +): + service, _ = _pitch_runtime(math.radians(tool_degrees), math.radians(ray_degrees)) + start = service.tool("left_arm") + point = service.point(0.162)[0] - service.origins + local_point = start[:, :3, :3].transpose(1, 2) @ ( + point - start[:, :3, 3] + ).unsqueeze(-1) + + result = _lower_pitch(service) + + assert not result.failed_preconditions + path = result.goal.xpos + rotations = path[:, :, :3, :3] + predicted_point = (rotations @ local_point[:, None]).squeeze(-1) + path[:, :, :3, 3] + torch.testing.assert_close( + predicted_point, point[:, None].expand_as(predicted_point) + ) + torch.testing.assert_close( + rotations.transpose(-1, -2) @ rotations, torch.eye(3).expand_as(rotations) + ) + torch.testing.assert_close( + torch.linalg.det(rotations), torch.ones(rotations.shape[:2]) + ) + forward = rotations[:, -1, :, 0] * torch.tensor([1.0, -1.0])[:, None] + torch.testing.assert_close( + torch.atan2(forward[:, 2], forward[:, 1]), + torch.full((2,), math.radians(expected_degrees)), + atol=1e-6, + rtol=0, + ) + + +def test_pitch_alignment_rejects_only_the_row_with_backward_distal_span(): + service, state = _pitch_runtime(0, math.pi / 6) + state[1, -1, 1] = 0.01 + initial = service.tool("left_arm") + + result = _lower_pitch(service) + + assert set(result.failed_preconditions) == {1} + torch.testing.assert_close( + result.goal.xpos[1], initial[1].expand_as(result.goal.xpos[1]) + ) + assert not torch.allclose(result.goal.xpos[0, -1], initial[0]) + + +def test_pitch_alignment_preserves_seated_rows_loaded_command(): + service, _ = _pitch_runtime(0, math.pi / 6) + measured = service.robot.get_qpos().clone() + commanded = measured.clone() + commanded[[3, 1], 2] -= 0.003 + service.robot.get_qpos = lambda *, target=False: commanded if target else measured + target = replace(service.cfg.target("pitch"), stop_when_seated=True) + service.cfg = replace(service.cfg, targets=(target,)) + service.seated = lambda: torch.tensor([True, False]) + loaded = service.tool("left_arm", commanded=True) + + result = _lower_pitch(service) + + assert torch.equal(result.goal.xpos[0], loaded[0].expand_as(result.goal.xpos[0])) + assert not torch.allclose(result.goal.xpos[1, -1], loaded[1]) + + +def _lay_down_runtime(tool_degrees=-20, minimum_z=0.7625): + service, state = _pitch_runtime(math.radians(tool_degrees), -math.pi / 12) + value = _declaration() + value["targets"] = {"lay": {"kind": "lay_down", "minimum_z": minimum_z}} + service.cfg = _decode_cable_service(value) + service.robot.get_qpos()[[3, 1]] = torch.tensor([0.586, 0.04, 0.7655]) + tip = torch.tensor([0.586, 0.077, 0.756]) + for row, native_row in enumerate((3, 1)): + state[native_row, -1, :3] = ( + tip - torch.tensor([0.006, 0.0, 0.0]) + service.origins[row] + ) + service.seated = lambda: torch.ones(2, dtype=torch.bool) + service.grasps["left_arm"] = ( + 0.162, + service.point_in_tool("left_arm", 0.162).clone(), + ) + return service, state + + +@pytest.mark.parametrize("tool_degrees,expected_degrees", [(-20, -10), (-5, 0), (0, 0)]) +def test_lay_down_returns_both_wrist_branches_toward_flat_without_overshoot( + tool_degrees, expected_degrees +): + service, _ = _lay_down_runtime(tool_degrees, minimum_z=0.74) + initial = service.tool("left_arm") + + result = _lower_pitch(service, "lay") + + assert not result.failed_preconditions + forward = result.goal.xpos[:, -1, :3, 0] * torch.tensor([1.0, -1.0])[:, None] + torch.testing.assert_close( + torch.atan2(forward[:, 2], forward[:, 1]), + torch.full((2,), math.radians(expected_degrees)), + atol=1e-6, + rtol=0, + ) + if tool_degrees == 0: + torch.testing.assert_close( + result.goal.xpos, initial[:, None].expand_as(result.goal.xpos) + ) + + +def test_lay_down_limits_rotation_at_floor_and_preserves_tip_in_every_sample(): + service, _ = _lay_down_runtime() + initial = service.tool("left_arm") + tip = cable_endpoints(service.state(), service.lengths)[1][:, -1] - service.origins + local_tip = initial[:, :3, :3].transpose(1, 2) @ ( + tip - initial[:, :3, 3] + ).unsqueeze(-1) + + result = _lower_pitch(service, "lay") + + assert not result.failed_preconditions + path = result.goal.xpos + transformed_tip = (path[:, :, :3, :3] @ local_tip[:, None]).squeeze(-1) + path[ + :, :, :3, 3 + ] + torch.testing.assert_close(transformed_tip, tip[:, None].expand_as(transformed_tip)) + # The floor is reached before the ten-degree rotation allowance; clamping + # TCP translation after rotation would violate the fixed-tip check above. + torch.testing.assert_close(path[:, -1, 2, 3], torch.full((2,), 0.7625)) + assert bool((path[:, :, 2, 3] >= 0.7625 - 1e-7).all()) + assert bool((path[:, 1:, 2, 3] <= path[:, :-1, 2, 3] + 1e-7).all()) + forward = path[:, -1, :3, 0] * torch.tensor([1.0, -1.0])[:, None] + final_pitch = torch.atan2(forward[:, 2], forward[:, 1]) + assert bool((final_pitch > math.radians(-20)).all()) + assert bool((final_pitch < math.radians(-10)).all()) + + +@pytest.mark.parametrize("below_floor", [0.0, 0.000005, 0.00009]) +def test_lay_down_at_floor_holds_loaded_command_without_unloading(below_floor): + service, _ = _lay_down_runtime() + measured = service.robot.get_qpos().clone() + measured[[3, 1], 2] = 0.7625 - below_floor + commanded = measured.clone() + commanded[[3, 1], 2] = 0.7625 + commanded[[3, 1], 1] += 0.001 + service.robot.get_qpos = lambda *, target=False: commanded if target else measured + loaded = service.tool("left_arm", commanded=True) + + result = _lower_pitch(service, "lay") + + assert not result.failed_preconditions + assert torch.equal(result.goal.xpos, loaded[:, None].expand_as(result.goal.xpos)) + + +@pytest.mark.parametrize( + "invalid", ["unseated", "backward", "nonfinite", "pivot", "below_floor"] +) +def test_lay_down_rejects_only_invalid_environment_and_keeps_its_pose(invalid): + service, state = _lay_down_runtime() + if invalid == "unseated": + service.seated = lambda: torch.tensor([True, False]) + elif invalid == "backward": + service.port_yaw["left_arm"][1] = math.pi / 2 + elif invalid == "nonfinite": + state[1, -1, 0] = float("nan") + elif invalid == "pivot": + state[1, -1, 1] = 0.03 + else: + service.robot.get_qpos()[1, 2] = 0.76239 + initial = service.tool("left_arm") + + result = _lower_pitch(service, "lay") + + assert service.env_ids.tolist() == [3, 1] + assert set(result.failed_preconditions) == {1} + assert torch.equal(result.goal.xpos[1], initial[1].expand_as(result.goal.xpos[1])) + assert not torch.allclose(result.goal.xpos[0, -1], initial[0]) + + +def test_lay_down_preserves_active_grasp_and_original_slip_reference(): + service, state = _lay_down_runtime() + baseline = service.point_in_tool("left_arm", 0.162).clone() + reference = (0.162, baseline) + service.grasps["left_arm"] = reference + service.hand_to_motion["left_eef"] = "left_arm" + service.grasp_slip["left_arm"] = torch.tensor([0.001, 0.002]) + service.max_slip[:] = service.grasp_slip["left_arm"] + pending = ("right_arm", service.cfg.target("lay")) + service.pending_pick = pending + + _lower_pitch(service, "lay") + + assert service.grasps["left_arm"] is reference + assert service.grasps["left_arm"][1] is baseline + assert service.pending_pick is pending + torch.testing.assert_close(service.max_slip, torch.tensor([0.001, 0.002])) + torch.testing.assert_close(service.grasp_slip["left_arm"], service.max_slip) + state[3, :, 0] += 0.013 + assert service.observe().tolist() == [True, False] + + +def test_lay_down_rejects_a_motion_part_without_an_active_grasp(): + service, _ = _lay_down_runtime() + service.grasps.pop("left_arm") + with pytest.raises(ValueError, match="active grasp"): + _lower_pitch(service, "lay") + + +@pytest.mark.parametrize( + "changes", + [ + {"minimum_z": None}, + {"minimum_z": float("nan")}, + {"maximum_rotation": 0}, + {"maximum_rotation": math.pi / 6}, + {"maximum_rotation": float("inf")}, + {"stop_when_seated": True}, + ], +) +def test_lay_down_requires_explicit_floor_and_bounded_unfrozen_rotation(changes): + value = _declaration() + target = {"kind": "lay_down", "minimum_z": 0.7625} + target.update(changes) + if target.get("minimum_z") is None: + target.pop("minimum_z") + value["targets"] = {"lay": target} + with pytest.raises((ValueError, TypeError)): + _decode_cable_service(value) + + +def _grip_adjust_runtime(): + service, state, _ = _runtime() + value = _declaration() + value["targets"]["adjust"] = { + "kind": "grip_adjust", + "samples": 60, + "finger_position": 0.003, + } + service.cfg = _decode_cable_service(value) + return service, state + + +def _lower_grip_adjust(service): + lowerer = _CableJointLowerer(service.cfg) + lowerer.runtime = service + endpoint = SimpleNamespace(runtime_target=SimpleNamespace(control_part="left_eef")) + bound = SimpleNamespace( + binding=SimpleNamespace( + resources={"primary": SimpleNamespace(endpoints={"motion": endpoint})} + ) + ) + return lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.cable_joint", arguments={"target": "adjust"} + ), + context=SimpleNamespace(env_ids=service.env_ids), + bound=bound, + option_template=MoveJointsOptions(), + ) + + +def test_grip_adjust_preserves_grasp_reference_and_slip_guard(): + service, state = _grip_adjust_runtime() + service.hand_to_motion["left_eef"] = "left_arm" + baseline = service.point_in_tool("left_arm", 0.162).clone() + reference = (0.162, baseline) + service.grasps["left_arm"] = reference + service.grasp_slip["left_arm"] = torch.tensor([0.001, 0.002]) + service.max_slip[:] = service.grasp_slip["left_arm"] + original_max_slip = service.max_slip + pending = ("right_arm", service.cfg.target("right_grasp_0.162")) + service.pending_pick = pending + + result = _lower_grip_adjust(service) + + assert service.grasps["left_arm"] is reference + assert service.grasps["left_arm"][1] is baseline + assert service.pending_pick is pending + assert service.max_slip is original_max_slip + torch.testing.assert_close(service.max_slip, torch.tensor([0.001, 0.002])) + torch.testing.assert_close( + service.grasp_slip["left_arm"], torch.tensor([0.001, 0.002]) + ) + torch.testing.assert_close(result.goal.target[:, 0], torch.zeros(2, 3)) + torch.testing.assert_close(result.goal.target[:, -1], torch.full((2, 3), 0.003)) + assert ( + "adjust" in _CableJointFactory(service.cfg).argument_contract.required["target"] + ) + assert ( + "adjust" + not in _CableMoveFactory(service.cfg).argument_contract.required["target"] + ) + # Adjustment must not rebase or disable the reference when contact slips. + state[3, :, 0] += 0.013 + assert service.observe().tolist() == [True, False] + + +@pytest.mark.parametrize("missing", ["mapping", "grasp"]) +def test_grip_adjust_requires_current_hand_grasp(missing): + service, _ = _grip_adjust_runtime() + if missing == "mapping": + service.grasps["left_arm"] = (0.162, service.point_in_tool("left_arm", 0.162)) + else: + service.hand_to_motion["left_eef"] = "left_arm" + with pytest.raises(ValueError, match="active grasp"): + _lower_grip_adjust(service) + + +def test_grip_adjust_rejects_release_semantics(): + value = _declaration() + value["targets"] = {"adjust": {"kind": "grip_adjust", "release_part": "left_arm"}} + with pytest.raises(ValueError, match="Unsupported fields"): + _decode_cable_service(value) + + def _lower_check(service, target): lowerer = _CableCheckLowerer(service.cfg) lowerer.runtime = service diff --git a/tests/sim/objects/test_visual_object.py b/tests/sim/objects/test_visual_object.py index 9ca3855e8c..2aa9945e15 100644 --- a/tests/sim/objects/test_visual_object.py +++ b/tests/sim/objects/test_visual_object.py @@ -282,6 +282,7 @@ def test_add_usd_classifies_visuals_without_promoting_collision_only_objects(tmp "_rigid_objects", "_rigid_object_groups", "_deformable_objects", + "_cable_objects", "_fluid_objects", "_articulations", "_robots", diff --git a/tests/test_task_program_package_data.py b/tests/test_task_program_package_data.py index 00ecd06ec8..53f7e247f6 100644 --- a/tests/test_task_program_package_data.py +++ b/tests/test_task_program_package_data.py @@ -259,6 +259,7 @@ Path("tasks/manipulation/gaussian_pick_place/task_program/integration.yaml"), Path("tasks/manipulation/gaussian_pick_place/catalog.yaml"), Path("tasks/manipulation/gaussian_pick_place/README.md"), + Path("tasks/manipulation/cable/route_the_cable/cable.usda"), Path("tasks/manipulation/tableware/pour_cup/task.franka.fluid.yaml"), Path("tasks/manipulation/tableware/pour_cup/env.fluid.yaml"), Path("tasks/manipulation/tableware/pour_cup/task_program/program.fluid.yaml"), @@ -486,6 +487,7 @@ def test_setup_stages_all_official_task_programs( assert get_package_dir()[_CONFIG_PACKAGE] == "extensions/embodichain_tasks/configs" assert staged_config_package.package_data[_CONFIG_PACKAGE] == [ "tasks/manipulation/cloth_fold/task_program/trajectory.npz", + "**/*.usda", "**/*.json", "**/*.yaml", "**/*.yml", From 5517b73832cf564bba39d3e911f7159ac3d307d7 Mon Sep 17 00:00:00 2001 From: KeqiYe Date: Thu, 19 Nov 2026 14:12:23 +0800 Subject: [PATCH 07/11] fix(yam): honor offline arm URDF overrides Use the configured arm component before resolving the default asset so offline configurations round-trip. Keep the release task prerequisite aligned with the DexSim version pinned on main. --- extensions/embodichain_robots/i2rt/yam/cfg.py | 58 ++++++++++++------- .../cable/route_the_cable/README.md | 2 +- tests/sim/robots/test_yam_i2rt.py | 16 ++++- 3 files changed, 52 insertions(+), 24 deletions(-) diff --git a/extensions/embodichain_robots/i2rt/yam/cfg.py b/extensions/embodichain_robots/i2rt/yam/cfg.py index c1e96bbb0e..48ec9410c7 100644 --- a/extensions/embodichain_robots/i2rt/yam/cfg.py +++ b/extensions/embodichain_robots/i2rt/yam/cfg.py @@ -20,7 +20,6 @@ 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 ( @@ -63,26 +62,45 @@ def from_dict(cls, init_dict: dict[str, Any]) -> YamI2rtCfg: return merge_robot_cfg(cfg, init_dict) def _build_defaults(self, init_dict: dict[str, Any] | None = None) -> None: + overrides = init_dict or {} 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.fpath = overrides.get("fpath") or self.fpath + arm_override = None + urdf_override = overrides.get("urdf_cfg") + if not self.fpath and isinstance(urdf_override, dict): + components = urdf_override.get("components") + if isinstance(components, dict): + arm = components.get("arm") + elif isinstance(components, list): + arm = next( + ( + component + for component in components + if isinstance(component, dict) + and component.get("component_type") == "arm" + and component.get("urdf_path") + ), + None, + ) + else: + arm = None + if isinstance(arm, dict): + arm_override = arm.get("urdf_path") + if arm_override: + # Seed the arm as the first component. The shared merge applies + # the caller's transform and other component metadata afterward. + urdf_path = arm_override + else: + 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().add_component("arm", urdf_path) self.init_pos = (0.2525, 0, 0.75) self.asset_physics_mode = "overlay" self.control_parts = { 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 88ffd012d7..14f2f9f805 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,7 +11,7 @@ in-episode state writes. ## Requirements and entry point -Use CUDA and the pinned DexSim `0.6.0rc2` wheel. The packaged +Use CUDA and the pinned DexSim `0.6.0` 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 diff --git a/tests/sim/robots/test_yam_i2rt.py b/tests/sim/robots/test_yam_i2rt.py index c4360a0e21..e153b2c15c 100644 --- a/tests/sim/robots/test_yam_i2rt.py +++ b/tests/sim/robots/test_yam_i2rt.py @@ -99,16 +99,26 @@ def test_missing_asset_reports_download_command( YamI2rtCfg.from_dict({}) +@pytest.mark.parametrize("use_component", [False, True], ids=["fpath", "arm-component"]) def test_local_override_roundtrips_without_network( - monkeypatch: pytest.MonkeyPatch, arm_asset: Path + monkeypatch: pytest.MonkeyPatch, arm_asset: Path, use_component: bool ) -> 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)}) + if use_component: + override = { + "urdf_cfg": { + "components": [{"component_type": "arm", "urdf_path": str(arm_asset)}] + } + } + else: + override = {"fpath": str(arm_asset)} + cfg = YamI2rtCfg.from_dict(override) restored = YamI2rtCfg.from_dict(cfg.to_dict()) - assert restored.fpath == str(arm_asset) + assert restored.fpath == (None if use_component else str(arm_asset)) + assert restored.urdf_cfg.assemble_urdf() == str(arm_asset) assert restored._pk_urdf_path == str(arm_asset) assert restored.control_parts == cfg.control_parts From 47c7411338049a2e16c48dc6ce835e8fc07247dc Mon Sep 17 00:00:00 2001 From: yuecideng Date: Thu, 19 Nov 2026 21:19:47 +0800 Subject: [PATCH 08/11] feat: add observed contact paths for compact cable routing --- .../topics/atomic-actions/atomic-actions.md | 9 + .../topics/atomic-actions/execution.md | 9 + docs/source/api_reference/public_api.rst | 69 + embodichain/lab/sim/atomic_skills/__init__.py | 4 + .../lab/sim/atomic_skills/continuation.py | 73 + embodichain/lab/sim/atomic_skills/core.py | 32 + embodichain/lab/sim/atomic_skills/engine.py | 116 +- .../lab/sim/atomic_skills/execution.py | 59 +- embodichain/lab/sim/atomic_skills/plans.py | 23 + .../sim/atomic_skills/primitives/__init__.py | 12 +- .../sim/atomic_skills/primitives/contact.py | 497 +++++++ .../sim/atomic_skills/primitives/pick_up.py | 96 +- .../lab/sim/atomic_skills/primitives/place.py | 53 +- embodichain/lab/sim/atomic_skills/runtime.py | 48 + embodichain/lab/sim/atomic_skills/state.py | 12 + .../task_program/integrations/configured.py | 14 + .../integrations/simulation/_cable_contact.py | 536 ++++++++ .../integrations/simulation/cable.py | 342 +++-- .../cable/route_the_cable/README.md | 42 + .../cable/route_the_cable/catalog.yaml | 2 + .../route_the_cable/task.yam.compact.yaml | 38 + .../execution_policy.compact.yaml | 25 + .../task_program/integration.compact.yaml | 1221 +++++++++++++++++ .../task_program/program.compact.yaml | 87 ++ .../envs/task_program/test_cable_material.py | 203 ++- .../test_configured_integration.py | 8 +- tests/sim/atomic_skills/test_actions.py | 10 +- tests/sim/atomic_skills/test_contact_paths.py | 381 +++++ 28 files changed, 3815 insertions(+), 206 deletions(-) create mode 100644 embodichain/lab/sim/atomic_skills/continuation.py create mode 100644 embodichain/lab/sim/atomic_skills/primitives/contact.py create mode 100644 embodichain/lab/task_program/integrations/simulation/_cable_contact.py create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.compact.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.compact.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.compact.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.compact.yaml create mode 100644 tests/sim/atomic_skills/test_contact_paths.py diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index ac5475e91f..c5dd1e7753 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -36,6 +36,15 @@ 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`. +`ContactPickUpGoal` and `ContactPlaceGoal` opt into a bounded observed path in +the same `PickUp`/`Place` invocation. `SkillPlan.continuation` carries an owned +cursor; `ExecutionSession` installs the next plan only after the host acknowledges +commands and physical checkpoints. The engine owns motion generation; the +episode-owned `ContactPhaseSource` resolves geometry and observations only. +Offline projection, external trajectory transforms, replay recovery and +invocation revisions are rejected for this path. Its phases share one timeout +and failure scope; `SkillPlan.segments` remain descriptive command ranges. + ## Package map | Concern | Source of truth | diff --git a/agent_context/topics/atomic-actions/execution.md b/agent_context/topics/atomic-actions/execution.md index 1558b9efbf..637262466b 100644 --- a/agent_context/topics/atomic-actions/execution.md +++ b/agent_context/topics/atomic-actions/execution.md @@ -47,6 +47,15 @@ implementation `_plan(request, context)` hook. - `ExecutionRunner` drives that session against observation, command, and clock ports without blocking `step()`. +An opt-in `SkillPlan.continuation` requests fresh-observation planning inside +one invocation. Progress is bounded and strictly advances; rejected rows cannot +return. `PLAN_CONTINUED` records each installed chunk without consuming a retry +or resetting the invocation timeout. Intermediate chunks commit no symbolic +effects. Contact paths require execution, reject offline `compile()`, external +nominal/expanded trajectories and invocation revisions, and retain loaded joint +targets across internal holds. `PlanningContext.planning_mask` scopes geometric +reductions and IK to the session's eligible rows before selecting shared timing. + For a prequalified replay, `MoveJoints` accepts a typed full-robot `TimedTrajectory`. Each command occupies one environment control step. The motion binding covers all joints, and the selected rows match the planning diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index ebe3772b1f..8e56e9aa8e 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -6559,3 +6559,72 @@ Official sensor presets VLP16Cfg VLP16_REGISTRATION + + +Observed contact manipulation +============================= + +The existing ``PickUp`` and ``Place`` skills accept explicit contact paths. +These keep one invocation while planning later chunks from fresh measurements; +no rigid held-object relation or independently recoverable sub-skill is implied. +The host must acknowledge commands and acquire the declared physical evidence. +Paths reject offline projection, external trajectory transformations, replay +recovery and mid-operation invocation revisions. + +Bounded plan continuation +------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_skills.continuation + +.. autosummary:: + + PlanContinuation + PlanContinuationResult + +.. autoclass:: PlanContinuation + :members: + +.. autoclass:: PlanContinuationResult + :members: + +Contact goals and observation sources +------------------------------------- + +.. currentmodule:: embodichain.lab.sim.atomic_skills.primitives.contact + +The source is registered through ``SkillPlanningServices.register_contact_source`` +and remains owned by its episode host. It resolves geometric snapshots and +accepts observed physical boundaries; motion generation and simulation stepping +remain with the engine and host respectively. Snapshots retain owned tensors +and scalar metadata, with explicit batch and row identity. + +.. autosummary:: + + ContactPhase + ContactPostcondition + ContactPickUpGoal + ContactPlaceGoal + ContactPhaseSource + ContactContinuation + ResolvedContactPhase + +.. autoclass:: ContactPhase + :members: + +.. autoclass:: ContactPostcondition + :members: + +.. autoclass:: ContactPickUpGoal + :members: + +.. autoclass:: ContactPlaceGoal + :members: + +.. autoclass:: ContactPhaseSource + :members: + +.. autoclass:: ContactContinuation + :members: + +.. autoclass:: ResolvedContactPhase + :members: diff --git a/embodichain/lab/sim/atomic_skills/__init__.py b/embodichain/lab/sim/atomic_skills/__init__.py index 238d58ef43..ea1b42d007 100644 --- a/embodichain/lab/sim/atomic_skills/__init__.py +++ b/embodichain/lab/sim/atomic_skills/__init__.py @@ -201,6 +201,8 @@ CoordinatedPlacementOptions, EndEffectorPoseGoal, ContactGraspGoal, + ContactPickUpGoal, + ContactPlaceGoal, GraspGoal, HandOver, HandOverGoal, @@ -389,6 +391,8 @@ "GRASP_COMMAND", "GRASP_CAPABILITY", "ContactGraspGoal", + "ContactPickUpGoal", + "ContactPlaceGoal", "GraspGoal", "HandOver", "HandOverGoal", diff --git a/embodichain/lab/sim/atomic_skills/continuation.py b/embodichain/lab/sim/atomic_skills/continuation.py new file mode 100644 index 0000000000..ed43dd06bc --- /dev/null +++ b/embodichain/lab/sim/atomic_skills/continuation.py @@ -0,0 +1,73 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Bounded observed continuation of one atomic invocation.""" + +from __future__ import annotations +from dataclasses import dataclass +from typing import TYPE_CHECKING, Protocol, runtime_checkable +import torch + +if TYPE_CHECKING: + from .plans import SkillPlan + + +@runtime_checkable +class PlanContinuation(Protocol): + """Independently owned progress within one skill invocation. + + ``position`` advances strictly within ``limit`` (at most 256 chunks). + ``phase_name`` identifies the current chunk for audit and host observations. + Snapshots must retain no live controller or simulator handles. + """ + + position: int + limit: int + phase_name: str + + def snapshot(self) -> PlanContinuation: + """Copy the bounded progress and its observation-derived data.""" + ... + + +@dataclass(frozen=True, slots=True, eq=False) +class PlanContinuationResult: + """Next plan or terminal per-row acceptance from fresh observations. + + Args: + accepted: Owned boolean subset of the invocation's active rows. + plan: Next bounded phase plan, or ``None`` at the terminal boundary. + messages: Physical rejection details suitable for execution event logs. + """ + + accepted: torch.Tensor + plan: SkillPlan | None = None + messages: tuple[str, ...] = () + + def __post_init__(self) -> None: + if ( + not isinstance(self.accepted, torch.Tensor) + or self.accepted.dtype != torch.bool + or self.accepted.ndim != 1 + ): + raise ValueError("Continuation acceptance must be a boolean row mask.") + if not all(isinstance(message, str) for message in self.messages): + raise TypeError("Continuation messages must be strings.") + object.__setattr__(self, "accepted", self.accepted.clone()) + object.__setattr__(self, "messages", tuple(self.messages)) + + +__all__ = ["PlanContinuation", "PlanContinuationResult"] diff --git a/embodichain/lab/sim/atomic_skills/core.py b/embodichain/lab/sim/atomic_skills/core.py index 20587e026b..8a662a5328 100644 --- a/embodichain/lab/sim/atomic_skills/core.py +++ b/embodichain/lab/sim/atomic_skills/core.py @@ -34,6 +34,7 @@ from .affordance import Affordance from .bindings import EndpointBinding, JointPositionTarget +from .continuation import PlanContinuationResult from .effects import StateDelta from .goals import collect_scene_dependencies from .invocation import ( @@ -1514,6 +1515,37 @@ def failed_plan( diagnostics=diagnostics, ) + def continue_plan( + self, + request: ResolvedSkillRequest, + context: PlanningContext, + plan: SkillPlan, + active_mask: torch.Tensor, + ) -> PlanContinuationResult: + """Plan a later chunk without beginning another logical skill. + + Args: + request: The unchanged resolved invocation. + context: Fresh observations with authoritative row identity and timing. + plan: Completed chunk and its owned continuation cursor. + active_mask: Eligible rows that executed the completed chunk. + + Returns: + Physical row acceptance and an optional later plan. + """ + raise ValueError(f"Skill {self.skill_id!r} does not support continuation.") + + def on_plan_installed( + self, request: ResolvedSkillRequest, context: PlanningContext, plan: SkillPlan + ) -> None: + """Publish bookkeeping only after an execution plan was accepted. + + Args: + request: The installed plan's resolved invocation. + context: Observations used for this plan. + plan: Validated plan after destination and lifecycle acceptance. + """ + @abstractmethod def _plan( self, diff --git a/embodichain/lab/sim/atomic_skills/engine.py b/embodichain/lab/sim/atomic_skills/engine.py index 23c5b50fb0..88392f6cbd 100644 --- a/embodichain/lab/sim/atomic_skills/engine.py +++ b/embodichain/lab/sim/atomic_skills/engine.py @@ -22,7 +22,6 @@ from dataclasses import replace from functools import wraps -from dataclasses import replace from copy import deepcopy from types import MappingProxyType from typing import Callable, Iterable, Mapping, TYPE_CHECKING @@ -33,6 +32,7 @@ from .bindings import SkillBinding from .core import AtomicSkill, SkillDescriptor from .control import SkillControlOverrides, ControlPartCommandProfile +from .continuation import PlanContinuationResult from .invocation import SkillInvocation, GoalT, OptionsT, ResolvedSkillRequest from .nominal import NominalMotionSource from .plans import ( @@ -608,32 +608,7 @@ def finalize_selected( external_nominal=nominal_source is not None, ) self._validate_plan(result, current, request) - if ( - request.motion_policy.require_final_velocity - and request.motion_policy.final_velocity_rejection == "mask_only" - ): - velocity_mask = self._final_velocity_mask(result) - success = result.plan_success & velocity_mask - failure = result.diagnostics.failure - if failure is None and not success.all(): - failure = PlanningFailure("final_velocity_limit", retryable=False) - result = replace( - result, - plan_success=success, - diagnostics=replace( - result.diagnostics, - failure=failure, - metadata={ - **result.diagnostics.metadata, - "final_velocity_qualification": { - "scope": "complete_request_post_recovery_and_transform", - "valid_mask": velocity_mask.detach().cpu().tolist(), - "physical_evidence": False, - }, - }, - ), - ) - self._validate_plan(result, current, request) + result = self._qualify_mask_only_velocity(request, current, result) failure = result.diagnostics.failure if ( not request.recovery_policy.retry_all_failed_plans @@ -837,6 +812,10 @@ def _transform_plan( ) -> SkillPlan: """Apply one owned caller transform with standard validation.""" if plan_transform is not None: + if plan.continuation is not None: + raise ValueError( + "Observed continuation does not support plan transforms." + ) recovery_outcome = plan.planning_recovery_outcome if not callable(plan_transform): raise TypeError("plan_transform must be callable or None.") @@ -859,6 +838,38 @@ def _transform_plan( self._validate_plan(plan, context, request) return plan + def _qualify_mask_only_velocity( + self, request: ResolvedSkillRequest, context: PlanningContext, result: SkillPlan + ) -> SkillPlan: + """Apply the same mask-only command qualification to every observed chunk.""" + if ( + request.motion_policy.require_final_velocity + and request.motion_policy.final_velocity_rejection == "mask_only" + ): + velocity_mask = self._final_velocity_mask(result) + success = result.plan_success & velocity_mask + failure = result.diagnostics.failure + if failure is None and not success.all(): + failure = PlanningFailure("final_velocity_limit", retryable=False) + result = replace( + result, + plan_success=success, + diagnostics=replace( + result.diagnostics, + failure=failure, + metadata={ + **result.diagnostics.metadata, + "final_velocity_qualification": { + "scope": "complete_request_post_recovery_and_transform", + "valid_mask": velocity_mask.detach().cpu().tolist(), + "physical_evidence": False, + }, + }, + ), + ) + self._validate_plan(result, context, request) + return result + def _final_velocity_mask(self, plan: SkillPlan) -> torch.Tensor: """Check actual full-robot samples against unchanged authored/runtime caps.""" from embodichain.compute.trajectory.limits import joint_velocity_validity @@ -1082,6 +1093,45 @@ def plan( nominal_source=nominal_source, ) + def continue_request( + self, + request: ResolvedSkillRequest, + context: PlanningContext, + previous: SkillPlan, + active_mask: torch.Tensor, + ) -> PlanContinuationResult: + """Prepare another bounded observed chunk of the current invocation.""" + action = self._skills[request.skill_id] + current = replace(context, planning_mask=active_mask) + decision = action.continue_plan(request, current, previous, active_mask) + if not isinstance(decision, PlanContinuationResult): + raise TypeError("Expected PlanContinuationResult.") + if ( + decision.accepted.shape != active_mask.shape + or decision.accepted.device != active_mask.device + or (decision.accepted & ~active_mask).any() + ): + raise ValueError("Continuation acceptance must be a subset of active rows.") + plan = decision.plan + if plan is not None: + if (plan.plan_success & ~decision.accepted).any(): + raise ValueError("Continuation cannot reactivate rejected rows.") + if ( + plan.continuation is None + or previous.continuation is None + or plan.continuation.position <= previous.continuation.position + or plan.continuation.limit != previous.continuation.limit + ): + raise ValueError("Continuation must advance within its original bound.") + plan = replace( + plan, commands=action._authorize_command_targets(request, plan.commands) + ) + plan = self._qualify_completed_plan(action, request, current, plan, None) + plan = self._qualify_mask_only_velocity(request, current, plan) + self._validate_plan(plan, current, request) + decision = replace(decision, plan=plan) + return decision + def initial_context( self, *, @@ -1202,6 +1252,10 @@ def compile( if preserve_failed_positions else plan.joint_trajectory.hold_rows(step_success, previous_qpos) ) + if plan.continuation is not None: + raise ValueError( + "Observed contact paths require an execution session; offline projection cannot supply future evidence." + ) plans.append(plan) trajectories.append(trajectory) @@ -1374,6 +1428,10 @@ def rebuild_plan_from_trajectory( """ if not isinstance(plan, SkillPlan): raise TypeError("plan must be an SkillPlan.") + if plan.continuation is not None: + raise ValueError( + "Observed continuation cannot be rebuilt from external motion." + ) if not isinstance(trajectory, TimedTrajectory): raise TypeError("trajectory must be a TimedTrajectory.") self._validate_context(context) @@ -1470,6 +1528,10 @@ def rebuild_plan_from_mapped_trajectory( ) from ._expansion_phases import FREE_PHASES + if plan.continuation is not None: + raise ValueError( + "Observed continuation cannot be rebuilt from external motion." + ) if not isinstance(reference, TrajectoryTemplate) or not isinstance( result, MappedRetimeResult ): diff --git a/embodichain/lab/sim/atomic_skills/execution.py b/embodichain/lab/sim/atomic_skills/execution.py index 7342c1188a..4744c9cc43 100644 --- a/embodichain/lab/sim/atomic_skills/execution.py +++ b/embodichain/lab/sim/atomic_skills/execution.py @@ -73,6 +73,7 @@ class ExecutionEventKind(str, Enum): SKILL_PLANNED = "skill_planned" INVOCATION_REVISED = "invocation_revised" + PLAN_CONTINUED = "plan_continued" REPLANNED = "replanned" TRACKING_DIVERGED = "tracking_diverged" TRACKING_FEEDBACK_FAILED = "tracking_feedback_failed" @@ -180,6 +181,7 @@ def __post_init__(self) -> None: if self.event_kind not in { ExecutionEventKind.SKILL_PLANNED, ExecutionEventKind.INVOCATION_REVISED, + ExecutionEventKind.PLAN_CONTINUED, ExecutionEventKind.REPLANNED, }: raise ValueError("event_kind must describe an installed action plan.") @@ -678,6 +680,10 @@ def _prepare_revision( raise TypeError("invocation must be an SkillInvocation.") if self._status is not ExecutionStatus.RUNNING: raise RuntimeError("Only a running execution session can be revised.") + if self._plan is not None and self._plan.continuation is not None: + raise ValueError( + "Observed continuation does not support invocation revisions." + ) self._require_accepted_plan_installation() if ( self._pending_effect is not None @@ -705,6 +711,10 @@ def _install_prepared_revision( raise TypeError("replacement must be a ResolvedSkillRequest.") if self._status is not ExecutionStatus.RUNNING: raise RuntimeError("Only a running execution session can be revised.") + if self._plan is not None and self._plan.continuation is not None: + raise ValueError( + "Observed continuation does not support invocation revisions." + ) self._require_accepted_plan_installation() if ( self._pending_effect is not None @@ -1303,7 +1313,7 @@ def _plan_current( request = self._requests[self._invocation_index] plan = self._engine._plan_request( request, - context, + replace(context, planning_mask=self._pending), plan_transform=self._plan_transform, nominal_source=self._nominal_source, ) @@ -1318,6 +1328,12 @@ def _install_plan( destination_continuity_validated: bool = False, ) -> None: """Install a plan, checking target continuity unless already checked.""" + if plan.continuation is not None and ( + self._plan_transform is not None or self._nominal_source is not None + ): + raise ValueError( + "Observed continuation does not support external nominal motion or plan transforms." + ) replacement_targets = { (target.transport_id, target.target_id): target for target in plan.commands.targets @@ -1337,6 +1353,7 @@ def _install_plan( not in ( ExecutionEventKind.REPLANNED, ExecutionEventKind.INVOCATION_REVISED, + ExecutionEventKind.PLAN_CONTINUED, ) or replacement_destinations ): @@ -1346,6 +1363,7 @@ def _install_plan( not in ( ExecutionEventKind.REPLANNED, ExecutionEventKind.INVOCATION_REVISED, + ExecutionEventKind.PLAN_CONTINUED, ) or replacement_tracking_routes ): @@ -1396,6 +1414,13 @@ def _install_plan( # cancel/hold and the once-only abort notification to the host. self._plan_installation_failure = f"{type(error).__name__}: {error}" raise + try: + self._engine._skills[plan.skill_id].on_plan_installed( + self._requests[self._invocation_index], context, plan + ) + except Exception as error: + self._plan_installation_failure = f"{type(error).__name__}: {error}" + raise self._plan_attempts.append(attempt) self._queued_events.append( self._event(event_kind, planned_mask, "Planned from the latest context.") @@ -1458,6 +1483,7 @@ def _validate_destination_continuity( if event_kind not in ( ExecutionEventKind.REPLANNED, ExecutionEventKind.INVOCATION_REVISED, + ExecutionEventKind.PLAN_CONTINUED, ): return replacement_targets = { @@ -1519,6 +1545,7 @@ def _validate_tracking_continuity( if event_kind not in ( ExecutionEventKind.REPLANNED, ExecutionEventKind.INVOCATION_REVISED, + ExecutionEventKind.PLAN_CONTINUED, ): return if self._plan is None: @@ -1775,6 +1802,36 @@ def _finish_action( return None, hold_targets, events return None, active_targets, events + if self._plan.continuation is not None: + decision = self._engine.continue_request( + self._requests[self._invocation_index], + self._context, + self._plan, + execution_mask & self._pending & self._eligible, + ) + rejected = self._pending & ~decision.accepted + if rejected.any(): + self.deactivate_rows( + rejected, + reason="; ".join(decision.messages) + or "Contact phase condition failed.", + ) + events.extend(self._drain_events()) + if self._status is not ExecutionStatus.RUNNING: + return None, active_targets, events + if decision.plan is not None and self._eligible.any(): + started = self._action_started_at + self._install_plan( + decision.plan, self._context, ExecutionEventKind.PLAN_CONTINUED + ) + self._action_started_at = started + events.extend(self._drain_events()) + if self._status is not ExecutionStatus.RUNNING: + return None, active_targets, events + return None, (), events + self._plan = replace(self._plan, continuation=None) + execution_mask &= decision.accepted + failed_effect = torch.zeros_like(execution_mask) unresolved = torch.zeros_like(execution_mask) made_progress = False diff --git a/embodichain/lab/sim/atomic_skills/plans.py b/embodichain/lab/sim/atomic_skills/plans.py index 3a8e7bb3fd..95a187cc0a 100644 --- a/embodichain/lab/sim/atomic_skills/plans.py +++ b/embodichain/lab/sim/atomic_skills/plans.py @@ -31,6 +31,7 @@ normalize_success_mask, ) +from .continuation import PlanContinuation from .effects import StateDelta from .policies import RecoveryPolicy from .runtime_commands import TimedCommandSequence @@ -636,7 +637,28 @@ class SkillPlan: discard it or commit only to an independently owned preview cursor. """ + continuation: PlanContinuation | None = field(default=None, kw_only=True) + """Next observed chunk in the same invocation and recovery scope.""" + def __post_init__(self) -> None: + if self.continuation is not None: + cursor = self.continuation + if not isinstance(cursor, PlanContinuation): + raise TypeError("continuation must implement PlanContinuation.") + if ( + type(cursor.position) is not int + or type(cursor.limit) is not int + or not 0 < cursor.position <= cursor.limit <= 256 + ): + raise ValueError("Continuation progress must be positive and bounded.") + if not cursor.phase_name or not isinstance(cursor.phase_name, str): + raise ValueError("Continuation phases require audit names.") + if ( + not self.expected_effects.is_empty + or self.effect_verification is not None + ): + raise ValueError("Intermediate chunks cannot commit terminal effects.") + object.__setattr__(self, "continuation", cursor.snapshot()) if not isinstance(self.skill_id, str) or not self.skill_id: raise ValueError("SkillPlan.skill_id must be non-empty.") outcome = self.planning_recovery_outcome @@ -1048,6 +1070,7 @@ def snapshot(self) -> SkillPlan: """ return SkillPlan( skill_id=self.skill_id, + continuation=self.continuation, plan_success=self.plan_success, commands=self.commands, recovery_policy=self.recovery_policy, diff --git a/embodichain/lab/sim/atomic_skills/primitives/__init__.py b/embodichain/lab/sim/atomic_skills/primitives/__init__.py index cf6f9878a6..11d509c388 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/__init__.py +++ b/embodichain/lab/sim/atomic_skills/primitives/__init__.py @@ -44,8 +44,14 @@ ) from .move_joints import JointPositionGoal, MoveJoints, MoveJointsOptions from .open_door import OpenDoor, OpenDoorGoal, OpenDoorOptions -from .pick_up import ContactGraspGoal, GraspGoal, PickUp, PickUpOptions -from .place import AssembleGoal, Place, PlaceGoal, PlaceOptions +from .pick_up import ( + ContactPickUpGoal, + ContactGraspGoal, + GraspGoal, + PickUp, + PickUpOptions, +) +from .place import ContactPlaceGoal, AssembleGoal, Place, PlaceGoal, PlaceOptions from .press import ( Press, PressGoal, @@ -104,6 +110,8 @@ "CoordinatedPlacementOptions", "EndEffectorPoseGoal", "ContactGraspGoal", + "ContactPickUpGoal", + "ContactPlaceGoal", "GraspGoal", "HandOver", "HandOverGoal", diff --git a/embodichain/lab/sim/atomic_skills/primitives/contact.py b/embodichain/lab/sim/atomic_skills/primitives/contact.py new file mode 100644 index 0000000000..22d38b0305 --- /dev/null +++ b/embodichain/lab/sim/atomic_skills/primitives/contact.py @@ -0,0 +1,497 @@ +# ---------------------------------------------------------------------------- +# 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 manipulation goals and observed phase planning shared by PickUp/Place.""" + +from __future__ import annotations + +from dataclasses import dataclass, replace +from typing import TYPE_CHECKING, ClassVar, Literal, Protocol, runtime_checkable + +import torch + +from embodichain.lab.sim.atomic_skills.bindings import JointPositionTarget +from embodichain.lab.sim.atomic_skills.continuation import PlanContinuationResult +from embodichain.lab.sim.atomic_skills.plans import SkillPlan, TimedTrajectory +from embodichain.lab.sim.atomic_skills.primitives._helpers import ( + plan_named_arm_trajectory, + require_shared_task_state_key, +) +from embodichain.lab.sim.atomic_skills.invocation import ResolvedSkillRequest +from embodichain.lab.sim.atomic_skills.state import PlanningContext + +if TYPE_CHECKING: + from embodichain.lab.sim.atomic_skills.core import AtomicSkill + + +@dataclass(frozen=True, slots=True) +class ContactPostcondition: + """Named physical check or explicit settling target in a contact path. + + Args: + kind: Physical checkpoint or explicit command-hold duration. + target: Identifier resolved by the episode's observation source. + """ + + kind: Literal["hold", "check"] + target: str + + def __post_init__(self) -> None: + if ( + self.kind not in {"hold", "check"} + or not isinstance(self.target, str) + or not self.target + ): + raise ValueError("Contact postconditions require a hold/check target.") + + +@dataclass(frozen=True, slots=True) +class ContactPhase: + """One contact path waypoint, grasp, or hand command with named evidence. + + Args: + kind: Cartesian path, pickup contact closure, or named joint path. + target: Observation-source target resolved immediately before planning. + after: Ordered settling and physical checkpoints on the command grid. + """ + + kind: Literal["pose", "grasp", "joint"] + target: str + after: tuple[ContactPostcondition, ...] = () + + def __post_init__(self) -> None: + if ( + self.kind not in {"pose", "grasp", "joint"} + or not isinstance(self.target, str) + or not self.target + ): + raise ValueError("Invalid contact phase.") + if not all(type(v) is ContactPostcondition for v in self.after): + raise TypeError("Contact phase tails must be typed postconditions.") + object.__setattr__(self, "after", tuple(self.after)) + + +def _validate_path( + source: str, phases: tuple[ContactPhase, ...], *, pickup: bool +) -> None: + if ( + not isinstance(source, str) + or not source + or not 1 <= len(phases) <= 128 + or not all(type(p) is ContactPhase for p in phases) + ): + raise ValueError( + "Contact goals require a bound source and a finite typed path." + ) + if sum(p.kind == "grasp" for p in phases) != (1 if pickup else 0): + raise ValueError( + "Pickup paths need exactly one grasp; placement paths cannot grasp." + ) + + +@dataclass(frozen=True, slots=True) +class ContactPickUpGoal: + """Approach, establish contact grasp and lift using an observed path source. + + Args: + source: Engine-local source identifier; its episode host retains ownership. + phases: Bounded path containing exactly one grasp and its required evidence. + + The source supplies physical acceptance; completion projects no rigid relation. + """ + + goal_kind: ClassVar[str] = "contact_pick_up" + source: str + phases: tuple[ContactPhase, ...] + + def __post_init__(self) -> None: + object.__setattr__(self, "phases", tuple(self.phases)) + _validate_path(self.source, self.phases, pickup=True) + + +@dataclass(frozen=True, slots=True) +class ContactPlaceGoal: + """Place a contact-held subject without projecting a rigid object/tool relation. + + Args: + source: Engine-local source that observes the existing physical grasp. + phases: Bounded transport, release and retreat path, with no new grasp. + """ + + goal_kind: ClassVar[str] = "contact_place" + source: str + phases: tuple[ContactPhase, ...] + + def __post_init__(self) -> None: + object.__setattr__(self, "phases", tuple(self.phases)) + _validate_path(self.source, self.phases, pickup=False) + + +@dataclass(frozen=True, slots=True, eq=False) +class ResolvedContactPhase: + """Owned same-observation geometric inputs; contains no controller handles. + + Args: + values: Cartesian poses or joint path in the source's selected row order. + held_qpos: Full loaded robot commands preserved on nonmoving joints. + valid: Boolean row validity from geometry and contact preconditions. + endpoint: Endpoint receiving joint paths; Cartesian paths use motion. + data: Owned tensor/scalar metadata committed only on plan installation. + """ + + values: torch.Tensor + held_qpos: torch.Tensor + valid: torch.Tensor + endpoint: Literal["motion", "grasp"] = "motion" + data: tuple[tuple[str, object], ...] = () + + def __post_init__(self) -> None: + if ( + not self.held_qpos.is_floating_point() + or not self.values.is_floating_point() + or self.values.dtype != self.held_qpos.dtype + or self.values.ndim < 2 + or self.held_qpos.ndim != 2 + or self.valid.dtype != torch.bool + or self.valid.shape != self.held_qpos.shape[:1] + ): + raise ValueError( + "Contact snapshots require full joint rows and matching boolean validity." + ) + if ( + self.values.shape[0] != self.held_qpos.shape[0] + or self.values.device != self.held_qpos.device + or self.valid.device != self.held_qpos.device + ): + raise ValueError("Contact snapshot tensors must share batch/device.") + if ( + not torch.isfinite(self.values).all() + or not torch.isfinite(self.held_qpos).all() + ): + raise ValueError( + "Resolved contact commands must be finite, including inactive holds." + ) + if self.endpoint not in {"motion", "grasp"} or len( + {k for k, _ in self.data} + ) != len(self.data): + raise ValueError("Invalid contact endpoint or duplicate snapshot keys.") + if any( + not isinstance(k, str) + or not k + or not (isinstance(v, (torch.Tensor, str, bool, int, float)) or v is None) + for k, v in self.data + ): + raise TypeError( + "Contact snapshot metadata must contain scalar values or owned tensors." + ) + for name in ["values", "held_qpos", "valid"]: + object.__setattr__(self, name, getattr(self, name).clone()) + object.__setattr__( + self, + "data", + tuple( + (k, v.clone() if isinstance(v, torch.Tensor) else v) + for k, v in self.data + ), + ) + + def snapshot(self) -> ResolvedContactPhase: + """Return owned observation data.""" + return replace(self) + + +@dataclass(frozen=True, slots=True, eq=False) +class ContactContinuation: + """Finite observed path cursor with immutable phase/checkpoint evidence. + + Args: + position: One-based index of the current phase. + limit: Total declared phase count in this invocation. + phase_name: Auditable current target identifier. + phase: Declarative target and ordered postconditions. + resolved: Owned geometric inputs from the planning observation. + checkpoints: Command indices and target identifiers requiring host evidence. + """ + + position: int + limit: int + phase_name: str + phase: ContactPhase + resolved: ResolvedContactPhase + checkpoints: tuple[tuple[int, str], ...] = () + + def snapshot(self) -> ContactContinuation: + """Copy the phase's derived tensors, retaining only declarative references.""" + return replace(self, resolved=self.resolved.snapshot()) + + +@runtime_checkable +class ContactPhaseSource(Protocol): + """Engine-bound observation/geometry adapter; never plans or steps physics.""" + + def resolve( + self, + phase: ContactPhase, + request: ResolvedSkillRequest, + context: PlanningContext, + active_mask: torch.Tensor, + ) -> ResolvedContactPhase: + """Resolve one target from context and active rows without committing state.""" + ... + + def hold_steps(self, target: str) -> int: + """Return a declared positive number of settling command samples.""" + ... + + def begin( + self, + cursor: ContactContinuation, + context: PlanningContext, + active_mask: torch.Tensor, + ) -> None: + """Install reference bookkeeping after the engine accepts a phase plan.""" + ... + + def finish( + self, + cursor: ContactContinuation, + context: PlanningContext, + active_mask: torch.Tensor, + ) -> tuple[torch.Tensor, tuple[str, ...]]: + """Return per-row acceptance only after all physical checkpoints are observed.""" + ... + + +def _source( + action: AtomicSkill, goal: ContactPickUpGoal | ContactPlaceGoal +) -> ContactPhaseSource: + return action.planning_services.contact_source(goal.source) + + +def _trajectory(context: PlanningContext, positions: torch.Tensor) -> TimedTrajectory: + dt = torch.full( + positions.shape[:2], + context.require_control_dt(), + dtype=torch.float64, + device=positions.device, + ) + dt[:, 0] = 0 + return TimedTrajectory(positions, None, None, dt, context.env_ids) + + +def _plan_contact_phase( + action: AtomicSkill, + request: ResolvedSkillRequest, + context: PlanningContext, + position: int = 0, + active_mask: torch.Tensor | None = None, +) -> SkillPlan: + """Plan one observed chunk directly, without nested atomic invocations.""" + goal = request.goal + if request.recovery_policy.planning_budget is not None: + raise ValueError( + "Observed contact paths do not support external planning budgets." + ) + if request.recovery_policy.max_replans or request.recovery_policy.max_skill_retries: + raise ValueError( + "Contact paths use normal continuation, not replay of partially executed skills." + ) + if ( + request.motion_policy.sample_count_retries + or request.motion_policy.segment_sample_count_retries + or request.motion_policy.segment_continuous_ik_recovery + ): + raise ValueError("Observed contact paths require a fixed sampling policy.") + if ( + request.motion_policy.dynamic_collision_mode != "off" + or request.motion_policy.strategy != "ik_interp" + ): + raise ValueError( + "Observed contact paths require the declared IK grid and native contact qualification." + ) + active = active_mask if active_mask is not None else context.planning_mask + if active is None: + active = torch.ones( + context.batch_size, dtype=torch.bool, device=context.env_ids.device + ) + key = require_shared_task_state_key( + request.binding.endpoint("primary", "motion"), + request.binding.endpoint("primary", "grasp"), + participant="Contact manipulation", + ) + if key is not None: + active = active & ~context.task.held_object_mask(key) + for pair, held in context.task.coordinated_held_objects.items(): + if key in pair: + active &= ~( + torch.ones_like(active) if held.env_mask is None else held.env_mask + ) + phase = goal.phases[position] + source = _source(action, goal) + resolved = source.resolve(phase, request, context, active) + valid = active & resolved.valid + motion = request.binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + hand = request.binding.endpoint("primary", "grasp").require_target( + JointPositionTarget + ) + if phase.kind == "pose": + success, arm = plan_named_arm_trajectory( + action.motion_generator, + motion.control_part, + context.robot.qpos[:, list(motion.joint_ids)], + resolved.values, + resolved.values.shape[1] + 1, + request.motion_policy, + context.require_control_dt(), + active_mask=valid, + cartesian_linear=True, + ) + positions = resolved.held_qpos[:, None].expand(-1, arm.shape[1], -1).clone() + positions[:, :, list(motion.joint_ids)] = arm + segments = {"move_end_effector": arm.shape[1]} + elif phase.kind == "joint": + target = motion if resolved.endpoint == "motion" else hand + positions = ( + resolved.held_qpos[:, None].expand(-1, resolved.values.shape[1], -1).clone() + ) + positions[:, :, list(target.joint_ids)] = resolved.values + success = valid & action.motion_generator.joint_steps_within_velocity_limits( + context.robot.qpos[:, list(target.joint_ids)], + resolved.values, + target.control_part, + velocity_limit=request.motion_policy.velocity_limit, + step_dt=context.require_control_dt(), + ) + segments = {"move_joints": positions.shape[1]} + else: + opened = request.binding.endpoint("primary", "grasp").joint_positions( + "open", + num_envs=context.batch_size, + device=action.device, + dtype=context.robot.qpos.dtype, + ) + closed = dict(resolved.data)["closed_qpos"] + direction = request.skill_options.approach_direction.to(context.robot.qpos) + direction = direction / torch.linalg.vector_norm( + direction, dim=-1, keepdim=True + ) + success, positions, segments = action._get_full_pickup_trajectory( + resolved.values, + context.robot.qpos[:, list(motion.joint_ids)], + context.robot.qpos, + request.motion_policy, + request.skill_options, + direction, + motion, + hand, + opened, + closed, + context.require_control_dt(), + contact_grasp=True, + env_ids=context.env_ids, + active_mask=valid, + ) + success &= valid + checkpoints = [] + count = positions.shape[1] + for post in phase.after: + if post.kind == "hold": + n = source.hold_steps(post.target) + else: + checkpoints.append((count - 1, post.target)) + n = 2 + if type(n) is not int or n < 1: + raise ValueError( + "Contact settling targets must supply positive integer steps." + ) + positions = torch.cat((positions, positions[:, -1:].expand(-1, n, -1)), 1) + segments[f"{post.kind}:{post.target}:{len(segments)}"] = n + count += n + plan = action.build_plan( + request, + context, + success=success & valid, + trajectory=_trajectory(context, positions), + segment_lengths=segments, + replannable=False, + ) + return replace( + plan, + continuation=ContactContinuation( + position + 1, + len(goal.phases), + phase.target, + phase, + resolved, + tuple(checkpoints), + ), + ) + + +def _continue_contact_phase( + action: AtomicSkill, + request: ResolvedSkillRequest, + context: PlanningContext, + plan: SkillPlan, + active_mask: torch.Tensor, +) -> PlanContinuationResult: + """Validate the observed chunk and keep the same invocation identity.""" + cursor = plan.continuation + if not isinstance(cursor, ContactContinuation): + raise TypeError("Expected contact path continuation.") + accepted, messages = _source(action, request.goal).finish( + cursor, context, active_mask + ) + accepted &= active_mask + next_plan = None + if cursor.position < len(request.goal.phases) and bool(accepted.any()): + next_plan = _plan_contact_phase( + action, request, context, cursor.position, accepted + ) + return PlanContinuationResult(accepted, next_plan, messages) + + +def _begin_contact_phase( + action: AtomicSkill, + request: ResolvedSkillRequest, + context: PlanningContext, + plan: SkillPlan, +) -> None: + """Commit only observation/reference bookkeeping after plan acceptance.""" + if isinstance(request.goal, (ContactPickUpGoal, ContactPlaceGoal)): + _source(action, request.goal).begin( + plan.continuation, + context, + plan.plan_success + & ( + context.planning_mask + if context.planning_mask is not None + else torch.ones_like(plan.plan_success) + ), + ) + + +__all__ = [ + "ContactPhase", + "ContactPostcondition", + "ContactPickUpGoal", + "ContactPlaceGoal", + "ContactPhaseSource", + "ContactContinuation", + "ResolvedContactPhase", +] diff --git a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py index a0ec0312e5..eaeb085d28 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py @@ -18,6 +18,14 @@ from __future__ import annotations +from embodichain.lab.sim.atomic_skills.continuation import PlanContinuationResult +from embodichain.lab.sim.atomic_skills.primitives.contact import ( + ContactPickUpGoal, + _plan_contact_phase, + _continue_contact_phase, + _begin_contact_phase, +) + import math from dataclasses import dataclass, field, replace from typing import ClassVar @@ -348,11 +356,17 @@ def __post_init__(self) -> None: ) -class PickUp(AtomicSkill[GraspGoal | ContactGraspGoal, PickUpOptions]): +class PickUp( + AtomicSkill[GraspGoal | ContactGraspGoal | ContactPickUpGoal, PickUpOptions] +): """Approach a grasp pose, close the gripper, lift.""" skill_id: ClassVar[str] = "pick_up" - GoalType: ClassVar[tuple[type, ...]] = (GraspGoal, ContactGraspGoal) + GoalType: ClassVar[tuple[type, ...]] = ( + GraspGoal, + ContactGraspGoal, + ContactPickUpGoal, + ) OptionsType: ClassVar[type] = PickUpOptions binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( @@ -373,13 +387,29 @@ class PickUp(AtomicSkill[GraspGoal | ContactGraspGoal, PickUpOptions]): ), ) + def continue_plan( + self, + request: ResolvedSkillRequest, + context: PlanningContext, + plan: SkillPlan, + active_mask: torch.Tensor, + ) -> PlanContinuationResult: + return _continue_contact_phase(self, request, context, plan, active_mask) + + def on_plan_installed( + self, request: ResolvedSkillRequest, context: PlanningContext, plan: SkillPlan + ) -> None: + _begin_contact_phase(self, request, context, plan) + def _scene_dependencies( self, - request: ResolvedSkillRequest[GraspGoal | ContactGraspGoal, PickUpOptions], + request: ResolvedSkillRequest[ + GraspGoal | ContactGraspGoal | ContactPickUpGoal, PickUpOptions + ], ) -> tuple[str, ...]: """Monitor the acquired object and late-bound downstream targets.""" dependencies = set(super()._scene_dependencies(request)) - if isinstance(request.goal, ContactGraspGoal): + if isinstance(request.goal, (ContactGraspGoal, ContactPickUpGoal)): return tuple(sorted(dependencies)) entity_id = request.goal.semantics.entity_id # An explicit object pose is a scene-independent planning input. Do @@ -410,6 +440,7 @@ def _get_full_pickup_trajectory( *, contact_grasp: bool = False, env_ids: torch.Tensor | None = None, + active_mask: torch.Tensor | None = None, ) -> tuple[torch.Tensor, torch.Tensor, dict[str, int]]: if options.approach_along_grasp_axis: approach_direction = grasp_xpos[:, :3, 2] @@ -436,6 +467,19 @@ def sampled_move( poses = interpolate_xpos_batched(start_pose, target_pose, sample_count)[ :, 1: ] + if active_mask is not None: + mask, sampled = plan_named_arm_trajectory( + self.motion_generator, + manipulator.control_part, + start_full[:, arm_ids], + poses, + sample_count, + motion_policy, + interpolation_dt, + active_mask=active_mask, + cartesian_linear=True, + ) + return mask, sampled result = self.motion_generator.generate( build_pose_plan_states(poses), options=replace( @@ -692,11 +736,16 @@ def sampled_move( def _plan( self, - request: ResolvedSkillRequest[GraspGoal | ContactGraspGoal, PickUpOptions], + request: ResolvedSkillRequest[ + GraspGoal | ContactGraspGoal | ContactPickUpGoal, PickUpOptions + ], context: PlanningContext, ) -> SkillPlan: """Plan approach, close, and lift segments without committing attachment.""" target = self.require_goal(request) + if isinstance(target, ContactPickUpGoal): + self._validate_contact_options(request.skill_options) + return _plan_contact_phase(self, request, context) if isinstance(target, ContactGraspGoal): self._validate_contact_goal(request, context) options = replace( @@ -875,12 +924,8 @@ def _plan( provenance=None, ) - def _validate_contact_goal( - self, - request: ResolvedSkillRequest[GraspGoal | ContactGraspGoal, PickUpOptions], - context: PlanningContext, - ) -> None: - options = request.skill_options + @staticmethod + def _validate_contact_options(options: PickUpOptions) -> None: if ( options.fixed_object_to_eef is not None or options.downstream_object_target_poses @@ -890,6 +935,15 @@ def _validate_contact_goal( raise ValueError( "Contact pickup requires an explicit grasp pose without rigid-object options." ) + + def _validate_contact_goal( + self, + request: ResolvedSkillRequest[ + GraspGoal | ContactGraspGoal | ContactPickUpGoal, PickUpOptions + ], + context: PlanningContext, + ) -> None: + self._validate_contact_options(request.skill_options) motion = request.binding.endpoint("primary", "motion") hand = request.binding.endpoint("primary", "grasp") key = require_shared_task_state_key( @@ -910,12 +964,16 @@ def _validate_contact_goal( def _plan_from_nominal( self, - request: ResolvedSkillRequest[GraspGoal | ContactGraspGoal, PickUpOptions], + request: ResolvedSkillRequest[ + GraspGoal | ContactGraspGoal | ContactPickUpGoal, PickUpOptions + ], context: PlanningContext, nominal_source: NominalMotionSource, ) -> SkillPlan: """Assemble a Pick from an explicitly supplied external motion.""" target = self.require_goal(request) + if isinstance(target, ContactPickUpGoal): + raise ValueError("Observed contact paths cannot replay nominal motion.") if isinstance(target, ContactGraspGoal): self._validate_contact_goal(request, context) motion = self._validate_nominal_motion( @@ -1032,7 +1090,9 @@ def _plan_from_nominal( def _assemble_pick_plan( self, - request: ResolvedSkillRequest[GraspGoal | ContactGraspGoal, PickUpOptions], + request: ResolvedSkillRequest[ + GraspGoal | ContactGraspGoal | ContactPickUpGoal, PickUpOptions + ], context: PlanningContext, *, success: torch.Tensor, @@ -1045,6 +1105,8 @@ def _assemble_pick_plan( provenance: dict[str, object] | None, ) -> SkillPlan: """Assemble Pick semantics shared by native and external motion.""" + if isinstance(request.goal, ContactPickUpGoal): + raise ValueError("Observed contact paths cannot replay nominal motion.") target = request.goal options = request.skill_options motion = request.binding.endpoint("primary", "motion") @@ -1412,4 +1474,10 @@ def _upright_adjusted_grasp_poses( return adjusted_grasp_xpos -__all__ = ["GraspGoal", "ContactGraspGoal", "PickUp", "PickUpOptions"] +__all__ = [ + "GraspGoal", + "ContactGraspGoal", + "ContactPickUpGoal", + "PickUp", + "PickUpOptions", +] diff --git a/embodichain/lab/sim/atomic_skills/primitives/place.py b/embodichain/lab/sim/atomic_skills/primitives/place.py index d65249429f..96880bbc0d 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/place.py +++ b/embodichain/lab/sim/atomic_skills/primitives/place.py @@ -18,6 +18,14 @@ from __future__ import annotations +from embodichain.lab.sim.atomic_skills.continuation import PlanContinuationResult +from embodichain.lab.sim.atomic_skills.primitives.contact import ( + ContactPlaceGoal, + _plan_contact_phase, + _continue_contact_phase, + _begin_contact_phase, +) + from dataclasses import dataclass, field from typing import ClassVar, Literal @@ -168,7 +176,7 @@ def __post_init__(self) -> None: raise TypeError("preserve_current_object_orientation must be a bool.") -class Place(AtomicSkill[PlaceGoal | AssembleGoal, PlaceOptions]): +class Place(AtomicSkill[PlaceGoal | AssembleGoal | ContactPlaceGoal, PlaceOptions]): """Lower the held object to a place pose, open the gripper, retract. The :class:`PlaceGoal` may carry either a single waypoint @@ -190,6 +198,7 @@ class Place(AtomicSkill[PlaceGoal | AssembleGoal, PlaceOptions]): GoalType: ClassVar[type | tuple[type, ...]] = ( PlaceGoal, AssembleGoal, + ContactPlaceGoal, ) OptionsType: ClassVar[type] = PlaceOptions binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( @@ -210,9 +219,25 @@ class Place(AtomicSkill[PlaceGoal | AssembleGoal, PlaceOptions]): ), ) + def continue_plan( + self, + request: ResolvedSkillRequest, + context: PlanningContext, + plan: SkillPlan, + active_mask: torch.Tensor, + ) -> PlanContinuationResult: + return _continue_contact_phase(self, request, context, plan, active_mask) + + def on_plan_installed( + self, request: ResolvedSkillRequest, context: PlanningContext, plan: SkillPlan + ) -> None: + _begin_contact_phase(self, request, context, plan) + def _scene_dependencies( self, - request: ResolvedSkillRequest[PlaceGoal | AssembleGoal, PlaceOptions], + request: ResolvedSkillRequest[ + PlaceGoal | AssembleGoal | ContactPlaceGoal, PlaceOptions + ], ) -> tuple[str, ...]: """Include an explicitly snapshot-grounded assembly base.""" dependencies = set(super()._scene_dependencies(request)) @@ -223,10 +248,14 @@ def _scene_dependencies( def _plan( self, - request: ResolvedSkillRequest[PlaceGoal | AssembleGoal, PlaceOptions], + request: ResolvedSkillRequest[ + PlaceGoal | AssembleGoal | ContactPlaceGoal, PlaceOptions + ], context: PlanningContext, ) -> SkillPlan: """Plan approach, release, and retract without committing detachment.""" + if isinstance(request.goal, ContactPlaceGoal): + return _plan_contact_phase(self, request, context) target = request.goal options = request.skill_options binding = request.binding @@ -488,12 +517,16 @@ def _plan( def _plan_from_nominal( self, - request: ResolvedSkillRequest[PlaceGoal | AssembleGoal, PlaceOptions], + request: ResolvedSkillRequest[ + PlaceGoal | AssembleGoal | ContactPlaceGoal, PlaceOptions + ], context: PlanningContext, nominal_source: NominalMotionSource, ) -> SkillPlan: """Assemble external placement without replanning the source path.""" target = self.require_goal(request) + if isinstance(request.goal, ContactPlaceGoal): + raise ValueError("Observed contact paths cannot replay nominal motion.") if not isinstance(target, PlaceGoal): raise ValueError("external placement currently requires a direct PlaceGoal") motion = self._validate_nominal_motion( @@ -606,7 +639,9 @@ def _plan_from_nominal( def _assemble_place_plan( self, - request: ResolvedSkillRequest[PlaceGoal | AssembleGoal, PlaceOptions], + request: ResolvedSkillRequest[ + PlaceGoal | AssembleGoal | ContactPlaceGoal, PlaceOptions + ], context: PlanningContext, *, success: torch.Tensor, @@ -661,11 +696,13 @@ def _assemble_place_plan( def _resolve_place_xpos( self, - target: PlaceGoal | AssembleGoal, + target: PlaceGoal | AssembleGoal | ContactPlaceGoal, state: PlanningContext, task_state_key: str, *, - request: ResolvedSkillRequest[PlaceGoal | AssembleGoal, PlaceOptions], + request: ResolvedSkillRequest[ + PlaceGoal | AssembleGoal | ContactPlaceGoal, PlaceOptions + ], ) -> tuple[torch.Tensor, AffordanceSample | None]: """Resolve the place EEF poses from a typed target. @@ -842,4 +879,4 @@ def _select_tcp_symmetric_place_variant( ] -__all__ = ["AssembleGoal", "Place", "PlaceGoal", "PlaceOptions"] +__all__ = ["AssembleGoal", "Place", "PlaceGoal", "ContactPlaceGoal", "PlaceOptions"] diff --git a/embodichain/lab/sim/atomic_skills/runtime.py b/embodichain/lab/sim/atomic_skills/runtime.py index f42d791644..dad0e1c260 100644 --- a/embodichain/lab/sim/atomic_skills/runtime.py +++ b/embodichain/lab/sim/atomic_skills/runtime.py @@ -25,6 +25,7 @@ from types import MappingProxyType from typing import TYPE_CHECKING from uuid import uuid4 +from weakref import WeakValueDictionary import numpy as np import torch @@ -58,6 +59,7 @@ ) if TYPE_CHECKING: + from .primitives.contact import ContactPhaseSource from embodichain.lab.sim.objects import Robot from embodichain.lab.sim.sim_manager import SimulationManager from embodichain.lab.sim.motion.motion_generator import MotionGenerator @@ -105,6 +107,9 @@ def __init__( hand_geometry_cfgs: Explicit Lab hand settings by grasp target ID; copied independently without capturing geometry or reading state. """ + self._contact_sources: WeakValueDictionary[str, ContactPhaseSource] = ( + WeakValueDictionary() + ) self._motion_generator = motion_generator self._robot: Robot = motion_generator.robot self._device = resolve_runtime_device(motion_generator.device) @@ -135,6 +140,49 @@ def __init__( {} if hand_geometry_cfgs is None else hand_geometry_cfgs ) + def register_contact_source( + self, source_id: str, source: "ContactPhaseSource" + ) -> None: + """Bind an episode-owned weak contact observation source. + + Args: + source_id: Nonempty identifier local to this engine. + source: Observation adapter; its episode host retains a strong reference. + + Raises: + ValueError: The identifier is already bound to another live source. + """ + from .primitives.contact import ContactPhaseSource + + if not isinstance(source_id, str) or not source_id: + raise ValueError("Contact source IDs must be nonempty strings.") + + if not isinstance(source, ContactPhaseSource): + raise TypeError("Invalid contact phase source.") + previous = self._contact_sources.get(source_id) + if previous is not None and previous is not source: + raise ValueError("Contact source IDs cannot be rebound while live.") + self._contact_sources[source_id] = source + + def contact_source(self, source_id: str) -> "ContactPhaseSource": + """Resolve a still-live source from this exact engine's registry. + + Args: + source_id: Identifier registered by the episode's composition root. + + Returns: + Live observation adapter owned by that episode. + + Raises: + ValueError: The source expired or was not registered on this engine. + """ + try: + return self._contact_sources[source_id] + except KeyError as error: + raise ValueError( + "Contact source is expired or belongs to another engine." + ) from error + @property def motion_generator(self) -> MotionGenerator: """Return the single motion generator owned by the engine.""" diff --git a/embodichain/lab/sim/atomic_skills/state.py b/embodichain/lab/sim/atomic_skills/state.py index 83aba095d6..0bc42d333a 100644 --- a/embodichain/lab/sim/atomic_skills/state.py +++ b/embodichain/lab/sim/atomic_skills/state.py @@ -951,6 +951,9 @@ class PlanningContext: control_dt: float | None = None """Explicit command period used by action-owned interpolation.""" + planning_mask: torch.Tensor | None = field(default=None, kw_only=True) + """Eligible planning rows; inactive rows cannot determine path duration.""" + affordance_sampling: AffordanceSamplingContext | None = None """Optional immutable Affordance sampling stream supplied by the host.""" @@ -966,6 +969,15 @@ class PlanningContext: """Optional native-world source stamp; projected joint states keep this world scope.""" def __post_init__(self) -> None: + if self.planning_mask is not None: + mask = self.planning_mask + if ( + mask.dtype != torch.bool + or mask.shape != self.env_ids.shape + or mask.device != self.env_ids.device + ): + raise ValueError("planning_mask must match the selected context rows.") + object.__setattr__(self, "planning_mask", mask.clone()) if type(self.is_preview) is not bool: raise TypeError("is_preview must be bool.") if self.world_source_stamp is not None: diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index 7c7928fb93..1e83dbd324 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -3383,6 +3383,20 @@ def _decode_runtime_services( _CableCheckFactory, ) ) + if cable is not None and cable.operations: + from .simulation._cable_contact import ( + CableCompactPickFactory, + CableCompactPlaceFactory, + ) + + lowerers += tuple( + factory(cable) + for kind, factory in ( + ("pick_up", CableCompactPickFactory), + ("place", CableCompactPlaceFactory), + ) + if any(operation.kind == kind for operation in cable.operations) + ) lowerer_call_ids = tuple(factory.call_id for factory in lowerers) if len(set(lowerer_call_ids)) != len(lowerer_call_ids): raise ValueError( diff --git a/embodichain/lab/task_program/integrations/simulation/_cable_contact.py b/embodichain/lab/task_program/integrations/simulation/_cable_contact.py new file mode 100644 index 0000000000..e9bf505b61 --- /dev/null +++ b/embodichain/lab/task_program/integrations/simulation/_cable_contact.py @@ -0,0 +1,536 @@ +# ---------------------------------------------------------------------------- +# 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 geometry adapter for generic observed PickUp/Place contact paths.""" + +from __future__ import annotations + +from copy import copy +from collections.abc import Mapping +from dataclasses import dataclass +from typing import ClassVar, TYPE_CHECKING +from uuid import uuid4 +import torch + +from embodichain.lab.sim.atomic_skills.primitives.contact import ( + ContactPhase, + ContactPostcondition, + ContactPickUpGoal, + ContactPlaceGoal, + ResolvedContactPhase, + ContactContinuation, +) +from embodichain.lab.sim.atomic_skills.primitives.pick_up import PickUp +from embodichain.lab.sim.atomic_skills.primitives.place import Place +from embodichain.lab.sim.atomic_skills import ( + PlanningContext, + ResolvedSkillRequest, + SkillDescriptor, + SkillOptions, +) +from embodichain.lab.task_program.compiler.lowering import SemanticLowering +from embodichain.lab.task_program.language.semantic_contracts import ( + SemanticCallContract, +) +from embodichain.lab.task_program.semantics import ( + RegisteredSemanticCall, + BoundSemanticCall, +) +from .cable import ( + _CableLowerer, + _CableFactory, + _CableRuntime, + _CableTarget, + _ground_cable_move, + _ground_cable_joint, +) + +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + from embodichain.lab.task_program.runtime import SemanticCallExecutor + + +class _ObservedRobot: + def __init__( + self, robot: Robot, context: PlanningContext, target: torch.Tensor + ) -> None: + self._robot = robot + self._target = target + self._observed = torch.zeros_like(target) + self._observed.index_copy_(0, context.env_ids, context.robot.qpos) + + def get_qpos(self, *, target: bool = False) -> torch.Tensor: + return self._target if target else self._observed + + def __getattr__(self, name: str) -> object: + return getattr(self._robot, name) + + +@dataclass(frozen=True, slots=True) +class CableContactOperation: + """Declared operation whose numeric plans remain owned by PickUp or Place.""" + + name: str + kind: str + phases: tuple[ContactPhase, ...] + + +def decode_operations( + value: object, targets: Mapping[str, _CableTarget] +) -> tuple[CableContactOperation, ...]: + """Validate contact operation paths without acquiring simulation resources.""" + from ..configured import _mapping, _sequence, _identifier + + result = [] + if not isinstance(value, Mapping): + raise TypeError("Contact operations must be a mapping.") + for name, raw in _mapping( + value, + path="cable_material.operations", + required=frozenset(), + optional=frozenset(value), + ).items(): + c = _mapping( + raw, + path=f"cable_material.operations.{name}", + required=frozenset({"kind", "phases"}), + ) + if c["kind"] not in {"pick_up", "place"}: + raise ValueError("Contact operation kind must be pick_up or place.") + phases = [] + for raw_phase in _sequence(c["phases"], path="contact operation phases"): + p = _mapping( + raw_phase, + path="contact phase", + required=frozenset({"kind", "target"}), + optional=frozenset({"after"}), + ) + key = _identifier(p["target"], path="contact phase target") + kind = p["kind"] + if key not in targets: + raise ValueError(f"Unknown contact target {key!r}.") + t = targets[key] + if ( + kind not in {"pose", "grasp", "joint"} + or (kind == "grasp" and t.kind != "material") + or (kind == "joint" and t.kind not in {"open", "grip_adjust"}) + or ( + kind == "pose" + and t.kind in {"check", "open", "hold", "grip_adjust"} + ) + ): + raise ValueError( + "Contact phase kind does not match its declared target." + ) + tail = [] + for raw_post in _sequence(p.get("after", ()), path="contact phase after"): + v = _mapping( + raw_post, + path="contact postcondition", + required=frozenset({"kind", "target"}), + ) + ref = _identifier(v["target"], path="contact postcondition target") + if ( + ref not in targets + or targets[ref].kind != v["kind"] + or v["kind"] not in {"hold", "check"} + ): + raise ValueError( + "Contact postcondition must name a compatible hold/check target." + ) + tail.append(ContactPostcondition(v["kind"], ref)) + phases.append(ContactPhase(kind, key, tuple(tail))) + if c["kind"] == "place" and not any( + p.kind == "joint" and targets[p.target].kind == "open" for p in phases + ): + raise ValueError("Contact placement must declare an opening phase.") + goal_type = ContactPickUpGoal if c["kind"] == "pick_up" else ContactPlaceGoal + goal_type("static-preflight", tuple(phases)) + result.append( + CableContactOperation( + _identifier(name, path="contact operation name"), + c["kind"], + tuple(phases), + ) + ) + return tuple(result) + + +class CableContactSource: + """Resolve geometry from one bridge, without owning the operation sequence.""" + + def __init__(self, runtime: _CableRuntime) -> None: + from weakref import proxy + + self.runtime = proxy(runtime) + self.source_id = uuid4().hex + self.current: ContactContinuation | None = None + self.observed_checks: set[tuple[int, str]] = set() + self.rejected: torch.Tensor | None = None + self.reports: list[dict[str, object]] = [] + self.owner: str | None = None + + def _view( + self, context: PlanningContext, active: torch.Tensor + ) -> tuple[_CableRuntime, torch.Tensor, torch.Tensor]: + runtime = self.runtime + rows = active.nonzero(as_tuple=True)[0] + if not torch.equal(context.env_ids, runtime.env_ids): + raise ValueError("Contact context row identity changed.") + target = runtime.robot.get_qpos(target=True).clone() + view = copy(runtime) + view.env_ids = runtime.env_ids[rows] + view.rows = runtime.rows[rows] + view.origins = runtime.origins[rows] + view.max_slip = runtime.max_slip.index_select(0, rows) + view.robot = _ObservedRobot(runtime.robot, context, target) + for name in ["home", "grasp_slip", "last_targets", "port_yaw", "grasp_active"]: + setattr( + view, + name, + { + k: v.index_select(0, rows) + for k, v in getattr(runtime, name, {}).items() + }, + ) + view.grasps = { + k: (d, b.index_select(0, rows)) for k, (d, b) in runtime.grasps.items() + } + view.pick_initial = { + k: (d, b.index_select(0, rows)) + for k, (d, b) in runtime.pick_initial.items() + } + view._metric_cache_time = None + view._metric_cache = {} + return view, rows, target.index_select(0, context.env_ids) + + def resolve( + self, + phase: ContactPhase, + request: ResolvedSkillRequest, + context: PlanningContext, + active_mask: torch.Tensor, + ) -> ResolvedContactPhase: + """Resolve only selected rows, before any batch-wide timing reduction.""" + runtime = self.runtime + runtime._scope_guard() + B = context.batch_size + motion = request.binding.endpoint("primary", "motion").target + hand = request.binding.endpoint("primary", "grasp").target + valid = active_mask.clone() + if ( + isinstance(request.goal, ContactPlaceGoal) + and phase is request.goal.phases[0] + ): + grasped = runtime.grasp_active.get(motion.control_part) + valid &= torch.zeros_like(valid) if grasped is None else grasped + + held = ( + runtime.robot.get_qpos(target=True).index_select(0, context.env_ids).clone() + ) + if not bool(valid.any()): + shape = ( + (B, 2, 4, 4) + if phase.kind == "pose" + else (B, 4, 4) if phase.kind == "grasp" else (B, 2, len(hand.joint_ids)) + ) + return ResolvedContactPhase( + torch.zeros(shape, device=held.device), held, valid + ) + view, rows, held = self._view(context, valid) + data = { + "rows": rows, + "part": motion.control_part, + "operation_id": request.invocation_id or repr(request.goal), + } + target = runtime.cfg.target(phase.target) + if phase.kind == "pose": + result = _ground_cable_move( + view, target, motion.control_part, context.require_control_dt() + ) + local = result.goal.xpos + values = ( + torch.eye(4, device=held.device).expand(B, local.shape[1], 4, 4).clone() + ) + values[rows] = local + for physical in result.failed_preconditions: + valid[torch.nonzero(context.env_ids == physical).flatten()] = False + data["last_target"] = view.last_targets[motion.control_part] + elif phase.kind == "grasp": + # Resolve only target geometry; reference state is installed when execution accepts the plan. + local = view.pose(target, motion.control_part) + values = torch.eye(4, device=held.device).expand(B, 4, 4).clone() + values[rows] = local + close = request.binding.endpoint("primary", "grasp").joint_positions( + "grasp", num_envs=B, device=held.device, dtype=held.dtype + ) + if target.firm: + close = torch.zeros_like(close) + data["closed_qpos"] = close + data["pick_z"] = view.point(target.material_distance)[0][:, 2] + data["hand"] = hand.control_part + if motion.control_part in view.port_yaw: + data["port_yaw"] = view.port_yaw[motion.control_part] + else: + result = _ground_cable_joint(view, target, hand.control_part, view.env_ids) + local = result.goal.target + values = ( + held[:, None, list(hand.joint_ids)] + .expand(-1, local.shape[1], -1) + .clone() + ) + values[rows] = local + if target.kind == "open": + data["release_part"] = target.release_part + return ResolvedContactPhase( + values, + held, + valid, + "grasp" if phase.kind == "joint" else "motion", + tuple(data.items()), + ) + + def hold_steps(self, target: str) -> int: + """Read explicit settling duration from its configured target.""" + t = self.runtime.cfg.target(target) + if t.kind != "hold": + raise ValueError("Expected declared hold target.") + return t.samples + + def begin( + self, + cursor: ContactContinuation, + context: PlanningContext, + active_mask: torch.Tensor, + ) -> None: + """Install accepted references without capturing pre-close grasp evidence.""" + runtime = self.runtime + runtime._scope_guard() + if not bool(active_mask.any()): + return + data = dict(cursor.resolved.data) + owner = data["operation_id"] + if self.owner is not None and self.owner != owner: + raise ValueError("One contact source cannot execute concurrent operations.") + self.current = cursor + self.observed_checks = set() + self.rejected = torch.zeros_like(active_mask) + self.owner = owner + rows = active_mask.nonzero(as_tuple=True)[0] + original_rows = data["rows"] + select = torch.isin(original_rows, rows) + dest = original_rows[select] + part = data["part"] + target = runtime.cfg.target(cursor.phase.target) + if "last_target" in data: + value = runtime.last_targets.get(part, runtime.tool(part)).clone() + value[dest] = data["last_target"][select] + runtime.last_targets[part] = value + if cursor.phase.kind == "grasp": + runtime.pending_pick = (part, target) + runtime.pending_pick_rows = active_mask.clone() + runtime.hand_to_motion[data["hand"]] = part + previous = runtime.pick_initial.get( + part, (target.material_distance, torch.zeros_like(runtime.max_slip)) + )[1].clone() + previous[dest] = data["pick_z"][select] + runtime.pick_initial[part] = (target.material_distance, previous) + if "port_yaw" in data: + yaw = runtime.port_yaw.get( + part, torch.zeros_like(runtime.max_slip) + ).clone() + yaw[dest] = data["port_yaw"][select] + runtime.port_yaw[part] = yaw + if "release_part" in data: + key = data["release_part"] + if key in runtime.grasp_active: + runtime.grasp_active[key][active_mask] = False + if key in runtime.grasps and not bool( + runtime.grasp_active.get(key, torch.zeros_like(active_mask)).any() + ): + runtime.grasps.pop(key, None) + runtime.grasp_slip.pop(key, None) + runtime.pending_pick = None + self.reports.append( + { + "event": "phase_started", + "phase": cursor.phase_name, + "time": context.robot.timestamp, + "active": active_mask.cpu().tolist(), + } + ) + + def observe(self, action: object, runtime: SemanticCallExecutor) -> None: + """Evaluate checkpoints only after their physical commands are acknowledged.""" + cursor = self.current + if cursor is None: + return + reference = runtime.last_sent_plan_reference + if reference is None: + return + index = reference[2] + for after, target in cursor.checkpoints: + checkpoint = (after, target) + if index != after or checkpoint in self.observed_checks: + continue + self.observed_checks.add(checkpoint) + part = dict(cursor.resolved.data)["part"] + passed, details = self.runtime.condition( + self.runtime.cfg.target(target), part + ) + self.rejected |= ~passed + self.reports.append( + { + "event": "condition", + "phase": cursor.phase_name, + "condition": target, + "after_index": after, + "passed": passed.cpu().tolist(), + "details": details, + } + ) + + def finish( + self, + cursor: ContactContinuation, + context: PlanningContext, + active_mask: torch.Tensor, + ) -> tuple[torch.Tensor, tuple[str, ...]]: + """Accept rows with all required physical evidence for the completed phase.""" + if self.current is not cursor and ( + self.current is None or self.current.position != cursor.position + ): + raise RuntimeError("Contact phase observation is stale.") + expected = set(cursor.checkpoints) + if expected != self.observed_checks: + raise RuntimeError( + "Contact path commands were not acknowledged by their observation host." + ) + accepted = active_mask & ~self.rejected + metrics = self.runtime.metrics() + self.reports.append( + { + "event": "phase_completed", + "phase": cursor.phase_name, + "time": context.robot.timestamp, + "accepted": accepted.cpu().tolist(), + "endpoint": metrics["end_position"].cpu().tolist(), + "max_slip": metrics["max_grasp_slip"].cpu().tolist(), + } + ) + messages = tuple( + f"env {int(context.env_ids[i])}: contact condition failed in {cursor.phase_name}" + for i in (active_mask & ~accepted).nonzero().flatten().tolist() + ) + if cursor.position == cursor.limit: + self.owner = None + return accepted, messages + + +class _CableCompactPickLowerer(_CableLowerer): + call_id: ClassVar[str] = "simulation.cable_pickup" + target_descriptor: ClassVar[SkillDescriptor] = PickUp.descriptor() + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: SkillOptions, + ) -> SemanticLowering: + return _lower_compact(self, call, "pick_up") + + +class _CableCompactPlaceLowerer(_CableLowerer): + call_id: ClassVar[str] = "simulation.cable_place" + target_descriptor: ClassVar[SkillDescriptor] = Place.descriptor() + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: SkillOptions, + ) -> SemanticLowering: + return _lower_compact(self, call, "place") + + +def _lower_compact( + lowerer: _CableLowerer, call: RegisteredSemanticCall, kind: str +) -> SemanticLowering: + if set(call.arguments) != {"target"}: + raise ValueError("Contact operations require one declared target.") + operation = next( + ( + v + for v in lowerer.cfg.operations + if v.name == call.arguments["target"] and v.kind == kind + ), + None, + ) + if operation is None: + raise ValueError("Unknown contact operation.") + if lowerer.runtime is None or lowerer.runtime._contact_source is None: + raise RuntimeError("Contact operation was not bound to its observation source.") + cls = ContactPickUpGoal if kind == "pick_up" else ContactPlaceGoal + return SemanticLowering( + goal=cls(lowerer.runtime._contact_source.source_id, operation.phases) + ) + + +@dataclass(frozen=True, slots=True) +class CableCompactPickFactory(_CableFactory): + """Bind a contact pickup declaration to the ordinary PickUp primitive.""" + + call_id: ClassVar[str] = _CableCompactPickLowerer.call_id + target_descriptor: ClassVar[SkillDescriptor] = PickUp.descriptor() + lowerer_type: ClassVar[type[_CableLowerer]] = _CableCompactPickLowerer + + @property + def argument_contract(self) -> SemanticCallContract: + return SemanticCallContract( + required={ + "target": tuple( + o.name for o in self.cfg.operations if o.kind == "pick_up" + ) + }, + optional={}, + ) + + +@dataclass(frozen=True, slots=True) +class CableCompactPlaceFactory(_CableFactory): + """Bind a contact placement declaration to the ordinary Place primitive.""" + + call_id: ClassVar[str] = _CableCompactPlaceLowerer.call_id + target_descriptor: ClassVar[SkillDescriptor] = Place.descriptor() + lowerer_type: ClassVar[type[_CableLowerer]] = _CableCompactPlaceLowerer + + @property + def argument_contract(self) -> SemanticCallContract: + return SemanticCallContract( + required={ + "target": tuple( + o.name for o in self.cfg.operations if o.kind == "place" + ) + }, + optional={}, + ) + + +__all__: list[str] = [] diff --git a/embodichain/lab/task_program/integrations/simulation/cable.py b/embodichain/lab/task_program/integrations/simulation/cable.py index 62dadb4ca5..ad9fdbcb3c 100644 --- a/embodichain/lab/task_program/integrations/simulation/cable.py +++ b/embodichain/lab/task_program/integrations/simulation/cable.py @@ -25,7 +25,7 @@ from collections.abc import Mapping from dataclasses import dataclass import math -from typing import ClassVar, Any +from typing import ClassVar, Any, TYPE_CHECKING import torch @@ -66,6 +66,9 @@ ) from ..extensions import RegisteredSemanticLowererFactory +if TYPE_CHECKING: + from ._cable_contact import CableContactOperation, CableContactSource + __all__: list[str] = [] from embodichain.compute.geometry._cable_route import _CABLE_METRICS as _METRICS @@ -105,6 +108,7 @@ class _CableService: geometry: tuple[tuple[str, object], ...] motion_parts: tuple[str, ...] slip_limit: float = 0.012 + operations: tuple[CableContactOperation, ...] = () def target(self, target_id: str) -> _CableTarget: for target in self.targets: @@ -157,6 +161,9 @@ def __init__( self.pending_pick: tuple[str, _CableTarget] | None = None self.hand_to_motion: dict[str, str] = {} self.grasps: dict[str, tuple[float, torch.Tensor]] = {} + self.grasp_active: dict[str, torch.Tensor] = {} + self.pending_pick_rows: torch.Tensor | None = None + self._contact_source: CableContactSource | None = None 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] = {} @@ -204,6 +211,10 @@ def point_in_tool(self, part: str, distance: float) -> torch.Tensor: def observe(self) -> torch.Tensor: for part, (distance, baseline) in self.grasps.items(): slip = (self.point_in_tool(part, distance) - baseline).norm(dim=-1) + if part in self.grasp_active: + slip = torch.where( + self.grasp_active[part], slip, torch.zeros_like(slip) + ) self.grasp_slip[part] = torch.maximum(self.grasp_slip[part], slip) self.max_slip = torch.maximum(self.max_slip, slip) state = self.state() @@ -248,6 +259,45 @@ def seated(self) -> torch.Tensor: ] ).all(0) + def condition( + self, target: _CableTarget, part: str + ) -> tuple[torch.Tensor, dict[str, list[float]]]: + """Measure a declared condition from current physical observations.""" + passed = ~self.observe() + details = {} + if target.condition == "pose": + expected, actual = self.last_targets[part], self.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: + # Above-channel release alignment still requires ordinary TCP + # tracking. Only an inserted endpoint may retain contact bias. + passed |= ( + self.seated() & self.metrics()["strict_port"] & ~self.observe() + ) + details = { + "position_error_m": error.tolist(), + "orientation_error_rad": angle.tolist(), + } + elif target.condition == "lift": + distance, before = self.pick_initial[part] + lift = self.point(distance)[0][:, 2] - before + passed &= (lift > 0.008) & (part in self.grasps) + details = {"material_lift_m": lift.tolist()} + elif target.condition == "seated": + passed &= self.seated() + else: + raise ValueError("Unknown declared cable precondition.") + return passed, details + 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() @@ -265,11 +315,23 @@ def after_action(self, action: Any, runtime: Any) -> tuple[torch.Tensor, str]: 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) + rows = self.pending_pick_rows + if rows is None: + rows = torch.ones_like(self.max_slip, dtype=torch.bool) + baseline = self.grasps.get( + part, (distance, torch.zeros((len(rows), 3), device=rows.device)) + )[1].clone() + baseline[rows] = self.point_in_tool(part, distance)[rows] + self.grasps[part] = (distance, baseline) + active = self.grasp_active.get(part, torch.zeros_like(rows)).clone() + active[rows] = True + self.grasp_active[part] = active + slip = self.grasp_slip.get( + part, torch.zeros_like(self.max_slip) + ).clone() + slip[rows] = 0 + self.grasp_slip[part] = slip + self.pending_pick_rows = None self.pending_pick = None self.closed_reference_samples.append( { @@ -279,7 +341,13 @@ def after_action(self, action: Any, runtime: Any) -> tuple[torch.Tensor, str]: "sample_index": phase[2], } ) - return self.observe(), "cable_material_state_or_slip_guard_failed" + rejected = self.observe() + source = getattr(self, "_contact_source", None) + if source is not None: + source.observe(action, runtime) + if source.rejected is not None: + rejected |= source.rejected + return rejected, "cable_material_state_or_slip_guard_failed" def _material_pose(self, target: _CableTarget, part: str) -> torch.Tensor: assert target.material_distance is not None @@ -564,6 +632,77 @@ def lower( ) +def _ground_cable_move( + service: _CableRuntime, target: _CableTarget, part: str, control_dt: float +) -> SemanticLowering: + env_ids = service.env_ids + start, end = service.tool(part), service.pose(target, part) + pivot_geometry = ( + service._pivot_delta(target, part, start) + if target.kind in {"pitch_align", "lay_down"} + else None + ) + 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) + if target.kind == "lay_down": + assert pivot_geometry is not None + holding = pivot_geometry[2] & (pivot_geometry[1] == 0) + start = torch.where(holding[:, None, None], end, start) + path_start, path_end = start, end + if pivot_geometry is not None: + valid = pivot_geometry[2][:, None, None] + identity = torch.eye(4, dtype=start.dtype, device=start.device) + # Invalid rows are rejected independently, including nonfinite + # observations; they must not poison the shared sample count. + path_start = torch.where(valid, start, identity) + path_end = torch.where(valid, end, identity) + distance = (path_start[:, :3, 3] - path_end[:, :3, 3]).norm(dim=-1).max().item() + trace = ( + (path_start[:, :3, :3].transpose(1, 2) @ path_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) / control_dt) + 1, + ) + service.last_targets[part] = end.clone() + path = interpolate_xpos_batched(path_start, path_end, count)[:, 1:] + failures = {} + if pivot_geometry is not None: + point, _, valid = pivot_geometry + local_point = start[:, :3, :3].transpose(1, 2) @ ( + point - start[:, :3, 3] + ).unsqueeze(-1) + pivot_path = point[:, None] - ( + path[:, :, :3, :3] @ local_point[:, None] + ).squeeze(-1) + path[:, :, :3, 3] = torch.where( + valid[:, None, None], pivot_path, path[:, :, :3, 3] + ) + frozen = ~valid + if target.kind == "lay_down": + frozen |= holding + if target.stop_when_seated: + frozen |= service.seated() + path = torch.where(frozen[:, None, None, None], start[:, None], path) + reason = ( + "Cable lay-down requires a seated tip and a finite, forward, " + "monotonically descending pivot arc above its TCP floor." + if target.kind == "lay_down" + else "The distal cable span or tool points away from the channel." + ) + failures = { + int(env_ids[row]): reason for row in (~valid).nonzero().flatten().tolist() + } + return SemanticLowering( + goal=EndEffectorPoseGoal(path), failed_preconditions=failures + ) + + class _CableMoveLowerer(_CableLowerer): call_id: ClassVar[str] = "simulation.cable_move" target_descriptor: ClassVar[SkillDescriptor] = MoveEndEffector.descriptor() @@ -578,75 +717,45 @@ def lower( ) -> SemanticLowering: service, target = self._target(call) part = self._part(bound) - start, end = service.tool(part), service.pose(target, part) - pivot_geometry = ( - service._pivot_delta(target, part, start) - if target.kind in {"pitch_align", "lay_down"} - else None - ) - 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) - if target.kind == "lay_down": - assert pivot_geometry is not None - holding = pivot_geometry[2] & (pivot_geometry[1] == 0) - start = torch.where(holding[:, None, None], end, start) - path_start, path_end = start, end - if pivot_geometry is not None: - valid = pivot_geometry[2][:, None, None] - identity = torch.eye(4, dtype=start.dtype, device=start.device) - # Invalid rows are rejected independently, including nonfinite - # observations; they must not poison the shared sample count. - path_start = torch.where(valid, start, identity) - path_end = torch.where(valid, end, identity) - distance = (path_start[:, :3, 3] - path_end[:, :3, 3]).norm(dim=-1).max().item() - trace = ( - (path_start[:, :3, :3].transpose(1, 2) @ path_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() + return _ground_cable_move(service, target, part, context.require_control_dt()) + + +def _ground_cable_joint( + service: _CableRuntime, + target: _CableTarget, + part: str, + env_ids: torch.Tensor, +) -> SemanticLowering: + ids = service.robot.get_joint_ids(name=part) + start = service.robot.get_qpos(target=True).index_select(0, env_ids)[:, ids] + path = start[:, None, :].repeat(1, target.samples, 1) + if target.kind in {"open", "grip_adjust"}: + if target.kind == "open": + # Only release the grasp whose configured gripper is being opened. + motion_part = target.release_part + if ( + motion_part not in service.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 + elif service.hand_to_motion.get(part) not in service.grasps: + raise ValueError( + "Adjusting a grip requires an active grasp for the declared hand." ) - + 1, + # Adjustment retains the original grasp reference and slip history; + # only the finger command changes while physical contact settles. + path = torch.lerp( + start[:, None], + torch.full_like(start[:, None], target.finger_position), + torch.linspace(0, 1, target.samples, device=start.device)[None, :, None], ) - service.last_targets[part] = end.clone() - path = interpolate_xpos_batched(path_start, path_end, count)[:, 1:] - failures = {} - if pivot_geometry is not None: - point, _, valid = pivot_geometry - local_point = start[:, :3, :3].transpose(1, 2) @ ( - point - start[:, :3, 3] - ).unsqueeze(-1) - pivot_path = point[:, None] - ( - path[:, :, :3, :3] @ local_point[:, None] - ).squeeze(-1) - path[:, :, :3, 3] = torch.where( - valid[:, None, None], pivot_path, path[:, :, :3, 3] - ) - frozen = ~valid - if target.kind == "lay_down": - frozen |= holding - if target.stop_when_seated: - frozen |= service.seated() - path = torch.where(frozen[:, None, None, None], start[:, None], path) - reason = ( - "Cable lay-down requires a seated tip and a finite, forward, " - "monotonically descending pivot arc above its TCP floor." - if target.kind == "lay_down" - else "The distal cable span or tool points away from the channel." - ) - failures = { - int(context.env_ids[row]): reason - for row in (~valid).nonzero().flatten().tolist() - } - return SemanticLowering( - goal=EndEffectorPoseGoal(path), failed_preconditions=failures + elif target.kind != "hold": + raise ValueError( + "Cable joint targets must be open, grip_adjust or hold declarations." ) + return SemanticLowering(goal=JointPositionGoal(path)) class _CableJointLowerer(_CableLowerer): @@ -663,42 +772,7 @@ def lower( ) -> 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 in {"open", "grip_adjust"}: - 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 - elif service.hand_to_motion.get(part) not in service.grasps: - raise ValueError( - "Adjusting a grip requires an active grasp for the declared hand." - ) - # Adjustment retains the original grasp reference and slip history; - # only the finger command changes while physical contact settles. - 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, grip_adjust or hold declarations." - ) - return SemanticLowering(goal=JointPositionGoal(path)) + return _ground_cable_joint(service, target, part, context.env_ids) class _CableCheckLowerer(_CableLowerer): @@ -715,41 +789,7 @@ def lower( ) -> 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: - # 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(), - } - 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.") + passed, details = service.condition(target, part) failures = {} for row in (~passed).nonzero().flatten().tolist(): measured = {name: values[row] for name, values in details.items()} @@ -813,6 +853,12 @@ def create( else torch.tensor(selected, dtype=torch.long, device=engine.device) ) lowerer.runtime = _CableRuntime(self.cfg, simulation, robot, ids) + if self.cfg.operations: + from ._cable_contact import CableContactSource + + source = CableContactSource(lowerer.runtime) + lowerer.runtime._contact_source = source + engine.planning_services.register_contact_source(source.source_id, source) return lowerer @@ -853,7 +899,7 @@ def _decode_cable_service(value: object) -> _CableService: required=frozenset( {"entity_id", "simulation_uid", "motion_parts", "geometry", "targets"} ), - optional=frozenset({"slip_limit"}), + optional=frozenset({"slip_limit", "operations"}), ) targets = [] fields = { @@ -1063,12 +1109,18 @@ def freeze(value: object) -> object: ) if not parts or len(set(parts)) != len(parts): raise ValueError("Cable motion parts must be nonempty and unique.") + from ._cable_contact import decode_operations + + operations = decode_operations( + config.get("operations", {}), {t.target_id: t for t in targets} + ) 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), + operations=operations, geometry=tuple(frozen_geometry.items()), motion_parts=parts, slip_limit=_real( 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 14f2f9f805..de0858618f 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 @@ -338,3 +338,45 @@ Static composition/catalog preflight and intermediate physical qualification do not certify a complete cable task. Full acceptance remains strict after release, retreat and settling. Failed samples retain their measured stopping stage; changing orchestration does not by itself establish physical insertion success. + + +## Experimental compact program + +`task.yam.compact.yaml` selects `RouteTheCableCompact-YAM-v1`, an opt-in variant +with **8 Semantic Calls / 8 Atomic Skill invocations**: three right-arm +`PickUp`/`Place` pairs, then one left-arm `PickUp`/`Place` pair. The ordinary +`task.yam.yaml` remains the default 57-segment / 148-call deployment. + +```bash +python -m embodichain run-task \ + --gym_config extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.compact.yaml \ + --max_episodes 1 --seed 73 --headless +``` + +The compact integration declares 55 observed phases. `PickUp`/`Place` plan them +directly inside one execution session; they do not start nested Atomic Skills. +The 48 original `cable_check` calls become acknowledged physical checkpoints, +and 45 hold targets become command ranges. Their original order, thresholds, +settling samples and initial control frames are retained. Home motion is part +of the corresponding placement retreat. Every feedback-dependent target is +resolved again from fresh observations before its phase is planned. + +The observation source supplies geometry and evidence; the existing Atomic +engine owns IK, joint commands, timing and row-local execution. Failed rows are +removed before geometry/timing reductions and IK. Physical grasp references are +captured after the acknowledged close/settle command and released per row; no +rigid cable/tool relation is projected. Newton/DexUni, scene, robot, cable +coefficients, native contact handling and strict task acceptance are unchanged. + +Observed continuation is bounded and shares one invocation timeout. This +variant sets that timeout to 120 seconds to cover the full left placement; +it retains the 12,000-control-step episode ceiling. Offline `compile()`, external +nominal trajectories/expansion, replay recovery and mid-operation invocation +revisions are explicitly unsupported. Cancellation and failed-row holds use the +ordinary runner. `PLAN_CONTINUED` events expose every installed phase plan. + +The extended accepted goal types intentionally change registration/integration +fingerprints, including integrations which reference these descriptors. Saved +registrations must be regenerated; the existing handover program fingerprint is +unchanged. Fewer invocations are a structural simplification, not a claim of +proportionally faster physics or of a general deformable-object policy. 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 4711589548..60f7ff8161 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 @@ -11,3 +11,5 @@ default_deployment: yam deployments: yam: config: task.yam.yaml + yam_compact: + config: task.yam.compact.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.compact.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.compact.yaml new file mode 100644 index 0000000000..b789947033 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.compact.yaml @@ -0,0 +1,38 @@ +id: RouteTheCableCompact-YAM-v1 +environment: + component: env.yaml +embodiment: + component: ../../../../components/embodiments/yam_i2rt.yaml + overrides: + init_qpos: + - -0.414 + - 0.359 + - 0.945 + - 0.943 + - 1.021 + - 1.019 + - -1.243 + - -1.236 + - -0.171 + - 0.149 + - 1.192 + - -1.243 + - 0.0475 + - 0.0475 + - 0.0475 + - 0.0475 +headless: true +render_cfg: + renderer: no-render +enable_entity_gizmo: false +visualization: + allow_commands: false +collection: + target_episodes: 1 + max_attempts: 1 +task_program: + program: task_program/program.compact.yaml + integration: task_program/integration.compact.yaml + execution_policy: task_program/execution_policy.compact.yaml +evaluation: + component: task_program/evaluation.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.compact.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.compact.yaml new file mode 100644 index 0000000000..bc59492964 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.compact.yaml @@ -0,0 +1,25 @@ +policy_id: cable_compact_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 + skill_timeout: 120.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.compact.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.compact.yaml new file mode 100644 index 0000000000..8929e2fb0a --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.compact.yaml @@ -0,0 +1,1221 @@ +integration_id: route_the_cable_compact_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 + place: + primary: right_manipulator + skill_options: + simulation.cable_pick: + kind: pick_up + hand_interp_steps: 48 + grasp_settle_steps: 21 + lift_height: 0.0 + 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 + simulation.cable_pickup: + kind: pick_up + hand_interp_steps: 48 + grasp_settle_steps: 21 + lift_height: 0.0 + pre_grasp_distance: 0.0 + simulation.cable_place: + kind: place + release_settle_steps: 0 + 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.055 + - 0.795 + 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.5845 + - 0.072 + - 0.753 + - - 0.5875 + - 0.083 + - 0.758 + 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.7605 + firm: true + port_screen: false + right_lifted_0.162: + kind: check + condition: lift + raise_0: + kind: material_lift + position: + - null + - null + - 0.774 + material_distance: 0.162 + retract_distance: 0.0015 + 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.7605 + firm: false + port_screen: false + right_lifted_0.282: + kind: check + condition: lift + raise_1: + kind: material_lift + position: + - null + - null + - 0.78 + material_distance: 0.282 + retract_distance: 0.003 + 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.7605 + firm: true + port_screen: false + right_lifted_0.438: + kind: check + condition: lift + raise_2: + kind: material_lift + position: + - null + - null + - 0.8 + material_distance: 0.438 + retract_distance: 0.008 + raise_2_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + carry_2: + kind: command + position: + - 0.555 + - -0.01 + - 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.7605 + firm: true + port_screen: true + left_lifted_0.57: + kind: check + condition: lift + stage_free_end: + kind: port_stage + position: + - 0.586 + - 0.055 + - 0.795 + stage_free_end_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.795 + 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.05 + 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.05 + 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.7875 + 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.05 + 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.05 + 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.78 + 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.05 + 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.05 + 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 + pitch_free_end_0: + kind: pitch_align + material_distance: 0.57 + pitch: -0.2617993877991494 + maximum_rotation: 0.17453292519943295 + minimum_tool_pitch: -0.3490658503988659 + tolerance: 0.006 + stop_when_seated: true + pitch_free_end_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + pitch_free_end_1: + kind: pitch_align + material_distance: 0.57 + pitch: -0.2617993877991494 + maximum_rotation: 0.17453292519943295 + minimum_tool_pitch: -0.3490658503988659 + tolerance: 0.006 + stop_when_seated: true + pitch_free_end_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.7685 + 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 + soften_free_end: + kind: grip_adjust + samples: 60 + finger_position: 0.004 + settle_soft_grasp: + kind: hold + samples: 30 + soften_free_end_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.7535 + maximum_correction: 0.005 + maximum_y: 0.05 + minimum_z: 0.7655 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: false + 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.7535 + maximum_correction: 0.005 + maximum_y: 0.05 + minimum_z: 0.7655 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: false + 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 + lay_down_free_end_0: + kind: lay_down + maximum_rotation: 0.17453292519943295 + minimum_z: 0.7625 + speed: 0.015 + tolerance: 0.006 + lay_down_free_end_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: false + lay_down_free_end_1: + kind: lay_down + maximum_rotation: 0.17453292519943295 + minimum_z: 0.7625 + speed: 0.015 + tolerance: 0.006 + lay_down_free_end_1_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: false + release_alignment: + kind: check + condition: seated + release_free_end: + kind: open + samples: 60 + release_part: left_arm + finger_position: 0.008 + settle_released_end: + kind: hold + samples: 30 + retreat_free_end: + kind: command + position: + - null + - null + - 0.8 + speed: 0.025 + retreat_free_end_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + raise_clear_of_port: + kind: command + position: + - null + - 0.015 + - 0.82 + speed: 0.08 + raise_clear_of_port_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + 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 + 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.008 + 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 + operations: + right_pick_0: + kind: pick_up + phases: + - kind: pose + target: right_approach_0.162 + after: + - kind: hold + target: settle_motion + - kind: check + target: right_approach_0.162_tracking + - kind: grasp + target: right_grasp_0.162 + - kind: pose + target: raise_0 + after: + - kind: hold + target: settle_motion + - kind: check + target: raise_0_tracking + - kind: check + target: right_lifted_0.162 + right_place_0: + kind: place + phases: + - kind: pose + target: carry_0 + after: + - kind: hold + target: settle_motion + - kind: check + target: carry_0_tracking + - kind: pose + target: lower_0 + after: + - kind: hold + target: settle_motion + - kind: check + target: lower_0_tracking + - kind: joint + target: release_0 + - kind: pose + target: retreat_0 + after: + - kind: hold + target: settle_motion + - kind: check + target: retreat_0_tracking + - kind: joint + target: open_clear_of_spool_0 + right_pick_1: + kind: pick_up + phases: + - kind: pose + target: right_approach_0.282 + after: + - kind: hold + target: settle_motion + - kind: check + target: right_approach_0.282_tracking + - kind: grasp + target: right_grasp_0.282 + - kind: pose + target: raise_1 + after: + - kind: hold + target: settle_motion + - kind: check + target: raise_1_tracking + - kind: check + target: right_lifted_0.282 + right_place_1: + kind: place + phases: + - kind: pose + target: carry_1 + after: + - kind: hold + target: settle_motion + - kind: check + target: carry_1_tracking + - kind: pose + target: lower_1 + after: + - kind: hold + target: settle_motion + - kind: check + target: lower_1_tracking + - kind: joint + target: release_1 + - kind: pose + target: retreat_1 + after: + - kind: hold + target: settle_motion + - kind: check + target: retreat_1_tracking + - kind: joint + target: open_clear_of_spool_1 + right_pick_2: + kind: pick_up + phases: + - kind: pose + target: right_approach_0.438 + after: + - kind: hold + target: settle_motion + - kind: check + target: right_approach_0.438_tracking + - kind: grasp + target: right_grasp_0.438 + - kind: pose + target: raise_2 + after: + - kind: hold + target: settle_motion + - kind: check + target: raise_2_tracking + - kind: check + target: right_lifted_0.438 + right_place_2: + kind: place + phases: + - kind: pose + target: carry_2 + after: + - kind: hold + target: settle_motion + - kind: check + target: carry_2_tracking + - kind: pose + target: lower_2 + after: + - kind: hold + target: settle_motion + - kind: check + target: lower_2_tracking + - kind: joint + target: release_2 + - kind: pose + target: retreat_2 + after: + - kind: hold + target: settle_motion + - kind: check + target: retreat_2_tracking + - kind: joint + target: open_clear_of_spool_2 + - kind: pose + target: home_right + after: + - kind: hold + target: settle_motion + - kind: check + target: home_right_tracking + left_pick: + kind: pick_up + phases: + - kind: pose + target: left_approach_0.57 + after: + - kind: hold + target: settle_motion + - kind: check + target: left_approach_0.57_tracking + - kind: grasp + target: left_grasp_0.57 + - kind: pose + target: raise_free_end + after: + - kind: hold + target: settle_motion + - kind: check + target: raise_free_end_tracking + - kind: check + target: left_lifted_0.57 + left_place: + kind: place + phases: + - kind: pose + target: stage_free_end + after: + - kind: hold + target: settle_motion + - kind: check + target: stage_free_end_tracking + - kind: pose + target: align_over_port_0 + after: + - kind: hold + target: settle_motion + - kind: check + target: align_over_port_0_tracking + - kind: pose + target: align_over_port_1 + after: + - kind: hold + target: settle_motion + - kind: check + target: align_over_port_1_tracking + - kind: pose + target: align_over_port_2 + after: + - kind: hold + target: settle_motion + - kind: check + target: align_over_port_2_tracking + - kind: pose + target: align_over_port_3 + after: + - kind: hold + target: settle_motion + - kind: check + target: align_over_port_3_tracking + - kind: pose + target: lower_channel_0 + after: + - kind: hold + target: settle_motion + - kind: check + target: lower_channel_0_tracking + - kind: pose + target: align_tip_0_0 + after: + - kind: hold + target: settle_motion + - kind: check + target: align_tip_0_0_tracking + - kind: pose + target: align_tip_0_1 + after: + - kind: hold + target: settle_motion + - kind: check + target: align_tip_0_1_tracking + - kind: pose + target: lower_channel_1 + after: + - kind: hold + target: settle_motion + - kind: check + target: lower_channel_1_tracking + - kind: pose + target: align_tip_1_0 + after: + - kind: hold + target: settle_motion + - kind: check + target: align_tip_1_0_tracking + - kind: pose + target: align_tip_1_1 + after: + - kind: hold + target: settle_motion + - kind: check + target: align_tip_1_1_tracking + - kind: pose + target: lower_channel_2 + after: + - kind: hold + target: settle_motion + - kind: check + target: lower_channel_2_tracking + - kind: pose + target: align_tip_2_0 + after: + - kind: hold + target: settle_motion + - kind: check + target: align_tip_2_0_tracking + - kind: pose + target: align_tip_2_1 + after: + - kind: hold + target: settle_motion + - kind: check + target: align_tip_2_1_tracking + - kind: pose + target: pitch_free_end_0 + after: + - kind: hold + target: settle_motion + - kind: check + target: pitch_free_end_0_tracking + - kind: pose + target: pitch_free_end_1 + after: + - kind: hold + target: settle_motion + - kind: check + target: pitch_free_end_1_tracking + - kind: pose + target: lower_channel_3 + after: + - kind: hold + target: settle_motion + - kind: check + target: lower_channel_3_tracking + - kind: joint + target: soften_free_end + after: + - kind: hold + target: settle_soft_grasp + - kind: check + target: soften_free_end_tracking + - kind: pose + target: align_tip_3_0 + after: + - kind: hold + target: settle_motion + - kind: check + target: align_tip_3_0_tracking + - kind: pose + target: align_tip_3_1 + after: + - kind: hold + target: settle_motion + - kind: check + target: align_tip_3_1_tracking + - kind: pose + target: lay_down_free_end_0 + after: + - kind: hold + target: settle_motion + - kind: check + target: lay_down_free_end_0_tracking + - kind: pose + target: lay_down_free_end_1 + after: + - kind: hold + target: settle_motion + - kind: check + target: lay_down_free_end_1_tracking + - kind: check + target: release_alignment + - kind: joint + target: release_free_end + after: + - kind: hold + target: settle_released_end + - kind: pose + target: retreat_free_end + after: + - kind: hold + target: settle_motion + - kind: check + target: retreat_free_end_tracking + - kind: pose + target: raise_clear_of_port + after: + - kind: hold + target: settle_motion + - kind: check + target: raise_clear_of_port_tracking + - kind: joint + target: open_clear_of_port + - kind: pose + target: home_left + after: + - kind: hold + target: settle_motion + - kind: check + target: home_left_tracking + - kind: hold + target: settle_and_validate diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.compact.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.compact.yaml new file mode 100644 index 0000000000..85fa9a521d --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.compact.yaml @@ -0,0 +1,87 @@ +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_pick_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pickup + arguments: + target: right_pick_0 + resources: + primary: right_manipulator +- segment: right_place_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_place + arguments: + target: right_place_0 + resources: + primary: right_manipulator +- segment: right_pick_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pickup + arguments: + target: right_pick_1 + resources: + primary: right_manipulator +- segment: right_place_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_place + arguments: + target: right_place_1 + resources: + primary: right_manipulator +- segment: right_pick_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pickup + arguments: + target: right_pick_2 + resources: + primary: right_manipulator +- segment: right_place_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_place + arguments: + target: right_place_2 + resources: + primary: right_manipulator +- segment: left_pick + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pickup + arguments: + target: left_pick + resources: + primary: left_manipulator +- segment: left_place + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_place + arguments: + target: left_place + resources: + primary: left_manipulator + validators: + - kind: condition + ref: route_success diff --git a/tests/gym/envs/task_program/test_cable_material.py b/tests/gym/envs/task_program/test_cable_material.py index 25a58a6990..3255d0875e 100644 --- a/tests/gym/envs/task_program/test_cable_material.py +++ b/tests/gym/envs/task_program/test_cable_material.py @@ -58,8 +58,8 @@ def _declaration(): ]["cable_material"] -def _deployment(): - path = _TASK / "task.yam.yaml" +def _deployment(*, compact=False): + path = _TASK / ("task.yam.compact.yaml" if compact else "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( @@ -89,6 +89,69 @@ def test_cable_deployment_preflights_all_calls_and_measured_final_acceptance(): assert factory.argument_contract.required["target"] +def test_compact_cable_preflight_has_eight_calls_and_retains_physical_checks(): + deployment = _deployment(compact=True) + registration = deployment.integration.registration + compiled = registration.catalog.preflight(deployment.load_program()) + assert compiled.segment_count == 8 + cfg = next( + factory.cfg + for factory in registration.registered_semantic_lowerer_factories + if factory.call_id == "simulation.cable_pickup" + ) + original = _decode_cable_service(_declaration()) + assert (cfg.targets, cfg.geometry, cfg.motion_parts, cfg.slip_limit) == ( + original.targets, + original.geometry, + original.motion_parts, + original.slip_limit, + ) + operations = cfg.operations + assert len(operations) == 8 + phases = [phase for operation in operations for phase in operation.phases] + assert len(phases) == 55 + assert sum(phase.kind == "grasp" for phase in phases) == 4 + assert sum(post.kind == "check" for phase in phases for post in phase.after) == 48 + assert sum(post.kind == "hold" for phase in phases for post in phase.after) == 45 + assert registration.evaluation_final_refs == ("route_success",) + + +@pytest.mark.parametrize( + "operation_name,call_id", + [ + ("right_pick_0", "simulation.cable_pickup"), + ("right_place_0", "simulation.cable_place"), + ], +) +def test_contact_catalog_accepts_a_single_declared_operation_kind( + operation_name, call_id +): + path = _TASK / "task.yam.compact.yaml" + physical = resolve_gym_components( + yaml.safe_load(path.read_text()), base_dir=path.parent + ) + declaration = yaml.safe_load( + (_TASK / "task_program/integration.compact.yaml").read_text() + ) + operations = declaration["runtime_services"]["cable_material"]["operations"] + declaration["runtime_services"]["cable_material"]["operations"] = { + operation_name: operations[operation_name] + } + selection = deepcopy(physical.config["task_program"]) + selection["integration"] = {"inline": declaration} + deployment = load_configured_task_program_deployment( + task_program=selection, + skill_profile=physical.embodiment_skill_profile, + evaluation=physical.evaluation, + base_dir=path.parent, + ) + compact_ids = { + factory.call_id + for factory in deployment.integration.registration.registered_semantic_lowerer_factories + } & {"simulation.cable_pickup", "simulation.cable_place"} + assert compact_ids == {call_id} + + @pytest.mark.parametrize( "change", ["unknown", "negative_coordinate", "bad_geometry", "executable"] ) @@ -311,6 +374,142 @@ 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_partial_close_captures_only_acknowledged_rows_and_keeps_slip_history(): + service, _, _ = _runtime() + part = "right_arm" + target = service.cfg.target("right_grasp_0.162") + original = torch.full((2, 3), 0.25) + service.grasps[part] = (target.material_distance, original.clone()) + service.grasp_active[part] = torch.tensor([False, False]) + service.grasp_slip[part] = torch.tensor([0.004, 0.003]) + service.max_slip[:] = torch.tensor([0.013, 0.003]) + service.pending_pick = (part, target) + service.pending_pick_rows = torch.tensor([False, True]) + service.after_action( + SimpleNamespace(metadata={"bridge_action_kind": "runtime_command"}), + SimpleNamespace(_last_sent_command_phase=lambda: ("close", True, 7)), + ) + torch.testing.assert_close(service.grasps[part][1][0], original[0]) + torch.testing.assert_close( + service.grasps[part][1][1], service.point_in_tool(part, 0.162)[1] + ) + assert service.grasp_active[part].tolist() == [False, True] + torch.testing.assert_close(service.max_slip, torch.tensor([0.013, 0.003])) + + +def test_contact_source_partial_view_keeps_metric_rows_in_physical_id_order(): + from embodichain.lab.task_program.integrations.simulation._cable_contact import ( + CableContactSource, + ) + + original, _, measured = _runtime() + service = _CableRuntime( + original.cfg, original.simulation, original.robot, torch.tensor([3, 1, 0]) + ) + part = "left_arm" + service.grasps[part] = (0.162, service.point_in_tool(part, 0.162).clone()) + service.grasp_slip[part] = torch.tensor([0.013, 0.002, 0.003]) + service.grasp_active[part] = torch.tensor([False, True, True]) + service.max_slip[:] = service.grasp_slip[part] + source = CableContactSource(service) + view, rows, _ = source._view( + SimpleNamespace( + env_ids=service.env_ids, + robot=SimpleNamespace(qpos=measured[service.env_ids]), + ), + torch.tensor([False, True, True]), + ) + assert rows.tolist() == [1, 2] + assert view.env_ids.tolist() == [1, 0] + metrics = view.metrics() + torch.testing.assert_close(metrics["max_grasp_slip"], torch.tensor([0.002, 0.003])) + assert metrics["end_position"].shape == (2, 3) + torch.testing.assert_close(service.max_slip, torch.tensor([0.013, 0.002, 0.003])) + + +def test_repeated_contact_check_is_measured_again_after_settling(): + from embodichain.lab.task_program.integrations.simulation._cable_contact import ( + CableContactSource, + ) + from embodichain.lab.sim.atomic_skills.primitives.contact import ( + ContactPhase, + ContactContinuation, + ResolvedContactPhase, + ) + + service, _, _ = _runtime() + source = CableContactSource(service) + observations = [] + + def condition(target, part): + observations.append(target.target_id) + return torch.tensor([len(observations) == 1, True]), {} + + service.condition = condition + source.current = ContactContinuation( + 1, + 1, + "waypoint", + ContactPhase("pose", "waypoint"), + ResolvedContactPhase( + torch.zeros(2, 2, 4, 4), + torch.zeros(2, 3), + torch.ones(2, dtype=torch.bool), + data=(("part", "left_arm"),), + ), + ((1, "raise_0_tracking"), (5, "raise_0_tracking")), + ) + source.rejected = torch.zeros(2, dtype=torch.bool) + runtime = SimpleNamespace(last_sent_plan_reference=(0, 0, 1)) + source.observe(None, runtime) + runtime.last_sent_plan_reference = (0, 0, 5) + source.observe(None, runtime) + accepted, _ = source.finish( + source.current, + SimpleNamespace(env_ids=service.env_ids, robot=SimpleNamespace(timestamp=1.0)), + torch.ones(2, dtype=torch.bool), + ) + assert observations == ["raise_0_tracking", "raise_0_tracking"] + assert accepted.tolist() == [False, True] + + +def test_rejected_concurrent_contact_operation_preserves_the_live_observer(): + from embodichain.lab.task_program.integrations.simulation._cable_contact import ( + CableContactSource, + ) + from embodichain.lab.sim.atomic_skills.primitives.contact import ( + ContactPhase, + ContactContinuation, + ResolvedContactPhase, + ) + + service, _, _ = _runtime() + source = CableContactSource(service) + phase = ContactPhase("pose", "right_approach_0.162") + resolved = ResolvedContactPhase( + torch.zeros(2, 2, 4, 4), + torch.zeros(2, 3), + torch.ones(2, dtype=torch.bool), + data=( + ("operation_id", "first"), + ("rows", torch.tensor([0, 1])), + ("part", "right_arm"), + ), + ) + first = ContactContinuation(1, 2, phase.target, phase, resolved) + context = SimpleNamespace(robot=SimpleNamespace(timestamp=0)) + source.begin(first, context, torch.ones(2, dtype=torch.bool)) + source.observed_checks.add((3, "tracking")) + other = replace( + first, resolved=replace(resolved, data=(("operation_id", "second"),)) + ) + with pytest.raises(ValueError, match="concurrent operations"): + source.begin(other, context, torch.ones(2, dtype=torch.bool)) + assert source.current is first + assert source.owner == "first" + assert source.observed_checks == {(3, "tracking")} + + def test_port_stage_replaces_measured_tilt_with_selected_downward_orientation(): service, _, _ = _runtime() original_fk = service.robot.compute_fk diff --git a/tests/gym/envs/task_program/test_configured_integration.py b/tests/gym/envs/task_program/test_configured_integration.py index e68f1efa95..0836d645af 100644 --- a/tests/gym/envs/task_program/test_configured_integration.py +++ b/tests/gym/envs/task_program/test_configured_integration.py @@ -4686,15 +4686,15 @@ def test_unrelated_task_program_identities_keep_pre_cable_program_values(): integration = deployment.integration registration = integration.registration assert "evaluation" not in registration.fingerprint_payload() - # Independently recomputed at 694ddda7d before the cable Task Program - # migration. Contact-capable PickUp had already changed the old main golden. + # Extending the accepted PickUp/Place goal descriptors intentionally changes + # registration identity. The handover program and inactive options stay fixed. assert ( registration.fingerprint - == "0b5253d265d1d88dc165138e915f56f3596f52525c9ae620943f68a3ecc1d79f" + == "d9cd591643cf8c29cbad04e532c6743208b78315a484796e90595d03ad98e6a8" ) assert ( integration.integration_fingerprint - == "400ee8ebb00e416a8c9b783f5a681b7f9566da92cef4cbc55ec966cd5c20b2c4" + == "785a9307300d193082e945cee747d37a688b0a7ba3a09d6569b418553b44379e" ) compiled = registration.catalog.preflight(deployment.load_program()) assert ( diff --git a/tests/sim/atomic_skills/test_actions.py b/tests/sim/atomic_skills/test_actions.py index 929e442a9e..4388164706 100644 --- a/tests/sim/atomic_skills/test_actions.py +++ b/tests/sim/atomic_skills/test_actions.py @@ -946,12 +946,16 @@ 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 - from embodichain.lab.sim.atomic_skills import ContactGraspGoal + from embodichain.lab.sim.atomic_skills import ( + ContactGraspGoal, + ContactPickUpGoal, + ContactPlaceGoal, + ) - assert PickUp.GoalType == (GraspGoal, ContactGraspGoal) + assert PickUp.GoalType == (GraspGoal, ContactGraspGoal, ContactPickUpGoal) assert AxisAlign.GoalType is AxisAlignGoal assert MoveHeldObject.GoalType is HeldObjectPoseGoal - assert Place.GoalType == (PlaceGoal, AssembleGoal) + assert Place.GoalType == (PlaceGoal, AssembleGoal, ContactPlaceGoal) assert Pour.GoalType is PourGoal assert Press.GoalType is PressGoal assert PushObject.GoalType is PushObjectGoal diff --git a/tests/sim/atomic_skills/test_contact_paths.py b/tests/sim/atomic_skills/test_contact_paths.py new file mode 100644 index 0000000000..202ffef169 --- /dev/null +++ b/tests/sim/atomic_skills/test_contact_paths.py @@ -0,0 +1,381 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Behavioral coverage for one-invocation observed contact paths.""" + +from __future__ import annotations +from dataclasses import replace +import gc +import pytest +import torch +from embodichain.lab.sim.atomic_skills import ( + Place, + PickUp, + PickUpOptions, + MotionPolicy, + RecoveryPolicy, +) +from embodichain.lab.sim.atomic_skills.primitives.contact import ( + ContactPlaceGoal, + ContactPickUpGoal, + ContactPhase, + ResolvedContactPhase, +) +from embodichain.lab.sim.atomic_skills.state import RobotObservation +from .test_actions import ( + _bind_action, + _motion_generator, + _ACTION_ENGINES, + _context, + _invocation, + _held, + _torch_interpolation, +) + + +class _Source: + def __init__(self, reject_first=False): + self.reject_first = reject_first + self.started = [] + self.masks = [] + + def resolve(self, phase, request, context, active_mask): + self.masks.append(active_mask.clone()) + ids = list(request.binding.endpoint("primary", "grasp").target.joint_ids) + values = context.robot.qpos[:, None, ids].expand(-1, 3, -1).clone() + values[:, 1:] += 0.001 + return ResolvedContactPhase(values, context.robot.qpos, active_mask, "grasp") + + def begin(self, cursor, context, active_mask): + self.started.append(cursor.phase_name) + + def hold_steps(self, target): + return 2 + + def finish(self, cursor, context, active_mask): + mask = active_mask.clone() + if self.reject_first and cursor.position == 1: + mask[0] = False + return mask, ("measured failure",) if self.reject_first else () + + +class _GeometrySource(_Source): + def __init__(self, closed=0.002): + super().__init__() + self.closed = closed + + def resolve(self, phase, request, context, active_mask): + B = context.batch_size + if phase.kind == "pose": + values = torch.eye(4).expand(B, 2, 4, 4).clone() + data = () + else: + values = torch.eye(4).expand(B, 4, 4).clone() + ids = request.binding.endpoint("primary", "grasp").target.joint_ids + data = (("closed_qpos", torch.full((B, len(ids)), self.closed)),) + return ResolvedContactPhase(values, context.robot.qpos, active_mask, data=data) + + +def _setup(source): + action = _bind_action(_motion_generator(), Place()) + engine = _ACTION_ENGINES[id(action)] + engine.planning_services.register_contact_source("fixture", source) + goal = ContactPlaceGoal( + "fixture", (ContactPhase("joint", "first"), ContactPhase("joint", "second")) + ) + invocation = replace( + _invocation(action, goal), + motion_policy=MotionPolicy(strategy="ik_interp", dynamic_collision_mode="off"), + recovery_policy=RecoveryPolicy( + max_replans=0, max_skill_retries=0, skill_timeout=120 + ), + ) + return engine, invocation, _context() + + +def _execute(engine, invocation, context): + session = engine.start((invocation,), context) + events = [] + frames = [] + count = 0 + for _ in range(50): + tick = session.tick(context) + events.extend(tick.events) + if tick.command is not None: + frames.append(tick.command) + qpos = context.robot.qpos.clone() + for command in tick.command.commands: + qpos[:, list(command.target.joint_ids)] = command.payload.positions + count += 1 + context = replace( + context, + robot=RobotObservation( + count * context.control_dt, qpos, torch.zeros_like(qpos) + ), + ) + if tick.status.value != "running": + break + else: + pytest.fail("Contact path failed to terminate within its finite bound") + return session, events, frames + + +def test_contact_path_is_one_invocation_with_multiple_observed_plans(): + source = _Source() + engine, invocation, context = _setup(source) + session, events, frames = _execute(engine, invocation, context) + assert session.status.value == "completed" + assert len(frames) == 6 + assert len(session.plan_attempts) == 2 + assert source.started == ["first", "second"] + assert sum(e.kind.value == "skill_completed" for e in events) == 1 + assert sum(e.kind.value == "plan_continued" for e in events) == 1 + assert {a.invocation_index for a in session.plan_attempts} == {0} + assert session.task_state.get_held_object("arm") is None + + +def test_measured_phase_failure_does_not_reactivate_or_block_peer(): + source = _Source(reject_first=True) + engine, invocation, context = _setup(source) + session, _, frames = _execute(engine, invocation, context) + assert session.status.value == "completed" + assert session.eligible_mask.tolist() == [False, True] + assert source.masks[-1].tolist() == [False, True] + assert all(frame.active_mask.tolist() == [False, True] for frame in frames[3:]) + + +def test_all_rows_rejected_by_a_contact_check_fail_with_a_normal_terminal_result(): + class RejectAll(_Source): + def finish(self, cursor, context, active_mask): + return torch.zeros_like(active_mask), ("All measured checks failed",) + + source = RejectAll() + engine, invocation, context = _setup(source) + session, events, frames = _execute(engine, invocation, context) + assert session.status.value == "failed" + assert len(frames) == 3 + assert source.started == ["first"] + assert any(event.kind.value == "session_failed" for event in events) + + +def test_contact_revision_rejects_before_regrounding_inactive_rows(): + source = _Source() + engine, invocation, context = _setup(source) + session = engine.start( + (invocation,), context, eligible_mask=torch.tensor([False, True]) + ) + with pytest.raises(ValueError, match="does not support invocation revisions"): + session.revise_current(replace(invocation, revision=1), context=context) + assert len(source.masks) == 1 + assert source.masks[0].tolist() == [False, True] + assert len(session.plan_attempts) == 1 + + +def test_preview_does_not_commit_reference_bookkeeping_and_snapshot_is_owned(): + source = _Source() + engine, invocation, context = _setup(source) + plan = engine.plan(invocation, context) + assert source.started == [] + copy = plan.snapshot() + copy.continuation.resolved.held_qpos.fill_(99) + assert not bool((plan.continuation.resolved.held_qpos == 99).any()) + + +def test_contact_sources_are_engine_local_and_do_not_leak_episode_lifetimes(): + source = _Source() + engine, invocation, context = _setup(source) + del source + gc.collect() + with pytest.raises(ValueError, match="expired or belongs"): + engine.plan(invocation, context) + + +def test_offline_projection_cannot_fake_future_contact_observations(): + source = _Source() + engine, invocation, context = _setup(source) + with pytest.raises(ValueError, match="execution session"): + engine.compile((invocation,), context) + + +def test_external_transform_cannot_drop_a_contact_continuation(): + source = _Source() + engine, invocation, context = _setup(source) + called = [] + + def transform(request, current, plan): + called.append(True) + return engine.rebuild_plan_from_trajectory( + request, current, plan, plan.joint_trajectory + ) + + with pytest.raises(ValueError, match="does not support plan transforms"): + engine.compile((invocation,), context, plan_transform=transform) + assert called == [] + assert source.started == [] + + +@pytest.mark.parametrize("skill", [PickUp, Place]) +def test_contact_nominal_rejection_precedes_external_source_acquisition(skill): + class Nominal: + def instantiate(self, *args, **kwargs): + pytest.fail( + "Unsupported contact replay must reject before acquiring external motion" + ) + + source = _GeometrySource() + action = _bind_action(_motion_generator(), skill()) + engine = _ACTION_ENGINES[id(action)] + engine.planning_services.register_contact_source("geometry", source) + goal = ( + ContactPickUpGoal("geometry", (ContactPhase("grasp", "close"),)) + if skill is PickUp + else ContactPlaceGoal("geometry", (ContactPhase("joint", "open"),)) + ) + with pytest.raises(ValueError, match="cannot replay nominal motion"): + engine.plan(_invocation(action, goal), _context(), nominal_source=Nominal()) + assert source.started == [] + + +def test_contact_path_rejects_replay_recovery_of_partially_executed_operation(): + source = _Source() + engine, invocation, context = _setup(source) + invocation = replace(invocation, recovery_policy=RecoveryPolicy(max_replans=1)) + with pytest.raises(ValueError, match="not replay"): + engine.plan(invocation, context) + + +def test_contact_path_timeout_covers_the_whole_invocation(): + source = _Source() + engine, invocation, context = _setup(source) + invocation = replace( + invocation, + recovery_policy=RecoveryPolicy( + max_replans=0, max_skill_retries=0, skill_timeout=4 * context.control_dt + ), + ) + session, events, frames = _execute(engine, invocation, context) + assert session.status.value == "failed" + assert len(session.plan_attempts) == 2 + assert len(frames) < 6 + assert any(event.kind.value == "skill_timeout" for event in events) + + +def test_contact_path_cannot_replace_an_existing_rigid_held_relation(): + source = _Source() + engine, invocation, context = _setup(source) + held = _held(env_mask=torch.tensor([True, False])) + context = replace(context, task=replace(context.task, held_objects={"arm": held})) + plan = engine.plan(invocation, context) + assert plan.plan_success.tolist() == [False, True] + assert source.masks[-1].tolist() == [False, True] + assert source.started == [] + + +def test_contact_pickup_plans_pose_close_and_lift_without_rigid_projection(): + source = _GeometrySource() + action = _bind_action(_motion_generator(), PickUp()) + engine = _ACTION_ENGINES[id(action)] + engine.planning_services.register_contact_source("geometry", source) + invocation = replace( + _invocation( + action, ContactPickUpGoal("geometry", (ContactPhase("grasp", "pinch"),)) + ), + motion_policy=MotionPolicy(strategy="ik_interp", dynamic_collision_mode="off"), + recovery_policy=RecoveryPolicy(max_replans=0, max_skill_retries=0), + skill_options=PickUpOptions( + pre_grasp_distance=0, lift_height=0, grasp_settle_steps=3 + ), + ) + context = _context() + plan = engine.plan(invocation, context) + assert plan.plan_success.tolist() == [True, True] + assert [segment.name for segment in plan.segments] == ["approach", "close", "lift"] + close = next(segment for segment in plan.segments if segment.name == "close") + ids = invocation.binding.endpoint("primary", "grasp").target.joint_ids + torch.testing.assert_close( + plan.joint_trajectory.positions[:, close.stop - 1, list(ids)], + torch.full((2, len(ids)), 0.002), + ) + assert source.started == [] + assert plan.expected_effects.is_empty + assert context.task.get_held_object("arm") is None + invocation = replace( + invocation, + goal=ContactPickUpGoal( + "geometry", + (ContactPhase("pose", "approach"), ContactPhase("grasp", "pinch")), + ), + ) + pose_plan = engine.plan(invocation, context) + assert pose_plan.plan_success.tolist() == [True, True] + assert pose_plan.continuation.phase_name == "approach" + # The observed start and two authored waypoints all occupy the control grid. + assert pose_plan.commands.frame_count == 3 + + +def test_velocity_qualification_also_rejects_an_unsafe_continued_grasp(monkeypatch): + source = _GeometrySource(closed=1.0) + action = _bind_action(_motion_generator(), PickUp()) + engine = _ACTION_ENGINES[id(action)] + engine.planning_services.register_contact_source("geometry", source) + context = _context() + monkeypatch.setattr( + "embodichain.lab.sim.motion.limits.authored_joint_velocity_limits", + lambda *_: torch.ones_like(context.robot.qpos), + ) + invocation = replace( + _invocation( + action, + ContactPickUpGoal( + "geometry", + ( + ContactPhase("pose", "approach"), + ContactPhase("grasp", "close"), + ), + ), + ), + motion_policy=MotionPolicy( + strategy="ik_interp", + dynamic_collision_mode="off", + sample_count=3, + require_final_velocity=True, + final_velocity_rejection="mask_only", + ), + recovery_policy=RecoveryPolicy(max_replans=0, max_skill_retries=0), + skill_options=PickUpOptions( + pre_grasp_distance=0, lift_height=0, hand_interp_steps=2 + ), + ) + session, events, frames = _execute(engine, invocation, context) + assert session.status.value == "failed" + assert len(session.plan_attempts) == 2 + assert session.plan_attempts[-1].plan.plan_success.tolist() == [False, False] + assert any(event.kind.value == "session_failed" for event in events) + for frame in frames: + for command in frame.commands: + assert not bool((command.payload.positions == 1).any()) + + +@pytest.mark.parametrize( + "goal,phases", + [ + (ContactPickUpGoal, (ContactPhase("joint", "x"),)), + (ContactPlaceGoal, (ContactPhase("grasp", "x"),)), + ], +) +def test_contact_goal_keeps_pickup_and_placement_intents_distinct(goal, phases): + with pytest.raises(ValueError): + goal("fixture", phases) From d9eff2f47c785845f19fa001659c3a7fa94520d8 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Thu, 19 Nov 2026 22:22:34 +0800 Subject: [PATCH 09/11] feat: combine compact cable routing with contact-aware fast pacing --- .../integrations/simulation/cable.py | 9 +- .../cable/route_the_cable/README.md | 49 +++--- .../execution_policy.compact.yaml | 7 +- .../task_program/integration.compact.yaml | 144 ++++++++++++------ .../envs/task_program/test_cable_material.py | 64 +++++++- 5 files changed, 203 insertions(+), 70 deletions(-) diff --git a/embodichain/lab/task_program/integrations/simulation/cable.py b/embodichain/lab/task_program/integrations/simulation/cable.py index ad9fdbcb3c..0209bd944f 100644 --- a/embodichain/lab/task_program/integrations/simulation/cable.py +++ b/embodichain/lab/task_program/integrations/simulation/cable.py @@ -82,6 +82,7 @@ class _CableTarget: retract_distance: float = 0.0 position: tuple[float | None, float | None, float | None] = (None, None, None) speed: float = 0.05 + angular_speed: float = 0.5 tolerance: float = 0.02 orientation_tolerance: float = 0.25 port_screen: bool = False @@ -667,7 +668,10 @@ def _ground_cable_move( angle = torch.acos(((trace - 1) / 2).clamp(-1, 1)).max().item() count = max( 3, - math.ceil(max(distance / target.speed, angle / 0.5) / control_dt) + 1, + math.ceil( + max(distance / target.speed, angle / target.angular_speed) / control_dt + ) + + 1, ) service.last_targets[part] = end.clone() path = interpolate_xpos_batched(path_start, path_end, count)[:, 1:] @@ -918,7 +922,7 @@ def _decode_cable_service(value: object) -> _CableService: item, path=path, required=frozenset({"kind"}), optional=frozenset(fields) ) kind = _identifier(row["kind"], path=path) - motion_fields = {"speed", "tolerance", "orientation_tolerance"} + motion_fields = {"speed", "angular_speed", "tolerance", "orientation_tolerance"} allowed = { "material": motion_fields | {"material_distance", "position", "port_screen", "firm"}, @@ -1000,6 +1004,7 @@ def _decode_cable_service(value: object) -> _CableService: target = _CableTarget(**kwargs) if ( target.speed <= 0 + or target.angular_speed <= 0 or target.tolerance <= 0 or target.orientation_tolerance <= 0 or target.maximum_correction <= 0 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 de0858618f..da4e21691c 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 @@ -340,7 +340,7 @@ retreat and settling. Failed samples retain their measured stopping stage; changing orchestration does not by itself establish physical insertion success. -## Experimental compact program +## Compact program with contact-aware pacing `task.yam.compact.yaml` selects `RouteTheCableCompact-YAM-v1`, an opt-in variant with **8 Semantic Calls / 8 Atomic Skill invocations**: three right-arm @@ -356,27 +356,36 @@ python -m embodichain run-task \ The compact integration declares 55 observed phases. `PickUp`/`Place` plan them directly inside one execution session; they do not start nested Atomic Skills. The 48 original `cable_check` calls become acknowledged physical checkpoints, -and 45 hold targets become command ranges. Their original order, thresholds, -settling samples and initial control frames are retained. Home motion is part -of the corresponding placement retreat. Every feedback-dependent target is -resolved again from fresh observations before its phase is planned. +and 45 hold targets become command ranges. Home motion is part of the +corresponding placement retreat. Every feedback-dependent target is resolved +again from fresh observations before its phase is planned. + +The fast profile combines compact orchestration with faster motion pacing. +Translational target speeds are scaled by 2.2 and ordinary angular pacing uses +1.1 rad/s. Each motion target may explicitly declare positive `angular_speed` +in rad/s; omitted values retain the legacy 0.5 rad/s. The two wrist lay-down +phases keep 0.5 rad/s and their original 15-control settling holds through the +separate `settle_contact` target. The profile also preserves all four original +21-control post-close dwells, the 30-control soft-grasp and post-release holds, +and the final 120-control acceptance hold. Ordinary process holds and gripper +transitions are shortened with minimum sample constraints. The motion-policy +acceleration budget follows squared time scaling; authored robot joint-speed +limits remain enforced. This targets a physical task cycle near 44 seconds, +including the final two-second acceptance hold. The observation source supplies geometry and evidence; the existing Atomic engine owns IK, joint commands, timing and row-local execution. Failed rows are removed before geometry/timing reductions and IK. Physical grasp references are captured after the acknowledged close/settle command and released per row; no -rigid cable/tool relation is projected. Newton/DexUni, scene, robot, cable -coefficients, native contact handling and strict task acceptance are unchanged. - -Observed continuation is bounded and shares one invocation timeout. This -variant sets that timeout to 120 seconds to cover the full left placement; -it retains the 12,000-control-step episode ceiling. Offline `compile()`, external -nominal trajectories/expansion, replay recovery and mid-operation invocation -revisions are explicitly unsupported. Cancellation and failed-row holds use the -ordinary runner. `PLAN_CONTINUED` events expose every installed phase plan. - -The extended accepted goal types intentionally change registration/integration -fingerprints, including integrations which reference these descriptors. Saved -registrations must be regenerated; the existing handover program fingerprint is -unchanged. Fewer invocations are a structural simplification, not a claim of -proportionally faster physics or of a general deformable-object policy. +rigid cable/tool relation is projected. Newton/DexUni, physics/control timesteps, +scene, robot, cable coefficients, native contact handling and strict task +acceptance are unchanged. The previous normal-pacing compact implementation +is available at commit `47c741133` for comparison. + +Observed continuation is bounded and shares one invocation timeout. Offline +`compile()`, external nominal trajectories/expansion, replay recovery and +mid-operation invocation revisions are explicitly unsupported. Cancellation and +failed-row holds use the ordinary runner. `PLAN_CONTINUED` events expose every +installed phase plan. Extended accepted goal types intentionally change related +registration/integration fingerprints; saved registrations must be regenerated. +Fewer invocations and shorter physical duration are separately measured results. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.compact.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.compact.yaml index bc59492964..dd9bb46dfe 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.compact.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.compact.yaml @@ -1,4 +1,4 @@ -policy_id: cable_compact_contact_v1 +policy_id: cable_compact_fast_contact_v1 preset_id: cable requires: embodiment_contract: dual_arm_parallel_gripper @@ -7,9 +7,10 @@ planner: motion: strategy: ik_interp velocity_targets: auto - sample_count: 73 - interpolate_position_step: 0.0008333333333333334 + sample_count: 34 + interpolate_position_step: 0.0018333333333333335 dynamic_collision_mode: 'off' + acceleration_limit: 2.42 tracking: kind: timed recovery: diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.compact.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.compact.yaml index 8929e2fb0a..1d119e5a29 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.compact.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.compact.yaml @@ -1,4 +1,4 @@ -integration_id: route_the_cable_compact_v1 +integration_id: route_the_cable_compact_fast_v1 program_id: route_the_cable requires: scene_contract: cable_board_v1 @@ -37,7 +37,7 @@ profile: preserve_samples: true simulation.cable_pickup: kind: pick_up - hand_interp_steps: 48 + hand_interp_steps: 22 grasp_settle_steps: 21 lift_height: 0.0 pre_grasp_distance: 0.0 @@ -144,11 +144,12 @@ runtime_services: - null - null - 0.81 - speed: 0.12 + speed: 0.264 port_screen: false + angular_speed: 1.1 settle_motion: kind: hold - samples: 15 + samples: 7 right_approach_0.162_tracking: kind: check condition: pose @@ -175,6 +176,8 @@ runtime_services: - 0.774 material_distance: 0.162 retract_distance: 0.0015 + speed: 0.11000000000000001 + angular_speed: 1.1 raise_0_tracking: kind: check condition: pose @@ -187,6 +190,8 @@ runtime_services: - 0.5 - -0.255 - null + speed: 0.11000000000000001 + angular_speed: 1.1 carry_0_tracking: kind: check condition: pose @@ -199,6 +204,8 @@ runtime_services: - null - null - 0.7645 + speed: 0.11000000000000001 + angular_speed: 1.1 lower_0_tracking: kind: check condition: pose @@ -207,7 +214,7 @@ runtime_services: stop_when_seated: false release_0: kind: open - samples: 36 + samples: 17 finger_position: 0.008 release_part: right_arm retreat_0: @@ -216,7 +223,8 @@ runtime_services: - null - null - 0.82 - speed: 0.08 + speed: 0.17600000000000002 + angular_speed: 1.1 retreat_0_tracking: kind: check condition: pose @@ -225,7 +233,7 @@ runtime_services: stop_when_seated: false open_clear_of_spool_0: kind: open - samples: 36 + samples: 17 release_part: right_arm right_approach_0.282: kind: material @@ -234,8 +242,9 @@ runtime_services: - null - null - 0.81 - speed: 0.12 + speed: 0.264 port_screen: false + angular_speed: 1.1 right_approach_0.282_tracking: kind: check condition: pose @@ -262,6 +271,8 @@ runtime_services: - 0.78 material_distance: 0.282 retract_distance: 0.003 + speed: 0.11000000000000001 + angular_speed: 1.1 raise_1_tracking: kind: check condition: pose @@ -274,6 +285,8 @@ runtime_services: - 0.528 - -0.14 - null + speed: 0.11000000000000001 + angular_speed: 1.1 carry_1_tracking: kind: check condition: pose @@ -286,6 +299,8 @@ runtime_services: - null - null - 0.7645 + speed: 0.11000000000000001 + angular_speed: 1.1 lower_1_tracking: kind: check condition: pose @@ -294,7 +309,7 @@ runtime_services: stop_when_seated: false release_1: kind: open - samples: 36 + samples: 17 finger_position: 0.008 release_part: right_arm retreat_1: @@ -303,7 +318,8 @@ runtime_services: - null - null - 0.82 - speed: 0.08 + speed: 0.17600000000000002 + angular_speed: 1.1 retreat_1_tracking: kind: check condition: pose @@ -312,7 +328,7 @@ runtime_services: stop_when_seated: false open_clear_of_spool_1: kind: open - samples: 36 + samples: 17 release_part: right_arm right_approach_0.438: kind: material @@ -321,8 +337,9 @@ runtime_services: - null - null - 0.81 - speed: 0.12 + speed: 0.264 port_screen: false + angular_speed: 1.1 right_approach_0.438_tracking: kind: check condition: pose @@ -349,6 +366,8 @@ runtime_services: - 0.8 material_distance: 0.438 retract_distance: 0.008 + speed: 0.11000000000000001 + angular_speed: 1.1 raise_2_tracking: kind: check condition: pose @@ -361,6 +380,8 @@ runtime_services: - 0.555 - -0.01 - null + speed: 0.11000000000000001 + angular_speed: 1.1 carry_2_tracking: kind: check condition: pose @@ -373,6 +394,8 @@ runtime_services: - null - null - 0.7645 + speed: 0.11000000000000001 + angular_speed: 1.1 lower_2_tracking: kind: check condition: pose @@ -381,7 +404,7 @@ runtime_services: stop_when_seated: false release_2: kind: open - samples: 36 + samples: 17 finger_position: 0.008 release_part: right_arm retreat_2: @@ -390,7 +413,8 @@ runtime_services: - null - null - 0.82 - speed: 0.08 + speed: 0.17600000000000002 + angular_speed: 1.1 retreat_2_tracking: kind: check condition: pose @@ -399,11 +423,12 @@ runtime_services: stop_when_seated: false open_clear_of_spool_2: kind: open - samples: 36 + samples: 17 release_part: right_arm home_right: kind: home - speed: 0.15 + speed: 0.33 + angular_speed: 1.1 home_right_tracking: kind: check condition: pose @@ -417,8 +442,9 @@ runtime_services: - null - null - 0.81 - speed: 0.12 + speed: 0.264 port_screen: true + angular_speed: 1.1 left_approach_0.57_tracking: kind: check condition: pose @@ -443,6 +469,8 @@ runtime_services: - 0.586 - 0.055 - 0.795 + speed: 0.11000000000000001 + angular_speed: 1.1 stage_free_end_tracking: kind: check condition: pose @@ -455,9 +483,10 @@ runtime_services: - null - null - 0.795 - speed: 0.025 + speed: 0.05500000000000001 tolerance: 0.006 stop_when_seated: true + angular_speed: 1.1 lower_channel_0_tracking: kind: check condition: pose @@ -472,10 +501,11 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.02 + speed: 0.044000000000000004 tolerance: 0.006 stop_when_seated: true maximum_z_correction: 0.003 + angular_speed: 1.1 align_tip_0_0_tracking: kind: check condition: pose @@ -490,10 +520,11 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.02 + speed: 0.044000000000000004 tolerance: 0.006 stop_when_seated: true maximum_z_correction: 0.003 + angular_speed: 1.1 align_tip_0_1_tracking: kind: check condition: pose @@ -506,9 +537,10 @@ runtime_services: - null - null - 0.7875 - speed: 0.025 + speed: 0.05500000000000001 tolerance: 0.006 stop_when_seated: true + angular_speed: 1.1 lower_channel_1_tracking: kind: check condition: pose @@ -523,10 +555,11 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.02 + speed: 0.044000000000000004 tolerance: 0.006 stop_when_seated: true maximum_z_correction: 0.003 + angular_speed: 1.1 align_tip_1_0_tracking: kind: check condition: pose @@ -541,10 +574,11 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.02 + speed: 0.044000000000000004 tolerance: 0.006 stop_when_seated: true maximum_z_correction: 0.003 + angular_speed: 1.1 align_tip_1_1_tracking: kind: check condition: pose @@ -557,9 +591,10 @@ runtime_services: - null - null - 0.78 - speed: 0.025 + speed: 0.05500000000000001 tolerance: 0.006 stop_when_seated: true + angular_speed: 1.1 lower_channel_2_tracking: kind: check condition: pose @@ -574,10 +609,11 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.02 + speed: 0.044000000000000004 tolerance: 0.006 stop_when_seated: true maximum_z_correction: 0.003 + angular_speed: 1.1 align_tip_2_0_tracking: kind: check condition: pose @@ -592,10 +628,11 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.02 + speed: 0.044000000000000004 tolerance: 0.006 stop_when_seated: true maximum_z_correction: 0.003 + angular_speed: 1.1 align_tip_2_1_tracking: kind: check condition: pose @@ -610,6 +647,8 @@ runtime_services: minimum_tool_pitch: -0.3490658503988659 tolerance: 0.006 stop_when_seated: true + speed: 0.11000000000000001 + angular_speed: 1.1 pitch_free_end_0_tracking: kind: check condition: pose @@ -624,6 +663,8 @@ runtime_services: minimum_tool_pitch: -0.3490658503988659 tolerance: 0.006 stop_when_seated: true + speed: 0.11000000000000001 + angular_speed: 1.1 pitch_free_end_1_tracking: kind: check condition: pose @@ -636,9 +677,10 @@ runtime_services: - null - null - 0.7685 - speed: 0.025 + speed: 0.05500000000000001 tolerance: 0.006 stop_when_seated: true + angular_speed: 1.1 lower_channel_3_tracking: kind: check condition: pose @@ -647,7 +689,7 @@ runtime_services: stop_when_seated: true soften_free_end: kind: grip_adjust - samples: 60 + samples: 28 finger_position: 0.004 settle_soft_grasp: kind: hold @@ -667,10 +709,11 @@ runtime_services: maximum_correction: 0.005 maximum_y: 0.05 minimum_z: 0.7655 - speed: 0.02 + speed: 0.044000000000000004 tolerance: 0.006 stop_when_seated: false maximum_z_correction: 0.003 + angular_speed: 1.1 align_tip_3_0_tracking: kind: check condition: pose @@ -686,10 +729,11 @@ runtime_services: maximum_correction: 0.005 maximum_y: 0.05 minimum_z: 0.7655 - speed: 0.02 + speed: 0.044000000000000004 tolerance: 0.006 stop_when_seated: false maximum_z_correction: 0.003 + angular_speed: 1.1 align_tip_3_1_tracking: kind: check condition: pose @@ -700,8 +744,9 @@ runtime_services: kind: lay_down maximum_rotation: 0.17453292519943295 minimum_z: 0.7625 - speed: 0.015 + speed: 0.033 tolerance: 0.006 + angular_speed: 0.5 lay_down_free_end_0_tracking: kind: check condition: pose @@ -712,8 +757,9 @@ runtime_services: kind: lay_down maximum_rotation: 0.17453292519943295 minimum_z: 0.7625 - speed: 0.015 + speed: 0.033 tolerance: 0.006 + angular_speed: 0.5 lay_down_free_end_1_tracking: kind: check condition: pose @@ -725,7 +771,7 @@ runtime_services: condition: seated release_free_end: kind: open - samples: 60 + samples: 28 release_part: left_arm finger_position: 0.008 settle_released_end: @@ -737,7 +783,8 @@ runtime_services: - null - null - 0.8 - speed: 0.025 + speed: 0.05500000000000001 + angular_speed: 1.1 retreat_free_end_tracking: kind: check condition: pose @@ -750,7 +797,8 @@ runtime_services: - null - 0.015 - 0.82 - speed: 0.08 + speed: 0.17600000000000002 + angular_speed: 1.1 raise_clear_of_port_tracking: kind: check condition: pose @@ -758,7 +806,8 @@ runtime_services: orientation_tolerance: 0.25 home_left: kind: home - speed: 0.15 + speed: 0.33 + angular_speed: 1.1 home_left_tracking: kind: check condition: pose @@ -776,9 +825,10 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.02 + speed: 0.044000000000000004 tolerance: 0.006 stop_when_seated: true + angular_speed: 1.1 align_over_port_0_tracking: kind: check condition: pose @@ -793,9 +843,10 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.02 + speed: 0.044000000000000004 tolerance: 0.006 stop_when_seated: true + angular_speed: 1.1 align_over_port_1_tracking: kind: check condition: pose @@ -810,9 +861,10 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.02 + speed: 0.044000000000000004 tolerance: 0.006 stop_when_seated: true + angular_speed: 1.1 align_over_port_2_tracking: kind: check condition: pose @@ -827,9 +879,10 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.02 + speed: 0.044000000000000004 tolerance: 0.006 stop_when_seated: true + angular_speed: 1.1 align_over_port_3_tracking: kind: check condition: pose @@ -844,6 +897,8 @@ runtime_services: - null - null - 0.78 + speed: 0.11000000000000001 + angular_speed: 1.1 raise_free_end_tracking: kind: check condition: pose @@ -852,8 +907,11 @@ runtime_services: stop_when_seated: false open_clear_of_port: kind: open - samples: 36 + samples: 17 release_part: left_arm + settle_contact: + kind: hold + samples: 15 operations: right_pick_0: kind: pick_up @@ -1177,14 +1235,14 @@ runtime_services: target: lay_down_free_end_0 after: - kind: hold - target: settle_motion + target: settle_contact - kind: check target: lay_down_free_end_0_tracking - kind: pose target: lay_down_free_end_1 after: - kind: hold - target: settle_motion + target: settle_contact - kind: check target: lay_down_free_end_1_tracking - kind: check diff --git a/tests/gym/envs/task_program/test_cable_material.py b/tests/gym/envs/task_program/test_cable_material.py index 3255d0875e..412c7ec795 100644 --- a/tests/gym/envs/task_program/test_cable_material.py +++ b/tests/gym/envs/task_program/test_cable_material.py @@ -100,12 +100,26 @@ def test_compact_cable_preflight_has_eight_calls_and_retains_physical_checks(): if factory.call_id == "simulation.cable_pickup" ) original = _decode_cable_service(_declaration()) - assert (cfg.targets, cfg.geometry, cfg.motion_parts, cfg.slip_limit) == ( - original.targets, + assert (cfg.geometry, cfg.motion_parts, cfg.slip_limit) == ( original.geometry, original.motion_parts, original.slip_limit, ) + # Timing changes must preserve every geometric target and physical gate. + targets = {target.target_id: target for target in cfg.targets} + assert set(targets) == {target.target_id for target in original.targets} | { + "settle_contact" + } + for target in original.targets: + assert ( + replace( + targets[target.target_id], + speed=target.speed, + angular_speed=target.angular_speed, + samples=target.samples, + ) + == target + ) operations = cfg.operations assert len(operations) == 8 phases = [phase for operation in operations for phase in operation.phases] @@ -113,6 +127,24 @@ def test_compact_cable_preflight_has_eight_calls_and_retains_physical_checks(): assert sum(phase.kind == "grasp" for phase in phases) == 4 assert sum(post.kind == "check" for phase in phases for post in phase.after) == 48 assert sum(post.kind == "hold" for phase in phases for post in phase.after) == 45 + assert { + name: targets[name].samples + for name in ( + "settle_soft_grasp", + "settle_contact", + "settle_released_end", + "settle_and_validate", + ) + } == { + "settle_soft_grasp": 30, + "settle_contact": 15, + "settle_released_end": 30, + "settle_and_validate": 120, + } + for phase in phases: + if targets[phase.target].kind == "lay_down": + assert targets[phase.target].angular_speed == 0.5 + assert phase.after[0].target == "settle_contact" assert registration.evaluation_final_refs == ("route_success",) @@ -694,6 +726,34 @@ def _lower_pitch(service, target="pitch"): ) +def test_declared_angular_speed_changes_duration_without_changing_pivot_goal(): + service, _ = _pitch_runtime(0, math.pi / 6) + ordinary = _lower_pitch(service).goal.xpos + target = service.cfg.target("pitch") + assert target.angular_speed == 0.5 + service.cfg = replace(service.cfg, targets=(replace(target, angular_speed=1.0),)) + accelerated = _lower_pitch(service).goal.xpos + # A ten-degree rotation at 60 Hz takes ceil(20.94) / ceil(10.47) samples. + assert ordinary.shape[1] == 21 + assert accelerated.shape[1] == 11 + torch.testing.assert_close(ordinary[:, -1], accelerated[:, -1]) + + +@pytest.mark.parametrize("rate", [0.0, -0.5]) +def test_cable_angular_speed_requires_a_positive_rate(rate): + value = _declaration() + value["targets"] = {"home": {"kind": "home", "angular_speed": rate}} + with pytest.raises(ValueError, match="positive"): + _decode_cable_service(value) + + +def test_cable_hold_cannot_declare_a_motion_angular_speed(): + value = _declaration() + value["targets"] = {"hold": {"kind": "hold", "samples": 15, "angular_speed": 1.0}} + with pytest.raises(ValueError, match="Unsupported fields"): + _decode_cable_service(value) + + @pytest.mark.parametrize( "tool_degrees,ray_degrees,expected_degrees", [(0, 30, -10), (-15, 30, -20), (-25, 30, -25), (0, -30, 0)], From 1c3080fac46c7abee17e5bdef5b682b2f47faab9 Mon Sep 17 00:00:00 2001 From: yuecideng Date: Thu, 19 Nov 2026 23:34:43 +0800 Subject: [PATCH 10/11] fix: retain post-lay-down convergence before cable release --- .../cable/route_the_cable/README.md | 7 +- .../task_program/execution_policy.yaml | 4 +- .../task_program/integration.yaml | 169 +++++++++--------- .../envs/task_program/test_cable_material.py | 8 +- .../task_program/test_task_vertical_slices.py | 8 +- 5 files changed, 104 insertions(+), 92 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 629f8c6a61..33887e664b 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 @@ -325,13 +325,14 @@ command boundaries, while the 45 explicit hold targets are internal command ranges. Home belongs to the corresponding placement retreat. Every feedback-dependent target is resolved from current observations before planning. -The profile scales translational rates by 2.2 and uses 1.1 rad/s for ordinary +The profile scales translational rates by 2.22 and uses 1.11 rad/s for ordinary angular pacing. Each target can declare positive `angular_speed` in rad/s; omission keeps 0.5 rad/s. The two wrist lay-down phases retain 0.5 rad/s and -15-control contact holds. All four 21-control post-close dwells, the 30-control +15-control and 60-control contact holds, respectively. The longer second hold +lets the transient endpoint rise settle before the unchanged release gate. All four 21-control post-close dwells, the 30-control soft-grasp and post-release holds, and the final 120-control acceptance hold are retained. Ordinary process holds and hand transitions are shortened with -minimum sample constraints. The motion acceleration budget is 2.42; robot +minimum sample constraints. The motion acceleration budget is 2.4642; robot joint-speed limits remain unchanged. Newton/DexUni coupling, physics/control timesteps, rod and robot parameters, 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 index dd9bb46dfe..4cb78f69f4 100644 --- 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 @@ -8,9 +8,9 @@ motion: strategy: ik_interp velocity_targets: auto sample_count: 34 - interpolate_position_step: 0.0018333333333333335 + interpolate_position_step: 0.0018500000000000003 dynamic_collision_mode: 'off' - acceleration_limit: 2.42 + acceleration_limit: 2.4642 tracking: kind: timed recovery: 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 d9eb447727..c5758c4492 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 @@ -125,9 +125,9 @@ runtime_services: - null - null - 0.81 - speed: 0.264 + speed: 0.2664 port_screen: false - angular_speed: 1.1 + angular_speed: 1.11 settle_motion: kind: hold samples: 7 @@ -157,8 +157,8 @@ runtime_services: - 0.774 material_distance: 0.162 retract_distance: 0.0015 - speed: 0.11000000000000001 - angular_speed: 1.1 + speed: 0.111 + angular_speed: 1.11 raise_0_tracking: kind: check condition: pose @@ -171,8 +171,8 @@ runtime_services: - 0.5 - -0.255 - null - speed: 0.11000000000000001 - angular_speed: 1.1 + speed: 0.111 + angular_speed: 1.11 carry_0_tracking: kind: check condition: pose @@ -185,8 +185,8 @@ runtime_services: - null - null - 0.7645 - speed: 0.11000000000000001 - angular_speed: 1.1 + speed: 0.111 + angular_speed: 1.11 lower_0_tracking: kind: check condition: pose @@ -204,8 +204,8 @@ runtime_services: - null - null - 0.82 - speed: 0.17600000000000002 - angular_speed: 1.1 + speed: 0.1776 + angular_speed: 1.11 retreat_0_tracking: kind: check condition: pose @@ -223,9 +223,9 @@ runtime_services: - null - null - 0.81 - speed: 0.264 + speed: 0.2664 port_screen: false - angular_speed: 1.1 + angular_speed: 1.11 right_approach_0.282_tracking: kind: check condition: pose @@ -252,8 +252,8 @@ runtime_services: - 0.78 material_distance: 0.282 retract_distance: 0.003 - speed: 0.11000000000000001 - angular_speed: 1.1 + speed: 0.111 + angular_speed: 1.11 raise_1_tracking: kind: check condition: pose @@ -266,8 +266,8 @@ runtime_services: - 0.528 - -0.14 - null - speed: 0.11000000000000001 - angular_speed: 1.1 + speed: 0.111 + angular_speed: 1.11 carry_1_tracking: kind: check condition: pose @@ -280,8 +280,8 @@ runtime_services: - null - null - 0.7645 - speed: 0.11000000000000001 - angular_speed: 1.1 + speed: 0.111 + angular_speed: 1.11 lower_1_tracking: kind: check condition: pose @@ -299,8 +299,8 @@ runtime_services: - null - null - 0.82 - speed: 0.17600000000000002 - angular_speed: 1.1 + speed: 0.1776 + angular_speed: 1.11 retreat_1_tracking: kind: check condition: pose @@ -318,9 +318,9 @@ runtime_services: - null - null - 0.81 - speed: 0.264 + speed: 0.2664 port_screen: false - angular_speed: 1.1 + angular_speed: 1.11 right_approach_0.438_tracking: kind: check condition: pose @@ -347,8 +347,8 @@ runtime_services: - 0.8 material_distance: 0.438 retract_distance: 0.008 - speed: 0.11000000000000001 - angular_speed: 1.1 + speed: 0.111 + angular_speed: 1.11 raise_2_tracking: kind: check condition: pose @@ -361,8 +361,8 @@ runtime_services: - 0.555 - -0.01 - null - speed: 0.11000000000000001 - angular_speed: 1.1 + speed: 0.111 + angular_speed: 1.11 carry_2_tracking: kind: check condition: pose @@ -375,8 +375,8 @@ runtime_services: - null - null - 0.7645 - speed: 0.11000000000000001 - angular_speed: 1.1 + speed: 0.111 + angular_speed: 1.11 lower_2_tracking: kind: check condition: pose @@ -394,8 +394,8 @@ runtime_services: - null - null - 0.82 - speed: 0.17600000000000002 - angular_speed: 1.1 + speed: 0.1776 + angular_speed: 1.11 retreat_2_tracking: kind: check condition: pose @@ -408,8 +408,8 @@ runtime_services: release_part: right_arm home_right: kind: home - speed: 0.33 - angular_speed: 1.1 + speed: 0.333 + angular_speed: 1.11 home_right_tracking: kind: check condition: pose @@ -423,9 +423,9 @@ runtime_services: - null - null - 0.81 - speed: 0.264 + speed: 0.2664 port_screen: true - angular_speed: 1.1 + angular_speed: 1.11 left_approach_0.57_tracking: kind: check condition: pose @@ -450,8 +450,8 @@ runtime_services: - 0.586 - 0.055 - 0.795 - speed: 0.11000000000000001 - angular_speed: 1.1 + speed: 0.111 + angular_speed: 1.11 stage_free_end_tracking: kind: check condition: pose @@ -464,10 +464,10 @@ runtime_services: - null - null - 0.795 - speed: 0.05500000000000001 + speed: 0.0555 tolerance: 0.006 stop_when_seated: true - angular_speed: 1.1 + angular_speed: 1.11 lower_channel_0_tracking: kind: check condition: pose @@ -482,11 +482,11 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.044000000000000004 + speed: 0.0444 tolerance: 0.006 stop_when_seated: true maximum_z_correction: 0.003 - angular_speed: 1.1 + angular_speed: 1.11 align_tip_0_0_tracking: kind: check condition: pose @@ -501,11 +501,11 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.044000000000000004 + speed: 0.0444 tolerance: 0.006 stop_when_seated: true maximum_z_correction: 0.003 - angular_speed: 1.1 + angular_speed: 1.11 align_tip_0_1_tracking: kind: check condition: pose @@ -518,10 +518,10 @@ runtime_services: - null - null - 0.7875 - speed: 0.05500000000000001 + speed: 0.0555 tolerance: 0.006 stop_when_seated: true - angular_speed: 1.1 + angular_speed: 1.11 lower_channel_1_tracking: kind: check condition: pose @@ -536,11 +536,11 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.044000000000000004 + speed: 0.0444 tolerance: 0.006 stop_when_seated: true maximum_z_correction: 0.003 - angular_speed: 1.1 + angular_speed: 1.11 align_tip_1_0_tracking: kind: check condition: pose @@ -555,11 +555,11 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.044000000000000004 + speed: 0.0444 tolerance: 0.006 stop_when_seated: true maximum_z_correction: 0.003 - angular_speed: 1.1 + angular_speed: 1.11 align_tip_1_1_tracking: kind: check condition: pose @@ -572,10 +572,10 @@ runtime_services: - null - null - 0.78 - speed: 0.05500000000000001 + speed: 0.0555 tolerance: 0.006 stop_when_seated: true - angular_speed: 1.1 + angular_speed: 1.11 lower_channel_2_tracking: kind: check condition: pose @@ -590,11 +590,11 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.044000000000000004 + speed: 0.0444 tolerance: 0.006 stop_when_seated: true maximum_z_correction: 0.003 - angular_speed: 1.1 + angular_speed: 1.11 align_tip_2_0_tracking: kind: check condition: pose @@ -609,11 +609,11 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.044000000000000004 + speed: 0.0444 tolerance: 0.006 stop_when_seated: true maximum_z_correction: 0.003 - angular_speed: 1.1 + angular_speed: 1.11 align_tip_2_1_tracking: kind: check condition: pose @@ -628,8 +628,8 @@ runtime_services: minimum_tool_pitch: -0.3490658503988659 tolerance: 0.006 stop_when_seated: true - speed: 0.11000000000000001 - angular_speed: 1.1 + speed: 0.111 + angular_speed: 1.11 pitch_free_end_0_tracking: kind: check condition: pose @@ -644,8 +644,8 @@ runtime_services: minimum_tool_pitch: -0.3490658503988659 tolerance: 0.006 stop_when_seated: true - speed: 0.11000000000000001 - angular_speed: 1.1 + speed: 0.111 + angular_speed: 1.11 pitch_free_end_1_tracking: kind: check condition: pose @@ -658,10 +658,10 @@ runtime_services: - null - null - 0.7685 - speed: 0.05500000000000001 + speed: 0.0555 tolerance: 0.006 stop_when_seated: true - angular_speed: 1.1 + angular_speed: 1.11 lower_channel_3_tracking: kind: check condition: pose @@ -690,11 +690,11 @@ runtime_services: maximum_correction: 0.005 maximum_y: 0.05 minimum_z: 0.7655 - speed: 0.044000000000000004 + speed: 0.0444 tolerance: 0.006 stop_when_seated: false maximum_z_correction: 0.003 - angular_speed: 1.1 + angular_speed: 1.11 align_tip_3_0_tracking: kind: check condition: pose @@ -710,11 +710,11 @@ runtime_services: maximum_correction: 0.005 maximum_y: 0.05 minimum_z: 0.7655 - speed: 0.044000000000000004 + speed: 0.0444 tolerance: 0.006 stop_when_seated: false maximum_z_correction: 0.003 - angular_speed: 1.1 + angular_speed: 1.11 align_tip_3_1_tracking: kind: check condition: pose @@ -725,7 +725,7 @@ runtime_services: kind: lay_down maximum_rotation: 0.17453292519943295 minimum_z: 0.7625 - speed: 0.033 + speed: 0.0333 tolerance: 0.006 angular_speed: 0.5 lay_down_free_end_0_tracking: @@ -738,7 +738,7 @@ runtime_services: kind: lay_down maximum_rotation: 0.17453292519943295 minimum_z: 0.7625 - speed: 0.033 + speed: 0.0333 tolerance: 0.006 angular_speed: 0.5 lay_down_free_end_1_tracking: @@ -764,8 +764,8 @@ runtime_services: - null - null - 0.8 - speed: 0.05500000000000001 - angular_speed: 1.1 + speed: 0.0555 + angular_speed: 1.11 retreat_free_end_tracking: kind: check condition: pose @@ -778,8 +778,8 @@ runtime_services: - null - 0.015 - 0.82 - speed: 0.17600000000000002 - angular_speed: 1.1 + speed: 0.1776 + angular_speed: 1.11 raise_clear_of_port_tracking: kind: check condition: pose @@ -787,8 +787,8 @@ runtime_services: orientation_tolerance: 0.25 home_left: kind: home - speed: 0.33 - angular_speed: 1.1 + speed: 0.333 + angular_speed: 1.11 home_left_tracking: kind: check condition: pose @@ -806,10 +806,10 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.044000000000000004 + speed: 0.0444 tolerance: 0.006 stop_when_seated: true - angular_speed: 1.1 + angular_speed: 1.11 align_over_port_0_tracking: kind: check condition: pose @@ -824,10 +824,10 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.044000000000000004 + speed: 0.0444 tolerance: 0.006 stop_when_seated: true - angular_speed: 1.1 + angular_speed: 1.11 align_over_port_1_tracking: kind: check condition: pose @@ -842,10 +842,10 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.044000000000000004 + speed: 0.0444 tolerance: 0.006 stop_when_seated: true - angular_speed: 1.1 + angular_speed: 1.11 align_over_port_2_tracking: kind: check condition: pose @@ -860,10 +860,10 @@ runtime_services: - null maximum_correction: 0.005 maximum_y: 0.05 - speed: 0.044000000000000004 + speed: 0.0444 tolerance: 0.006 stop_when_seated: true - angular_speed: 1.1 + angular_speed: 1.11 align_over_port_3_tracking: kind: check condition: pose @@ -878,8 +878,8 @@ runtime_services: - null - null - 0.78 - speed: 0.11000000000000001 - angular_speed: 1.1 + speed: 0.111 + angular_speed: 1.11 raise_free_end_tracking: kind: check condition: pose @@ -893,6 +893,9 @@ runtime_services: settle_contact: kind: hold samples: 15 + settle_release_alignment: + kind: hold + samples: 60 operations: right_pick_0: kind: pick_up @@ -1223,7 +1226,7 @@ runtime_services: target: lay_down_free_end_1 after: - kind: hold - target: settle_contact + target: settle_release_alignment - kind: check target: lay_down_free_end_1_tracking - kind: check diff --git a/tests/gym/envs/task_program/test_cable_material.py b/tests/gym/envs/task_program/test_cable_material.py index a3c4c68d0c..e722e79e69 100644 --- a/tests/gym/envs/task_program/test_cable_material.py +++ b/tests/gym/envs/task_program/test_cable_material.py @@ -118,19 +118,25 @@ def test_cable_preflight_has_eight_calls_and_retains_physical_checks(): for name in ( "settle_soft_grasp", "settle_contact", + "settle_release_alignment", "settle_released_end", "settle_and_validate", ) } == { "settle_soft_grasp": 30, "settle_contact": 15, + "settle_release_alignment": 60, "settle_released_end": 30, "settle_and_validate": 120, } for phase in phases: if targets[phase.target].kind == "lay_down": assert targets[phase.target].angular_speed == 0.5 - assert phase.after[0].target == "settle_contact" + assert phase.after[0].target == ( + "settle_contact" + if phase.target.endswith("_0") + else "settle_release_alignment" + ) assert registration.evaluation_final_refs == ("route_success",) 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 deb3a1a833..97451f5485 100644 --- a/tests/gym/envs/task_program/test_task_vertical_slices.py +++ b/tests/gym/envs/task_program/test_task_vertical_slices.py @@ -532,14 +532,16 @@ def test_open_drawer_config_owns_registered_lowerer_factory() -> None: registration = integration.registration # The handle-only deployment also fingerprints the shared PickUp goal types - # and the interpolation step declared by its motion policies. + # and the interpolation step declared by its motion policies. The added + # contact-path goal types intentionally extend those descriptors; restoring + # the pre-continuation types independently reproduces the previous goldens. assert ( registration.fingerprint - == "d4a935e630c9c912eeedd1e6c443617dfeae9a725cec15778a14bb383c7e2366" + == "8c26f478773895d74399f6f50e7784625719062b20ce76dc3c3c94b8ac7a7005" ) assert ( integration.integration_fingerprint - == "7b3223ed6d865f6863d3596a8b4350308d13a55182217e096ab5090b698a1a75" + == "a40a3110b8db70c9ce1b4a1879dc41f12c0dd876dce09e1844f2dc01c4ebc8d7" ) assert tuple(registration.catalog.registered_semantic_lowerer_declarations) == ( _OPEN_DRAWER_CALL_ID, From cbbee29a2219ba39702565f308d81821eb20e04a Mon Sep 17 00:00:00 2001 From: yuecideng Date: Thu, 19 Nov 2026 23:43:31 +0800 Subject: [PATCH 11/11] docs: document canonical cable pacing and qualification limits --- .../manipulation/cable/route_the_cable/README.md | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 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 33887e664b..5e63b3f145 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 @@ -139,7 +139,7 @@ aim the observed distal span 15° downward. Each can turn downward by at most span is never rotated upward. The complete interpolated motion pivots around the observed grasped material point, and a seated row retains its loaded command. After the last nominal descent, `soften_free_end` adjusts each finger toward a -4 mm joint target over 60 control samples, then holds for 30 samples before the +4 mm joint target over 28 control samples, then holds for 30 samples before the last two endpoint corrections. This reduces the commanded pinch load while retaining the original grasp reference and accumulated slip measurements. The adjustment neither releases the grasp reference nor resets the 12 mm slip @@ -176,7 +176,7 @@ penetration no greater than 1 mm. These release and final acceptance bounds are unchanged. Ordinary measured TCP tracking remains required until the endpoint is seated inside the strict channel. -The left hand releases toward an 8 mm joint target per finger over 60 control +The left hand releases toward an 8 mm joint target per finger over 28 control samples, then holds the arm stationary for 30 samples. It withdraws vertically to TCP `[same X, same Y, 0.80]` m before a separate retreat moves the TCP to `[same X, 0.015, 0.82]` m. `open_clear_of_port` opens the hand fully after @@ -276,7 +276,7 @@ spool: python -m pytest tests/gym/envs/tasks/test_route_the_cable_cuda.py --run-gpu -m gpu -q ``` -The optimized eight-call implementation was qualified at seed 73 on DexSim +The initial optimized eight-call implementation (`d9eff2f47`) was qualified at seed 73 on DexSim `0.6.0rc2` build `dbaca660471d5aaa7e750093e7fe1c207465c4af` and an RTX 5090. Fresh 2-environment, 16-environment and repeated 16-environment batches passed 2/2, 16/16 and 16/16 tasks. Their physical task times were 44.22, 45.07 and @@ -286,6 +286,13 @@ final settling and five additional seconds of live physics. Maximum observed grip slip was 1.741 mm against the original 12 mm limit. Final sampled cable penetration against the bench, spools and port was zero. +A later main-integrated batch correctly rejected one row during the second +lay-down's transient endpoint rise. The current profile retains a full second +of settling there before the unchanged release gate; a fresh 2-row and 16-row +qualification passed at 44.60 and 45.77 physical seconds, respectively, with +five additional seconds of released-endpoint persistence. The earlier failed +batch remains recorded in the PR. + These fixed-seed batches qualify the tested setup, not a population success rate. CPU coverage checks the bounded pivot, original grasp reference, exact row scope, strictly decoded pacing and contact dwells. GPU acceptance covers