diff --git a/NOTICE b/NOTICE index e2a23acf3..33cc37edf 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 6f10b4b2e..5c0e8c02f 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -444,7 +444,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 c538354cf..47d3150b3 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -24,6 +24,27 @@ 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`. + +`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. + Row-local planning within a larger World must carry the same explicit `context.env_ids` through FK/IK, root poses and diagnostics. HandOver applies this to both source/destination arms, including its continuation and failed-waypoint diff --git a/agent_context/topics/atomic-actions/execution.md b/agent_context/topics/atomic-actions/execution.md index 1558b9efb..637262466 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/agent_context/topics/simulation-system/deformables.md b/agent_context/topics/simulation-system/deformables.md index 165ad49df..db52a5268 100644 --- a/agent_context/topics/simulation-system/deformables.md +++ b/agent_context/topics/simulation-system/deformables.md @@ -108,8 +108,14 @@ Focused coverage: `tests/sim/test_deformable_cfg.py`, `CableObjectCfg` and `CableObject(BatchEntity)` are independent of particle-set deformables. `SimulationManager.add_cable_object()` lowers through `spawn/descriptors.py` into `CableDesc`, declares in the existing `SpawnScene`, -and binds the same facade in place during `prepare()`. Backend capabilities -reject Default, CPU, other Newton solvers and gradient mode; use DexUni on CUDA. +and binds the same facade in place during `prepare()`. `add_usd(UsdImportCfg)` +also returns a `CableObject` per imported cable under its source resource key, +with the same UID lookup, segment state and reset as a procedural declaration. +Cable geometry, placement and visuals come from the parsed `CableDesc`. +A sparse `asset_cfgs` entry keyed by the simulation curve prim can override +the UID, `fixed_segments`, `collision_filter_span` and individual `attrs` physics and +contact fields before declaration. Omitted fields retain the parsed values. +Cable declarations require DexUni on CUDA with gradient mode disabled. With this backend, an arena containing a cable cannot also contain fluid, cloth or soft-body particles: cable episode reset does not support those particles. Spawn checks the native cable and particle descriptors for direct diff --git a/agent_context/topics/task-programs/configuration.md b/agent_context/topics/task-programs/configuration.md index 941c80e51..0fce568e3 100644 --- a/agent_context/topics/task-programs/configuration.md +++ b/agent_context/topics/task-programs/configuration.md @@ -254,6 +254,15 @@ 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`. 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; omission retains the existing adaptive policy and acceleration default. @@ -268,6 +277,24 @@ 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. Declared contact operations lower +to `PickUp(ContactPickUpGoal)` and `Place(ContactPlaceGoal)`; bounded observed +continuations keep feedback phases inside the same invocation. The program +owns operation ordering. Compatibility move/joint/check adapters retain their +explicit configured call contracts. Registered grounding receives eligible rows +through `PlanningContext.planning_mask`, before shared duration reductions. +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 +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 dd4473520..6a635cf38 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 -------------------------------------- @@ -3486,6 +3499,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 ----------------------------------------------- @@ -5137,6 +5166,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 ---------------------------------------- @@ -7062,3 +7104,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/docs/source/overview/sim/atomic_skills/builtin_skills.md b/docs/source/overview/sim/atomic_skills/builtin_skills.md index d487dd9b0..b585dd973 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 000000000..b05c6ae09 --- /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 000000000..f26085fe6 --- /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 e63a006b8..c3712a3a9 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 f7183e4df..6317a1c11 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 ac46d07fd..111fd3f68 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 fa4ef217f..ffd84ec67 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 ae38d02e7..9156113f0 100644 --- a/embodichain/lab/gym/envs/task_program/bridge.py +++ b/embodichain/lab/gym/envs/task_program/bridge.py @@ -1812,6 +1812,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 201b5b972..9938c1aa7 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 b010e1932..ab7761329 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 cefb9aa90..f17d002d3 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 6c7a7dbd9..ea1b42d00 100644 --- a/embodichain/lab/sim/atomic_skills/__init__.py +++ b/embodichain/lab/sim/atomic_skills/__init__.py @@ -200,6 +200,9 @@ CoordinatedPlacementGoal, CoordinatedPlacementOptions, EndEffectorPoseGoal, + ContactGraspGoal, + ContactPickUpGoal, + ContactPlaceGoal, GraspGoal, HandOver, HandOverGoal, @@ -387,6 +390,9 @@ "FeedbackTerminalAcceptance", "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 000000000..ed43dd06b --- /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 fcebdab5d..3f1c0adb2 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 ( @@ -1516,6 +1517,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 23c5b50fb..88392f6cb 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 7342c1188..4744c9cc4 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 3a8e7bb3f..95a187cc0 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/policies.py b/embodichain/lab/sim/atomic_skills/policies.py index d868fbbcf..3c7a4a19a 100644 --- a/embodichain/lab/sim/atomic_skills/policies.py +++ b/embodichain/lab/sim/atomic_skills/policies.py @@ -85,6 +85,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.""" @@ -231,9 +238,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" @@ -327,6 +338,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 fef056f58..11d509c38 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 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, @@ -103,6 +109,9 @@ "CoordinatedPlacementGoal", "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 000000000..50db11855 --- /dev/null +++ b/embodichain/lab/sim/atomic_skills/primitives/contact.py @@ -0,0 +1,498 @@ +# ---------------------------------------------------------------------------- +# 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 + or request.motion_policy.phase_paths + ): + 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/coordinated_pickment.py b/embodichain/lab/sim/atomic_skills/primitives/coordinated_pickment.py index e80eccaec..ba095aa61 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_end_effector.py b/embodichain/lab/sim/atomic_skills/primitives/move_end_effector.py index 3dde68460..5685f152d 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 d1d28cc13..40cd00277 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 + """Keep authored joint samples at host cadence, rejecting overspeed rows.""" + + 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,41 @@ 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.") + step_dt = context.require_control_dt() + dt = torch.full( + target_qpos.shape[:2], + step_dt, + dtype=torch.float64, + device=target_qpos.device, + ) + dt[:, 0] = 0 + result = PlanResult( + 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, + ) + 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 ce65f7fb9..31c43c393 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py @@ -18,12 +18,21 @@ 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 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 +75,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 +95,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 +141,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) @@ -327,11 +359,17 @@ def __post_init__(self) -> None: ) -class PickUp(AtomicSkill[GraspGoal, PickUpOptions]): +class PickUp( + AtomicSkill[GraspGoal | ContactGraspGoal | ContactPickUpGoal, 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, + ContactPickUpGoal, + ) OptionsType: ClassVar[type] = PickUpOptions binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( @@ -352,12 +390,30 @@ class PickUp(AtomicSkill[GraspGoal, 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, 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, ContactPickUpGoal)): + 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 @@ -384,6 +440,10 @@ 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, + active_mask: torch.Tensor | None = None, ) -> tuple[torch.Tensor, torch.Tensor, dict[str, int]]: if options.cartesian_lift and motion_policy.phase_paths: raise ValueError("cartesian_lift and phase_paths are mutually exclusive.") @@ -393,121 +453,257 @@ 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.phase_paths: + raise ValueError( + "Contact pickup uses its fixed Cartesian grid, without phase_paths." + ) + 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: + ] + 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( + 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] - phase_names = ["pre_grasp", "grasp", "lift"] - 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 ) - initial_lift = translate_pose_world( - start_xpos, - start_xpos.new_tensor([0.0, 0.0, options.initial_lift_height]), + 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 ) - waypoints.insert(0, initial_lift) - phase_names.insert(0, "initial_lift") - - if any(name in motion_policy.phase_paths for name in phase_names): - motion_options.waypoint_modes = tuple( - motion_policy.phase_paths.get(name, "joint") for name in phase_names + 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]) ) - - 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, + 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 ) - if n_lift < 2: - raise ValueError("cartesian_lift requires at least two lift samples.") - lift_targets = axis_translation_keyframes( + is_success = prefix_valid & is_success & lift_success + n_approach, n_close, n_lift = ( + approach_arm.shape[1], + options.hand_interp_steps, + lift_arm.shape[1], + ) + grasp_arm_qpos = approach_arm[:, -1] + else: + n_approach, n_close, n_lift = split_three_segments( + motion_policy.sample_count, + options.hand_interp_steps, + first_segment_name="approach", + third_segment_name="lift", + ) + + lift_xpos = translate_pose_world( grasp_xpos, - 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] + phase_names = ["pre_grasp", "grasp", "lift"] + 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) + phase_names.insert(0, "initial_lift") + + if any(name in motion_policy.phase_paths for name in phase_names): + motion_options.waypoint_modes = tuple( + motion_policy.phase_paths.get(name, "joint") for name in phase_names + ) + + 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 @@ -530,7 +726,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: @@ -554,11 +752,18 @@ def _get_full_pickup_trajectory( def _plan( self, - request: ResolvedSkillRequest[GraspGoal, 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( request.skill_options, downstream_object_target_poses=tuple( @@ -606,76 +811,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, @@ -700,6 +914,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, @@ -724,14 +940,58 @@ def _plan( provenance=None, ) + @staticmethod + def _validate_contact_options(options: PickUpOptions) -> None: + 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." + ) + + 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( + 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 | 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( request, context, nominal_source, segments=("approach", "close", "lift") ) @@ -784,27 +1044,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 ): @@ -837,19 +1106,23 @@ def _plan_from_nominal( def _assemble_pick_plan( self, - request: ResolvedSkillRequest[GraspGoal, PickUpOptions], + request: ResolvedSkillRequest[ + GraspGoal | ContactGraspGoal | ContactPickUpGoal, 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, 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") @@ -857,31 +1130,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 @@ -1203,4 +1490,10 @@ def _upright_adjusted_grasp_poses( return adjusted_grasp_xpos -__all__ = ["GraspGoal", "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 26f1e0836..5b4b499fc 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 import math from typing import ClassVar, Literal @@ -187,7 +195,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 @@ -209,6 +217,7 @@ class Place(AtomicSkill[PlaceGoal | AssembleGoal, PlaceOptions]): GoalType: ClassVar[type | tuple[type, ...]] = ( PlaceGoal, AssembleGoal, + ContactPlaceGoal, ) OptionsType: ClassVar[type] = PlaceOptions binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( @@ -229,9 +238,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)) @@ -242,10 +267,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 @@ -523,12 +552,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( @@ -641,7 +674,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, @@ -696,11 +731,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. @@ -877,4 +914,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 f42d79164..dad0e1c26 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 83aba095d..0bc42d333 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/sim/cfg/usd.py b/embodichain/lab/sim/cfg/usd.py index 72a9b6e7f..41ee08a67 100644 --- a/embodichain/lab/sim/cfg/usd.py +++ b/embodichain/lab/sim/cfg/usd.py @@ -74,7 +74,10 @@ class UsdImportCfg: In sparse mappings with ``auto`` placement, explicit ``init_pos`` or ``init_rot`` keys select the configured pose, including zero values. - Omitted pose fields retain source placement. + Omitted pose fields retain source placement. For cables, sparse mappings + accept ``uid``, ``fixed_segments``, ``collision_filter_span`` and fields of + ``CablePhysicsCfg`` under ``attrs``. Overrides apply to all curves on + the targeted prim, preserving its authored geometry and placement. """ robot_cfgs: dict[str, RobotCfg] = field(default_factory=dict) diff --git a/embodichain/lab/sim/motion/motion_generator.py b/embodichain/lab/sim/motion/motion_generator.py index 23fdb6354..a926c22bb 100644 --- a/embodichain/lab/sim/motion/motion_generator.py +++ b/embodichain/lab/sim/motion/motion_generator.py @@ -719,6 +719,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/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index d114487e9..da20506ab 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -3349,7 +3349,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. @@ -3378,7 +3384,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 @@ -3430,6 +3437,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 @@ -3928,22 +3936,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": @@ -3960,6 +3954,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 cd397b8e8..0553f1258 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 23d739bc1..22656e439 100644 --- a/embodichain/lab/sim/spawn/usd_import.py +++ b/embodichain/lab/sim/spawn/usd_import.py @@ -19,14 +19,16 @@ from __future__ import annotations from collections import Counter +from collections.abc import Mapping from copy import deepcopy -from dataclasses import dataclass, field, fields +from dataclasses import dataclass, field, fields, replace from typing import TYPE_CHECKING, Any import numpy as np from dexsim.spawn import ( ArticulationDesc, + CableDesc, ConstraintDesc, MeshObjectDesc, SoftBodyDesc, @@ -36,6 +38,9 @@ from embodichain.lab.sim.cfg import ( ArticulationCfg, ArticulationRootPropertiesCfg, + CableAttachmentCfg, + CableObjectCfg, + CablePhysicsCfg, RigidObjectCfg, RobotCfg, VisualObjectCfg, @@ -46,6 +51,7 @@ ) from embodichain.lab.sim.objects import ( Articulation, + CableObject, RigidObject, Robot, VisualObject, @@ -54,10 +60,17 @@ ) from embodichain.lab.sim.sensors import Camera, CameraCfg, StereoCamera, StereoCameraCfg from embodichain.lab.sim.sensors.registry import _resolve_sensor_factory -from .descriptors import _pose_from_cfg, configure_articulation_desc +from .descriptors import ( + _cable_contact_spec, + _compile_newton_collision, + _pose_from_cfg, + _required_uid, + configure_articulation_desc, +) from .usd import ( _configure_articulation_usd, _configure_rigid_usd, + _overlay_optional_properties, _parse_usd_source, _source_pose, _set_resolved_usd_pose, @@ -91,6 +104,67 @@ def _source_path(descriptor: Any) -> str: return usd.prim_path if usd is not None and usd.prim_path else descriptor.name +def _usd_cable_cfg( + descriptor: CableDesc, overrides: Mapping[str, Any] | None +) -> CableObjectCfg: + """Apply sparse cable properties before declaring the descriptor and its facade.""" + if overrides is None: + overrides = {} + if not isinstance(overrides, Mapping): + raise TypeError("USD cable overrides must be a sparse mapping.") + unknown = overrides.keys() - { + "uid", + "attrs", + "fixed_segments", + "collision_filter_span", + } + if unknown: + raise ValueError(f"Unknown USD cable override fields: {sorted(unknown)}.") + attrs = overrides.get("attrs", {}) + if not isinstance(attrs, Mapping): + raise TypeError("USD cable attrs must be a sparse mapping.") + parsed_physics = { + item.name: getattr(descriptor.physics, item.name) + for item in fields(descriptor.physics) + } + physics_cfg = CablePhysicsCfg(**{**parsed_physics, **attrs}) + if overrides: + if "uid" in overrides: + descriptor.name = _required_uid(overrides["uid"], "Cable object") + # Preserve the difference between an omitted coefficient and an explicit zero/None. + descriptor.physics = replace( + descriptor.physics, + **{key: attrs[key] for key in attrs.keys() & parsed_physics.keys()}, + ) + contact = _cable_contact_spec(physics_cfg) + descriptor.collision = _overlay_optional_properties( + descriptor.collision, + _compile_newton_collision(contact, newton_solver_type="dexuni"), + ) + if contact.collision_enabled is not None: + descriptor.enable_collision = contact.collision_enabled + for name in ("fixed_segments", "collision_filter_span"): + if name in overrides: + setattr(descriptor, name, overrides[name]) + # Revalidate indices, attachments and thickness against the source + # geometry before publishing any part of this freshly parsed scene. + descriptor.__post_init__() + + frames = descriptor.segment_quaternions + return CableObjectCfg( + uid=descriptor.name, + points=descriptor.points, + radius=descriptor.radius, + init_local_pose=descriptor.pose, + segment_quaternions=None if frames is None else frames[:, [1, 2, 3, 0]], + closed=descriptor.closed, + fixed_segments=descriptor.fixed_segments, + attrs=physics_cfg, + collision_filter_span=descriptor.collision_filter_span, + render_mode=descriptor.render_mode, + ) + + def _compile_usd_import( manager: SimulationManager | None, recipe: UsdImportCfg, @@ -143,6 +217,8 @@ def _compile_usd_import( manager._spawn_scene._require_compatible_particles( cables=bool(source.cables), particles=bool(source.particle_sets) ) + if source.cables: + manager._require_cable_declaration() resources = list(source.iter_entities()) source_paths_by_id = {id(item): _source_path(item) for item in resources} counts = Counter(source_paths_by_id.values()) @@ -163,7 +239,12 @@ def _compile_usd_import( if item.usd is not None and item.usd.prim_path else item.name ) - for item in (*source.mesh_objects, *source.articulations, *source.particle_sets) + for item in ( + *source.mesh_objects, + *source.articulations, + *source.particle_sets, + *source.cables, + ) } overlap = robot_cfgs.keys() & asset_cfgs.keys() missing = (robot_cfgs.keys() | asset_cfgs.keys()) - source_paths @@ -251,6 +332,21 @@ def _compile_usd_import( and descriptor.physics is None and not descriptor.collisions ) + if isinstance(descriptor, CableDesc): + if root_pose is not None: + descriptor.pose = root_pose @ descriptor.pose + cfg = _usd_cable_cfg(descriptor, configured) + if descriptor.name in occupied: + raise ValueError(f"USD asset UID already exists: {descriptor.name!r}.") + occupied.add(descriptor.name) + renamed[original_name] = descriptor.name + facade = ( + None if manager is None else CableObject(cfg, device=manager.device) + ) + declarations.append( + (resource_keys[id(descriptor)], "cable_object", descriptor, facade) + ) + continue if configured is not None and not isinstance( descriptor, (MeshObjectDesc, ArticulationDesc, SoftBodyDesc, ClothDesc) ): @@ -547,6 +643,23 @@ def renamed_endpoint(endpoint: str) -> str: owner, separator, suffix = endpoint.partition("/") return renamed.get(owner, owner) + separator + suffix + for _, kind, descriptor, facade in declarations: + if kind == "cable_object": + for name in ("start_attachment", "end_attachment"): + anchor = getattr(descriptor, name) + if anchor is not None: + anchor.target = renamed.get(anchor.target, anchor.target) + if facade is not None: + setattr( + facade.cfg, + name, + CableAttachmentCfg( + target=anchor.target, + position=anchor.position.copy(), + link_name=anchor.link_name, + ), + ) + for constraint in source.constraints: if constraint.body0 is not None: constraint.body0 = renamed_endpoint(constraint.body0) @@ -577,6 +690,7 @@ def renamed_endpoint(endpoint: str) -> str: public_kinds = { "soft_object": "volume_deformable", "cloth_object": "surface_deformable", + "cable_object": "cable", } resource_types = { path: "robot" if path in robot_paths else public_kinds.get(kind, kind) @@ -586,7 +700,6 @@ def renamed_endpoint(endpoint: str) -> str: ("cameras", "camera"), ("lights", "light"), ("records", "record"), - ("cables", "cable"), ("constraints", "constraint"), ): resource_types.update( diff --git a/embodichain/lab/sim/spawn/usd_result.py b/embodichain/lab/sim/spawn/usd_result.py index dd562cb59..198281319 100644 --- a/embodichain/lab/sim/spawn/usd_result.py +++ b/embodichain/lab/sim/spawn/usd_result.py @@ -25,6 +25,7 @@ if TYPE_CHECKING: from embodichain.lab.sim.objects import ( Articulation, + CableObject, DeformableObject, RigidObject, Robot, @@ -35,11 +36,15 @@ class UsdImportResult( - Mapping[str, "VisualObject | RigidObject | Articulation | Robot | DeformableObject"] + Mapping[ + str, + "VisualObject | RigidObject | Articulation | Robot | DeformableObject | CableObject", + ] ): """Map source resource keys to stable, declared EmbodiChain facades. - Facades bind in place at ``SimulationManager.prepare()``. Resource maps + Facades, including imported cables, bind in place at + ``SimulationManager.prepare()``. Resource maps include native-only scene resources and constraints; their values are logical Spawn UIDs, not native handles or builder indices. A unique source keeps its prim path as the resource key. When a source prim produces several @@ -51,7 +56,13 @@ class UsdImportResult( def __init__( self, assets: Mapping[ - str, VisualObject | RigidObject | Articulation | Robot | DeformableObject + str, + VisualObject + | RigidObject + | Articulation + | Robot + | DeformableObject + | CableObject, ], *, resource_uids: Mapping[str, str], @@ -83,7 +94,14 @@ def __init__( def __getitem__( self, source_path: str - ) -> VisualObject | RigidObject | Articulation | Robot | DeformableObject: + ) -> ( + VisualObject + | RigidObject + | Articulation + | Robot + | DeformableObject + | CableObject + ): return self._assets[source_path] def __iter__(self) -> Iterator[str]: diff --git a/embodichain/lab/task_program/compiler/lowering.py b/embodichain/lab/task_program/compiler/lowering.py index da685143b..1a655593e 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 @@ -1652,7 +1672,9 @@ def ground( eligible, path=(*path, call_index, "call"), ) - lowering = self._lower_registered(analyzed, context, path=path) + lowering = self._lower_registered( + analyzed, replace(context, planning_mask=eligible), path=path + ) self._validate_registered_held_effects( analyzed, lowering, @@ -1750,6 +1772,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 +1790,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/_configured_composition.py b/embodichain/lab/task_program/integrations/_configured_composition.py index 55647ff70..a44b98083 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 5ddea4c5c..f93686b8d 100644 --- a/embodichain/lab/task_program/integrations/catalog.py +++ b/embodichain/lab/task_program/integrations/catalog.py @@ -242,6 +242,23 @@ def _canonical_value(value: object) -> object: # An inactive phase-path extension preserves existing registration seals. if not value.phase_paths: metadata.pop("phase_paths") + # 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 c5afef500..5ab4ec52c 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", @@ -1348,15 +1369,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, @@ -1799,6 +1832,7 @@ def _decode_motion_policy(value: object, *, path: str) -> MotionPolicy: "velocity_targets", "plan_opts", "interpolation_mode", + "interpolate_position_step", "ik_seed_mode", "phase_paths", "sample_count_retries", @@ -1868,7 +1902,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 @@ -3152,6 +3190,7 @@ class _DecodedRuntimeServices: ] = () control_part_evidence: _JointPositionConstraintEvidenceProviderFactory | None = None nominal_motion_sources: tuple[_ConfiguredPrototypeSource, ...] = () + cable_material: object | None = None settle_presets: ( tuple[ tuple[ @@ -3268,6 +3307,7 @@ def _decode_runtime_services( "registered_semantic_lowerers", "control_part_evidence", "nominal_motion_sources", + "cable_material", "settle_presets", } ), @@ -3318,6 +3358,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, @@ -3330,6 +3383,30 @@ def _decode_runtime_services( ) ) ) + if cable is not None: + lowerers += tuple( + factory(cable) + for factory in ( + _CablePickFactory, + _CableMoveFactory, + _CableJointFactory, + _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( @@ -3485,6 +3562,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=( @@ -3539,6 +3617,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: @@ -3555,6 +3634,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 } @@ -3722,6 +3808,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 @@ -3742,6 +3838,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/_cable_contact.py b/embodichain/lab/task_program/integrations/simulation/_cable_contact.py new file mode 100644 index 000000000..cda61c49f --- /dev/null +++ b/embodichain/lab/task_program/integrations/simulation/_cable_contact.py @@ -0,0 +1,538 @@ +# ---------------------------------------------------------------------------- +# 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_valid = torch.ones(len(rows), dtype=torch.bool, device=held.device) + local = view.pose(target, motion.control_part, valid_mask=local_valid) + valid[rows] &= local_valid + 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/bindings.py b/embodichain/lab/task_program/integrations/simulation/bindings.py index e09c521ab..fa9db7642 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 000000000..0dac9bde5 --- /dev/null +++ b/embodichain/lab/task_program/integrations/simulation/cable.py @@ -0,0 +1,1206 @@ +# ---------------------------------------------------------------------------- +# 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, TYPE_CHECKING + +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 + +if TYPE_CHECKING: + from ._cable_contact import CableContactOperation, CableContactSource + +__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 + angular_speed: float = 0.5 + 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 + 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 + 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 + operations: tuple[CableContactOperation, ...] = () + + 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.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] = {} + 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) + 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() + 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 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() + 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 + 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( + { + "part": part, + "material_distance": distance, + "phase": "close", + "sample_index": phase[2], + } + ) + 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, *, valid_mask: torch.Tensor | None = None + ) -> 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 + ) + success = success.reshape(-1).bool() + 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=torch.where(success[:, None], qpos, seed), + 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)) + ) + reachable = first[0] | second[0] + if valid_mask is not None: + reachable &= valid_mask + valid_mask &= reachable + elif not bool(reachable.all()): + raise ValueError("No reachable wrist branch for the cable material target.") + if target.port_screen: + yaw = torch.where(choose, -math.pi / 2, math.pi / 2) + self.port_yaw[part] = torch.where( + reachable, yaw, self.port_yaw.get(part, torch.zeros_like(yaw)) + ) + chosen = torch.where(choose[:, None, None], second[2], first[2]) + return torch.where(reachable[:, None, None], chosen, self.tool(part)) + + 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, *, valid_mask: torch.Tensor | None = None + ) -> torch.Tensor: + if target.kind == "material": + return self._material_pose(target, part, valid_mask=valid_mask) + 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 {"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 + 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: + valid = torch.isfinite(direction).all(-1) & (direction[:, 1] > 1e-6) + if valid_mask is not None: + valid_mask &= valid + elif not bool(valid.all()): + raise ValueError( + "The distal cable span points away from the channel." + ) + mouth_x, mouth_y = self.geometry["port_mouth"] + denominator = torch.where( + valid, direction[:, 1], torch.ones_like(direction[:, 1]) + ) + meets = point[:, 0] + direction[:, 0] / denominator * ( + mouth_y - point[:, 1] + ) + pose[:, 0, 3] += torch.where( + valid, mouth_x - meets, torch.zeros_like(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) + valid = torch.isfinite(tangent).all(-1) & (horizontal_norm[:, 0] > 1e-6) + if valid_mask is not None: + valid_mask &= valid + elif not bool(valid.all()): + raise ValueError( + "Material lift requires a cable tangent with a horizontal direction." + ) + safe_tangent = torch.where( + valid[:, None], tangent, torch.zeros_like(tangent) + ) + denominator = torch.where( + valid[:, None], horizontal_norm, torch.ones_like(horizontal_norm) + ) + pose[:, :2, 3] -= target.retract_distance * safe_tangent / denominator + pose[:, 2, 3] = torch.where( + valid, pose.new_tensor(target.position[2]), pose[:, 2, 3] + ) + 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) + valid = torch.ones_like(service.max_slip, dtype=torch.bool) + pose = service.pose(target, part, valid_mask=valid) + service.pending_pick = (part, target) + service.pending_pick_rows = valid + 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(pose), + control_overrides=overrides, + failed_preconditions={ + int(context.env_ids[row]): "Unreachable cable grasp target." + for row in (~valid).nonzero().flatten().tolist() + }, + ) + + +def _ground_cable_move( + service: _CableRuntime, + target: _CableTarget, + part: str, + control_dt: float, + active_mask: torch.Tensor | None = None, +) -> SemanticLowering: + env_ids = service.env_ids + active = ( + torch.ones(len(env_ids), dtype=torch.bool, device=env_ids.device) + if active_mask is None + else active_mask + ) + geometry_valid = active.clone() + start = service.tool(part) + end = service.pose(target, part, valid_mask=geometry_valid) + 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 + path_valid = ( + geometry_valid if pivot_geometry is None else geometry_valid & pivot_geometry[2] + ) + identity = torch.eye(4, dtype=start.dtype, device=start.device) + # Invalid rows must not influence shared duration or contribute nonfinite paths. + path_start = torch.where(path_valid[:, None, None], start, identity) + path_end = torch.where(path_valid[:, None, None], 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 / target.angular_speed) / control_dt + ) + + 1, + ) + previous_target = service.last_targets.get(part, service.tool(part, commanded=True)) + service.last_targets[part] = torch.where( + geometry_valid[:, None, None], end, previous_target + ).clone() + path = interpolate_xpos_batched(path_start, path_end, count)[:, 1:] + failures = { + int(env_ids[row]): "Unreachable or invalid cable target geometry." + for row in (active & ~geometry_valid).nonzero().flatten().tolist() + } + 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.update( + { + int(env_ids[row]): reason + for row in (active & ~valid).nonzero().flatten().tolist() + } + ) + path = torch.where(geometry_valid[:, None, None, None], path, start[:, None]) + return SemanticLowering( + goal=EndEffectorPoseGoal(path), failed_preconditions=failures + ) + + +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) + return _ground_cable_move( + service, + target, + part, + context.require_control_dt(), + active_mask=getattr(context, "planning_mask", None), + ) + + +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." + ) + # 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)) + + +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) + return _ground_cable_joint(service, target, part, context.env_ids) + + +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, details = service.condition(target, part) + 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)), + failed_preconditions=failures, + ) + + +@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", "grip_adjust", "hold"} + if self.call_id == "simulation.cable_check": + return target.kind == "check" + return target.kind not in {"open", "grip_adjust", "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) + 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 + + +@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", "operations"}), + ) + 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", "angular_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"}, + "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", + "maximum_correction", + "maximum_z_correction", + "minimum_z", + "maximum_y", + "stop_when_seated", + }, + "hold": {"samples"}, + "open": {"samples", "finger_position", "release_part"}, + "grip_adjust": {"samples", "finger_position"}, + "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", + "pitch_align", + "lay_down", + "tip_align", + "hold", + "open", + "grip_adjust", + "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.angular_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", + "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.") + 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.") + 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( + 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 064b4e2b2..8c862f2e9 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 3e92e7ce4..4710a2975 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 c0e0686cd..2352e4eb4 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, @@ -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.""" @@ -1203,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( @@ -1215,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 new file mode 100644 index 000000000..ab006df76 --- /dev/null +++ b/examples/sim/demo/cable_two_posts.py @@ -0,0 +1,428 @@ +# ---------------------------------------------------------------------------- +# 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 + +from embodichain.lab.visualization import ( + add_viser_args_to_parser, + visualization_cfg_from_args, +) + +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, + DenoisingCfg, + 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, + visualization=visualization_cfg_from_args(args), + render_cfg=RenderCfg( + renderer="no-render" if args.no_video else "hybrid", + spp=4, + denoising=DenoisingCfg(window="off", offscreen="off"), + ), + 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): + write_video_frame = camera is not None and frame % 2 == 0 + if frame: + 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() + 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 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() + 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__) + 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) + 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/extensions/embodichain_robots/i2rt/yam/cfg.py b/extensions/embodichain_robots/i2rt/yam/cfg.py index c1e96bbb0..48ec9410c 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/components/embodiments/yam_i2rt.yaml b/extensions/embodichain_tasks/configs/components/embodiments/yam_i2rt.yaml new file mode 100644 index 000000000..ca423d557 --- /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 000000000..5e63b3f14 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md @@ -0,0 +1,357 @@ +# 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.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 +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. 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 8 segments and 8 Atomic Skill invocations: four `PickUp` / `Place` pairs with 55 observed phase plans and 48 physical checkpoints. + +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 +pinches use a 0.7605 m TCP height. `PickUp` closes and settles with +`lift_height: 0`; its observed `material_lift` phase then raises the held +material while retracting opposite its measured 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 | 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.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.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. 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 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 +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 +`[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. 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 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 +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 +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 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 +45.15 seconds, including the final two-second settling interval. All released +endpoints stayed within the unchanged strict channel through retreat, home, +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 +raw robot/fixture contacts, the original spool/lift milestones and released-tip +persistence. Each qualification uses a fresh process. + +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. + +## Eight-call task and contact-aware pacing + +`task.yam.yaml` is the single supported deployment and registers +`RouteTheCable-YAM-v1`. The previous 57-segment / 148-call program and temporary +compact deployment files have been removed. The canonical program contains +three right-arm `PickUp`/`Place` pairs and one left-arm `PickUp`/`Place` pair. +The ordinary task entry point therefore runs the optimized task directly. + +The skills plan 55 observed phases within 8 execution sessions, with no nested +Atomic Skill invocation. The original 48 physical checks run at acknowledged +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.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 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.4642; robot +joint-speed limits remain unchanged. + +Newton/DexUni coupling, physics/control timesteps, rod and robot parameters, +scene, native contact handling and physical success criteria are preserved. +The source supplies geometry and evidence; the engine owns IK, command timing +and row-local execution. Failed rows are removed before duration reductions +and IK. References are acquired after acknowledged closing/settling and released +per row, without projecting a rigid cable/tool attachment. + +Observed continuation is bounded and shares one invocation timeout. Offline +`compile()`, external nominal/expanded trajectories, per-phase path overrides, +replay recovery and mid-operation invocation revisions are unsupported. The +runner owns cancellation and failed-row holds; `PLAN_CONTINUED` records every +installed continuation. Saved integration registrations must be regenerated +when their supported goal descriptors or pacing declarations change. 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 000000000..711b15d23 --- /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 000000000..217abe560 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/catalog.yaml @@ -0,0 +1,12 @@ +task_key: route_the_cable +title: Route the Cable +summary: Eight-call contact routing with fast pacing and preserved critical settling. +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 000000000..d4b80fd06 --- /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 000000000..b357fa33e --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml @@ -0,0 +1,28 @@ +id: RouteTheCable-YAM-v1 +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 +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 000000000..6d47a2728 --- /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 000000000..4cb78f69f --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.yaml @@ -0,0 +1,26 @@ +policy_id: cable_compact_fast_contact_v1 +preset_id: cable +requires: + embodiment_contract: dual_arm_parallel_gripper +planner: + type: trapezoidal +motion: + strategy: ik_interp + velocity_targets: auto + sample_count: 34 + interpolate_position_step: 0.0018500000000000003 + dynamic_collision_mode: 'off' + acceleration_limit: 2.4642 +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.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml new file mode 100644 index 000000000..c5758c449 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml @@ -0,0 +1,1263 @@ +integration_id: route_the_cable_compact_fast_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 + place: + primary: right_manipulator + skill_options: + simulation.cable_pickup: + kind: pick_up + hand_interp_steps: 22 + 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.2664 + port_screen: false + angular_speed: 1.11 + settle_motion: + kind: hold + samples: 7 + 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 + speed: 0.111 + angular_speed: 1.11 + 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 + speed: 0.111 + angular_speed: 1.11 + 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 + speed: 0.111 + angular_speed: 1.11 + lower_0_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + release_0: + kind: open + samples: 17 + finger_position: 0.008 + release_part: right_arm + retreat_0: + kind: command + position: + - null + - null + - 0.82 + speed: 0.1776 + angular_speed: 1.11 + 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: 17 + release_part: right_arm + right_approach_0.282: + kind: material + material_distance: 0.282 + position: + - null + - null + - 0.81 + speed: 0.2664 + port_screen: false + angular_speed: 1.11 + 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 + speed: 0.111 + angular_speed: 1.11 + 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 + speed: 0.111 + angular_speed: 1.11 + 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 + speed: 0.111 + angular_speed: 1.11 + lower_1_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + release_1: + kind: open + samples: 17 + finger_position: 0.008 + release_part: right_arm + retreat_1: + kind: command + position: + - null + - null + - 0.82 + speed: 0.1776 + angular_speed: 1.11 + 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: 17 + release_part: right_arm + right_approach_0.438: + kind: material + material_distance: 0.438 + position: + - null + - null + - 0.81 + speed: 0.2664 + port_screen: false + angular_speed: 1.11 + 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 + speed: 0.111 + angular_speed: 1.11 + 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 + speed: 0.111 + angular_speed: 1.11 + 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 + speed: 0.111 + angular_speed: 1.11 + lower_2_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + release_2: + kind: open + samples: 17 + finger_position: 0.008 + release_part: right_arm + retreat_2: + kind: command + position: + - null + - null + - 0.82 + speed: 0.1776 + angular_speed: 1.11 + 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: 17 + release_part: right_arm + home_right: + kind: home + speed: 0.333 + angular_speed: 1.11 + 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.2664 + port_screen: true + angular_speed: 1.11 + 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 + speed: 0.111 + angular_speed: 1.11 + 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.0555 + tolerance: 0.006 + stop_when_seated: true + angular_speed: 1.11 + 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.0444 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + angular_speed: 1.11 + 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.0444 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + angular_speed: 1.11 + 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.0555 + tolerance: 0.006 + stop_when_seated: true + angular_speed: 1.11 + 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.0444 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + angular_speed: 1.11 + 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.0444 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + angular_speed: 1.11 + 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.0555 + tolerance: 0.006 + stop_when_seated: true + angular_speed: 1.11 + 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.0444 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + angular_speed: 1.11 + 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.0444 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + angular_speed: 1.11 + 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 + speed: 0.111 + angular_speed: 1.11 + 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 + speed: 0.111 + angular_speed: 1.11 + 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.0555 + tolerance: 0.006 + stop_when_seated: true + angular_speed: 1.11 + 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: 28 + 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.0444 + tolerance: 0.006 + stop_when_seated: false + maximum_z_correction: 0.003 + angular_speed: 1.11 + 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.0444 + tolerance: 0.006 + stop_when_seated: false + maximum_z_correction: 0.003 + angular_speed: 1.11 + 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.0333 + tolerance: 0.006 + angular_speed: 0.5 + 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.0333 + tolerance: 0.006 + angular_speed: 0.5 + 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: 28 + 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.0555 + angular_speed: 1.11 + 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.1776 + angular_speed: 1.11 + raise_clear_of_port_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + home_left: + kind: home + speed: 0.333 + angular_speed: 1.11 + 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.0444 + tolerance: 0.006 + stop_when_seated: true + angular_speed: 1.11 + 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.0444 + tolerance: 0.006 + stop_when_seated: true + angular_speed: 1.11 + 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.0444 + tolerance: 0.006 + stop_when_seated: true + angular_speed: 1.11 + 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.0444 + tolerance: 0.006 + stop_when_seated: true + angular_speed: 1.11 + 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 + speed: 0.111 + angular_speed: 1.11 + 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: 17 + release_part: left_arm + settle_contact: + kind: hold + samples: 15 + settle_release_alignment: + kind: hold + samples: 60 + 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_contact + - kind: check + target: lay_down_free_end_0_tracking + - kind: pose + target: lay_down_free_end_1 + after: + - kind: hold + target: settle_release_alignment + - 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.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.yaml new file mode 100644 index 000000000..85fa9a521 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.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/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/__init__.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/__init__.py new file mode 100644 index 000000000..3818cfd8e --- /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 dd7b09a75..37429f7ec 100644 --- a/scripts/validation/accept_usd_import.py +++ b/scripts/validation/accept_usd_import.py @@ -169,7 +169,10 @@ def _accept_case(case: dict[str, Any], output: Path) -> dict[str, Any]: "An imported entity was lost or a source path is duplicated." ) expected_assets = ( - len(raw.mesh_objects) + len(raw.articulations) + len(raw.particle_sets) + len(raw.mesh_objects) + + len(raw.articulations) + + len(raw.particle_sets) + + len(raw.cables) ) record["resources"] = dict(imported.resource_uids) record["resource_source_paths"] = dict(imported.resource_source_paths) @@ -286,6 +289,12 @@ def check_state() -> None: ) elif type(asset).__name__ in ("Articulation", "Robot"): value = asset.get_qpos() + elif type(asset).__name__ == "CableObject": + value = asset.data.segment_state_w + if value.numel() == 0: + raise AssertionError( + f"Cable {asset.uid} has no simulation segments." + ) else: value = asset.get_local_pose() if not torch.isfinite(value).all(): diff --git a/setup.py b/setup.py index aee874174..203a535f3 100644 --- a/setup.py +++ b/setup.py @@ -178,6 +178,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 000000000..9b0820cf0 --- /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 a461f3e8d..63b0344cc 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 000000000..e722e79e6 --- /dev/null +++ b/tests/gym/envs/task_program/test_cable_material.py @@ -0,0 +1,1340 @@ +# ---------------------------------------------------------------------------- +# 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.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, +) +from embodichain.lab.task_program.integrations.simulation.cable import ( + _decode_cable_service, + _CableRuntime, + _CableCheckLowerer, + _CableJointLowerer, + _CableJointFactory, + _CableMoveFactory, + _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(): + value = yaml.safe_load((_TASK / "task_program/integration.yaml").read_text())[ + "runtime_services" + ]["cable_material"] + value.pop("operations", None) + return value + + +def _deployment(*, legacy_checks=False): + path = _TASK / "task.yam.yaml" + raw = yaml.safe_load(path.read_text()) + physical = resolve_gym_components(raw, base_dir=path.parent) + selection = deepcopy(physical.config["task_program"]) + if legacy_checks: + declaration = yaml.safe_load( + (_TASK / "task_program/integration.yaml").read_text() + ) + declaration["profile"]["skill_options"].update( + { + call_id: {"kind": "move_joints", "preserve_samples": True} + for call_id in ("simulation.cable_joint", "simulation.cable_check") + } + ) + 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, + ) + physical.validate_scene_bindings(deployment.scene_binding) + return deployment + + +def test_cable_preflight_has_eight_calls_and_retains_physical_checks(): + deployment = _deployment() + 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" + ) + geometry = dict(cfg.geometry) + assert geometry["strict_region"] == ( + (0.5845, 0.064, 0.753), + (0.5875, 0.0896, 0.759), + ) + assert cfg.slip_limit == 0.012 + targets = {target.target_id: target for target in cfg.targets} + assert targets["align_tip_3_1"].maximum_correction == 0.005 + assert targets["align_tip_3_1"].minimum_z == 0.7655 + assert targets["lay_down_free_end_0"].minimum_z == 0.7625 + 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 { + name: targets[name].samples + 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" + if phase.target.endswith("_0") + else "settle_release_alignment" + ) + 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.yaml" + physical = resolve_gym_components( + yaml.safe_load(path.read_text()), base_dir=path.parent + ) + declaration = yaml.safe_load((_TASK / "task_program/integration.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"] +) +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) + + +@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, + ) + + 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("kind", ["material", "material_lift", "ray_align"]) +def test_geometry_failure_is_row_local_and_cannot_lengthen_a_healthy_path(kind): + service, state, measured = _runtime() + value = _declaration() + spec = {"kind": kind, "material_distance": 0.162} + if kind in {"material", "material_lift"}: + spec["position"] = [None, None, 0.78] + value["targets"] = {"mixed": spec} + service.cfg = _decode_cable_service(value) + if kind == "material": + + def ik(pose, *, joint_seed, env_ids, **kwargs): + return env_ids == 3, joint_seed.clone() + + service.robot.compute_ik = ik + elif kind == "material_lift": + state[1, 13, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0]) + else: + state[3, -1, 1] = 0.1 + first = _lower_pitch(service, "mixed") + assert set(first.failed_preconditions) == {1} + assert torch.isfinite(first.goal.xpos).all() + # Only the failed physical row changes; healthy env 3 keeps the same path/grid. + measured[1] = torch.tensor([100.0, 20.0, 30.0]) + second = _lower_pitch(service, "mixed") + assert set(second.failed_preconditions) == {1} + torch.testing.assert_close(first.goal.xpos[0], second.goal.xpos[0]) + + +def test_contact_source_preserves_reachable_peer_when_one_wrist_has_no_ik(): + from embodichain.lab.task_program.integrations.simulation._cable_contact import ( + CableContactSource, + ) + from embodichain.lab.sim.atomic_skills.primitives.contact import ContactPhase + + service, _, measured = _runtime() + service.robot.compute_ik = lambda pose, *, joint_seed, env_ids, **kwargs: ( + env_ids == 3, + joint_seed.clone(), + ) + source = CableContactSource(service) + target = SimpleNamespace(control_part="right_arm", joint_ids=(0, 1, 2)) + request = SimpleNamespace( + goal=None, + invocation_id="mixed-ik", + binding=SimpleNamespace(endpoint=lambda *args: SimpleNamespace(target=target)), + ) + context = SimpleNamespace( + batch_size=2, + env_ids=service.env_ids, + robot=SimpleNamespace(qpos=measured[service.env_ids]), + require_control_dt=lambda: 1 / 60, + ) + resolved = source.resolve( + ContactPhase("pose", "right_approach_0.162"), + request, + context, + torch.tensor([True, True]), + ) + assert resolved.valid.tolist() == [True, False] + assert torch.isfinite(resolved.values).all() + assert service.last_targets == {} + + +@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_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 + 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] + + +@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", *, active_mask=None): + 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, + planning_mask=active_mask, + ), + bound=bound, + option_template=MoveEndEffectorOptions(), + ) + + +def test_inactive_legacy_move_row_cannot_lengthen_healthy_trajectory(): + service, _, measured = _runtime() + value = _declaration() + value["targets"] = {"home": {"kind": "home"}} + service.cfg = _decode_cable_service(value) + active = torch.tensor([False, True]) + first = _lower_pitch(service, "home", active_mask=active).goal.xpos + measured[3, 0] = 0.5 + second = _lower_pitch(service, "home", active_mask=active).goal.xpos + assert first.shape[1] == second.shape[1] == 2 + torch.testing.assert_close(first[1], second[1]) + torch.testing.assert_close( + second[0], service.tool("left_arm")[0].expand_as(second[0]) + ) + + +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)], +) +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 + 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_check", arguments={"target": target} + ), + 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"]) +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])} + 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": + 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)]] + ) + 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(legacy_checks=True) + full = deployment.load_program() + first = full.program.items[0] + hold = replace( + first.steps, + call=replace( + first.steps.call, + call_id="simulation.cable_joint", + arguments={"target": "settle_motion"}, + ), + ) + check = replace( + first.steps, + call=replace( + first.steps.call, + call_id="simulation.cable_check", + arguments={"target": "right_approach_0.162_tracking"}, + ), + ) + 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(): + 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 dc237d6fb..0836d645a 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. + # Extending the accepted PickUp/Place goal descriptors intentionally changes + # registration identity. The handover program and inactive options stay fixed. assert ( registration.fingerprint - == "61cf38ce9128dfcc7063cec3933d900674863facbc6b244f31dec07ea24ca730" + == "d9cd591643cf8c29cbad04e532c6743208b78315a484796e90595d03ad98e6a8" ) assert ( integration.integration_fingerprint - == "d8d328c33f7149a5ed76b861bead1155977ed7f4ef213c9ceae560b36c20034e" + == "785a9307300d193082e945cee747d37a688b0a7ba3a09d6569b418553b44379e" ) compiled = registration.catalog.preflight(deployment.load_program()) assert ( 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 4996eb218..97451f548 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,17 @@ 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. The added + # contact-path goal types intentionally extend those descriptors; restoring + # the pre-continuation types independently reproduces the previous goldens. assert ( registration.fingerprint - == "c8767354951735e47897e046ba35bc26b8e8f47433915174655d4c5247ec2a88" + == "8c26f478773895d74399f6f50e7784625719062b20ce76dc3c3c94b8ac7a7005" ) assert ( integration.integration_fingerprint - == "aede0eed53368155b3d58bf288188079f9278fd883e342197ee38c101123b7da" + == "a40a3110b8db70c9ce1b4a1879dc41f12c0dd876dce09e1844f2dc01c4ebc8d7" ) assert tuple(registration.catalog.registered_semantic_lowerer_declarations) == ( _OPEN_DRAWER_CALL_ID, 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 000000000..773b5f645 --- /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 000000000..7f41f78c7 --- /dev/null +++ b/tests/gym/envs/tasks/test_route_the_cable_cuda.py @@ -0,0 +1,438 @@ +# ---------------------------------------------------------------------------- +# 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.gym.envs.demo import execute_demo_episode +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() + released = False + 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 == "right_place_0": + released = True + 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. + assert released, "The expert did not complete the first release" + 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 = {"right_pick_2", "right_place_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 == "right_pick_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 == "right_place_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", "home_left"} + completed_boundaries: set[str] = set() + stage_metadata = [] + release_started = False + wrapped = False + lower = torch.tensor((0.5845, 0.064, 0.753), device=env.device) + upper = torch.tensor((0.5875, 0.0896, 0.759), device=env.device) + + def assert_endpoint() -> None: + endpoint = _material_runtime(env).metrics()["end_position"] + assert bool(((endpoint >= lower) & (endpoint <= upper)).all()), endpoint + + def observe(active, action, observation) -> None: + nonlocal worst_distance, worst_segment, release_started, wrapped + service = _material_runtime(env) + source = service._contact_source + assert source is not None + if not wrapped: + original_finish = source.finish + + def finish(cursor, context, mask): + result = original_finish(cursor, context, mask) + if cursor.phase_name in required_boundaries: + assert_endpoint() + completed_boundaries.add(cursor.phase_name) + for axis, value in zip( + "xyz", service.metrics()["end_position"][0], strict=True + ): + record_property( + f"{cursor.phase_name}_endpoint_{axis}_m", float(value) + ) + return result + + source.finish = finish + wrapped = True + if source.current.phase_name == "release_free_end": + release_started = True + if release_started: + assert_endpoint() + contacts = query.fetch_solver_report() + assert contacts.dropped_count == 0 + pairs = contacts.actor_ids[: contacts.count] + selected = torch.isin(pairs, robot_ids).any(-1) & torch.isin( + pairs, obstacle_ids + ).any(-1) + distances = contacts.distance[: contacts.count][selected] + if distances.numel() and float(distances.min()) < worst_distance: + worst_distance = float(distances.min()) + worst_segment = source.current.phase_name + assert worst_distance >= -0.001, (worst_segment, worst_distance) + + 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 + assert bool((qpos[:, env.robot.get_joint_ids(f"{side}_eef")] > 0.045).all()) + + result = execute_demo_episode( + env, + step_observer=observe, + on_segment_completed=lambda stage: stage_metadata.append( + stage.to_metadata() + ), + ) + assert result.completed and all(result.success), result.to_metadata() + assert len(stage_metadata) == 8 + assert stage_metadata[-1]["name"] == "left_place" + assert completed_boundaries == required_boundaries + events = [ + event + for stage in stage_metadata + for event in stage["metadata"]["runtime"]["events"] + ] + assert sum(event["kind"] == "skill_planned" for event in events) == 8 + starts = [ + event + for event in events + if event["kind"] == "trajectory_segment_entered" + and (event.get("segment_name") or "").startswith( + "hold:settle_and_validate:" + ) + ] + assert len(starts) == 1 + ends = [ + event["timestamp"] + for event in events + if event["kind"] == "trajectory_completed" + and event["invocation_id"] == starts[0]["invocation_id"] + and event["timestamp"] >= starts[0]["timestamp"] + ] + settling_seconds = min(ends) - starts[0]["timestamp"] + record_property("settling_seconds", settling_seconds) + assert settling_seconds >= 2.0 - 1e-9 + record_property("full_route_robot_obstacle_distance_m", worst_distance) + record_property("task_seconds", float(env.sim.simulation_time)) + assert 42 <= env.sim.simulation_time <= 46 + # Continue real physics after completion; all additional samples must stay seated. + for _ in range(round(5.0 / env.step_dt)): + env.sim.update( + physics_dt=env.physics_dt, + step=env.cfg.sim_steps_per_control, + after_substep=lambda dt: assert_endpoint(), + render_final_step=False, + ) + measurements = _material_runtime(env).metrics() + 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 81c39b5ea..41fd6750a 100644 --- a/tests/gym/envs/test_cable_recording_cuda.py +++ b/tests/gym/envs/test_cable_recording_cuda.py @@ -30,7 +30,7 @@ pytestmark = [pytest.mark.subprocess_sim, pytest.mark.gpu] -def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( +def test_usd_and_procedural_cables_record_and_replay_across_arenas( tmp_path: Path, ) -> None: # A fresh process preserves NoRender without the shared test engine's @@ -44,8 +44,32 @@ def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( """, encoding="utf-8", ) + usd = tmp_path / "wire.usda" + usd.write_text( + """#usda 1.0 +( + defaultPrim = "World" + metersPerUnit = 1 + upAxis = "Z" +) + +def Xform "World" +{ + def BasisCurves "wire" ( + prepend apiSchemas = ["PhysicsCurvesDeformableSimAPI"] + ) + { + int[] curveVertexCounts = [3] + point3f[] points = [(0, 0, 1), (0.1, 0, 1), (0.2, 0, 1)] + uniform token type = "linear" + uniform token wrap = "nonperiodic" + } +} +""", + encoding="utf-8", + ) result = subprocess.run( - [sys.executable, str(Path(__file__).resolve()), str(urdf)], + [sys.executable, str(Path(__file__).resolve()), str(urdf), str(usd)], env=os.environ.copy(), capture_output=True, text=True, @@ -54,7 +78,7 @@ def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( assert result.returncode == 0, result.stdout + result.stderr -def _run_recording(urdf: str) -> None: +def _run_recording(urdf: str, usd: str) -> None: import torch from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg @@ -66,10 +90,13 @@ def _run_recording(urdf: str) -> None: NewtonPhysicsCfg, RenderCfg, RobotCfg, + UsdImportCfg, + UsdSourceCfg, ) cfg = EmbodiedEnvCfg( num_envs=2, + enable_ground_plane=True, max_episode_steps=4, sim_steps_per_control=1, ignore_terminations=True, @@ -90,6 +117,11 @@ def _run_recording(urdf: str) -> None: solver_cfg={"solver_type": "dexuni"}, ), ), + usd_scene=UsdImportCfg( + name="scene", + source=UsdSourceCfg(file_path=usd), + asset_cfgs={"/World/wire": {"uid": "wire", "collision_filter_span": 2}}, + ), robot=RobotCfg( uid="slider", fpath=urdf, @@ -98,12 +130,11 @@ def _run_recording(urdf: str) -> None: ), cable_object=[ CableObjectCfg( - uid=uid, - points=[[0.1 * index, 0, 0] for index in range(segments + 1)], + uid="long_wire", + points=[[0.1 * index, 0, 0] for index in range(4)], radius=0.005, - init_pos=(0.0, y, 1.0), + init_pos=(0.0, 0.5, 1.0), ) - for uid, segments, y in (("wire", 2, 0.0), ("long_wire", 3, 0.5)) ], observations={ "wire": ObservationCfg( @@ -121,11 +152,16 @@ def _run_recording(urdf: str) -> None: env = None try: env = EmbodiedEnv(cfg) + cable = env.usd_scene_import["/World/wire"] + assert cable is env.sim.get_cable_object("wire") + assert cable.cfg.collision_filter_span == 2 assert not env.sim.has_native_renderer obs, _ = env.reset(seed=73) manager = env.observation_manager assert manager is not None and manager.has_history cables = {uid: env.sim.get_cable_object(uid) for uid in cfg.trajectory_uids} + assert cables["wire"].data.num_segments == 2 + assert cables["long_wire"].data.num_segments == 3 fields = {uid: f"cables/{uid}/segment_state_w" for uid in cables} episodes = env.episode_context.episode_ids expected = { @@ -225,6 +261,12 @@ def _run_recording(urdf: str) -> None: assert saved["meta"]["arena_origins_w"] == [source_origin.tolist()] for uid, field in fields.items(): assert saved["raw_observations"]["fields"][field]["values"].shape[1] == 4 + torch.testing.assert_close( + saved["terminal_states"]["cables", uid, "segment_state_w"].cpu(), + latest.values[field][[1]], + rtol=0, + atol=0, + ) env.close(exit_process=False) env = None @@ -258,4 +300,4 @@ def _run_recording(urdf: str) -> None: if __name__ == "__main__": faulthandler.enable() faulthandler.dump_traceback_later(120) - _run_recording(sys.argv[1]) + _run_recording(sys.argv[1], sys.argv[2]) diff --git a/tests/lab/evaluation/test_rule_graph.py b/tests/lab/evaluation/test_rule_graph.py index 297fe938f..df3e2dbe4 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/lab/task_program/test_semantic_compiler.py b/tests/lab/task_program/test_semantic_compiler.py index 786919f89..2f907f9a5 100644 --- a/tests/lab/task_program/test_semantic_compiler.py +++ b/tests/lab/task_program/test_semantic_compiler.py @@ -232,6 +232,7 @@ class _InspectLowerer(RegisteredSemanticLowerer): def __init__(self) -> None: self.option_templates: list[SkillOptions] = [] + self.planning_masks: list[torch.Tensor | None] = [] def lower( self, @@ -241,7 +242,8 @@ def lower( bound: object, option_template: SkillOptions, ) -> SemanticLowering: - del call, context, bound + del call, bound + self.planning_masks.append(context.planning_mask) self.option_templates.append(option_template) return SemanticLowering( goal=GraspGoal( @@ -1344,7 +1346,9 @@ def test_registered_call_without_monitor_has_no_effect_contract() -> None: ) workflow = compiler.analyze((RegisteredSemanticCall(call_id="vendor.inspect"),)) - grounded = compiler.ground(workflow, 0, _context(registry)) + mask = torch.tensor([True, False]) + grounded = compiler.ground(workflow, 0, _context(registry), eligible_mask=mask) + assert torch.equal(lowerer.planning_masks[0], mask) assert workflow.calls[0].symbolic_writes == frozenset() assert workflow.calls[0].opaque_symbolic_effect diff --git a/tests/sim/atomic_skills/test_actions.py b/tests/sim/atomic_skills/test_actions.py index fc3defc09..d0940a379 100644 --- a/tests/sim/atomic_skills/test_actions.py +++ b/tests/sim/atomic_skills/test_actions.py @@ -954,10 +954,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 - assert PickUp.GoalType is GraspGoal + from embodichain.lab.sim.atomic_skills import ( + ContactGraspGoal, + ContactPickUpGoal, + ContactPlaceGoal, + ) + + 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 @@ -2448,7 +2454,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() @@ -2466,9 +2477,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( @@ -2485,6 +2500,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 = ( @@ -5508,7 +5527,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", @@ -10061,3 +10079,411 @@ 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 + ) + + +@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 = [] + 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/atomic_skills/test_contact_paths.py b/tests/sim/atomic_skills/test_contact_paths.py new file mode 100644 index 000000000..1068d856f --- /dev/null +++ b/tests/sim/atomic_skills/test_contact_paths.py @@ -0,0 +1,396 @@ +# ---------------------------------------------------------------------------- +# 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_rejects_phase_path_overrides_before_observing_geometry(): + source = _Source() + engine, invocation, context = _setup(source) + policy = replace( + invocation.motion_policy, + interpolation_mode="fixed_grid", + acceleration_limit=None, + ik_seed_mode="sequential", + phase_paths={"approach": "cartesian"}, + ) + with pytest.raises(ValueError, match="fixed sampling policy"): + engine.plan(replace(invocation, motion_policy=policy), context) + assert source.masks == [] + + +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) diff --git a/tests/sim/objects/test_cable_object.py b/tests/sim/objects/test_cable_object.py index d8a4d98b3..8c78e4cc6 100644 --- a/tests/sim/objects/test_cable_object.py +++ b/tests/sim/objects/test_cable_object.py @@ -390,11 +390,10 @@ def add_cable(): source=UsdSourceCfg(file_path=str(_write_cable_usd(tmp_path))), ) ) - assert dict(imported) == {} + cable = imported["/Cable"] assert list(imported.resource_types.values()) == ["cable"] assert len(builder.cables) == 1 - assert not sim._cable_objects and not scene._assets - return None + return cable if cable_first: first = add_cable() diff --git a/tests/sim/objects/test_cable_object_cuda.py b/tests/sim/objects/test_cable_object_cuda.py index 2a25e8594..7904112f5 100644 --- a/tests/sim/objects/test_cable_object_cuda.py +++ b/tests/sim/objects/test_cable_object_cuda.py @@ -226,6 +226,92 @@ def test_imported_particles_survive_rejected_cable(self, tmp_path: Path) -> None torch.testing.assert_close(reset[0], moved[0], rtol=0, atol=0) torch.testing.assert_close(reset[1], initial[1]) + def test_usd_cable_state_selective_reset_and_viser(self, tmp_path: Path) -> None: + from embodichain.lab.visualization import SceneExporter, VisualizationCfg + + path = tmp_path / "wire.usda" + path.write_text( + """#usda 1.0 +( + defaultPrim = "World" + metersPerUnit = 1 + upAxis = "Z" +) +def Xform "World" { + def BasisCurves "Wire" ( + prepend apiSchemas = ["PhysicsCurvesDeformableSimAPI"] + ) { + int[] curveVertexCounts = [3] + point3f[] points = [(0, 0, 0), (0.1, 0, 0), (0.3, 0, 0)] + uniform token type = "linear" + uniform token wrap = "nonperiodic" + float[] physics:thicknesses = [0.02] + token physics:thicknesses:elementType = "constant" + double3 xformOp:translate = (0.25, 0.5, 1) + uniform token[] xformOpOrder = ["xformOp:translate"] + } +} +""", + encoding="utf-8", + ) + imported = self.sim.add_usd( + UsdImportCfg(name="wire", source=UsdSourceCfg(file_path=str(path))) + ) + key = "/World/Wire" + cable = imported[key] + uid = imported.resource_uids[key] + assert self.sim.get_asset(uid) is self.sim.get_cable_object(uid) is cable + assert cable.num_instances == 2 and cable.data is None + + self.sim.prepare() + initial = cable.data.default_segment_state_w + assert initial.shape == (2, 2, 13) + expected_centres = initial.new_tensor([[0.3, 0.5, 1], [0.45, 0.5, 1]]) + torch.testing.assert_close( + initial[..., :3] - self.sim.arena_offsets[:, None, :], + expected_centres.expand(2, 2, 3), + atol=1e-6, + rtol=0, + ) + + self.sim.update(step=3, render_final_step=False) + moved = cable.data.segment_state_w + assert torch.all(moved[..., 2] < initial[..., 2]) + self.sim.reset_objects_state(env_ids=[1]) + reset = cable.data.segment_state_w + torch.testing.assert_close(reset[0], moved[0], rtol=0, atol=0) + torch.testing.assert_close(reset[1], initial[1], rtol=0, atol=0) + + state = reset[[1]].clone() + state[..., 0] += 0.125 + cable.write_segment_state_to_sim(state, env_ids=[1]) + torch.testing.assert_close( + cable.data.segment_state_w[0], moved[0], rtol=0, atol=0 + ) + torch.testing.assert_close(cable.data.segment_state_w[[1]], state) + + exporter = SceneExporter( + self.sim, VisualizationCfg(backend="viser", env_ids=[1]) + ) + manifest = exporter.build_manifest() + frame = exporter.capture(sim_step=3, sim_time=self.sim.simulation_time).frame + nodes = [node for node in manifest.nodes if node.kind == "cable_segment"] + assert len(nodes) == 2 + geometries = { + geometry.geometry_id: geometry for geometry in manifest.geometries + } + for segment_index, node in enumerate(nodes): + assert node.env_id == 1 + assert exporter.resolve_node_target(node.node_id) == (uid, "cable") + index = frame.node_ids.index(node.node_id) + pose = state[0, segment_index].cpu().numpy() + np.testing.assert_allclose(frame.positions[index], pose[:3], atol=1e-6) + np.testing.assert_allclose(frame.wxyz[index], pose[[6, 3, 4, 5]], atol=1e-6) + vertices = geometries[node.geometry_id].vertices + np.testing.assert_allclose( + np.ptp(vertices, axis=0), [0.02, 0.02, (0.12, 0.22)[segment_index]] + ) + def test_attached_articulation_expert_trajectory_repeats_after_reset( self, tmp_path: Path ) -> None: @@ -451,6 +537,11 @@ def test_imported_particles_survive_rejected_cable( "test_imported_particles_survive_rejected_cable", volume_particle_usd.parent ) + def test_usd_cable_state_selective_reset_and_viser(self, tmp_path: Path) -> None: + _assert_cable_case_in_subprocess( + "test_usd_cable_state_selective_reset_and_viser", tmp_path + ) + @pytest.mark.usefixtures("requires_dexuni_two_way") def test_attached_articulation_expert_trajectory_repeats_after_reset( self, tmp_path: Path diff --git a/tests/sim/objects/test_visual_object.py b/tests/sim/objects/test_visual_object.py index 9ca3855e8..2aa9945e1 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/sim/robots/test_yam_i2rt.py b/tests/sim/robots/test_yam_i2rt.py index c4360a0e2..e153b2c15 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 diff --git a/tests/sim/spawn/test_usd_scene_overrides.py b/tests/sim/spawn/test_usd_scene_overrides.py index 723db485a..3296d28f0 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 diff --git a/tests/test_task_program_package_data.py b/tests/test_task_program_package_data.py index 00ecd06ec..53f7e247f 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",