diff --git a/NOTICE b/NOTICE index e2a23acf3d..33cc37edf2 100644 --- a/NOTICE +++ b/NOTICE @@ -1,6 +1,11 @@ EmbodiChain Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +The cable routing application includes geometry and routines adapted from +RoboSimStudio, Copyright 2026 Kaiyuan Chen, licensed under Apache-2.0. +Source: https://github.com/robosimstudio/robosimstudio +Revision: 73739abe212f963e8a9f5862b27bca370df091a7 + The external YAM i2rt model contains the YAM arm and linear_4310 gripper, Copyright (c) I2RT Robotics, under the MIT license. The source model directory and RoboSimStudio NOTICE identify this attribution and license. diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 89ca0ee993..ddfe6fd4b5 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -438,7 +438,7 @@ topics: - id: atomic-actions title: Atomic Skills aliases: [atomic skill, atomic skills, atomic action, 原子技能, 原子动作] - keywords: [PlanningBudget, PlanningRecovery, ExecutionPlanLifecycle, AtomicSkillEngine, run-atomic-skill, PlanningContext, ExecutionSession, ExecutionRunner, SkillInvocation, + keywords: [PlanningBudget, PlanningRecovery, ExecutionPlanLifecycle, PickUp, GraspGoal, ContactGraspGoal, AtomicSkillEngine, run-atomic-skill, PlanningContext, ExecutionSession, ExecutionRunner, SkillInvocation, hand_geometry_cfgs, GraspOpeningTarget, prepare_grasp_query, scoped grasp preparation, RecoveryPolicy, HeldObjectGuardRequest, PhaseEffectGateRequest, articulation_geometry, control-grid retiming, AffordanceSamplingContext, AffordancePoseCandidates, AffordanceSample, affordance sampling] diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index f8927e9e9b..ac5475e91f 100644 --- a/agent_context/topics/atomic-actions/atomic-actions.md +++ b/agent_context/topics/atomic-actions/atomic-actions.md @@ -24,6 +24,18 @@ Hosts acquire versioned measurements; verification is the boundary that commits or invalidates observed effects. Preserve required stop/hold boundaries on the executed control grid instead of relying only on native planner timing. +`PickUp` accepts a rigid `GraspGoal` or an explicit `ContactGraspGoal`. +The rigid path projects its existing held-object relation. Contact pickup +requires a free task-state resource, shares the approach/close/lift command +assembly and declares `EffectVerificationRequirement(kind="contact_grasp")` +without a rigid held-object update. Hosts supply physical evidence such as +material-point lift and tool-frame retention before accepting that boundary. +The routing host samples its material reference at the end of the `close` +phase, including `grasp_settle_steps`, before lifting. Native contact IK paths +use `MotionPolicy.interpolate_position_step` for lift sampling on the control +grid; joint limits can lengthen them. Numerical material-coordinate geometry +belongs to `compute/geometry/cable.py`. + ## Package map | Concern | Source of truth | diff --git a/agent_context/topics/atomic-actions/execution.md b/agent_context/topics/atomic-actions/execution.md index a1f9c0f8b9..79f251ea92 100644 --- a/agent_context/topics/atomic-actions/execution.md +++ b/agent_context/topics/atomic-actions/execution.md @@ -192,9 +192,11 @@ physically qualified task needs a lower speed than the selected robot's limits. default) or `zero` (explicit position-only control). Configured execution policies decode the same field under `motion`. `AtomicSkill.build_plan` derives missing velocities after composite skills -have assembled their final full-robot positions and timing. The first command, -both ends of every stationary joint interval, and terminal settling commands -use zero velocity; failed rows remain held. +have assembled their final full-robot positions and timing. By default the first +command, both ends of a stationary joint interval and terminal settling commands +use zero velocity. For consecutive explicitly timed trajectories, +`MoveJointsOptions.preserve_velocity_samples` preserves the supplied arrival +velocities across phase boundaries. Failed rows remain held. `SimulationExecutionAdapter` always clears absent velocity references to zero; it never substitutes observed velocities or depends on peer active masks. diff --git a/agent_context/topics/simulation-system/deformables.md b/agent_context/topics/simulation-system/deformables.md index 165ad49dfe..db52a5268f 100644 --- a/agent_context/topics/simulation-system/deformables.md +++ b/agent_context/topics/simulation-system/deformables.md @@ -108,8 +108,14 @@ Focused coverage: `tests/sim/test_deformable_cfg.py`, `CableObjectCfg` and `CableObject(BatchEntity)` are independent of particle-set deformables. `SimulationManager.add_cable_object()` lowers through `spawn/descriptors.py` into `CableDesc`, declares in the existing `SpawnScene`, -and binds the same facade in place during `prepare()`. Backend capabilities -reject Default, CPU, other Newton solvers and gradient mode; use DexUni on CUDA. +and binds the same facade in place during `prepare()`. `add_usd(UsdImportCfg)` +also returns a `CableObject` per imported cable under its source resource key, +with the same UID lookup, segment state and reset as a procedural declaration. +Cable geometry, placement and visuals come from the parsed `CableDesc`. +A sparse `asset_cfgs` entry keyed by the simulation curve prim can override +the UID, `fixed_segments`, `collision_filter_span` and individual `attrs` physics and +contact fields before declaration. Omitted fields retain the parsed values. +Cable declarations require DexUni on CUDA with gradient mode disabled. With this backend, an arena containing a cable cannot also contain fluid, cloth or soft-body particles: cable episode reset does not support those particles. Spawn checks the native cable and particle descriptors for direct diff --git a/agent_context/topics/task-programs/configuration.md b/agent_context/topics/task-programs/configuration.md index 5282a66a48..861b1c140d 100644 --- a/agent_context/topics/task-programs/configuration.md +++ b/agent_context/topics/task-programs/configuration.md @@ -203,6 +203,13 @@ the closed-field decoder. Omitting `velocity_limit` preserves the Robot's ordered per-joint speed caps; an explicit scalar request is intersected with those caps rather than replacing them. +Configured motion accepts `interpolate_position_step` for native Cartesian +sampling. `MoveEndEffectorOptions.preserve_cartesian_samples` retains authored +Cartesian samples; `MoveJointsOptions.preserve_samples` consumes explicitly +sampled joint commands at the host cadence using `ik_interp`. These declarations +retain normal Atomic velocity qualification and do not certify collision-free +motion. Inactive sampling additions preserve existing integration fingerprints. + Configured motion also accepts explicit `interpolation_mode` and `ik_seed_mode`. `fixed_grid` requires `acceleration_limit: null` and no scalar speed override; omission retains the existing adaptive policy and acceleration default. @@ -217,6 +224,25 @@ declarations in the integration and timing in the execution policy, rather than placing a precomputed bottle world pose in the program. Omitting `target` retains the original internal-axis pouring behavior. +`scene_binding.cables` declares material-state native identities without rigid +object semantics or root-pose providers. The allowlisted +`runtime_services.cable_material` binds those identities to measured material +coordinates and closed target declarations. Its calls lower to existing +`PickUp(ContactGraspGoal)`, `MoveEndEffector`, and `MoveJoints` skills; the program +owns stage ordering. The bridge captures contact references only after an +acknowledged close frame and checks material slip after every consumed action. +References are fresh per bridge, guarded by its episode scope, and mapped from +selected robot rows to native cable arenas. Evaluation consumes qualified +`cables//` channels, including strict endpoint geometry and +episode slip. Keep routing acceptance in the deployment's named evaluation rules. + +`FrankaRopeWrap-v1` uses the registered `simulation.cable_wrap_phase` call. +Each phase lowers its checked portion of the full Franka arm and finger trajectory +to `MoveJoints`. Scene reset owns pre-roll. The monitor latches its slip reference +after closure and a simultaneous two-finger contact sample. The final +`wrap_success` condition reads all 19 measured checks. Task components live +under `configs/tasks/manipulation/cable/franka_rope_wrap/` in the task extension. + The configured lowerer allowlist also exposes profile-owned Park, AxisAlign, relative Place and coordinated transport. Their declarations in `integrations/_configured_services.py` select exact Atomic Skill descriptors; diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 6c413b5227..9e6365c040 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -33,6 +33,20 @@ Offline job preflight, run, resume and verified inspection commands. main +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 -------------------------------------- @@ -3156,6 +3170,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 ----------------------------------------------- @@ -4763,6 +4793,88 @@ reference-state resets and terminal snapshots before automatic row resets. .. automodule:: embodichain_tasks.locomotion.tracking.g1_motion :members: +embodichain_tasks.manipulation.cable.franka_rope_wrap +----------------------------------------------------- + +Build the Franka floor-pickup scene. The configuration defines the cable centerline, +fixed first segment, post and jaw geometry. The reset functor settles the cable +before the motion clock starts. + +.. currentmodule:: embodichain_tasks.manipulation.cable.franka_rope_wrap + +.. autosummary:: + + cable_points + load_config + materialize_deployment + prepare_models + settle_franka_rope + task_directory + +embodichain_tasks.manipulation.cable.route_the_cable +---------------------------------------------------- + +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.cable.wrap_evaluation +---------------------------------------------------- + +Attach episode-scoped contact sampling after the scene settles. The monitor records +both fingers at the same physical sample and tracks slip of the grasped material point. + +.. currentmodule:: embodichain_tasks.manipulation.cable.wrap_evaluation + +.. autosummary:: + + attach_wrap_monitor + +embodichain_tasks.manipulation.cable.wrap_integration +----------------------------------------------------- + +Bind a named wrap phase to the full-robot joint position and velocity commands +executed by the standard Task Program bridge. + +.. currentmodule:: embodichain_tasks.manipulation.cable.wrap_integration + +.. autosummary:: + + CableWrapPhaseFactory + +embodichain_tasks.manipulation.cable.wrap_measurements +------------------------------------------------------ + +Evaluate recorded control frames against all 19 physical acceptance checks, +including bilateral grasp, line winding, post contact and the final hold. + +.. currentmodule:: embodichain_tasks.manipulation.cable.wrap_measurements + +.. autosummary:: + + evaluate_wrap + +embodichain_tasks.manipulation.cable.wrap_motion +------------------------------------------------ + +Plan the continuous tool path and solve its joint targets in time order. +The phase boundaries partition the trajectory on the configured control grid. +The first sample describes the settled start at zero seconds. + +.. currentmodule:: embodichain_tasks.manipulation.cable.wrap_motion + +.. autosummary:: + + PHASE_END_S + WrapMotion + plan_wrap_motion + embodichain_tasks.manipulation.push_cube ---------------------------------------- diff --git a/docs/source/api_reference/robot_catalogs.rst b/docs/source/api_reference/robot_catalogs.rst index 9179bd9eff..bd9ced2287 100644 --- a/docs/source/api_reference/robot_catalogs.rst +++ b/docs/source/api_reference/robot_catalogs.rst @@ -629,6 +629,13 @@ and cache prefixes while using the shared core download and locking services. .. autoclass:: embodichain_robots.franka.assets.FrankaPandaArm +.. autoclass:: embodichain_robots.franka.assets.FrankaRopePads + + The cable-wrap embodiment resolves the flat-pad model through + ``FrankaRopePads/Panda/Fr3RopePadsV1.urdf``. + +.. autofunction:: embodichain_robots.franka.rope_pads.apply_rope_pads + .. autofunction:: embodichain_robots.robotiq.two_finger.robotiq85_joint_positions diff --git a/docs/source/overview/sim/atomic_skills/builtin_skills.md b/docs/source/overview/sim/atomic_skills/builtin_skills.md index d487dd9b03..b585dd973b 100644 --- a/docs/source/overview/sim/atomic_skills/builtin_skills.md +++ b/docs/source/overview/sim/atomic_skills/builtin_skills.md @@ -195,7 +195,7 @@ The animations below are the runnable simulator demonstrations under |---|---|---|---|---|---| | `move_end_effector` | `EndEffectorPoseGoal` | `primary.motion` | none | none | none | | `move_joints` | `JointPositionGoal` | `primary.motion` | named target only: command matching `target` on `primary.motion` | none | none | -| `pick_up` | `GraspGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | semantic object/entity | attach object to the `primary.motion` target | +| `pick_up` | `GraspGoal` or `ContactGraspGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | semantic object or explicit contact pose | rigid held relation or terminal contact verification | | `axis_align` | `AxisAlignGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `open`, `grasp` | unheld object with `AxisAlignAffordance` | open-loop pick and align while retaining the grasp | | `move_held_object` | `HeldObjectPoseGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `grasp` | object held exclusively by the `primary.motion` target | preserve attachment | | `pour` | `PourGoal` | `primary.motion`, `primary.grasp` | `primary.grasp`: `grasp` | exclusively held object with `AxisAlignAffordance` | preserve attachment; open-loop rotate and return | @@ -410,17 +410,31 @@ named_goal = JointPositionGoal(target="home") ## `PickUp` -Plans **approach -> close hand -> lift** and declares the object attached to the -bound motion target. +Plans **approach -> close hand -> lift**. A rigid `GraspGoal` declares a held-object +relation for the bound motion target. `ContactGraspGoal(grasp_xpos=...)` uses an +observed TCP pose and requires physical contact verification, with no fixed +object-to-tool relation. | Contract | Value | |---|---| | Skill ID | `pick_up` | -| Goal | `GraspGoal(semantics=..., grasp_xpos=None, object_pose=None)` | +| Goal | `GraspGoal(semantics=..., grasp_xpos=None, object_pose=None)` or `ContactGraspGoal(grasp_xpos=...)` | | Binding contract | `primary.motion` plus disjoint `primary.grasp` | -| Precondition | When `object_pose` is omitted, `ObjectSemantics.entity_id` must resolve in the planning snapshot (the deprecated live `entity` fallback remains temporarily); an explicit `SceneEntityPose` must reference a snapshot entity; an `AntipodalAffordance` is required when neither an explicit grasp pose nor `fixed_object_to_eef` is supplied | -| Effect | write `HeldObjectState` for the bound motion target | -| Verification | the attachment effect must be verified during closed-loop execution | +| Precondition | Rigid goals resolve their object pose and require an affordance when no grasp is supplied; contact goals require a free task-state resource; late-bound poses resolve in the planning snapshot | +| Effect | Rigid goals write `HeldObjectState`; contact goals declare a physical verification requirement | +| Verification | Rigid held effects and contact-only grasp effects require physical verification during closed-loop execution | + +Contact goals require an unoccupied single/coordinated task-state resource. +Their plan declares `EffectVerificationRequirement(kind="contact_grasp")`, +which the host must resolve with physical evidence. The cable routing host +measures material-point lift and retention in the tool frame. Rigid-object +calibrations and downstream object targets belong to `GraspGoal` pickup. + +Contact pickup uses the existing `hand_interp_steps`, `grasp_settle_steps` and +`lift_height` options. Its native IK lift is sampled using +`MotionPolicy.interpolate_position_step` and the control period, then qualified +against joint limits. The motion-generator and external-nominal paths retain +their backend timing. Both goal types use the same effect assembly boundary. `grasp_xpos` may be `(4, 4)`, `(B, 4, 4)`, or a `SceneEntityPose`. A scene reference resolves the latest grasp pose and registers its entity as a recovery diff --git a/embodichain/compute/geometry/_cable_route.py b/embodichain/compute/geometry/_cable_route.py new file mode 100644 index 0000000000..b05c6ae098 --- /dev/null +++ b/embodichain/compute/geometry/_cable_route.py @@ -0,0 +1,180 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Pure cable routing measurements for declared board geometry. + +Geometry measurements adapted from robosimstudio's Apache-2.0 example at +73739abe212f963e8a9f5862b27bca370df091a7; see the repository NOTICE. +https://github.com/robosimstudio/robosimstudio/tree/73739abe212f963e8a9f5862b27bca370df091a7/examples/08_demos/route_the_cable +""" + +from __future__ import annotations + +import torch +from .cable import cable_endpoints + +__all__: list[str] = [] + +_CABLE_METRICS = ( + "seat_0", + "seat_1", + "port_count", + "end_in_port", + "strict_port", + "release_ready", + "reference_routed", + "hands_clearance", + "max_speed", + "bench_penetration", + "spool_penetration", + "port_penetration", + "max_grasp_slip", + "end_position", +) + + +_CABLE_METRIC_UNITS = { + name: ( + "m/s" + if name == "max_speed" + else ( + "m" + if name + in { + "end_position", + "hands_clearance", + "bench_penetration", + "spool_penetration", + "port_penetration", + "max_grasp_slip", + } + else "unitless" + ) + ) + for name in _CABLE_METRICS +} + + +def _measure_cable_route( + state: torch.Tensor, + lengths: torch.Tensor, + hands_w: torch.Tensor, + origins: torch.Tensor, + geometry: dict[str, object], + radius: float, +) -> dict[str, torch.Tensor]: + """Measure the reference board goal and sampled geometry diagnostics. + + Args: + state: World cable segment states ``(B,50,13)``. + lengths: Capsule axis rest lengths ``(50,)``. + hands_w: World tool centres ``(B,2,3)``. + origins: World Arena translations ``(B,3)``. + + Returns: + Per-environment counts, speed, clearance and routing acceptance. + """ + bench = geometry["bench_height"] + port_x, port_y = geometry["port_center"] + p = state[..., :3] - origins[:, None, :] + starts, ends = cable_endpoints(state, lengths) + start_local = starts - origins[:, None, :] + end_local = ends - origins[:, None, :] + valid = torch.isfinite(state).all(dim=(1, 2)) & ( + state[..., 3:7].norm(dim=-1) > 1e-8 + ).all(-1) + out = {} + for i, (x, y, side) in enumerate(geometry["seats"]): + lo, hi = sorted((x + side * 0.003, x + side * 0.045)) + inside = ( + (p[:, :, 0] >= lo) + & (p[:, :, 0] <= hi) + & ((p[:, :, 1] - y).abs() <= 0.035) + & (p[:, :, 2] >= bench) + & (p[:, :, 2] <= bench + 0.15) + ) + out[f"seat_{i}"] = inside.sum(-1) + in_port = ( + ((p[:, :, 0] - port_x).abs() <= 0.045) + & ((p[:, :, 1] - port_y).abs() <= 0.045) + & (p[:, :, 2] >= bench) + & (p[:, :, 2] <= bench + 0.15) + ) + end = end_local[:, -1] + out["end_position"] = end + out["port_count"] = in_port.sum(-1) + out["end_in_port"] = ( + ((end[:, :2] - end.new_tensor([port_x, port_y])).abs() <= 0.045).all(-1) + & (end[:, 2] >= bench) + & (end[:, 2] <= bench + 0.15) + ) + out["hands_clearance"] = torch.cdist(hands_w, state[..., :3]).amin(dim=(1, 2)) + out["max_speed"] = torch.linalg.vector_norm(state[..., 7:10], dim=-1).amax(-1) + out["bench_penetration"] = ( + bench + radius - torch.minimum(start_local[..., 2], end_local[..., 2]).amin(-1) + ).clamp_min(0) + # Five points per capsule axis, using the exact capped-cylinder SDF. + axis_samples = ( + start_local[:, :, None, :] + + torch.linspace(0, 1, 5, device=state.device)[None, None, :, None] + * (end_local - start_local)[:, :, None, :] + ) + penetration = torch.zeros(state.shape[0], device=state.device) + for x, y, cylinder_radius, z0, z1 in geometry["cylinders"]: + radius_at_spool = cylinder_radius + radial = ( + torch.linalg.vector_norm( + axis_samples[..., :2] - state.new_tensor([x, y]), dim=-1 + ) + - radius_at_spool + ) + vertical = (axis_samples[..., 2] - (bench + (z0 + z1) / 2)).abs() - ( + z1 - z0 + ) / 2 + q = torch.stack((radial, vertical), -1) + sdf = torch.linalg.vector_norm(q.clamp_min(0), dim=-1) + q.amax(-1).clamp_max(0) + penetration = torch.maximum( + penetration, (radius - sdf).clamp_min(0).amax(dim=(1, 2)) + ) + out["spool_penetration"] = penetration + port_penetration = torch.zeros_like(penetration) + for center, half_size in geometry["boxes"]: + q = (axis_samples - state.new_tensor(center)).abs() - state.new_tensor( + half_size + ) + sdf = torch.linalg.vector_norm(q.clamp_min(0), dim=-1) + q.amax(-1).clamp_max(0) + port_penetration = torch.maximum( + port_penetration, (radius - sdf).clamp_min(0).amax(dim=(1, 2)) + ) + out["port_penetration"] = port_penetration + strict_lo, strict_hi = (state.new_tensor(v) for v in geometry["strict_region"]) + release_lo, release_hi = (state.new_tensor(v) for v in geometry["release_region"]) + out["strict_port"] = ((end >= strict_lo) & (end <= strict_hi)).all(-1) + inside_release = ((end >= release_lo) & (end <= release_hi)).all(-1) + # An endpoint already seated deeply inside the physical channel need not + # be pushed toward the narrower corridor used for release from above. + seated_deeply = out["strict_port"] & (end[:, 1] >= release_lo[1]) + out["release_ready"] = valid & (inside_release | seated_deeply) + out["reference_routed"] = ( + valid + & (out["seat_0"] >= 2) + & (out["seat_1"] >= 2) + & (out["port_count"] >= 3) + & out["end_in_port"] + & (out["hands_clearance"] > 0.05) + & (out["max_speed"] <= 0.05) + ) + return out diff --git a/embodichain/compute/geometry/cable.py b/embodichain/compute/geometry/cable.py new file mode 100644 index 0000000000..f26085fe6f --- /dev/null +++ b/embodichain/compute/geometry/cable.py @@ -0,0 +1,105 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Cable material coordinates and geometric observations, independent of simulation.""" + +from __future__ import annotations + +import torch + +__all__ = ["sample_cable_material", "cable_endpoints", "cable_grasp_slip"] + + +def cable_endpoints( + state: torch.Tensor, rest_lengths: torch.Tensor +) -> tuple[torch.Tensor, torch.Tensor]: + """Return proximal/distal capsule ends from xyz+xyzw segment states. + + Args: + state: World segment states with shape ``(..., S, 13)``. + rest_lengths: Positive capsule axis lengths with shape ``(S,)``. + + Returns: + Two tensors of shape ``(..., S, 3)``, in material order. + """ + if ( + state.ndim < 2 + or state.shape[-1] != 13 + or state.shape[-2] != rest_lengths.numel() + ): + raise ValueError("Cable state and rest lengths must agree on segment count.") + if ( + rest_lengths.ndim != 1 + or rest_lengths.numel() == 0 + or not bool((torch.isfinite(rest_lengths) & (rest_lengths > 0)).all()) + ): + raise ValueError("Cable rest lengths must be a positive vector.") + q = state[..., 3:7] + # The capsule's local +Z axis in world coordinates; quaternions are XYZW. + x, y, z, w = q.unbind(-1) + axis = torch.stack( + (2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y)), -1 + ) + half_axis = 0.5 * rest_lengths.to(state)[..., None] * axis + return state[..., :3] - half_axis, state[..., :3] + half_axis + + +def sample_cable_material( + state: torch.Tensor, rest_lengths: torch.Tensor, distance: float +) -> tuple[torch.Tensor, torch.Tensor, int]: + """Observe a fixed material coordinate, rather than a nearest world point. + + Args: + state: World states with shape ``(B,S,13)``. + rest_lengths: Positive capsule axis lengths ``(S,)``. + distance: Material arc length from the proximal cable end, in metres. + + Returns: + World point, unit material tangent, and stable segment index. + """ + if state.ndim != 3: + raise ValueError("Material sampling requires batched cable state (B,S,13).") + starts, ends = cable_endpoints(state, rest_lengths) + cumulative = rest_lengths.to(state).cumsum(0) + if not 0 <= distance <= float(cumulative[-1]): + raise ValueError("Material distance is outside this cable.") + index = min( + int(torch.searchsorted(cumulative, cumulative.new_tensor(distance))), + state.shape[-2] - 1, + ) + previous = cumulative[index - 1] if index else cumulative.new_tensor(0) + fraction = (distance - previous) / rest_lengths.to(state)[index] + direction = ends[:, index] - starts[:, index] + return ( + starts[:, index] + fraction * direction, + torch.nn.functional.normalize(direction, dim=-1), + index, + ) + + +def cable_grasp_slip( + point_in_tool: torch.Tensor, initial_point_in_tool: torch.Tensor +) -> torch.Tensor: + """Measure material-point slip in the tool frame, invariant to wrist motion. + + Args: + point_in_tool: Current material point in the TCP frame, ``(B,3)``. + initial_point_in_tool: Independent snapshot when the fingers closed. + + Returns: + Slip distance in metres for each environment. + """ + return torch.linalg.vector_norm(point_in_tool - initial_point_in_tool, dim=-1) diff --git a/embodichain/lab/evaluation/_predicates.py b/embodichain/lab/evaluation/_predicates.py index e63a006b8f..c3712a3a92 100644 --- a/embodichain/lab/evaluation/_predicates.py +++ b/embodichain/lab/evaluation/_predicates.py @@ -488,10 +488,45 @@ def _region(arguments, params): ) +def _scalar_range_params(params): + _keys(params, ("minimum", "maximum"), ("minimum_inclusive", "maximum_inclusive")) + lo, hi = (_finite(params[key], key) for key in ("minimum", "maximum")) + if lo > hi: + raise ValueError("Scalar range minimum exceeds maximum.") + flags = { + key: params.get(key, True) for key in ("minimum_inclusive", "maximum_inclusive") + } + if any(type(value) is not bool for value in flags.values()): + raise ValueError("Scalar range boundary flags must be booleans.") + return {"minimum": lo, "maximum": hi, **flags} + + +def _scalar_range(arguments, params): + value = arguments["value"] + if value.ndim != 2 or value.shape[1] != 1: + raise ValueError("Scalar observations must have shape (B, 1).") + value = value[:, 0] + lo, hi = params["minimum"], params["maximum"] + import torch + + return PredicateMeasurement( + ((value >= lo) if params["minimum_inclusive"] else (value > lo)) + & ((value <= hi) if params["maximum_inclusive"] else (value < hi)), + torch.isfinite(value), + {"value": value}, + ) + + def builtin_predicates() -> PredicateRegistry: """Return an independent registry with the shared built-in predicates.""" registry = PredicateRegistry() for kind, definition in ( + ( + "scalar_range", + PredicateDefinition( + 1, ("value",), _scalar_range_params, _scalar_range, ("scalar",) + ), + ), ("near_target", _NEAR), ( "near_relative_target", diff --git a/embodichain/lab/evaluation/_recording.py b/embodichain/lab/evaluation/_recording.py index f7183e4df0..bb1c860ef1 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,27 @@ 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.startswith("wrap_"): + from embodichain_tasks.manipulation.cable.wrap_measurements import ( + WRAP_CHECKS, + ) + + if quantity.removeprefix("wrap_") not in WRAP_CHECKS: + return None + contract = ("arena", "scalar", ("1",), (1,)) + elif quantity not in _METRICS: + return None + elif quantity == "end_position": + contract = ("arena", "xyz", ("m",), (3,)) + else: + contract = ("arena", "scalar", (_CABLE_METRIC_UNITS[quantity],), (1,)) + elif quantity == "centroid" and field.startswith("deformables/"): contract = ("world", "xyz", ("m",), (3,)) elif quantity in ("pose", "segment_start_pose"): contract = ("arena", "xyz_xyzw", ("m", "unitless"), (7,)) diff --git a/embodichain/lab/evaluation/observations.py b/embodichain/lab/evaluation/observations.py index ac46d07fdd..111fd3f683 100644 --- a/embodichain/lab/evaluation/observations.py +++ b/embodichain/lab/evaluation/observations.py @@ -41,6 +41,8 @@ def _raw_field_kind(field: str) -> str: owner, _, channel = field.partition("/") quantity = channel.rsplit("/", 1)[1] + if owner == "cables": + return "position" if quantity == "end_position" else "scalar" if owner == "deformables": from ._deformable import _centroid_selection diff --git a/embodichain/lab/gym/envs/demo.py b/embodichain/lab/gym/envs/demo.py index d898c20fa7..e09247d48b 100644 --- a/embodichain/lab/gym/envs/demo.py +++ b/embodichain/lab/gym/envs/demo.py @@ -387,7 +387,7 @@ def _segment_outcome_kind( return "not_attempted" if success: return "succeeded" - if failure_reason == "truncated": + if failure_reason in {"truncated", DEMO_STEP_LIMIT}: return "truncated" if failure_reason in {"interrupted", "batch_aborted", "empty_segment"}: return "cancelled" @@ -805,6 +805,7 @@ def to_metadata(self) -> dict[str, Any]: ProgressWrapper = Callable[[Iterable[Any], str], Iterable[Any]] +DEMO_STEP_LIMIT = "simulation_step_limit" StopPredicate = Callable[[], bool] @@ -1083,6 +1084,7 @@ def execute_demo_episode( attempt_id: int = 0, instruction: str | None = None, should_stop: StopPredicate | None = None, + max_steps: int | None = None, progress: ProgressWrapper | None = None, segments: Iterable[DemoSegment] | None = None, active_env_ids: tuple[int, ...] | None = None, @@ -1106,6 +1108,12 @@ def execute_demo_episode( attempt_id: Zero-based identifier for this collection attempt. instruction: Optional overall language override bound to this episode. should_stop: Optional callback checked before every action. + max_steps: Optional hard limit on simulation steps for a partial episode. + An episode that completes exactly at the limit retains its result. + Requesting another physical step truncates the active rows. Reset + or close the environment after this truncation. + Ordinary cancellation keeps its physical safe-stop actions while + simulation steps are available. progress: Optional wrapper such as ``tqdm`` for action iterables. segments: Optional trusted segment iterable. When provided, bypasses task planner selection while retaining ordinary segment validation, @@ -1128,6 +1136,8 @@ def execute_demo_episode( execution_cfg = DemoExecutionCfg() elif not isinstance(execution_cfg, DemoExecutionCfg): raise TypeError("execution_cfg must be a DemoExecutionCfg or None.") + if max_steps is not None and (type(max_steps) is not int or max_steps < 0): + raise ValueError("max_steps must be a non-negative integer or None.") if attempt_id < 0: raise ValueError("attempt_id must be non-negative.") @@ -1193,6 +1203,32 @@ def publish_active_mask() -> None: recording_started = False outcome_published = False + physical_steps = 0 + + def budget_exhausted() -> bool: + return max_steps is not None and physical_steps >= max_steps + + def stop_requested( + failures: list[str | None] | None = None, *, check_step_limit: bool + ) -> bool: + nonlocal fatal_reason + if check_step_limit and budget_exhausted(): + fatal_reason = DEMO_STEP_LIMIT + elif should_stop is not None and should_stop(): + fatal_reason = "interrupted" + else: + return False + for env_id, is_active in enumerate(active): + if is_active: + terminal_reasons[env_id] = fatal_reason + if failures is not None: + failures[env_id] = fatal_reason + if fatal_reason == DEMO_STEP_LIMIT: + truncated[env_id] = True + active[env_id] = False + publish_active_mask() + return True + def capture_frozen_rows(before: tuple[bool, ...]) -> None: if capture_terminal_physical is not None and ( "task_outcome" in last_info or "physical_objective" in last_info @@ -1250,13 +1286,7 @@ def episode_result( else _validated_demo_segments(env, segments, **plan_kwargs) ) while any(active): - if should_stop is not None and should_stop(): - fatal_reason = "interrupted" - for env_id, is_active in enumerate(active): - if is_active: - terminal_reasons[env_id] = fatal_reason - active[env_id] = False - publish_active_mask() + if stop_requested(check_step_limit=max_steps == 0): break try: segment = next(segment_iterator) @@ -1307,16 +1337,9 @@ def episode_result( segment_reason = "action_generation_failed" break last_action_consumed = False - if should_stop is not None and should_stop(): + if stop_requested(segment_failure_reasons, check_step_limit=True): actions_exhausted = False - fatal_reason = "interrupted" segment_reason = fatal_reason - for env_id, is_active in enumerate(active): - if is_active: - terminal_reasons[env_id] = fatal_reason - segment_failure_reasons[env_id] = fatal_reason - active[env_id] = False - publish_active_mask() break try: @@ -1341,6 +1364,7 @@ def episode_result( observation, _, terminated_value, truncated_value, info = env.step( action ) + physical_steps += 1 last_action_consumed = True if step_observer is not None: step_observer(active_before_step, action, observation) @@ -1430,16 +1454,11 @@ def episode_result( # and do not request another lazy segment. actions_exhausted = False break - if should_stop is not None and should_stop(): + # Allow the lazy action iterator and final validators to finish at + # the exact cap. A subsequent action is checked before env.step. + if stop_requested(segment_failure_reasons, check_step_limit=False): actions_exhausted = False - fatal_reason = "interrupted" segment_reason = fatal_reason - for env_id, is_active in enumerate(active): - if is_active: - terminal_reasons[env_id] = fatal_reason - segment_failure_reasons[env_id] = fatal_reason - active[env_id] = False - publish_active_mask() break if not actions_exhausted: @@ -1461,6 +1480,8 @@ def episode_result( emergency_iterator = iter(emergency_actions) try: for emergency_action in emergency_iterator: + if budget_exhausted(): + break if normalize_action is not None: emergency_action = normalize_action( emergency_action @@ -1469,6 +1490,7 @@ def episode_result( emergency_observation, _, _, _, emergency_info = ( env.step(emergency_action) ) + physical_steps += 1 if step_observer is not None: step_observer( tuple(participants), diff --git a/embodichain/lab/gym/envs/managers/observations/_common.py b/embodichain/lab/gym/envs/managers/observations/_common.py index fa4ef217f2..e825fd20be 100644 --- a/embodichain/lab/gym/envs/managers/observations/_common.py +++ b/embodichain/lab/gym/envs/managers/observations/_common.py @@ -234,6 +234,78 @@ 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.startswith("wrap_"): + monitor = getattr(env, "_wrap_monitor", None) + available = ( + monitor is not None and monitor.active and monitor.cable.cfg.uid == uid + ) + report = monitor.terminal_report if available else None + passed = ( + report is not None and report["checks"][quantity.removeprefix("wrap_")] + ) + values = torch.full((env.num_envs, 1), float(passed), device=env.device) + valid = torch.full( + (env.num_envs,), available, dtype=torch.bool, device=env.device + ) + return ObservationSample( + ObservationDescriptor( + uid, quantity, "arena", "scalar", ("1",), (1,), values.dtype + ), + values, + valid, + (None if available else "Wrap episode has not started.",) + * env.num_envs, + ) + if quantity not in _METRICS: + raise ValueError(f"Unknown cable observation {quantity!r}.") + bridge = getattr(env, "_active_task_program_bridge", None) + service = getattr( + getattr(bridge, "post_policy_port", None), "_cable_runtime", None + ) + shape = (3,) if quantity == "end_position" else (1,) + if service is None or service.cfg.simulation_uid != uid: + values = torch.zeros((env.num_envs, *shape), device=env.device) + valid = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + else: + measured = ( + service.metrics()[quantity] + .float() + .reshape(len(service.env_ids), *shape) + ) + values = torch.zeros((env.num_envs, *shape), device=env.device) + valid = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + values[service.env_ids] = measured + state = service.state() + valid[service.env_ids] = ( + torch.isfinite(measured).all(-1) + & torch.isfinite(state).all(dim=(1, 2)) + & (state[..., 3:7].norm(dim=-1) > 1e-8).all(-1) + ) + descriptor = ObservationDescriptor( + uid, + quantity, + "arena", + "xyz" if shape == (3,) else "scalar", + (_CABLE_METRIC_UNITS[quantity],), + shape, + values.dtype, + ) + return ObservationSample( + descriptor, + values, + valid, + tuple( + None if v else "Cable material session is unavailable." + for v in valid.tolist() + ), + ) if parts[0] in ("links", "robot_links"): owner = parts[0] link = parts[-2] diff --git a/embodichain/lab/gym/envs/task_program/bridge.py b/embodichain/lab/gym/envs/task_program/bridge.py index 9de1c60d4d..e7228184d7 100644 --- a/embodichain/lab/gym/envs/task_program/bridge.py +++ b/embodichain/lab/gym/envs/task_program/bridge.py @@ -36,7 +36,7 @@ from tensordict import TensorDict from embodichain.lab.gym.envs._json import json_safe_copy as _json_safe_copy -from embodichain.lab.gym.envs.demo import DemoSegment +from embodichain.lab.gym.envs.demo import DEMO_STEP_LIMIT, DemoSegment from embodichain.lab.gym.envs.expert_trajectory import JointCommandMode from embodichain.lab.gym.envs.types import ControllerAction from ._segment_contract import _check_outcomes, _check_prerequisites, _unrun_outcomes @@ -1784,6 +1784,19 @@ def _yield_and_advance( self._record_consumed_action(action) self._clock.advance_after_env_step() lifecycle.pending_action = None + observer = getattr( + getattr(self, "_validator_port", None), "_observe_consumed_action", None + ) + decision = None if observer is None else observer(action, runtime=self._runtime) + if decision is not None: + rejected, hold_qpos, reason = decision + if self._eligible_mask is not None: + rejected &= self._eligible_mask + if bool(rejected.any()): + hold = ControllerAction(self._sink.encode_post_policy_hold(hold_qpos)) + self.request_row_rejection( + tuple(self.env_ids[rejected].tolist()), hold, reason=reason + ) process = getattr(self, "_process_rejection", None) if process is not None: self._process_rejection = None @@ -2138,6 +2151,13 @@ def _abort_segment( result = _validate_runtime_result(runtime.cancel(reason)) self._record_runtime_result(lifecycle, result) + if reason == DEMO_STEP_LIMIT: + # The partial episode ends at its requested physical frame. Cancel + # the controller without emitting or acknowledging an unstepped hold. + self._sink.discard_pending() + lifecycle.abort_complete = True + return + pending_kind = ( None if pending is None else pending.metadata.get("bridge_action_kind") ) diff --git a/embodichain/lab/gym/utils/_component_composition.py b/embodichain/lab/gym/utils/_component_composition.py index 21281407ba..e6c7f95d31 100644 --- a/embodichain/lab/gym/utils/_component_composition.py +++ b/embodichain/lab/gym/utils/_component_composition.py @@ -435,6 +435,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"), @@ -454,6 +455,7 @@ def _validate_scene_binding_targets( "USD scene binding validation requires its preflight inventory." ) for binding_field, kind in ( + ("cables", "cable"), ("rigid_objects", "rigid_object"), ("articulations", "articulation"), ("rigidized_articulations", "articulation"), diff --git a/embodichain/lab/gym/utils/trajectory_state.py b/embodichain/lab/gym/utils/trajectory_state.py index b010e19320..ab77613290 100644 --- a/embodichain/lab/gym/utils/trajectory_state.py +++ b/embodichain/lab/gym/utils/trajectory_state.py @@ -305,7 +305,7 @@ def _evaluation_observation_cfgs( params={"field": name}, consumers=("recording",), ) - elif name.startswith(("links/", "robot/", "robot_links/")): + elif name.startswith(("links/", "robot/", "robot_links/", "cables/")): result[f"recording:{name}"] = ObservationCfg( func=_get_evaluation_state, mode="add", diff --git a/embodichain/lab/sim/atomic_actions/__init__.py b/embodichain/lab/sim/atomic_actions/__init__.py index cefb9aa904..f17d002d33 100644 --- a/embodichain/lab/sim/atomic_actions/__init__.py +++ b/embodichain/lab/sim/atomic_actions/__init__.py @@ -132,6 +132,7 @@ class identity, registry state and exception types intact during migration. "FeedbackTerminalAcceptance", "GRASP_COMMAND", "GRASP_CAPABILITY", + "ContactGraspGoal", "GraspGoal", "HandOver", "HandOverGoal", diff --git a/embodichain/lab/sim/atomic_skills/__init__.py b/embodichain/lab/sim/atomic_skills/__init__.py index 6c7a7dbd99..238d58ef43 100644 --- a/embodichain/lab/sim/atomic_skills/__init__.py +++ b/embodichain/lab/sim/atomic_skills/__init__.py @@ -200,6 +200,7 @@ CoordinatedPlacementGoal, CoordinatedPlacementOptions, EndEffectorPoseGoal, + ContactGraspGoal, GraspGoal, HandOver, HandOverGoal, @@ -387,6 +388,7 @@ "FeedbackTerminalAcceptance", "GRASP_COMMAND", "GRASP_CAPABILITY", + "ContactGraspGoal", "GraspGoal", "HandOver", "HandOverGoal", diff --git a/embodichain/lab/sim/atomic_skills/core.py b/embodichain/lab/sim/atomic_skills/core.py index 20587e026b..a4887a3167 100644 --- a/embodichain/lab/sim/atomic_skills/core.py +++ b/embodichain/lab/sim/atomic_skills/core.py @@ -981,6 +981,7 @@ def build_plan( effect_candidates: StateDelta | None = None, effect_verification: EffectVerificationRequirement | None = None, replannable: bool = True, + preserve_velocity_samples: bool = False, diagnostics: PlannerDiagnostics | None = None, segment_lengths: Mapping[str, int] | None = None, scene_dependency_monitor_until: Mapping[str, int] | None = None, @@ -999,6 +1000,9 @@ def build_plan( effect_verification: Optional explicit physical-effect boundary. Use this when verification is required without a symbolic task- state delta. + preserve_velocity_samples: Keep explicitly authored arrival velocities across + consecutive timed segments. Derived velocity targets retain their usual + boundary and stationary treatment. replannable: Whether the execution runtime may replan this action. diagnostics: Optional retained planner diagnostics. segment_lengths: Optional ordered mapping from semantic segment @@ -1053,7 +1057,9 @@ def build_plan( velocities = differentiate_positions(timed.positions, timed.dt) else: velocities = velocities.clone() - if timed.waypoint_count: + if timed.waypoint_count and not ( + preserve_velocity_samples and timed.velocities is not None + ): # Commands are held until the next arrival. A stationary interval # and the final settling window must never retain a moving target. stationary = timed.positions[:, 1:] == timed.positions[:, :-1] diff --git a/embodichain/lab/sim/atomic_skills/policies.py b/embodichain/lab/sim/atomic_skills/policies.py index 22fb610e65..bbf51381d4 100644 --- a/embodichain/lab/sim/atomic_skills/policies.py +++ b/embodichain/lab/sim/atomic_skills/policies.py @@ -75,6 +75,13 @@ class MotionPolicy: plan_opts: PlanOptions | None = None """Optional typed planner-specific options.""" + interpolate_position_step: float = field(default=0.002, kw_only=True) + """Cartesian sampling distance in metres, forwarded to motion generation. + + PickUp contact motions also use this distance to sample their lift on the + control grid. Joint-limit qualification can lengthen the resulting path. + """ + velocity_limit: float | None = None """Optional scalar speed request, capped by effective Robot joint limits.""" @@ -205,9 +212,13 @@ def __post_init__(self) -> None: raise ValueError("velocity_targets must be auto or zero.") if self.sample_count < 2: raise ValueError("sample_count must be at least 2.") - for name in ("velocity_limit", "acceleration_limit"): + for name in ( + "velocity_limit", + "acceleration_limit", + "interpolate_position_step", + ): value = getattr(self, name) - if value is None: + if value is None and name != "interpolate_position_step": if ( name == "velocity_limit" or self.strategy == "motion_gen" @@ -301,6 +312,7 @@ def to_motion_gen_options( interpolation_dt=interpolation_dt, is_linear=cartesian_linear or cartesian_keyframes, preserve_cartesian_samples=cartesian_linear, + interpolate_position_step=self.interpolate_position_step, cartesian_path_sampler=cartesian_path_sampler, interpolation_mode=self.interpolation_mode, ik_seed_mode=self.ik_seed_mode, diff --git a/embodichain/lab/sim/atomic_skills/primitives/__init__.py b/embodichain/lab/sim/atomic_skills/primitives/__init__.py index fef056f58a..cf6f9878a6 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/__init__.py +++ b/embodichain/lab/sim/atomic_skills/primitives/__init__.py @@ -44,7 +44,7 @@ ) from .move_joints import JointPositionGoal, MoveJoints, MoveJointsOptions from .open_door import OpenDoor, OpenDoorGoal, OpenDoorOptions -from .pick_up import GraspGoal, PickUp, PickUpOptions +from .pick_up import ContactGraspGoal, GraspGoal, PickUp, PickUpOptions from .place import AssembleGoal, Place, PlaceGoal, PlaceOptions from .press import ( Press, @@ -103,6 +103,7 @@ "CoordinatedPlacementGoal", "CoordinatedPlacementOptions", "EndEffectorPoseGoal", + "ContactGraspGoal", "GraspGoal", "HandOver", "HandOverGoal", diff --git a/embodichain/lab/sim/atomic_skills/primitives/move_end_effector.py b/embodichain/lab/sim/atomic_skills/primitives/move_end_effector.py index 3dde684606..5685f152d4 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/move_end_effector.py +++ b/embodichain/lab/sim/atomic_skills/primitives/move_end_effector.py @@ -63,6 +63,13 @@ def __post_init__(self) -> None: class MoveEndEffectorOptions(SkillOptions): """Per-invocation behavior for :class:`MoveEndEffector`.""" + preserve_cartesian_samples: bool = False + """Execute the supplied Cartesian samples on the control grid.""" + + def __post_init__(self) -> None: + if type(self.preserve_cartesian_samples) is not bool: + raise TypeError("preserve_cartesian_samples must be a bool.") + class MoveEndEffector(AtomicSkill[EndEffectorPoseGoal, MoveEndEffectorOptions]): """Plan a free-space move for a bound manipulator.""" @@ -96,6 +103,10 @@ def _plan( num_envs=context.batch_size, device=self.device, ) + if request.skill_options.preserve_cartesian_samples and move_xpos.ndim != 4: + raise ValueError( + "preserve_cartesian_samples requires batched pose samples." + ) start_qpos = context.robot.qpos[:, joint_ids] result = self.motion_generator.generate( build_pose_plan_states(move_xpos), @@ -103,6 +114,12 @@ def _plan( start_qpos=start_qpos, control_part=control_part, interpolation_dt=context.control_dt, + cartesian_linear=request.skill_options.preserve_cartesian_samples, + sample_count=( + move_xpos.shape[1] + 1 + if request.skill_options.preserve_cartesian_samples + else None + ), ), ) success, trajectory = to_full_robot_trajectory( diff --git a/embodichain/lab/sim/atomic_skills/primitives/move_joints.py b/embodichain/lab/sim/atomic_skills/primitives/move_joints.py index d1d28cc130..53d6751279 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/move_joints.py +++ b/embodichain/lab/sim/atomic_skills/primitives/move_joints.py @@ -85,6 +85,18 @@ def __post_init__(self) -> None: class MoveJointsOptions(SkillOptions): """Per-invocation behavior for :class:`MoveJoints`.""" + preserve_samples: bool = False + """Consume explicitly sampled joint positions at the host control cadence.""" + + preserve_velocity_samples: bool = False + """Retain explicit arrival velocities, including the ends of consecutive timed phases.""" + + def __post_init__(self) -> None: + if type(self.preserve_velocity_samples) is not bool: + raise TypeError("preserve_velocity_samples must be a bool.") + if type(self.preserve_samples) is not bool: + raise TypeError("preserve_samples must be a bool.") + class MoveJoints(AtomicSkill[JointPositionGoal, MoveJointsOptions]): """Plan joint motion from the observed state to one or more waypoints.""" @@ -113,6 +125,13 @@ def _plan( motion_target = motion.require_target(JointPositionTarget) control_part = motion_target.control_part joint_ids = list(motion_target.joint_ids) + if request.skill_options.preserve_velocity_samples and ( + not isinstance(goal.target, TimedTrajectory) + or goal.target.velocities is None + ): + raise ValueError( + "Preserving velocity samples requires an explicitly timed velocity trajectory." + ) if isinstance(goal.target, TimedTrajectory): trajectory = goal.target if sorted(joint_ids) != list(range(context.robot.robot_dof)): @@ -140,7 +159,12 @@ def _plan( "Each timed replay command must occupy one control step." ) return self.build_plan( - request, context, success=True, trajectory=trajectory, replannable=False + request, + context, + success=True, + trajectory=trajectory, + replannable=False, + preserve_velocity_samples=request.skill_options.preserve_velocity_samples, ) joint_dof = len(motion_target.joint_ids) @@ -156,14 +180,34 @@ def _plan( device=self.device, ) start_qpos = context.robot.qpos[:, joint_ids] - result = self.motion_generator.generate( - build_joint_plan_states(target_qpos), - options=request.motion_policy.to_motion_gen_options( - start_qpos=start_qpos, - control_part=control_part, - interpolation_dt=context.control_dt, - ), - ) + if request.skill_options.preserve_samples: + if request.motion_policy.strategy != "ik_interp": + raise ValueError("preserve_samples requires strategy='ik_interp'.") + from embodichain.lab.sim.motion.planners import PlanResult + + if target_qpos.ndim != 3: + raise ValueError("preserve_samples requires batched joint samples.") + dt = torch.full( + target_qpos.shape[:2], + context.require_control_dt(), + dtype=torch.float64, + device=target_qpos.device, + ) + dt[:, 0] = 0 + result = PlanResult( + success=torch.isfinite(target_qpos).all(dim=(1, 2)), + positions=target_qpos, + dt=dt, + ) + else: + result = self.motion_generator.generate( + build_joint_plan_states(target_qpos), + options=request.motion_policy.to_motion_gen_options( + start_qpos=start_qpos, + control_part=control_part, + interpolation_dt=context.control_dt, + ), + ) success, trajectory = to_full_robot_trajectory( result, base_qpos=context.robot.qpos, diff --git a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py index 7b5bc8f525..2a35a4de92 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py @@ -24,6 +24,7 @@ import torch +from embodichain.lab.sim.motion.planners.utils import interpolate_xpos_batched from embodichain.utils import logger from embodichain.utils.math import ( axis_angle_to_rotation_matrix, @@ -65,6 +66,7 @@ from embodichain.lab.sim.atomic_skills.nominal import NominalMotionSource from embodichain.lab.sim.atomic_skills.plans import ( SkillPlan, + EffectVerificationRequirement, PlannerDiagnostics, TimedTrajectory, normalize_success_mask, @@ -83,6 +85,7 @@ resolve_pose_target, split_three_segments, translate_pose_world, + to_full_robot_trajectory, ) from embodichain.lab.sim.atomic_skills.primitives._binding_contracts import ( make_manipulation_slot, @@ -128,6 +131,27 @@ def __post_init__(self) -> None: validate_pose_goal(self.grasp_xpos, "grasp_xpos", allow_waypoints=False) +@dataclass(frozen=True, slots=True, eq=False) +class ContactGraspGoal: + """Contact-only pickup at an observed TCP pose in the planning frame. + + Args: + grasp_xpos: Explicit or late-bound grasp pose. The host supplies physical + evidence that the target remains pinched during lift. The goal does + not project a fixed object-to-tool relationship. + """ + + goal_kind: ClassVar[str] = "contact_grasp" + grasp_xpos: PoseGoalValue + + def __post_init__(self) -> None: + validate_pose_goal(self.grasp_xpos, "grasp_xpos", allow_waypoints=False) + if isinstance(self.grasp_xpos, torch.Tensor) and not bool( + torch.isfinite(self.grasp_xpos).all() + ): + raise ValueError("grasp_xpos must be finite.") + + def _validate_single_se3(value: torch.Tensor, name: str) -> None: """Validate one finite, proper SE(3) transform.""" validate_pose_goal(value, name, allow_waypoints=False) @@ -317,11 +341,11 @@ def __post_init__(self) -> None: ) -class PickUp(AtomicSkill[GraspGoal, PickUpOptions]): +class PickUp(AtomicSkill[GraspGoal | ContactGraspGoal, PickUpOptions]): """Approach a grasp pose, close the gripper, lift.""" skill_id: ClassVar[str] = "pick_up" - GoalType: ClassVar[type] = GraspGoal + GoalType: ClassVar[tuple[type, ...]] = (GraspGoal, ContactGraspGoal) OptionsType: ClassVar[type] = PickUpOptions binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( @@ -344,10 +368,12 @@ class PickUp(AtomicSkill[GraspGoal, PickUpOptions]): def _scene_dependencies( self, - request: ResolvedSkillRequest[GraspGoal, PickUpOptions], + request: ResolvedSkillRequest[GraspGoal | ContactGraspGoal, PickUpOptions], ) -> tuple[str, ...]: """Monitor the acquired object and late-bound downstream targets.""" dependencies = set(super()._scene_dependencies(request)) + if isinstance(request.goal, ContactGraspGoal): + return tuple(sorted(dependencies)) entity_id = request.goal.semantics.entity_id # An explicit object pose is a scene-independent planning input. Do # not retain the semantic entity as a dynamic dependency in that @@ -374,6 +400,9 @@ def _get_full_pickup_trajectory( hand_open_qpos: torch.Tensor, hand_grasp_qpos: torch.Tensor, interpolation_dt: float, + *, + contact_grasp: bool = False, + env_ids: torch.Tensor | None = None, ) -> tuple[torch.Tensor, torch.Tensor, dict[str, int]]: if options.approach_along_grasp_axis: approach_direction = grasp_xpos[:, :3, 2] @@ -381,75 +410,192 @@ def _get_full_pickup_trajectory( grasp_xpos, -approach_direction * options.pre_grasp_distance ) - n_approach, n_close, n_lift = split_three_segments( - motion_policy.sample_count, - options.hand_interp_steps, - first_segment_name="approach", - third_segment_name="lift", - ) + if contact_grasp and motion_policy.strategy == "ik_interp": - lift_xpos = translate_pose_world( - grasp_xpos, - torch.tensor([0, 0, 1], device=self.device) * options.lift_height, - ) - motion_options = motion_policy.to_motion_gen_options( - start_qpos=start_arm_qpos, - control_part=manipulator.control_part, - sample_count=n_approach + n_lift, - interpolation_dt=interpolation_dt, - ) - if motion_policy.strategy == "motion_gen": - # Keep the native combined path for the pose-based split. The - # motion generator resolves backend defaults when no explicit - # planner options were supplied. - motion_options.sample_count = None - waypoints = [pre_grasp_xpos, grasp_xpos, lift_xpos] - if options.initial_lift_height > 0.0: - start_xpos = self.robot.compute_fk( - qpos=start_arm_qpos, - name=manipulator.control_part, - to_matrix=True, - **( - {"env_ids": self.planning_services.env_ids} - if self.planning_services.env_ids is not None - else {} - ), + def sampled_move( + start_full: torch.Tensor, target_pose: torch.Tensor, sample_count: int + ) -> tuple[torch.Tensor, torch.Tensor]: + arm_ids = list(manipulator.joint_ids) + start_pose = self.robot.compute_fk( + start_full[:, arm_ids], + name=manipulator.control_part, + to_matrix=True, + **( + {"env_ids": self.planning_services.env_ids} + if self.planning_services.env_ids is not None + else {} + ), + ) + poses = interpolate_xpos_batched(start_pose, target_pose, sample_count)[ + :, 1: + ] + result = self.motion_generator.generate( + build_pose_plan_states(poses), + options=replace( + motion_policy, sample_count=sample_count + ).to_motion_gen_options( + start_qpos=start_full[:, arm_ids], + control_part=manipulator.control_part, + interpolation_dt=interpolation_dt, + cartesian_linear=True, + ), + ) + valid, trajectory = to_full_robot_trajectory( + result, + base_qpos=start_full, + joint_ids=arm_ids, + env_ids=env_ids, + control_dt=interpolation_dt, + ) + return valid, trajectory.positions[:, :, arm_ids] + + approach_start = last_qpos + prefix = None + prefix_valid = torch.ones( + self.num_envs, dtype=torch.bool, device=self.device + ) + if options.initial_lift_height > 0.0: + start_xpos = self.robot.compute_fk( + qpos=start_arm_qpos, + name=manipulator.control_part, + to_matrix=True, + **( + {"env_ids": self.planning_services.env_ids} + if self.planning_services.env_ids is not None + else {} + ), + ) + initial_lift = translate_pose_world( + start_xpos, + start_xpos.new_tensor([0.0, 0.0, options.initial_lift_height]), + ) + prefix_valid, prefix = sampled_move( + approach_start, initial_lift, motion_policy.sample_count + ) + approach_start = last_qpos.clone() + approach_start[:, list(manipulator.joint_ids)] = prefix[:, -1] + if options.pre_grasp_distance > 0: + pre_grasp_valid, pre_grasp = sampled_move( + approach_start, pre_grasp_xpos, motion_policy.sample_count + ) + prefix_valid &= pre_grasp_valid + prefix = ( + pre_grasp + if prefix is None + else torch.cat((prefix, pre_grasp), dim=1) + ) + approach_start = last_qpos.clone() + approach_start[:, list(manipulator.joint_ids)] = prefix[:, -1] + is_success, approach_arm = sampled_move( + approach_start, grasp_xpos, motion_policy.sample_count + ) + if prefix is not None: + approach_arm = torch.cat((prefix, approach_arm), dim=1) + lift_start_qpos = last_qpos.clone() + lift_start_qpos[:, list(manipulator.joint_ids)] = approach_arm[:, -1] + lift_start_qpos[:, list(end_effector.joint_ids)] = hand_grasp_qpos + lift_xpos = translate_pose_world( + grasp_xpos, grasp_xpos.new_tensor([0, 0, options.lift_height]) ) - initial_lift = translate_pose_world( - start_xpos, - start_xpos.new_tensor([0.0, 0.0, options.initial_lift_height]), + lift_samples = max( + 2, + math.ceil(options.lift_height / motion_policy.interpolate_position_step) + + 1, + ) + lift_success, lift_arm = sampled_move( + lift_start_qpos, lift_xpos, lift_samples + ) + is_success = prefix_valid & is_success & lift_success + n_approach, n_close, n_lift = ( + approach_arm.shape[1], + options.hand_interp_steps, + lift_arm.shape[1], + ) + grasp_arm_qpos = approach_arm[:, -1] + else: + n_approach, n_close, n_lift = split_three_segments( + motion_policy.sample_count, + options.hand_interp_steps, + first_segment_name="approach", + third_segment_name="lift", ) - waypoints.insert(0, initial_lift) - 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, - ) - n_approach = approach_arm.shape[1] - n_lift = lift_arm.shape[1] - grasp_arm_qpos = approach_arm[:, -1, :] - is_success = motion_result.success + lift_xpos = translate_pose_world( + grasp_xpos, + torch.tensor([0, 0, 1], device=self.device) * options.lift_height, + ) + motion_options = motion_policy.to_motion_gen_options( + start_qpos=start_arm_qpos, + control_part=manipulator.control_part, + sample_count=n_approach + n_lift, + interpolation_dt=interpolation_dt, + ) + if motion_policy.strategy == "motion_gen": + # Keep the native combined path for the pose-based split. The + # motion generator resolves backend defaults when no explicit + # planner options were supplied. + motion_options.sample_count = None + waypoints = [pre_grasp_xpos, grasp_xpos, lift_xpos] + if options.initial_lift_height > 0.0: + start_xpos = self.robot.compute_fk( + qpos=start_arm_qpos, + name=manipulator.control_part, + to_matrix=True, + **( + {"env_ids": self.planning_services.env_ids} + if self.planning_services.env_ids is not None + else {} + ), + ) + initial_lift = translate_pose_world( + start_xpos, + start_xpos.new_tensor([0.0, 0.0, options.initial_lift_height]), + ) + waypoints.insert(0, initial_lift) - hand_close_path = interpolate_hand_qpos( - hand_open_qpos, hand_grasp_qpos, n_waypoints=n_close - ) + 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, + ) + n_approach = approach_arm.shape[1] + n_lift = lift_arm.shape[1] + grasp_arm_qpos = approach_arm[:, -1, :] + is_success = motion_result.success + + if contact_grasp: + # Keep endpoint-weighted contact commands on their original grid. + weights = ( + torch.linspace( + 0, 1, n_close, device=self.device, dtype=hand_open_qpos.dtype + ) + if n_close > 1 + else torch.ones(1, device=self.device, dtype=hand_open_qpos.dtype) + )[None, :, None] + hand_close_path = ( + hand_open_qpos[:, None] * (1 - weights) + + hand_grasp_qpos[:, None] * weights + ) + else: + hand_close_path = interpolate_hand_qpos( + hand_open_qpos, hand_grasp_qpos, n_waypoints=n_close + ) n_settle = options.grasp_settle_steps close_start = n_approach settle_start = close_start + n_close @@ -472,7 +618,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: @@ -496,11 +644,13 @@ def _get_full_pickup_trajectory( def _plan( self, - request: ResolvedSkillRequest[GraspGoal, PickUpOptions], + request: ResolvedSkillRequest[GraspGoal | ContactGraspGoal, PickUpOptions], context: PlanningContext, ) -> SkillPlan: """Plan approach, close, and lift segments without committing attachment.""" target = self.require_goal(request) + if isinstance(target, ContactGraspGoal): + self._validate_contact_goal(request, context) options = replace( request.skill_options, downstream_object_target_poses=tuple( @@ -548,76 +698,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, @@ -642,6 +801,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, @@ -666,14 +827,49 @@ def _plan( provenance=None, ) + def _validate_contact_goal( + self, + request: ResolvedSkillRequest[GraspGoal | ContactGraspGoal, PickUpOptions], + context: PlanningContext, + ) -> None: + options = request.skill_options + if ( + options.fixed_object_to_eef is not None + or options.downstream_object_target_poses + or options.rotate_upright is not None + or options.obj_upright_direction is not None + ): + raise ValueError( + "Contact pickup requires an explicit grasp pose without rigid-object options." + ) + motion = request.binding.endpoint("primary", "motion") + hand = request.binding.endpoint("primary", "grasp") + key = require_shared_task_state_key( + motion, hand, participant="PickUp primary participant" + ) + held = context.task.get_held_object(key) + occupied = held is not None and ( + held.env_mask is None or bool(held.env_mask.any()) + ) + coordinated = any( + key in pair and (value.env_mask is None or bool(value.env_mask.any())) + for pair, value in context.coordinated_held_objects.items() + ) + if occupied or coordinated: + raise ValueError( + "Contact pickup requires an unoccupied task-state resource." + ) + def _plan_from_nominal( self, - request: ResolvedSkillRequest[GraspGoal, PickUpOptions], + request: ResolvedSkillRequest[GraspGoal | ContactGraspGoal, PickUpOptions], context: PlanningContext, nominal_source: NominalMotionSource, ) -> SkillPlan: """Assemble a Pick from an explicitly supplied external motion.""" target = self.require_goal(request) + if isinstance(target, ContactGraspGoal): + self._validate_contact_goal(request, context) motion = self._validate_nominal_motion( request, context, nominal_source, segments=("approach", "close", "lift") ) @@ -726,27 +922,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 ): @@ -779,13 +984,13 @@ def _plan_from_nominal( def _assemble_pick_plan( self, - request: ResolvedSkillRequest[GraspGoal, PickUpOptions], + request: ResolvedSkillRequest[GraspGoal | ContactGraspGoal, PickUpOptions], context: PlanningContext, *, success: torch.Tensor, trajectory: TimedTrajectory, grasp_xpos: torch.Tensor, - object_pose: torch.Tensor, + object_pose: torch.Tensor | None, segment_lengths: dict[str, int], affordance_sample: AffordanceSample | None, replannable: bool, @@ -799,31 +1004,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 @@ -1145,4 +1364,4 @@ def _upright_adjusted_grasp_poses( return adjusted_grasp_xpos -__all__ = ["GraspGoal", "PickUp", "PickUpOptions"] +__all__ = ["GraspGoal", "ContactGraspGoal", "PickUp", "PickUpOptions"] diff --git a/embodichain/lab/sim/cfg/usd.py b/embodichain/lab/sim/cfg/usd.py index 72a9b6e7fd..41ee08a679 100644 --- a/embodichain/lab/sim/cfg/usd.py +++ b/embodichain/lab/sim/cfg/usd.py @@ -74,7 +74,10 @@ class UsdImportCfg: In sparse mappings with ``auto`` placement, explicit ``init_pos`` or ``init_rot`` keys select the configured pose, including zero values. - Omitted pose fields retain source placement. + Omitted pose fields retain source placement. For cables, sparse mappings + accept ``uid``, ``fixed_segments``, ``collision_filter_span`` and fields of + ``CablePhysicsCfg`` under ``attrs``. Overrides apply to all curves on + the targeted prim, preserving its authored geometry and placement. """ robot_cfgs: dict[str, RobotCfg] = field(default_factory=dict) diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index 24bfa1d7c3..358d4648b7 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -3065,7 +3065,13 @@ def add_usd( ) -> ( UsdImportResult | dict[ - str, VisualObject | RigidObject | Articulation | Robot | DeformableObject + str, + VisualObject + | RigidObject + | Articulation + | Robot + | DeformableObject + | CableObject, ] ): """Declare a USD import through a validated configuration recipe. @@ -3094,7 +3100,8 @@ def add_usd( asset_cfgs: Legacy per-asset overrides; use ``cfg.asset_cfgs``. Returns: - Source-path mapping of declared facades and imported resource UIDs. + Source-path mapping of declared facades, including USD cables, + and imported resource UIDs. Legacy argument forms retain the mutable asset dictionary result. """ from .cfg.usd import _normalize_usd_import @@ -3146,6 +3153,7 @@ def add_usd( ), "soft_object": self._deformable_objects, "cloth_object": self._deformable_objects, + "cable_object": self._cable_objects, }[kind] registry[descriptor.name] = facade assets[source_path] = facade @@ -3644,22 +3652,8 @@ def get_fluid_diagnostics(self, uid: str) -> dict[str, object]: raise RuntimeError("Fluid diagnostics require prepare().") return self.physics.fluid_diagnostics(fluid) - def add_cable_object(self, cfg: CableObjectCfg) -> CableObject: - """Declare a cable before ``prepare()`` using DexSim's public Spawn API. - - Args: - cfg: Typed cable geometry, four-mode material and visual configuration. - - Returns: - A declared facade whose data becomes available after ``prepare()``. - - Raises: - NotImplementedError: Backend/device/solver/gradient mode is unsupported, - or the physical scene has already been finalized. - ValueError: Required configuration or the asset UID is invalid. - """ - if not isinstance(cfg, CableObjectCfg): - raise TypeError("Cable objects require CableObjectCfg.") + def _require_cable_declaration(self) -> None: + """Check the shared runtime requirements for declaring cables.""" if not self.physics.supports_cables: raise NotImplementedError("Cable objects require the Newton backend.") if self.device.type != "cuda": @@ -3676,6 +3670,24 @@ def add_cable_object(self, cfg: CableObjectCfg) -> CableObject: raise NotImplementedError( "Declare cable objects before scene finalization." ) + + def add_cable_object(self, cfg: CableObjectCfg) -> CableObject: + """Declare a cable before ``prepare()`` using DexSim's public Spawn API. + + Args: + cfg: Typed cable geometry, four-mode material and visual configuration. + + Returns: + A declared facade whose data becomes available after ``prepare()``. + + Raises: + NotImplementedError: Backend/device/solver/gradient mode is unsupported, + or the physical scene has already been finalized. + ValueError: Required configuration or the asset UID is invalid. + """ + if not isinstance(cfg, CableObjectCfg): + raise TypeError("Cable objects require CableObjectCfg.") + self._require_cable_declaration() descriptor, materials = cable_desc_from_cfg(cfg, per_env=True) if descriptor.name in self.asset_uids or descriptor.name in self._spawn_scene: raise ValueError( diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index 6e78c429d9..30b3a410fd 100644 --- a/embodichain/lab/sim/spawn/descriptors.py +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -809,6 +809,22 @@ def _explicit_tet_mesh_from_cfg( } +def _cable_contact_spec(attrs: CablePhysicsCfg) -> _RigidPhysicsSpec: + if not isinstance(attrs, CablePhysicsCfg): + raise TypeError("Cable attrs must be CablePhysicsCfg.") + if isinstance(attrs.collision_props, DefaultCollisionPropertiesCfg): + raise NotImplementedError( + "Cable contact properties require common or Newton configuration." + ) + return _resolve_rigid_physics( + RigidBodyPhysicsCfg( + collision_props=attrs.collision_props, + material_props=attrs.material_props, + ), + newton_solver_type="dexuni", + ) + + def cable_desc_from_cfg( cfg: CableObjectCfg, *, @@ -835,19 +851,7 @@ def cable_desc_from_cfg( ) from exc cfg.validate() uid = _required_uid(cfg.uid, "Cable object") - if not isinstance(cfg.attrs, CablePhysicsCfg): - raise TypeError("Cable attrs must be CablePhysicsCfg.") - if isinstance(cfg.attrs.collision_props, DefaultCollisionPropertiesCfg): - raise NotImplementedError( - "Cable contact properties require common or Newton configuration." - ) - contact = _resolve_rigid_physics( - RigidBodyPhysicsCfg( - collision_props=cfg.attrs.collision_props, - material_props=cfg.attrs.material_props, - ), - newton_solver_type="dexuni", - ) + contact = _cable_contact_spec(cfg.attrs) collision = _compile_newton_collision( contact, newton_solver_type="dexuni", author_shape_defaults=True ) diff --git a/embodichain/lab/sim/spawn/usd_import.py b/embodichain/lab/sim/spawn/usd_import.py index 4ca8310785..6731eb18e8 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 dd562cb593..198281319e 100644 --- a/embodichain/lab/sim/spawn/usd_result.py +++ b/embodichain/lab/sim/spawn/usd_result.py @@ -25,6 +25,7 @@ if TYPE_CHECKING: from embodichain.lab.sim.objects import ( Articulation, + CableObject, DeformableObject, RigidObject, Robot, @@ -35,11 +36,15 @@ class UsdImportResult( - Mapping[str, "VisualObject | RigidObject | Articulation | Robot | DeformableObject"] + Mapping[ + str, + "VisualObject | RigidObject | Articulation | Robot | DeformableObject | CableObject", + ] ): """Map source resource keys to stable, declared EmbodiChain facades. - Facades bind in place at ``SimulationManager.prepare()``. Resource maps + Facades, including imported cables, bind in place at + ``SimulationManager.prepare()``. Resource maps include native-only scene resources and constraints; their values are logical Spawn UIDs, not native handles or builder indices. A unique source keeps its prim path as the resource key. When a source prim produces several @@ -51,7 +56,13 @@ class UsdImportResult( def __init__( self, assets: Mapping[ - str, VisualObject | RigidObject | Articulation | Robot | DeformableObject + str, + VisualObject + | RigidObject + | Articulation + | Robot + | DeformableObject + | CableObject, ], *, resource_uids: Mapping[str, str], @@ -83,7 +94,14 @@ def __init__( def __getitem__( self, source_path: str - ) -> VisualObject | RigidObject | Articulation | Robot | DeformableObject: + ) -> ( + VisualObject + | RigidObject + | Articulation + | Robot + | DeformableObject + | CableObject + ): return self._assets[source_path] def __iter__(self) -> Iterator[str]: diff --git a/embodichain/lab/task_program/integrations/_configured_composition.py b/embodichain/lab/task_program/integrations/_configured_composition.py index 61d4027a92..288279b301 100644 --- a/embodichain/lab/task_program/integrations/_configured_composition.py +++ b/embodichain/lab/task_program/integrations/_configured_composition.py @@ -58,6 +58,7 @@ "registered_semantic_lowerers", "control_part_evidence", "settle_presets", + "cable_material", } ) _TASK_RUNTIME_SERVICE_FIELDS = _RUNTIME_SERVICE_FIELDS | frozenset( @@ -265,6 +266,12 @@ def _merge_runtime_services( evidence = task_evidence if task_evidence is not None else skill_profile_evidence if evidence is not None: result["control_part_evidence"] = deepcopy(evidence) + if "cable_material" in skill_profile and "cable_material" in task: + raise ValueError("cable_material must have exactly one owner.") + if "cable_material" in task or "cable_material" in skill_profile: + result["cable_material"] = deepcopy( + task.get("cable_material", skill_profile.get("cable_material")) + ) if "nominal_motion_sources" in task: result["nominal_motion_sources"] = _owned_sequence( task["nominal_motion_sources"], @@ -662,6 +669,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 3fd384b1a2..9b0bbc139c 100644 --- a/embodichain/lab/task_program/integrations/catalog.py +++ b/embodichain/lab/task_program/integrations/catalog.py @@ -233,6 +233,23 @@ def _canonical_value(value: object) -> object: metadata.pop("supports_segment_motion_recovery") if type(value) is MotionPolicy and value.final_velocity_rejection == "hold": metadata.pop("final_velocity_rejection") + # Inactive additions do not change an existing integration's identity. + from embodichain.lab.sim.atomic_skills.primitives.move_joints import ( + MoveJointsOptions, + ) + from embodichain.lab.sim.atomic_skills.primitives.move_end_effector import ( + MoveEndEffectorOptions, + ) + + if type(value) is SimulationSceneBinding and not value.cables: + metadata.pop("cables") + if type(value) is MoveJointsOptions and not value.preserve_samples: + metadata.pop("preserve_samples") + if ( + type(value) is MoveEndEffectorOptions + and not value.preserve_cartesian_samples + ): + metadata.pop("preserve_cartesian_samples") if type(value) is RecoveryPolicy and value.retry_all_failed_plans is True: metadata.pop("retry_all_failed_plans") if isinstance(value, PickUpOptions) and value.initial_lift_height == 0.0: diff --git a/embodichain/lab/task_program/integrations/configured.py b/embodichain/lab/task_program/integrations/configured.py index 9af6c9ecd6..65c3707e4a 100644 --- a/embodichain/lab/task_program/integrations/configured.py +++ b/embodichain/lab/task_program/integrations/configured.py @@ -143,6 +143,9 @@ ) if TYPE_CHECKING: + from embodichain_tasks.manipulation.cable.wrap_integration import ( + CableWrapPhaseFactory, + ) from .environment import TaskProgramEnvironmentAdapter __all__: list[str] = [] @@ -825,6 +828,7 @@ def _decode_scene(value: object) -> SimulationSceneBinding: optional=frozenset( { "rigid_objects", + "cables", "rigidized_articulations", "articulations", "links", @@ -833,6 +837,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] = [] @@ -881,6 +902,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, @@ -1015,6 +1037,9 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: optional=frozenset( { "hand_interp_steps", + "preserve_samples", + "preserve_velocity_samples", + "preserve_cartesian_samples", "hold_steps", "release", "release_at_target", @@ -1265,15 +1290,30 @@ 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", "preserve_velocity_samples"}), + ) + return MoveJointsOptions( + preserve_samples=_boolean(config.get("preserve_samples", False), path=path), + preserve_velocity_samples=_boolean( + config.get("preserve_velocity_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, @@ -1705,6 +1745,7 @@ def _decode_motion_policy(value: object, *, path: str) -> MotionPolicy: "velocity_targets", "plan_opts", "interpolation_mode", + "interpolate_position_step", "ik_seed_mode", "sample_count_retries", "segment_sample_count_retries", @@ -1772,7 +1813,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 @@ -2539,8 +2584,16 @@ def _decode_registered_lowerer( | _PushObjectLowererFactory | _RelativePlaceLowererFactory | _KinematicReplayFactory + | CableWrapPhaseFactory ): """Decode one allowlisted registered semantic lowerer factory.""" + if isinstance(value, Mapping) and value.get("kind") == "cable_wrap_phase": + from embodichain_tasks.manipulation.cable.wrap_integration import ( + CableWrapPhaseFactory, + ) + + _mapping(value, path=path, required=frozenset({"kind"})) + return CableWrapPhaseFactory() if isinstance(value, Mapping) and value.get("kind") == "kinematic_joint_replay": return _decode_kinematic_replay(value, path=path) if isinstance(value, Mapping) and value.get("kind") in { @@ -3011,11 +3064,13 @@ class _DecodedRuntimeServices: | _PourLowererFactory | _PushObjectLowererFactory | _RelativePlaceLowererFactory - | _KinematicReplayFactory, + | _KinematicReplayFactory + | CableWrapPhaseFactory, ..., ] = () control_part_evidence: _JointPositionConstraintEvidenceProviderFactory | None = None nominal_motion_sources: tuple[_ConfiguredPrototypeSource, ...] = () + cable_material: object | None = None settle_presets: ( tuple[ tuple[ @@ -3132,6 +3187,7 @@ def _decode_runtime_services( "registered_semantic_lowerers", "control_part_evidence", "nominal_motion_sources", + "cable_material", "settle_presets", } ), @@ -3182,6 +3238,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, @@ -3194,6 +3263,16 @@ def _decode_runtime_services( ) ) ) + if cable is not None: + lowerers += tuple( + factory(cable) + for factory in ( + _CablePickFactory, + _CableMoveFactory, + _CableJointFactory, + _CableCheckFactory, + ) + ) lowerer_call_ids = tuple(factory.call_id for factory in lowerers) if len(set(lowerer_call_ids)) != len(lowerer_call_ids): raise ValueError( @@ -3350,6 +3429,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=( @@ -3400,6 +3480,8 @@ def _decode_evaluation( robot_profile: SimulationRobotSkillProfileBinding | None = None, robot_links: Mapping[str, str] | None = None, registry: PredicateRegistry | None = None, + cable_material: object | None = None, + cable_wrap: bool = False, transport_targets: Mapping[str, _MoveHeldObjectLowererFactory] | None = None, ) -> tuple[dict[str, CompiledEvaluation], tuple[str, ...], tuple[str, ...]]: if value is None: @@ -3416,6 +3498,21 @@ 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}" + ) + if cable_wrap: + from embodichain_tasks.manipulation.cable.wrap_measurements import WRAP_CHECKS + + for binding in scene.cables: + for name in WRAP_CHECKS: + channels[f"cables/{binding.entity_id}/wrap_{name}"] = ( + f"cables/{binding.simulation_uid}/wrap_{name}" + ) articulations = { binding.entity_id: binding.simulation_uid for binding in scene.articulations } @@ -3583,6 +3680,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 @@ -3603,6 +3710,11 @@ def _decode_configured_task_program_integration( robot_profile=robot_profile_binding, robot_links=evaluation_robot_links, registry=evaluation_registry, + cable_material=services.cable_material, + cable_wrap=any( + factory.call_id == "simulation.cable_wrap_phase" + for factory in services.registered_semantic_lowerers + ), transport_targets={ factory.target_id: factory for factory in services.registered_semantic_lowerers diff --git a/embodichain/lab/task_program/integrations/simulation/bindings.py b/embodichain/lab/task_program/integrations/simulation/bindings.py index e09c521ab2..fa9db7642b 100644 --- a/embodichain/lab/task_program/integrations/simulation/bindings.py +++ b/embodichain/lab/task_program/integrations/simulation/bindings.py @@ -202,6 +202,18 @@ def _validate_scene_classification( raise TypeError("collision_role must be a SceneCollisionRole value.") +@dataclass(frozen=True, slots=True) +class _SimulationCableBinding: + """Material-state identity, deliberately without a rigid object pose.""" + + entity_id: str + simulation_uid: str + + def __post_init__(self) -> None: + _identifier(self.entity_id, field_name="entity_id") + _identifier(self.simulation_uid, field_name="simulation_uid") + + @dataclass(frozen=True, slots=True) class SimulationRigidObjectBinding: """Explicit binding for one simulation rigid object.""" @@ -816,6 +828,7 @@ class SimulationSceneBinding: """Build one authoritative registry from explicit simulation bindings.""" registry_id: str + cables: tuple[_SimulationCableBinding, ...] = field(default=(), kw_only=True) rigid_objects: tuple[SimulationRigidObjectBinding, ...] = () rigidized_articulations: tuple[ SimulationRigidizedArticulationObjectBinding, ... @@ -833,6 +846,7 @@ class SimulationSceneBinding: def __post_init__(self) -> None: _identifier(self.registry_id, field_name="registry_id") expected_types = { + "cables": _SimulationCableBinding, "rigid_objects": SimulationRigidObjectBinding, "rigidized_articulations": SimulationRigidizedArticulationObjectBinding, "articulations": SimulationArticulationBinding, @@ -1059,6 +1073,11 @@ def build(self, simulation: SimulationManager) -> SceneRegistry: Returns: Immutable registry with typed roots, links, and affordances. """ + for binding in self.cables: + if simulation.get_cable_object(binding.simulation_uid) is None: + raise ValueError( + f"Cable {binding.simulation_uid!r} is absent from the live scene." + ) objects = {item.entity_id: item for item in self.rigid_objects} rigidized_articulations = { item.entity_id: item for item in self.rigidized_articulations diff --git a/embodichain/lab/task_program/integrations/simulation/cable.py b/embodichain/lab/task_program/integrations/simulation/cable.py new file mode 100644 index 0000000000..117be006a3 --- /dev/null +++ b/embodichain/lab/task_program/integrations/simulation/cable.py @@ -0,0 +1,898 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Material-coordinate grounding and measured contact for configured programs. + +This service owns no task sequence, controller, motion generator or simulator +step. Program calls use the ordinary Atomic Skills and Gym command bridge. +""" + +from __future__ import annotations + +from collections.abc import Mapping +from dataclasses import dataclass +import math +from typing import ClassVar, Any + +import torch + +from embodichain.compute.geometry.cable import sample_cable_material, cable_endpoints +from embodichain.compute.geometry._cable_route import _measure_cable_route +from embodichain.lab.sim.atomic_skills import ( + AtomicSkillEngine, + SkillControlOverrides, + JointPositionCommand, + SkillDescriptor, + PlanningContext, + SkillOptions, +) +from embodichain.lab.sim.atomic_skills.primitives.pick_up import ( + PickUp, + ContactGraspGoal, +) +from embodichain.lab.sim.atomic_skills.primitives.move_end_effector import ( + MoveEndEffector, + EndEffectorPoseGoal, +) +from embodichain.lab.sim.atomic_skills.primitives.move_joints import ( + MoveJoints, + JointPositionGoal, +) +from embodichain.lab.sim.motion.planners.utils import interpolate_xpos_batched +from embodichain.lab.task_program.compiler.lowering import ( + RegisteredSemanticLowerer, + SemanticLowering, +) +from embodichain.lab.task_program.language.semantic_contracts import ( + SemanticCallContract, +) +from embodichain.lab.task_program.semantics import ( + RegisteredSemanticCall, + BoundSemanticCall, + SceneRegistry, +) +from ..extensions import RegisteredSemanticLowererFactory + +__all__: list[str] = [] + +from embodichain.compute.geometry._cable_route import _CABLE_METRICS as _METRICS + + +@dataclass(frozen=True, slots=True) +class _CableTarget: + target_id: str + kind: str + material_distance: float | None = None + retract_distance: float = 0.0 + position: tuple[float | None, float | None, float | None] = (None, None, None) + speed: float = 0.05 + tolerance: float = 0.02 + orientation_tolerance: float = 0.25 + port_screen: bool = False + firm: bool = False + stop_when_seated: bool = False + maximum_correction: float = 0.005 + maximum_z_correction: float = 0.003 + minimum_z: float | None = None + maximum_y: float | None = None + samples: int = 2 + finger_position: float = 0.0475 + condition: str | None = None + release_part: str | None = None + + +@dataclass(frozen=True, slots=True) +class _CableService: + entity_id: str + simulation_uid: str + targets: tuple[_CableTarget, ...] + geometry: tuple[tuple[str, object], ...] + motion_parts: tuple[str, ...] + slip_limit: float = 0.012 + + def target(self, target_id: str) -> _CableTarget: + for target in self.targets: + if target.target_id == target_id: + return target + raise ValueError(f"Unknown cable target {target_id!r}.") + + +class _CableRuntime: + """One bridge's references and metrics, in explicit native arena order.""" + + def __init__( + self, cfg: _CableService, simulation: Any, robot: Any, env_ids: torch.Tensor + ) -> None: + self.cfg, self.simulation, self.robot = cfg, simulation, robot + self.env_ids = env_ids.clone() + self.cable = simulation.get_cable_object(cfg.simulation_uid) + if self.cable is None: + raise ValueError(f"Cable {cfg.simulation_uid!r} is absent.") + arenas = tuple(self.cable.data.batch.arena_indices) + by_arena = {int(value): row for row, value in enumerate(arenas)} + try: + selected_arenas = [ + simulation.spawn_result.arenas.index( + robot.body_data.entities[int(value)].arena_name + ) + for value in env_ids.tolist() + ] + rows = [by_arena[value] for value in selected_arenas] + except KeyError as exc: + raise ValueError( + "The cable does not cover the selected environment rows." + ) from exc + self.rows = torch.tensor(rows, device=env_ids.device) + points = torch.as_tensor( + self.cable.cfg.points, device=env_ids.device, dtype=torch.float32 + ) + self.lengths = points.diff(dim=0).norm(dim=-1) + origins = torch.as_tensor( + simulation.spawn_result.arenas.root_offsets, + device=env_ids.device, + dtype=torch.float32, + ) + self.origins = origins.index_select( + 0, torch.tensor(selected_arenas, device=env_ids.device) + ) + self.geometry = dict(cfg.geometry) + self._scope_guard = lambda: None + self.home = {part: self.tool(part).clone() for part in cfg.motion_parts} + self.pending_pick: tuple[str, _CableTarget] | None = None + self.hand_to_motion: dict[str, str] = {} + self.grasps: dict[str, tuple[float, torch.Tensor]] = {} + self.max_slip = torch.zeros(len(rows), device=env_ids.device) + self.grasp_slip: dict[str, torch.Tensor] = {} + self.last_targets: dict[str, torch.Tensor] = {} + self.pick_initial: dict[str, tuple[float, torch.Tensor]] = {} + self.port_yaw: dict[str, torch.Tensor] = {} + self.closed_reference_samples: list[dict[str, object]] = [] + self._metric_cache_time: float | None = None + self._metric_cache: dict[str, torch.Tensor] = {} + for target in cfg.targets: + if target.material_distance is not None: + # Configuration errors fail before the first command. + sample_cable_material( + self.state(), self.lengths, target.material_distance + ) + + def state(self) -> torch.Tensor: + self._scope_guard() + return self.cable.data.segment_state_w.index_select(0, self.rows) + + def tool( + self, part: str, *, commanded: bool = False, world: bool = False + ) -> torch.Tensor: + self._scope_guard() + joints = self.robot.get_joint_ids(name=part) + qpos = self.robot.get_qpos(target=commanded).index_select(0, self.env_ids)[ + :, joints + ] + pose = self.robot.compute_fk( + qpos, name=part, to_matrix=True, env_ids=self.env_ids + ).clone() + if world: + pose[:, :3, 3] += self.origins + return pose + + def point(self, distance: float) -> tuple[torch.Tensor, torch.Tensor, int]: + return sample_cable_material(self.state(), self.lengths, distance) + + def point_in_tool(self, part: str, distance: float) -> torch.Tensor: + point = self.point(distance)[0] + tool = self.tool(part, world=True) + return ( + tool[:, :3, :3].transpose(1, 2) @ (point - tool[:, :3, 3]).unsqueeze(-1) + ).squeeze(-1) + + def observe(self) -> torch.Tensor: + for part, (distance, baseline) in self.grasps.items(): + slip = (self.point_in_tool(part, distance) - baseline).norm(dim=-1) + self.grasp_slip[part] = torch.maximum(self.grasp_slip[part], slip) + self.max_slip = torch.maximum(self.max_slip, slip) + state = self.state() + invalid = ~torch.isfinite(state).all(dim=(1, 2)) | ~( + state[..., 3:7].norm(dim=-1) > 1e-8 + ).all(-1) + return (self.max_slip > self.cfg.slip_limit) | invalid + + def metrics(self) -> dict[str, torch.Tensor]: + self._scope_guard() + timestamp = getattr(self.simulation, "simulation_time", None) + if timestamp is not None and timestamp == self._metric_cache_time: + return {key: value.clone() for key, value in self._metric_cache.items()} + self.observe() + hands = torch.stack( + [self.tool(part, world=True)[:, :3, 3] for part in self.cfg.motion_parts], 1 + ) + result = _measure_cable_route( + self.state(), + self.lengths, + hands, + self.origins, + self.geometry, + float(self.cable.cfg.radius), + ) + result["max_grasp_slip"] = self.max_slip.clone() + if timestamp is not None: + self._metric_cache_time = timestamp + self._metric_cache = {key: value.clone() for key, value in result.items()} + return result + + def seated(self) -> torch.Tensor: + measurements = self.metrics() + return measurements["release_ready"] & torch.stack( + [ + measurements[key] <= 0.001 + for key in ( + "port_penetration", + "bench_penetration", + "spool_penetration", + ) + ] + ).all(0) + + def after_action(self, action: Any, runtime: Any) -> tuple[torch.Tensor, str]: + """Capture only after Gym acknowledges the final physical close step.""" + self._scope_guard() + phase = ( + runtime._last_sent_command_phase() + if hasattr(runtime, "_last_sent_command_phase") + else None + ) + if ( + self.pending_pick is not None + and action.metadata.get("bridge_action_kind") == "runtime_command" + and phase is not None + ): + if phase[0] == "close" and phase[1]: + part, target = self.pending_pick + distance = target.material_distance + assert distance is not None + self.grasps[part] = ( + distance, + self.point_in_tool(part, distance).clone(), + ) + self.grasp_slip[part] = torch.zeros_like(self.max_slip) + self.pending_pick = None + self.closed_reference_samples.append( + { + "part": part, + "material_distance": distance, + "phase": "close", + "sample_index": phase[2], + } + ) + return self.observe(), "cable_material_state_or_slip_guard_failed" + + def _material_pose(self, target: _CableTarget, part: str) -> torch.Tensor: + assert target.material_distance is not None + point, tangent, _ = self.point(target.material_distance) + yaw = torch.atan2(tangent[:, 1], tangent[:, 0]) + candidates = [] + joints = self.robot.get_joint_ids(name=part) + seed = self.robot.get_qpos().index_select(0, self.env_ids)[:, joints] + for branch, angle in enumerate((yaw, yaw + math.pi)): + pose = torch.eye(4, device=point.device).repeat(len(point), 1, 1) + c, s = torch.cos(angle), torch.sin(angle) + pose[:, 0, 0], pose[:, 0, 1] = c, s + pose[:, 1, 0], pose[:, 1, 1] = s, -c + pose[:, 2, 2] = -1 + pose[:, :3, 3] = point - self.origins + if target.position[2] is not None: + pose[:, 2, 3] = target.position[2] + success, qpos = self.robot.compute_ik( + pose, joint_seed=seed, name=part, env_ids=self.env_ids + ) + if target.port_screen: + stage = pose.clone() + port_angle = math.pi / 2 if branch == 0 else -math.pi / 2 + stage[:, 0, 0], stage[:, 0, 1] = math.cos(port_angle), math.sin( + port_angle + ) + stage[:, 1, 0], stage[:, 1, 1] = math.sin(port_angle), -math.cos( + port_angle + ) + stage[:, :3, 3] = point.new_tensor(self.geometry["port_stage"]) + later, _ = self.robot.compute_ik( + stage, joint_seed=qpos, name=part, env_ids=self.env_ids + ) + success = success.reshape(-1).bool() & later.reshape(-1).bool() + candidates.append((success.reshape(-1).bool(), qpos, pose)) + first, second = candidates + choose = second[0] & ( + ~first[0] + | ((second[1] - seed).square().sum(-1) < (first[1] - seed).square().sum(-1)) + ) + if not bool((first[0] | second[0]).all()): + raise ValueError("No reachable wrist branch for the cable material target.") + if target.port_screen: + self.port_yaw[part] = torch.where(choose, -math.pi / 2, math.pi / 2) + return torch.where(choose[:, None, None], second[2], first[2]) + + def pose(self, target: _CableTarget, part: str) -> torch.Tensor: + if target.kind == "material": + return self._material_pose(target, part) + if target.kind == "home": + return self.home[part].clone() + pose = self.tool(part, commanded=target.kind in {"command", "tip_align"}) + if target.kind == "port_stage": + angle = self.port_yaw[part] + c, s = torch.cos(angle), torch.sin(angle) + # Replace the complete rotation; measured tilt is not part of this + # downward staging orientation and would make it non-orthogonal. + pose[:, :3, :3] = 0 + pose[:, 0, 0], pose[:, 0, 1] = c, s + pose[:, 1, 0], pose[:, 1, 1] = s, -c + pose[:, 2, 2] = -1 + if target.kind in {"yaw_align", "ray_align"}: + assert target.material_distance is not None + point = self.point(target.material_distance)[0] - self.origins + end = cable_endpoints(self.state(), self.lengths)[1][:, -1] - self.origins + direction = end - point + if target.kind == "yaw_align": + angle = torch.atan2(direction[:, 0], direction[:, 1]) + c, s = torch.cos(angle), torch.sin(angle) + rotation = torch.eye(3, device=pose.device).repeat(len(pose), 1, 1) + rotation[:, 0, 0], rotation[:, 0, 1] = c, -s + rotation[:, 1, 0], rotation[:, 1, 1] = s, c + pose[:, :3, :3] = rotation @ pose[:, :3, :3] + else: + if not bool((direction[:, 1] > 1e-6).all()): + raise ValueError( + "The distal cable span points away from the channel." + ) + mouth_x, mouth_y = self.geometry["port_mouth"] + meets = point[:, 0] + direction[:, 0] / direction[:, 1] * ( + mouth_y - point[:, 1] + ) + pose[:, 0, 3] += mouth_x - meets + elif target.kind == "material_lift": + assert target.material_distance is not None + assert target.position[2] is not None + tangent = self.point(target.material_distance)[1][:, :2] + horizontal_norm = tangent.norm(dim=-1, keepdim=True) + if not bool((horizontal_norm > 1e-6).all()): + raise ValueError( + "Material lift requires a cable tangent with a horizontal direction." + ) + pose[:, :2, 3] -= target.retract_distance * tangent / horizontal_norm + pose[:, 2, 3] = target.position[2] + elif target.kind == "tip_align": + end = cable_endpoints(self.state(), self.lengths)[1][:, -1] - self.origins + for axis, value in enumerate(target.position): + if value is not None: + pose[:, axis, 3] += (value - end[:, axis]).clamp( + -( + target.maximum_z_correction + if axis == 2 + else target.maximum_correction + ), + ( + target.maximum_z_correction + if axis == 2 + else target.maximum_correction + ), + ) + if target.maximum_y is not None: + pose[:, 1, 3].clamp_(max=target.maximum_y) + if target.minimum_z is not None: + pose[:, 2, 3].clamp_(min=target.minimum_z) + else: + for axis, value in enumerate(target.position): + if value is not None: + pose[:, axis, 3] = value + if target.stop_when_seated: + # Retain the loading target after seating; holding the measured + # pose would unload contact forces before the configured release. + commanded = self.tool(part, commanded=True) + pose = torch.where(self.seated()[:, None, None], commanded, pose) + return pose + + +class _CableLowerer(RegisteredSemanticLowerer): + preserves_symbolic_state: ClassVar[bool] = True + + def __init__(self, cfg: _CableService) -> None: + self.cfg = cfg + self.runtime: _CableRuntime | None = None + + def _target( + self, call: RegisteredSemanticCall + ) -> tuple[_CableRuntime, _CableTarget]: + if dict(call.arguments).keys() != {"target"}: + raise ValueError( + "Cable calls require exactly one declared target identifier." + ) + if self.runtime is None: + raise RuntimeError("Cable service was not bound to this runtime assembly.") + return self.runtime, self.cfg.target(call.arguments["target"]) + + @staticmethod + def _part(bound: BoundSemanticCall, endpoint: str = "motion") -> str: + return ( + bound.binding.resources["primary"] + .endpoints[endpoint] + .runtime_target.control_part + ) + + +class _CablePickLowerer(_CableLowerer): + call_id: ClassVar[str] = "simulation.cable_pick" + target_descriptor: ClassVar[SkillDescriptor] = PickUp.descriptor() + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: SkillOptions, + ) -> SemanticLowering: + service, target = self._target(call) + if target.kind != "material": + raise ValueError("Cable pickup requires a material target.") + part = self._part(bound) + service.pending_pick = (part, target) + service.hand_to_motion[self._part(bound, "grasp")] = part + assert target.material_distance is not None + service.pick_initial[part] = ( + target.material_distance, + service.point(target.material_distance)[0][:, 2].clone(), + ) + overrides = SkillControlOverrides() + if target.firm: + grasp_part = self._part(bound, "grasp") + count = len(service.robot.get_joint_ids(name=grasp_part)) + overrides = SkillControlOverrides( + endpoints={ + "primary": { + "grasp": { + "grasp": JointPositionCommand( + torch.zeros(count, device=context.env_ids.device) + ) + } + } + } + ) + return SemanticLowering( + goal=ContactGraspGoal(service.pose(target, part)), + control_overrides=overrides, + ) + + +class _CableMoveLowerer(_CableLowerer): + call_id: ClassVar[str] = "simulation.cable_move" + target_descriptor: ClassVar[SkillDescriptor] = MoveEndEffector.descriptor() + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: SkillOptions, + ) -> SemanticLowering: + service, target = self._target(call) + part = self._part(bound) + start, end = service.tool(part), service.pose(target, part) + if target.stop_when_seated: + # Seated rows retain the loaded command throughout the path; + # interpolating from their measured pose would first unload it. + start = torch.where(service.seated()[:, None, None], end, start) + distance = (start[:, :3, 3] - end[:, :3, 3]).norm(dim=-1).max().item() + trace = ( + (start[:, :3, :3].transpose(1, 2) @ end[:, :3, :3]) + .diagonal(dim1=-2, dim2=-1) + .sum(-1) + ) + angle = torch.acos(((trace - 1) / 2).clamp(-1, 1)).max().item() + count = max( + 3, + math.ceil( + max(distance / target.speed, angle / 0.5) / context.require_control_dt() + ) + + 1, + ) + service.last_targets[part] = end.clone() + return SemanticLowering( + goal=EndEffectorPoseGoal(interpolate_xpos_batched(start, end, count)[:, 1:]) + ) + + +class _CableJointLowerer(_CableLowerer): + call_id: ClassVar[str] = "simulation.cable_joint" + target_descriptor: ClassVar[SkillDescriptor] = MoveJoints.descriptor() + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: SkillOptions, + ) -> SemanticLowering: + service, target = self._target(call) + part = self._part(bound) + ids = service.robot.get_joint_ids(name=part) + start = service.robot.get_qpos(target=True).index_select(0, context.env_ids)[ + :, ids + ] + path = start[:, None, :].repeat(1, target.samples, 1) + if target.kind == "open": + # Only release the grasp whose configured gripper is being opened. + motion_part = target.release_part + if ( + motion_part not in self.cfg.motion_parts + or service.hand_to_motion.get(part) != motion_part + ): + raise ValueError("Opening a hand requires its declared motion part.") + service.grasps.pop(motion_part, None) + service.pending_pick = None + path = torch.lerp( + start[:, None], + torch.full_like(start[:, None], target.finger_position), + torch.linspace(0, 1, target.samples, device=start.device)[ + None, :, None + ], + ) + elif target.kind != "hold": + raise ValueError("Cable joint targets must be open or hold declarations.") + return SemanticLowering(goal=JointPositionGoal(path)) + + +class _CableCheckLowerer(_CableLowerer): + call_id: ClassVar[str] = "simulation.cable_check" + target_descriptor: ClassVar[SkillDescriptor] = MoveJoints.descriptor() + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: SkillOptions, + ) -> SemanticLowering: + service, target = self._target(call) + part = self._part(bound) + passed = ~service.observe() + details = {} + if target.condition == "pose": + expected, actual = service.last_targets[part], service.tool(part) + error = (actual[:, :3, 3] - expected[:, :3, 3]).norm(dim=-1) + trace = ( + (actual[:, :3, :3].transpose(1, 2) @ expected[:, :3, :3]) + .diagonal(dim1=-2, dim2=-1) + .sum(-1) + ) + angle = torch.acos(((trace - 1) / 2).clamp(-1, 1)) + passed &= (error <= target.tolerance) & ( + angle <= target.orientation_tolerance + ) + if target.stop_when_seated: + # Above-channel release alignment still requires ordinary TCP + # tracking. Only an inserted endpoint may retain contact bias. + passed |= ( + service.seated() + & service.metrics()["strict_port"] + & ~service.observe() + ) + details = { + "position_error_m": error.tolist(), + "orientation_error_rad": angle.tolist(), + } + elif target.condition == "lift": + distance, before = service.pick_initial[part] + lift = service.point(distance)[0][:, 2] - before + passed &= (lift > 0.008) & (part in service.grasps) + details = {"material_lift_m": lift.tolist()} + elif target.condition == "seated": + passed &= service.seated() + else: + raise ValueError("Unknown declared cable precondition.") + if not bool(passed.all()): + raise ValueError( + f"Cable condition {target.target_id!r} failed in envs {context.env_ids[~passed].tolist()}: {details}; max_slip_m={service.max_slip.tolist()}" + ) + ids = service.robot.get_joint_ids(name=part) + qpos = service.robot.get_qpos(target=True).index_select(0, context.env_ids)[ + :, ids + ] + return SemanticLowering(goal=JointPositionGoal(qpos[:, None].repeat(1, 2, 1))) + + +@dataclass(frozen=True, slots=True) +class _CableFactory(RegisteredSemanticLowererFactory): + cfg: _CableService + revision: ClassVar[str] = "1" + lowerer_type: ClassVar[type[_CableLowerer]] + + @property + def argument_contract(self) -> SemanticCallContract: + return SemanticCallContract( + required={ + "target": tuple( + target.target_id + for target in self.cfg.targets + if self._supports(target) + ) + }, + optional={}, + ) + + def _supports(self, target: _CableTarget) -> bool: + if self.call_id == "simulation.cable_pick": + return target.kind == "material" + if self.call_id == "simulation.cable_joint": + return target.kind in {"open", "hold"} + if self.call_id == "simulation.cable_check": + return target.kind == "check" + return target.kind not in {"open", "hold", "check"} + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicSkillEngine, + ) -> RegisteredSemanticLowerer: + if engine.robot is not robot: + raise ValueError("Cable services must use the engine's exact robot.") + lowerer = self.lowerer_type(self.cfg) + selected = engine.planning_services.env_ids + ids = ( + torch.arange(robot.get_qpos().shape[0], device=engine.device) + if selected is None + else torch.tensor(selected, dtype=torch.long, device=engine.device) + ) + lowerer.runtime = _CableRuntime(self.cfg, simulation, robot, ids) + return lowerer + + +@dataclass(frozen=True, slots=True) +class _CablePickFactory(_CableFactory): + call_id: ClassVar[str] = _CablePickLowerer.call_id + target_descriptor: ClassVar[SkillDescriptor] = PickUp.descriptor() + lowerer_type: ClassVar[type[_CableLowerer]] = _CablePickLowerer + + +@dataclass(frozen=True, slots=True) +class _CableMoveFactory(_CableFactory): + call_id: ClassVar[str] = _CableMoveLowerer.call_id + target_descriptor: ClassVar[SkillDescriptor] = MoveEndEffector.descriptor() + lowerer_type: ClassVar[type[_CableLowerer]] = _CableMoveLowerer + + +@dataclass(frozen=True, slots=True) +class _CableJointFactory(_CableFactory): + call_id: ClassVar[str] = _CableJointLowerer.call_id + target_descriptor: ClassVar[SkillDescriptor] = MoveJoints.descriptor() + lowerer_type: ClassVar[type[_CableLowerer]] = _CableJointLowerer + + +@dataclass(frozen=True, slots=True) +class _CableCheckFactory(_CableFactory): + call_id: ClassVar[str] = _CableCheckLowerer.call_id + target_descriptor: ClassVar[SkillDescriptor] = MoveJoints.descriptor() + lowerer_type: ClassVar[type[_CableLowerer]] = _CableCheckLowerer + + +def _decode_cable_service(value: object) -> _CableService: + from ..configured import _mapping, _identifier, _real, _integer, _boolean, _sequence + + config = _mapping( + value, + path="cable_material", + required=frozenset( + {"entity_id", "simulation_uid", "motion_parts", "geometry", "targets"} + ), + optional=frozenset({"slip_limit"}), + ) + targets = [] + fields = { + field + for field in _CableTarget.__dataclass_fields__ + if field not in {"target_id", "kind"} + } + for name, item in _mapping( + config["targets"], + path="cable_material.targets", + required=frozenset(), + optional=frozenset(config["targets"]), + ).items(): + path = f"cable_material.targets.{name}" + row = _mapping( + item, path=path, required=frozenset({"kind"}), optional=frozenset(fields) + ) + kind = _identifier(row["kind"], path=path) + motion_fields = {"speed", "tolerance", "orientation_tolerance"} + allowed = { + "material": motion_fields + | {"material_distance", "position", "port_screen", "firm"}, + "material_lift": motion_fields + | {"material_distance", "retract_distance", "position"}, + "measured": motion_fields | {"position", "stop_when_seated"}, + "command": motion_fields | {"position", "stop_when_seated"}, + "home": motion_fields, + "port_stage": motion_fields | {"position"}, + "yaw_align": motion_fields | {"material_distance"}, + "ray_align": motion_fields | {"material_distance"}, + "tip_align": motion_fields + | { + "position", + "maximum_correction", + "maximum_z_correction", + "minimum_z", + "maximum_y", + "stop_when_seated", + }, + "hold": {"samples"}, + "open": {"samples", "finger_position", "release_part"}, + "check": { + "condition", + "tolerance", + "orientation_tolerance", + "stop_when_seated", + }, + } + if kind in allowed and set(row) - {"kind"} - allowed[kind]: + raise ValueError(f"Unsupported fields for cable target kind {kind!r}.") + if kind not in { + "material", + "material_lift", + "measured", + "command", + "home", + "port_stage", + "yaw_align", + "ray_align", + "tip_align", + "hold", + "open", + "check", + }: + raise ValueError(f"Unsupported cable target kind {kind!r}") + kwargs = {"target_id": _identifier(name, path=path), "kind": kind} + for key, val in row.items(): + if key == "kind": + continue + if key == "position": + values = _sequence(val, path=path) + if len(values) != 3: + raise ValueError("Cable position needs exactly three axes.") + kwargs[key] = tuple( + None if v is None else _real(v, path=path) for v in values + ) + elif key in {"port_screen", "firm", "stop_when_seated"}: + kwargs[key] = _boolean(val, path=path) + elif key == "samples": + kwargs[key] = _integer(val, path=path, minimum=1) + elif key in {"condition", "release_part"}: + kwargs[key] = _identifier(val, path=path) + else: + kwargs[key] = _real(val, path=path) + target = _CableTarget(**kwargs) + if ( + target.speed <= 0 + or target.tolerance <= 0 + or target.orientation_tolerance <= 0 + or target.maximum_correction <= 0 + ): + raise ValueError("Cable motion limits must be positive.") + if kind in {"material", "material_lift", "yaw_align", "ray_align"} and ( + target.material_distance is None or target.material_distance < 0 + ): + raise ValueError("A cable material coordinate must be nonnegative.") + if kind == "material_lift": + if target.retract_distance < 0: + raise ValueError("Cable retraction distance must be nonnegative.") + if target.position[:2] != (None, None) or target.position[2] is None: + raise ValueError( + "Material lift position must specify only its Z height." + ) + if kind == "open" and target.release_part is None: + raise ValueError("An opening target requires its explicit release_part.") + if kind == "check" and target.condition not in {"pose", "lift", "seated"}: + raise ValueError("Unknown cable check condition.") + targets.append(target) + geometry = _mapping( + config["geometry"], + path="cable_material.geometry", + required=frozenset( + { + "bench_height", + "port_center", + "port_stage", + "port_mouth", + "seats", + "cylinders", + "boxes", + "strict_region", + "release_region", + } + ), + ) + + def freeze(value: object) -> object: + if isinstance(value, (list, tuple)): + return tuple(freeze(item) for item in value) + return _real(value, path="cable_material.geometry") + + frozen_geometry = {key: freeze(value) for key, value in geometry.items()} + expected_widths = {"port_center": 2, "port_stage": 3, "port_mouth": 2} + for key, width in expected_widths.items(): + if ( + not isinstance(frozen_geometry[key], tuple) + or len(frozen_geometry[key]) != width + ): + raise ValueError(f"Cable geometry {key!r} requires {width} coordinates.") + for key in ("strict_region", "release_region"): + bounds = frozen_geometry[key] + if ( + not isinstance(bounds, tuple) + or len(bounds) != 2 + or any(not isinstance(row, tuple) or len(row) != 3 for row in bounds) + or any(lo > hi for lo, hi in zip(*bounds)) + ): + raise ValueError(f"Cable geometry {key!r} requires ordered XYZ bounds.") + for key, width in (("seats", 3), ("cylinders", 5)): + if not isinstance(frozen_geometry[key], tuple) or any( + not isinstance(row, tuple) or len(row) != width + for row in frozen_geometry[key] + ): + raise ValueError(f"Cable geometry {key!r} has invalid rows.") + if any( + radius <= 0 or z0 >= z1 for _, _, radius, z0, z1 in frozen_geometry["cylinders"] + ): + raise ValueError("Cable obstacle cylinders require positive radii and heights.") + if len(frozen_geometry["seats"]) != 2: + raise ValueError( + "The two-seat cable route measurements require exactly two seats." + ) + if not isinstance(frozen_geometry["boxes"], tuple) or any( + not isinstance(row, tuple) + or len(row) != 2 + or any(not isinstance(vector, tuple) or len(vector) != 3 for vector in row) + or any(v <= 0 for v in row[1]) + for row in frozen_geometry["boxes"] + ): + raise ValueError( + "Cable obstacle boxes require centres and positive half sizes." + ) + parts = tuple( + _identifier(part, path="cable_material.motion_parts") + for part in _sequence( + config["motion_parts"], path="cable_material.motion_parts" + ) + ) + if not parts or len(set(parts)) != len(parts): + raise ValueError("Cable motion parts must be nonempty and unique.") + return _CableService( + entity_id=_identifier(config["entity_id"], path="cable_material.entity_id"), + simulation_uid=_identifier( + config["simulation_uid"], path="cable_material.simulation_uid" + ), + targets=tuple(targets), + geometry=tuple(frozen_geometry.items()), + motion_parts=parts, + slip_limit=_real( + config.get("slip_limit", 0.012), + path="cable_material.slip_limit", + minimum=0.0, + strict_minimum=True, + ), + ) diff --git a/embodichain/lab/task_program/integrations/simulation/environment.py b/embodichain/lab/task_program/integrations/simulation/environment.py index dfee0f64d9..5918bc3dd1 100644 --- a/embodichain/lab/task_program/integrations/simulation/environment.py +++ b/embodichain/lab/task_program/integrations/simulation/environment.py @@ -35,7 +35,7 @@ from copy import deepcopy from dataclasses import dataclass, fields import math -from weakref import ReferenceType, ref +from weakref import ReferenceType, ref, proxy from types import MappingProxyType from typing import Literal, Protocol, TYPE_CHECKING @@ -916,7 +916,13 @@ def validate_runtime_bridge_creation(self) -> None: Non-support factories retain their existing independent assembly behavior. """ self._owner_token.require_open() - if self._support_registry is None or self._runtime_bridge_ref is None: + has_cable = any( + factory.call_id.startswith("simulation.cable_") + for factory in self._registration.registered_semantic_lowerer_factories + ) + if ( + self._support_registry is None and not has_cable + ) or self._runtime_bridge_ref is None: return previous = self._runtime_bridge_ref() if previous is not None and not previous.execution_finished: @@ -939,7 +945,10 @@ def bind_runtime_bridge(self, bridge: TaskProgramDemoBridge) -> None: raise ValueError( "The runtime bridge must use this factory's assembled clock." ) - if self._support_registry is not None: + cable = getattr(self, "_pending_cable_runtime", None) + if cable is not None: + bridge.post_policy_port._cable_runtime = cable + if self._support_registry is not None or cable is not None: self._runtime_bridge_ref = ref(bridge) def close_runtime_resources(self) -> None: @@ -1115,7 +1124,8 @@ def create_registered_semantic_lowerers( engine: AtomicSkillEngine, ) -> tuple[RegisteredSemanticLowerer, ...]: """Create fresh registration-owned lowerers for this runtime assembly.""" - return self._registration.create_registered_semantic_lowerers( + self.validate_runtime_bridge_creation() + lowerers = self._registration.create_registered_semantic_lowerers( environment=( self._environment_ref() if self._environment_ref is not None else None ), @@ -1125,6 +1135,30 @@ def create_registered_semantic_lowerers( engine=engine, ) + from .cable import _CableLowerer, _CableRuntime + + cable_lowerers = tuple( + value for value in lowerers if isinstance(value, _CableLowerer) + ) + self._pending_cable_runtime = None + if cable_lowerers: + cfg = cable_lowerers[0].cfg + if any(value.cfg != cfg for value in cable_lowerers): + raise ValueError( + "One bridge must use one consistent cable service declaration." + ) + session = cable_lowerers[0].runtime + if session is None or not torch.equal(session.env_ids, self._env_ids): + raise ValueError( + "Cable service rows must match this factory's explicit selection." + ) + for lowerer in cable_lowerers: + # Registration remembers lowerer identities; it must not retain + # every episode's live cable service through that history. + lowerer.runtime = proxy(session) + self._pending_cable_runtime = session + return lowerers + def create_scene_registry(self) -> SceneRegistry: """Build one fresh authoritative registry from the task registration.""" self._owner_token.require_open() @@ -1210,6 +1244,11 @@ def create_planning_observation_provider( require_open=self._owner_token.require_open, runtime_scope_guard=runtime_scope_guard, ) + cable = getattr(self, "_pending_cable_runtime", None) + if cable is not None: + cable._scope_guard = _runtime_scope_validator( + self._owner_token.require_open, runtime_scope_guard + ) self._segment_policy_port.bind_clock(clock) self._support_clock = clock return SimulationPlanningObservationProvider( @@ -1220,7 +1259,9 @@ def create_planning_observation_provider( owner_token=self._owner_token, runtime_scope_guard=runtime_scope_guard, capture_root_pose=self._capture_root_pose, - hold_qpos_source=self._hold_qpos_source, + hold_qpos_source=( + "drive_target" if cable is not None else self._hold_qpos_source + ), ) def create_effect_evidence_providers( diff --git a/embodichain/lab/task_program/integrations/simulation/policies.py b/embodichain/lab/task_program/integrations/simulation/policies.py index 339d6185ba..cd6e14306b 100644 --- a/embodichain/lab/task_program/integrations/simulation/policies.py +++ b/embodichain/lab/task_program/integrations/simulation/policies.py @@ -450,6 +450,23 @@ def settle_preset_ids(self) -> tuple[str, ...]: """Return installed post-policy preset IDs in declaration order.""" return tuple(self._settle_presets) + def _observe_consumed_action( + self, action: Any, *, runtime: Any + ) -> tuple[torch.Tensor, torch.Tensor, str] | None: + """Measure provider-owned invariants after a host-acknowledged command. + + Returns row rejection and a proposed hold. The bridge remains the only + owner of command encoding and dispatch; this port never steps physics. + """ + cable = getattr(self, "_cable_runtime", None) + if cable is None: + return None + rejected, reason = cable.after_action(action, runtime) + if not bool(rejected.any()): + return None + hold = self._robot.get_qpos(target=True).index_select(0, self._env_ids) + return rejected, hold, reason + def new_task_program_session(self) -> SimulationSegmentPolicyPort: """Create fresh episode state while retaining the validated scene bindings. @@ -460,6 +477,7 @@ def new_task_program_session(self) -> SimulationSegmentPolicyPort: self._require_open() session = copy(self) session._hold_observer = None + session._cable_runtime = None session._target_observer = None session._post_policy_results = {} session._post_policy_success = {} diff --git a/embodichain/lab/task_program/runtime/executor.py b/embodichain/lab/task_program/runtime/executor.py index c0e0686cda..e6460e15f8 100644 --- a/embodichain/lab/task_program/runtime/executor.py +++ b/embodichain/lab/task_program/runtime/executor.py @@ -492,6 +492,17 @@ def last_sent_plan_reference(self) -> tuple[int, str | None, int] | None: """ return self._last_sent_plan_reference + def _last_sent_command_phase(self) -> tuple[str, bool, int] | None: + reference = self._last_sent_plan_reference + if reference is None or self._runner is None: + return None + plan = self._runner.session._plan + if plan is None: + return None + index = reference[2] + segment = plan.segment_at(index) + return segment.name, index == segment.stop - 1, index + @property def result(self) -> SemanticExecutionResult: """Return an immutable snapshot of the current workflow.""" diff --git a/examples/README.md b/examples/README.md index 16cc174e66..fbe9b1dfdd 100644 --- a/examples/README.md +++ b/examples/README.md @@ -151,3 +151,13 @@ capsules in the browser: ```bash python -m embodichain run-example sim/demo/cable_viser --num-envs 2 --port 8080 --duration 12 ``` + +## Franka rope wrapping + +The [Franka rope wrap Task Program](sim/demo/franka_rope_wrap/README.md) picks up a +cable from the floor, wraps it around a post and measures the grasp and winding. +Its compatibility entry point exports recorded frames and all 19 physical checks: + +```bash +python -m embodichain run-example sim/demo/franka_rope_wrap --output /tmp/franka-rope-wrap-run +``` diff --git a/examples/sim/demo/cable_two_posts.py b/examples/sim/demo/cable_two_posts.py new file mode 100644 index 0000000000..c10843a667 --- /dev/null +++ b/examples/sim/demo/cable_two_posts.py @@ -0,0 +1,416 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Contact-driven two-post cable preview through EmbodiChain's manager. + +The Cartesian gripper is a prescribed articulation. Only its fingers are +controlled: the cable has one fixed root and no moving attachments. +""" + +from __future__ import annotations + +import argparse +import hashlib +import json +from pathlib import Path +import subprocess +import time + +import cv2 +import numpy as np +from scipy.interpolate import make_interp_spline + +FPS = 60 +DURATION = 14.0 +RADIUS = 0.008 +POST_RADIUS = 0.085 +POSTS = ((0.0, 0.0), (0.0, 0.4)) +# t, x, y, z, finger centre distance, phase +WAYPOINTS = ( + (0.0, 0.0, -0.28, 0.020, 0.045, "APPROACH"), + (0.5, 0.0, -0.28, 0.020, 0.045, "APPROACH"), + (1.2, 0.0, -0.28, 0.020, 0.0125, "GRIP"), + (2.0, 0.0, -0.25, 0.035, 0.0125, "LIFT"), + (3.3, 0.17, -0.12, 0.035, 0.0125, "WRAP POST 1"), + (4.7, 0.17, 0.10, 0.035, 0.0125, "WRAP POST 1"), + (5.6, 0.09, 0.20, 0.035, 0.0125, "BETWEEN POSTS"), + (6.8, -0.17, 0.24, 0.035, 0.0125, "WRAP POST 2"), + (8.2, -0.17, 0.49, 0.035, 0.0125, "WRAP POST 2"), + (9.2, 0.0, 0.73, 0.035, 0.0125, "WRAP POST 2"), + (10.2, 0.0, 0.73, 0.035, 0.0125, "HOLD"), + (10.7, 0.0, 0.73, 0.035, 0.045, "RELEASE"), + (11.5, 0.0, 0.73, 0.22, 0.045, "RETREAT"), + (14.0, 0.0, 0.73, 0.22, 0.045, "OBSERVE"), +) + + +def control_at(t: float) -> tuple[np.ndarray, str]: + """Sample a smooth deterministic Cartesian gripper command.""" + for a, b in zip(WAYPOINTS[:-1], WAYPOINTS[1:]): + if t <= b[0]: + u = np.clip((t - a[0]) / (b[0] - a[0]), 0, 1) + u = u * u * (3 - 2 * u) + values = (1 - u) * np.array(a[1:5]) + u * np.array(b[1:5]) + return np.array([*values[:3], values[3], values[3]], np.float32), b[5] + return np.array([0, 0.73, 0.22, 0.045, 0.045], np.float32), "OBSERVE" + + +def initial_points(segments: int) -> np.ndarray: + """Sample one continuous slack shape uniformly by arc length.""" + waypoints = np.array( + [ + [0, -0.52], + [-0.36, -0.65], + [-0.57, -0.50], + [-0.40, -0.34], + [-0.12, -0.43], + [0, -0.39], + [0, -0.28], + ] + ) + spline = make_interp_spline(np.arange(len(waypoints)), waypoints, k=2) + dense = spline(np.linspace(0, len(waypoints) - 1, 3000)) + arc = np.r_[0, np.cumsum(np.linalg.norm(np.diff(dense, axis=0), axis=1))] + sample = np.linspace(0, arc[-1], segments + 1) + return np.column_stack( + [ + np.interp(sample, arc, dense[:, 0]), + np.interp(sample, arc, dense[:, 1]), + np.full_like(sample, 0.011), + ] + ).astype(np.float32) + + +def write_stage(path: Path) -> None: + """Author fixed posts and a five-axis parallel-jaw gripper in one URDF.""" + parts = [''] + + def link( + name: str, geometry: str = "", xyz: str = "0 0 0", color: str = ".3 .4 .5 1" + ) -> None: + parts.append( + f'' + ) + if geometry: + parts.append( + f'{geometry}' + ) + parts.append( + f'{geometry}' + ) + parts.append("") + + def joint( + name: str, + parent: str, + child: str, + kind: str = "fixed", + xyz: str = "0 0 0", + axis: str = "1 0 0", + ) -> None: + parts.append( + f'' + ) + + link("base", '', "-.15 .10 -.02", ".15 .20 .25 1") + for i, (x, y) in enumerate(POSTS): + link( + f"post_{i+1}", + f'', + "0 0 .11", + ".15 .60 .73 1", + ) + joint(f"post_{i+1}_fixed", "base", f"post_{i+1}", xyz=f"{x} {y} 0") + link("anchor", '', "0 0 0", ".95 .57 .12 1") + joint("anchor_fixed", "base", "anchor", xyz="0 -.52 .013") + for name, parent, axis in ( + ("x", "base", "1 0 0"), + ("y", "slide_x", "0 1 0"), + ("z", "slide_y", "0 0 1"), + ): + link(f"slide_{name}") + joint(f"slide_{name}_joint", parent, f"slide_{name}", "prismatic", axis=axis) + link("palm", '', "0 0 .085", ".7 .75 .8 1") + joint("palm_fixed", "slide_z", "palm") + for name, axis in (("left", "1 0 0"), ("right", "-1 0 0")): + link(f"finger_{name}", '', "0 0 .025", ".87 .9 .94 1") + joint(f"finger_{name}_joint", "palm", f"finger_{name}", "prismatic", axis=axis) + parts.append("") + path.write_text("\n".join(parts)) + + +def run(args: argparse.Namespace) -> None: + """Build with EmbodiChain, step cable physics, and record camera observations.""" + import dexsim + import newton + import warp as wp + from dexsim.engine.newton_physics import DexUniSolverCfg + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + from embodichain.lab.sim.cfg import ( + ArticulationCfg, + CableObjectCfg, + CablePhysicsCfg, + NewtonCollisionPropertiesCfg, + NewtonPhysicsCfg, + NewtonRigidBodyMaterialCfg, + RenderCfg, + DLSSCfg, + RigidBodyPhysicsCfg, + ) + from embodichain.lab.sim.sensors import CameraCfg + from embodichain.lab.sim.material import VisualMaterialCfg + from embodichain.utils.math import xyz_quat_to_4x4_matrix + + out = args.output.resolve() + out.mkdir(parents=True, exist_ok=True) + urdf = out / "two_post_stage.urdf" + write_stage(urdf) + points = initial_points(args.segments) + frames = round(min(args.duration, DURATION) * FPS) + times = np.arange(frames + 1) / FPS + controls = np.array([control_at(t)[0] for t in times]) + np.savez(out / "inputs.npz", points=points, controls=controls, times=times) + sim = SimulationManager( + SimulationManagerCfg( + headless=True, + startup_summary="compact", + robot_ik_gizmo=None, + render_cfg=RenderCfg( + renderer="hybrid", spp=4, dlss=DLSSCfg(dlss_enabled=False) + ), + physics_cfg=NewtonPhysicsCfg( + physics_dt=1 / FPS, + num_substeps=args.substeps, + solver_cfg=DexUniSolverCfg( + joint_mode="kinematic", + vbd_options={ + "iterations": args.iterations, + "rigid_contact_history": True, + "friction_epsilon": 0.001, + }, + collision_options={"contact_matching": "latest"}, + ), + collision_cfg=None, + sync_to_renderer=False, + use_cuda_graph=True, + ), + ) + ) + video = camera = cable = None + poses, speeds, slips = [], [], [] + start = time.perf_counter() + try: + sim.add_articulation( + ArticulationCfg( + uid="stage", + fpath=str(urdf), + init_qpos=controls[0], + asset_physics_mode="overlay", + attrs=RigidBodyPhysicsCfg.from_dict( + { + "material_props": { + "static_friction": 1.2, + "dynamic_friction": 1.0, + "restitution": 0.0, + }, + "collision_props": { + "contact_offset": 0.002, + "rest_offset": 0.0, + }, + } + ), + ) + ) + sim.register_kinematic_joint_trajectory("stage", controls[None], fps=FPS) + sim.add_cable_object( + CableObjectCfg( + uid="cable", + points=points, + radius=RADIUS, + fixed_segments=(0,), + collision_filter_span=1, + attrs=CablePhysicsCfg( + density=800, + collision_props=NewtonCollisionPropertiesCfg(margin=0, gap=0.002), + material_props=NewtonRigidBodyMaterialCfg(dynamic_friction=1.2), + stretch_stiffness=1e5, + shear_stiffness=1e5, + bend_stiffness=0.012, + twist_stiffness=0.012, + stretch_damping=1, + shear_damping=1, + bend_damping=0.002, + twist_damping=0.002, + ), + visual_material=VisualMaterialCfg( + uid="orange_cable", + base_color=(1.0, 0.25, 0.035, 1.0), + roughness=0.6, + ), + ) + ) + if not args.no_video: + camera = sim.add_sensor( + CameraCfg( + uid="overview", + width=1280, + height=960, + intrinsics=(1050.0, 1050.0, 640.0, 480.0), + enable_color=True, + extrinsics=CameraCfg.ExtrinsicsCfg( + eye=(-0.12, -0.60, 2.30), target=(-0.16, 0.05, 0.025) + ), + ) + ) + sim.prepare() + cable = sim.get_cable_object("cable") + assert cable.data is not None + print("READY", sim.physics.solver_type, cable.data.num_segments, flush=True) + if camera is not None: + video = cv2.VideoWriter( + str(out / "preview_raw.mp4"), + cv2.VideoWriter_fourcc(*"mp4v"), + 30, + (1280, 960), + ) + if not video.isOpened(): + raise RuntimeError("Cannot open video writer") + phase = None + initial_tip = cable.data.segment_pos_w[0, -1].detach().cpu().numpy() + tip_offset = initial_tip - controls[0, :3] + for frame, t in enumerate(times): + if frame: + sim.update(step=1, render_final_step=False) + state = cable.data.segment_state_w[0].detach().cpu() + p = xyz_quat_to_4x4_matrix(state[:, :7]).numpy() + v = state[:, 7:].numpy() + if not np.isfinite(p).all() or not np.isfinite(v).all(): + raise RuntimeError(f"Non-finite cable state at {t}") + poses.append(p) + speeds.append(np.sqrt(np.mean(np.sum(v[:, :3] ** 2, axis=1)))) + slips.append( + float(np.linalg.norm(p[-1, :3, 3] - controls[frame, :3] - tip_offset)) + ) + current = control_at(t)[1] + if current != phase: + print( + f"{t:.2f}s {current}, tip={p[-1,:3,3]}, slip={slips[-1]:.4f}", + flush=True, + ) + phase = current + if camera is not None and frame % 2 == 0: + with sim.render_frame(capture_camera_images=False): + sim.sync_render_state() + camera.update() + rgb = camera.get_data()["color"][0, ..., :3].cpu().numpy() + bgr = cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR) + cv2.rectangle(bgr, (0, 0), (1280, 88), (25, 29, 34), -1) + cv2.putText( + bgr, + "P-11 | CABLE AROUND TWO POSTS", + (28, 35), + cv2.FONT_HERSHEY_SIMPLEX, + 0.85, + (245, 245, 245), + 2, + cv2.LINE_AA, + ) + cv2.putText( + bgr, + f"{t:05.2f} s {current} | EmbodiChain + cable DexSim", + (28, 68), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (170, 200, 215), + 1, + cv2.LINE_AA, + ) + video.write(bgr) + if frame % 60 == 0: + cv2.imwrite(str(out / f"frame_{frame:04d}.jpg"), bgr) + wp.synchronize() + np.savez_compressed( + out / "states.npz", + times=times, + poses=poses, + speed_rms=speeds, + grip_tip_error=slips, + ) + metrics = { + "demo_only": True, + "grasp": "finger frictional contact; no endpoint attachment", + "fixed_root_segment": 0, + "dexsim_commit": dexsim.__commit_id__, + "dexsim_path": dexsim.__file__, + "newton": newton.__version__, + "warp": wp.__version__, + "solver": sim.physics.solver_type, + "segments": args.segments, + "initial_length_m": float( + np.linalg.norm(np.diff(points, axis=0), axis=1).sum() + ), + "dt": 1 / FPS, + "substeps": args.substeps, + "iterations": args.iterations, + "duration_s": float(times[-1]), + "wall_s": time.perf_counter() - start, + "max_grip_tip_error_before_release_m": ( + float(np.max(np.array(slips)[(times >= 1.3) & (times <= 10.2)])) + if frames >= 78 + else None + ), + "final_speed_rms_m_s": float(speeds[-1]), + "urdf_sha256": hashlib.sha256(urdf.read_bytes()).hexdigest(), + } + (out / "manifest.json").write_text(json.dumps(metrics, indent=2) + "\n") + print(json.dumps(metrics, indent=2), flush=True) + finally: + if video is not None: + video.release() + cable = camera = None + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + if not args.no_video: + subprocess.run( + [ + "ffmpeg", + "-y", + "-loglevel", + "error", + "-i", + str(out / "preview_raw.mp4"), + "-c:v", + "libx264", + "-crf", + "19", + "-pix_fmt", + "yuv420p", + "-movflags", + "+faststart", + str(out / "cable_two_posts.mp4"), + ], + check=True, + ) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--segments", type=int, default=64) + parser.add_argument("--duration", type=float, default=DURATION) + parser.add_argument("--substeps", type=int, default=16) + parser.add_argument("--iterations", type=int, default=20) + parser.add_argument("--no-video", action="store_true") + run(parser.parse_args()) diff --git a/examples/sim/demo/cable_wall_franka.py b/examples/sim/demo/cable_wall_franka.py new file mode 100644 index 0000000000..ed5dee6d9c --- /dev/null +++ b/examples/sim/demo/cable_wall_franka.py @@ -0,0 +1,966 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Preview a Franka top-feed winding trajectory with gravity and cable contact. + +The commanded path makes three turns per post. Frictional grasp can still slip; +this preview does not certify successful wrapping or P-11 acceptance. +""" + +from __future__ import annotations + +import argparse +import hashlib +import json +from pathlib import Path +import subprocess +import time +import xml.etree.ElementTree as ET + +import cv2 +import numpy as np +from scipy.spatial.transform import Rotation + +FPS = 60 +CONTROL_FPS = 30 +TIME_SCALE = 3.5 +DURATION = 41.0 * TIME_SCALE +BASE = np.array([0.25, 0.0, 2.4]) +POSTS = np.array([[0.75, -0.20, 2.75], [0.75, 0.20, 2.75]]) +POST_RADIUS = 0.05 +POST_HALF_LENGTH = 0.05 +RADIUS = 0.004 +ORBIT_RADIUS = 0.16 +CABLE_LENGTH = 3.10 +CLOSED_FINGER = 0.0155 +FEED_FINGER = 0.0155 +BEND_STIFFNESS = 0.0003 +ANCHOR = np.array([0.76, -0.40, 2.92]) +JOINT_NAMES = tuple(f"fr3_joint{i}" for i in range(1, 8)) + ( + "fr3_finger_joint1", + "fr3_finger_joint2", +) +HAND_QUAT = Rotation.from_euler("x", np.pi).as_quat() +TOOL_POINT = np.array([0.13, 0.0, 0.0]) + + +def smooth(t: float) -> float: + u = float(np.clip(t, 0, 1)) + return u * u * (3 - 2 * u) + + +def orbit(post: int, u: float) -> np.ndarray: + theta = np.pi / 4 - 6 * np.pi * u + return np.array( + [ + 0.76 - 0.042 * u, + POSTS[post, 1] + ORBIT_RADIUS * np.cos(theta), + POSTS[post, 2] + ORBIT_RADIUS * np.sin(theta), + ] + ) + + +def target_at(t: float) -> tuple[np.ndarray, float, str, tuple[float, float]]: + """Three complete end-effector revolutions at each post, then release.""" + t = t / TIME_SCALE + if t <= 1: + return orbit(0, 0), FEED_FINGER, "THREAD / SETTLE", (0.0, 0.0) + if t <= 16: + u = smooth((t - 1) / 15) + return orbit(0, u), FEED_FINGER, "WIND + FEED POST 1", (3 * u, 0.0) + if t <= 19: + u = smooth((t - 16) / 3) + p = (1 - u) * orbit(0, 1) + u * orbit(1, 0) + p[2] += 0.10 * np.sin(np.pi * u) + return p, FEED_FINGER, "TRANSFER + FEED", (3.0, 0.0) + if t <= 34: + u = smooth((t - 19) / 15) + return orbit(1, u), FEED_FINGER, "WIND + FEED POST 2", (3.0, 3 * u) + if t <= 36: + return orbit(1, 1), CLOSED_FINGER, "HOLD", (3.0, 3.0) + if t <= 36.6: + return ( + orbit(1, 1), + CLOSED_FINGER + (0.04 - CLOSED_FINGER) * smooth((t - 36) / 0.6), + "RELEASE", + (3.0, 3.0), + ) + p = orbit(1, 1) + np.array([-0.07 * smooth((t - 36.6) / 1.4), 0, 0]) + return p, 0.04, "OBSERVE GRAVITY", (3.0, 3.0) + + +def initial_points(segments: int) -> np.ndarray: + """Short working lead passes through the hand; the remaining tail is free.""" + grip = orbit(0, 0) + # Enter the guide from the wall side and feed out toward the observer. + # The hanging tail then stays in front of both post tips. + u = np.linspace(0, 1, 2000)[:, None] + a, b = ANCHOR, ANCHOR + np.array([0.02, 0.16, 0.06]) + c, d = grip + np.array([0.035, 0, 0]), grip + lead = ( + (1 - u) ** 3 * a + 3 * (1 - u) ** 2 * u * b + 3 * (1 - u) * u**2 * c + u**3 * d + ) + lead_length = np.linalg.norm(np.diff(lead, axis=0), axis=1).sum() + outlet = grip - np.array([0.19, 0, 0]) + straight = np.linspace(grip, outlet, 500) + theta = np.linspace(0, np.pi / 2, 500) + bend = outlet + np.column_stack( + (-0.035 * np.sin(theta), np.zeros(500), -0.035 * (1 - np.cos(theta))) + ) + tail_length = CABLE_LENGTH - lead_length - 0.19 - 0.035 * np.pi / 2 + tail = bend[-1] + np.column_stack( + (np.zeros(2000), np.zeros(2000), -np.linspace(0, tail_length, 2000)) + ) + dense = np.vstack((lead[:-1], straight[:-1], bend[:-1], tail)) + arc = np.r_[0, np.cumsum(np.linalg.norm(np.diff(dense, axis=0), axis=1))] + return np.column_stack( + [ + np.interp(np.linspace(0, arc[-1], segments + 1), arc, dense[:, axis]) + for axis in range(3) + ] + ).astype(np.float32) + + +def write_feed_gripper(source: Path, path: Path) -> None: + """Add two visible grooved finger pads, open along the cable feed axis. + + The rectangular channel closes transversely around the rope and routes the + hanging tail in front of the post tips. + Nothing constrains a cable node or assigns its material position. + """ + root = ET.parse(source).getroot() + mesh_root = path.parent / "meshes" + source_mesh_root = (source.parent / "meshes").resolve() + if not mesh_root.exists(): + mesh_root.symlink_to(source_mesh_root, target_is_directory=True) + if mesh_root.resolve() != source_mesh_root: + raise ValueError("Output meshes path conflicts with the original Franka assets") + for mesh in root.findall(".//mesh"): + if not (path.parent / mesh.get("filename")).is_file(): + raise FileNotFoundError(mesh.get("filename")) + for name in ("fr3_leftfinger", "fr3_rightfinger"): + # Right finger is authored with a half turn around local Z. + center = 0.07 if name == "fr3_leftfinger" else -0.07 + boxes = [ + ((center, 0.002, 0.045), (0.180, 0.004, 0.035)), + ((center, -0.00575, 0.0275), (0.180, 0.0195, 0.004)), + ((center, -0.00575, 0.0625), (0.180, 0.0195, 0.004)), + ] + for index, (xyz, size) in enumerate(boxes): + pad_name = f"{name}_feed_pad_{index}" + link = ET.SubElement(root, "link", name=pad_name) + inertial = ET.SubElement(link, "inertial") + ET.SubElement(inertial, "mass", value=".0005") + ET.SubElement( + inertial, + "inertia", + ixx="1e-7", + iyy="1e-7", + izz="1e-7", + ixy="0", + ixz="0", + iyz="0", + ) + joint = ET.SubElement(root, "joint", name=pad_name + "_fixed", type="fixed") + ET.SubElement(joint, "parent", link=name) + ET.SubElement(joint, "child", link=pad_name) + ET.SubElement(joint, "origin", xyz=" ".join(map(str, xyz)), rpy="0 0 0") + for purpose in ("collision", "visual"): + item = ET.SubElement(link, purpose, name=f"feed_pad_{index}") + geometry = ET.SubElement(item, "geometry") + ET.SubElement(geometry, "box", size=" ".join(map(str, size))) + if purpose == "visual": + mat = ET.SubElement(item, "material", name="feed_pad") + ET.SubElement(mat, "color", rgba=".85 .58 .12 1") + ET.indent(root) + ET.ElementTree(root).write(path, encoding="unicode", xml_declaration=True) + + +def material_at_gripper( + poses: np.ndarray, rest_lengths: np.ndarray, grip: np.ndarray +) -> tuple[float, float, int]: + """Closest point on complete capsule axes, expressed in material arc length.""" + axis = poses[:, :3, 2] + starts = poses[:, :3, 3] - 0.5 * rest_lengths[:, None] * axis + local = np.clip(np.sum((grip - starts) * axis, axis=1), 0, rest_lengths) + distances = np.linalg.norm(starts + local[:, None] * axis - grip, axis=1) + index = int(np.argmin(distances)) + return ( + float(rest_lengths[:index].sum() + local[index]), + float(distances[index]), + index, + ) + + +def summarize_feeding( + times: np.ndarray, + poses: np.ndarray, + feed_points: np.ndarray, + material_s: np.ndarray, + distances: np.ndarray, + segment_ids: np.ndarray, + windings: np.ndarray, + post_penetrations: np.ndarray, +) -> dict[str, object]: + """Preview diagnostics; retain failure and incomplete runs as such.""" + pre = times < 36 * TIME_SCALE + lost = (distances > 0.02) & pre + result = { + "scope": "demo diagnostics, not P-11 physical acceptance", + "grip_loss_episodes": int(np.sum(lost & ~np.r_[False, lost[:-1]])), + "feed_after_settle_m": float( + material_s[np.flatnonzero(pre)[-1]] + - material_s[np.argmin(abs(times - TIME_SCALE))] + ), + "max_material_jump_per_frame_m": ( + float(np.max(abs(np.diff(material_s[pre])))) if pre.sum() > 1 else 0.0 + ), + "complete": bool(times[-1] >= DURATION - 1 / FPS), + "maximum_sampled_post_penetration_m": float(post_penetrations.max()), + "wraps_before_release": None, + "near_post_turns_before_release": None, + } + if times[-1] >= 35 * TIME_SCALE: + k = int(np.argmin(abs(times - 35 * TIME_SCALE))) + line = np.vstack((poses[k, : segment_ids[k] + 1, :3, 3], feed_points[k])) + wraps = [] + contact_turns = [] + for j, post in enumerate(POSTS): + angles = np.arctan2(line[:, 2] - post[2], line[:, 1] - post[1]) + endpoint_delta = np.angle(np.exp(1j * (angles[-1] - angles[0]))) + wraps.append(int(round(windings[k, j] - endpoint_delta / (2 * np.pi)))) + radial = np.linalg.norm(line[:, 1:] - post[1:], axis=1) + near = ( + (abs(line[:, 0] - post[0]) < POST_HALF_LENGTH) + & (radial < POST_RADIUS + RADIUS + 0.018) + & (radial > POST_RADIUS - 0.006) + ) + increments = np.angle(np.exp(1j * np.diff(angles))) + contact_turns.append( + float(abs(np.sum(increments[near[:-1] & near[1:]])) / (2 * np.pi)) + ) + result["wraps_before_release"] = wraps + result["near_post_turns_before_release"] = contact_turns + result["preview_checks_passed"] = bool( + result["complete"] + and not lost.any() + and all(x == -3 for x in wraps) + and min(contact_turns) >= 2.7 + and post_penetrations.max() < 0.01 + ) + else: + result["preview_checks_passed"] = False + return result + + +def sampled_post_penetration(poses: np.ndarray, rest_lengths: np.ndarray) -> np.ndarray: + """Seven axial samples per capsule against ideal finite cylinders. + + This catches gross crossings at every saved frame; it is not an exact + mesh-distance calculation or a continuous collision proof. + """ + offsets = np.linspace(-0.5, 0.5, 7) + samples = ( + poses[:, None, :3, 3] + + rest_lengths[:, None, None] * offsets[None, :, None] * poses[:, None, :3, 2] + ).reshape(-1, 3) + depths = [] + for post in POSTS: + d = np.column_stack( + ( + abs(samples[:, 0] - post[0]) - POST_HALF_LENGTH, + np.linalg.norm(samples[:, 1:] - post[1:], axis=1) - POST_RADIUS, + ) + ) + sdf = np.linalg.norm(np.maximum(d, 0), axis=1) + np.minimum(d.max(axis=1), 0) + depths.append(max(0.0, float(RADIUS - sdf.min()))) + return np.asarray(depths) + + +def write_stage(path: Path) -> None: + """Wall, normal-axis posts, visible fixed root and arm pedestal.""" + root = ET.Element("robot", name="wall_fixture") + + def link( + name: str, + kind: str, + dims: dict[str, str], + xyz: np.ndarray, + rpy: str, + color: str, + ) -> None: + body = ET.SubElement(root, "link", name=name) + inertial = ET.SubElement(body, "inertial") + ET.SubElement(inertial, "mass", value="1") + ET.SubElement( + inertial, "inertia", ixx=".1", iyy=".1", izz=".1", ixy="0", ixz="0", iyz="0" + ) + for purpose in ("visual", "collision"): + item = ET.SubElement(body, purpose) + ET.SubElement(item, "origin", xyz=" ".join(map(str, xyz)), rpy=rpy) + geo = ET.SubElement(item, "geometry") + ET.SubElement(geo, kind, **dims) + if purpose == "visual": + mat = ET.SubElement(item, "material", name=name + "_mat") + ET.SubElement(mat, "color", rgba=color) + if name != "wall": + joint = ET.SubElement(root, "joint", name=name + "_fixed", type="fixed") + ET.SubElement(joint, "parent", link="wall") + ET.SubElement(joint, "child", link=name) + + link( + "wall", + "box", + {"size": ".05 1.65 3.25"}, + np.array([0.825, 0, 1.625]), + "0 0 0", + ".17 .22 .28 1", + ) + for n, p in enumerate(POSTS, 1): + link( + f"post_{n}", + "cylinder", + {"radius": str(POST_RADIUS), "length": str(2 * POST_HALF_LENGTH)}, + p, + "0 1.570796326794897 0", + ".10 .57 .68 1", + ) + link( + "anchor", + "box", + {"size": ".06 .042 .042"}, + ANCHOR + np.array([0.015, 0, 0]), + "0 0 0", + ".96 .57 .10 1", + ) + link( + "pedestal", + "box", + {"size": f".32 .32 {BASE[2]}"}, + np.array([BASE[0], 0, BASE[2] / 2]), + "0 0 0", + ".35 .39 .43 1", + ) + ET.indent(root) + ET.ElementTree(root).write(path, encoding="unicode", xml_declaration=True) + + +def robot_fk(urdf: Path, q: np.ndarray) -> dict[str, np.ndarray]: + """Independent XML FK, with named joints and unmodified asset limits.""" + root = ET.parse(urdf).getroot() + qmap = dict(zip(JOINT_NAMES, q, strict=True)) + transforms = {"base": np.eye(4)} + transforms["base"][:3, 3] = BASE + remaining = list(root.findall("joint")) + while remaining: + progressed = False + for joint in remaining[:]: + parent = joint.find("parent").get("link") + child = joint.find("child").get("link") + if parent not in transforms: + continue + origin = joint.find("origin") + local = np.eye(4) + if origin is not None: + local[:3, 3] = np.fromstring(origin.get("xyz", "0 0 0"), sep=" ") + local[:3, :3] = Rotation.from_euler( + "xyz", np.fromstring(origin.get("rpy", "0 0 0"), sep=" ") + ).as_matrix() + motion = np.eye(4) + value = qmap.get(joint.get("name"), 0.0) + axis = joint.find("axis") + if axis is not None: + vector = np.fromstring(axis.get("xyz"), sep=" ") + if joint.get("type") == "revolute": + motion[:3, :3] = Rotation.from_rotvec(vector * value).as_matrix() + if joint.get("type") == "prismatic": + motion[:3, 3] = vector * value + transforms[child] = transforms[parent] @ local @ motion + remaining.remove(joint) + progressed = True + if not progressed: + raise RuntimeError("URDF joint tree is disconnected") + return transforms + + +def make_trajectory( + urdf: Path, out: Path, duration: float, q7: float = 0.75 +) -> tuple[np.ndarray, np.ndarray]: + """Use EmbodiChain FEP IK; independently check FK and asset joint limits.""" + import torch + from embodichain.lab.sim.motion.solvers import FEPSolverCfg + + times = np.arange(round(duration * CONTROL_FPS) + 1) / CONTROL_FPS + targets = np.array([target_at(t)[0] for t in times]) + ik = FEPSolverCfg( + urdf_path=str(urdf), + root_link_name="base", + end_link_name="fr3_hand_tcp", + redundancy_search=False, + ).init_solver("cpu") + seed = torch.tensor([[0, -0.6, 0, -2.2, 0, 1.6, q7]], dtype=torch.float32) + pose = torch.eye(4, dtype=torch.float32) + pose[:3, :3] = torch.tensor( + Rotation.from_quat(HAND_QUAT).as_matrix(), dtype=torch.float32 + ) + qs = [] + errors = [] + rot_errors = [] + for index, t in enumerate(times): + target, finger, _, _ = target_at(t) + tcp_target = target - Rotation.from_quat(HAND_QUAT).apply(TOOL_POINT) + pose[:3, 3] = torch.tensor(tcp_target - BASE, dtype=torch.float32) + valid, seed = ik.get_ik(pose, seed) + if not bool(valid.all()): + raise RuntimeError(f"FEP IK failed at {t:.3f}s") + q = np.r_[seed.numpy().reshape(-1), finger, finger] + fk = robot_fk(urdf, q)["fr3_hand_tcp"] + errors.append(np.linalg.norm(fk[:3, 3] + fk[:3, :3] @ TOOL_POINT - target)) + rot_errors.append( + ( + Rotation.from_matrix(fk[:3, :3]).inv() * Rotation.from_quat(HAND_QUAT) + ).magnitude() + ) + qs.append(q) + if index % 150 == 0: + print("IK", t, "error_m", errors[-1], flush=True) + qs = np.array(qs, dtype=np.float32) + root = ET.parse(urdf).getroot() + limits = { + j.get("name"): j.find("limit") + for j in root.findall("joint") + if j.get("name") in JOINT_NAMES + } + lower = np.array([float(limits[n].get("lower")) for n in JOINT_NAMES]) + upper = np.array([float(limits[n].get("upper")) for n in JOINT_NAMES]) + limit_violation = float(max(np.max(lower - qs), np.max(qs - upper), 0)) + joint_speed = np.abs(np.diff(qs[:, :7], axis=0)) * CONTROL_FPS + speed_limits = np.array([float(limits[n].get("velocity")) for n in JOINT_NAMES[:7]]) + stats = { + "ik_solver": f"EmbodiChain FEP, fixed q7={q7} rad", + "max_position_error_m": float(max(errors)), + "max_rotation_error_rad": float(max(rot_errors)), + "joint_limit_violation_rad": limit_violation, + "max_joint_step_rad": float(np.abs(np.diff(qs[:, :7], axis=0)).max()), + "max_joint_speed_rad_s": float(joint_speed.max()), + "max_speed_limit_ratio": float((joint_speed / speed_limits).max()), + } + (out / "ik_checks.json").write_text(json.dumps(stats, indent=2) + "\n") + np.savez( + out / "robot_trajectory.npz", + times=times, + qpos=qs, + joint_names=JOINT_NAMES, + tcp_targets=targets, + hand_quaternion=HAND_QUAT, + tool_point=TOOL_POINT, + position_error=errors, + ) + print("IK_CHECKS", stats, flush=True) + if ( + max(errors) > 0.004 + or max(rot_errors) > 0.04 + or limit_violation > 0.001 + or stats["max_speed_limit_ratio"] > 1 + ): + raise RuntimeError( + "Franka trajectory failed independent FK / joint-limit checks" + ) + return times, qs + + +def run(args: argparse.Namespace) -> None: + import dexsim, newton, warp as wp + from dexsim.engine.newton_physics import DexUniSolverCfg + from embodichain.lab.sim import SimulationManager, SimulationManagerCfg + from embodichain.lab.sim.cfg import ( + ArticulationCfg, + CableObjectCfg, + CablePhysicsCfg, + NewtonCollisionPropertiesCfg, + NewtonPhysicsCfg, + NewtonRigidBodyMaterialCfg, + RenderCfg, + DLSSCfg, + RigidBodyPhysicsCfg, + ) + from embodichain.lab.sim.cfg.articulation import LinkPhysicsOverrideCfg + from embodichain.lab.sim.sensors import CameraCfg + from embodichain.lab.sim.material import VisualMaterialCfg + from embodichain.utils.math import xyz_quat_to_4x4_matrix + + out = args.output.resolve() + out.mkdir(parents=True, exist_ok=True) + source_urdf = args.robot.resolve() + urdf = out / "franka_feed_gripper.urdf" + write_feed_gripper(source_urdf, urdf) + duration = min(args.duration, DURATION) + if args.trajectory: + saved = np.load(args.trajectory) + qs = saved["qpos"] + control_times = saved["times"] + qs[:, -2:] = np.array([target_at(t)[1] for t in control_times])[:, None] + if control_times[-1] < duration: + raise ValueError("Trajectory shorter than requested duration") + expected = np.array([target_at(t)[0] for t in control_times]) + if not np.allclose(saved["tcp_targets"], expected, atol=1e-6, rtol=0): + raise ValueError( + "Cached trajectory does not match the current winding path" + ) + if ( + "hand_quaternion" not in saved + or "tool_point" not in saved + or not np.allclose(saved["hand_quaternion"], HAND_QUAT, atol=1e-6, rtol=0) + or not np.allclose(saved["tool_point"], TOOL_POINT, atol=1e-6, rtol=0) + ): + raise ValueError( + "Cached trajectory does not match the current feed tool pose" + ) + checks = args.trajectory.parent / "ik_checks.json" + if checks.is_file(): + (out / "ik_checks.json").write_bytes(checks.read_bytes()) + else: + control_times, qs = make_trajectory(urdf, out, duration, args.ik_q7) + if args.plan_only: + return + np.savez( + out / "robot_trajectory.npz", + times=control_times, + qpos=qs, + joint_names=JOINT_NAMES, + tcp_targets=np.array([target_at(t)[0] for t in control_times]), + hand_quaternion=HAND_QUAT, + tool_point=TOOL_POINT, + ) + (out / "source_snapshot.py").write_bytes(Path(__file__).read_bytes()) + config = { + "length_m": CABLE_LENGTH, + "radius_m": RADIUS, + "post_centers": POSTS.tolist(), + "post_radius_m": POST_RADIUS, + "post_half_length_m": POST_HALF_LENGTH, + "anchor": ANCHOR.tolist(), + "franka_base": BASE.tolist(), + "hand_quaternion_xyzw": HAND_QUAT.tolist(), + "tool_point_in_tcp_m": TOOL_POINT.tolist(), + "orbit_radius_m": ORBIT_RADIUS, + "time_scale": TIME_SCALE, + "feed_finger_q_m": FEED_FINGER, + "hold_finger_q_m": CLOSED_FINGER, + "bend_stiffness": BEND_STIFFNESS, + "gripper_mu": 0.08, + "rope_mu": 0.15, + "stage_mu": 0.15, + "contact_ke": 1e5, + "joint_coefficient_scale": args.segments / 256, + "rigid_body_contact_buffer_size": 512, + "physical_tip_attachment": False, + "arguments": { + key: str(value) if isinstance(value, Path) else value + for key, value in vars(args).items() + }, + } + (out / "config.json").write_text(json.dumps(config, indent=2) + "\n") + stage = out / "wall_fixture.urdf" + write_stage(stage) + points = initial_points(args.segments) + rest_lengths = np.linalg.norm(np.diff(points, axis=0), axis=1) + np.savez(out / "inputs.npz", points=points, posts=POSTS, anchor=ANCHOR) + sim = SimulationManager( + SimulationManagerCfg( + headless=not args.viewer, + startup_summary="compact", + robot_ik_gizmo=None, + render_cfg=RenderCfg( + renderer="hybrid", spp=4, dlss=DLSSCfg(dlss_enabled=False) + ), + physics_cfg=NewtonPhysicsCfg( + physics_dt=1 / FPS, + num_substeps=args.substeps, + solver_cfg=DexUniSolverCfg( + joint_mode="kinematic", + vbd_options={ + "iterations": args.iterations, + "rigid_contact_history": True, + "rigid_body_contact_buffer_size": 512, + "friction_epsilon": 0.001, + }, + collision_options={"contact_matching": "latest"}, + ), + collision_cfg=None, + sync_to_renderer=False, + use_cuda_graph=True, + ), + ) + ) + video = camera = detail_camera = cable = robot = None + poses = [] + speeds = [] + free_tip_distances = [] + actual_tcp = [] + actual_feed_point = [] + grip_material_s = [] + grip_distances = [] + grip_segments = [] + prefix_windings = [] + post_penetrations = [] + start = time.perf_counter() + try: + material = RigidBodyPhysicsCfg.from_dict( + { + "material_props": { + "backend": "newton", + "dynamic_friction": 0.15, + "ke": 1e5, + "kd": 100.0, + }, + "collision_props": {"contact_offset": 0.001, "rest_offset": 0.0}, + } + ) + grip_material = RigidBodyPhysicsCfg.from_dict( + { + "material_props": { + "backend": "newton", + "dynamic_friction": 0.08, + "ke": 2e4, + "kd": 100.0, + }, + "collision_props": {"contact_offset": 0.001, "rest_offset": 0.0}, + } + ) + sim.add_articulation( + ArticulationCfg( + uid="wall_fixture", + fpath=str(stage), + asset_physics_mode="overlay", + attrs=material, + ) + ) + robot = sim.add_articulation( + ArticulationCfg( + uid="franka", + fpath=str(urdf), + init_pos=tuple(BASE), + init_qpos=qs[0], + asset_physics_mode="overlay", + attrs=material, + link_attrs={ + "finger_pads": LinkPhysicsOverrideCfg( + link_names_expr=[ + "fr3_leftfinger", + "fr3_rightfinger", + "fr3_.*finger_feed_pad_.*", + ], + attrs=grip_material, + ) + }, + ) + ) + sim.register_kinematic_joint_trajectory("franka", qs[None], fps=CONTROL_FPS) + sim.add_cable_object( + CableObjectCfg( + uid="cable", + points=points, + radius=RADIUS, + render_mode="capsules", + fixed_segments=(0,), + collision_filter_span=1, + attrs=CablePhysicsCfg( + density=800, + collision_props=NewtonCollisionPropertiesCfg(margin=0, gap=0.001), + material_props=NewtonRigidBodyMaterialCfg( + dynamic_friction=0.15, ke=1e5, kd=100.0 + ), + stretch_stiffness=5e5 * args.segments / 256, + shear_stiffness=5e5 * args.segments / 256, + bend_stiffness=BEND_STIFFNESS * args.segments / 256, + twist_stiffness=BEND_STIFFNESS * args.segments / 256, + stretch_damping=args.segments / 256, + shear_damping=args.segments / 256, + bend_damping=0.001 * args.segments / 256, + twist_damping=0.001 * args.segments / 256, + ), + visual_material=VisualMaterialCfg( + uid="orange_cable", + base_color=(1.0, 0.26, 0.025, 1.0), + roughness=0.6, + ), + ) + ) + if not args.no_video: + camera = sim.add_sensor( + CameraCfg( + uid="wall_view", + width=1600, + height=1200, + intrinsics=(1180.0, 1180.0, 800.0, 600.0), + extrinsics=CameraCfg.ExtrinsicsCfg( + eye=(-1.7, 1.0, 3.3), target=(0.65, 0, 2.45) + ), + ) + ) + detail_camera = sim.add_sensor( + CameraCfg( + uid="contact_detail", + width=760, + height=560, + intrinsics=(800.0, 800.0, 380.0, 280.0), + extrinsics=CameraCfg.ExtrinsicsCfg( + eye=(0.20, -0.55, 3.15), target=(0.75, 0, 2.75) + ), + ) + ) + sim.prepare() + cable = sim.get_cable_object("cable") + assert cable.data is not None + print("ROBOT_JOINT_ORDER", robot.joint_names, flush=True) + assert [x.lower() for x in robot.joint_names] == list(JOINT_NAMES) + if camera is not None: + video = cv2.VideoWriter( + str(out / "preview_raw.mp4"), + cv2.VideoWriter_fourcc(*"mp4v"), + 30, + (1600, 1200), + ) + if not video.isOpened(): + raise RuntimeError("Video writer unavailable") + frames = round(duration * FPS) + times = np.arange(frames + 1) / FPS + phase = None + for frame, t in enumerate(times): + if frame: + sim.update(step=1, render_final_step=args.viewer and camera is None) + state = cable.data.segment_state_w[0].detach().cpu() + p = xyz_quat_to_4x4_matrix(state[:, :7]).numpy() + v = state[:, 7:].numpy() + if not np.isfinite(p).all() or not np.isfinite(v).all(): + raise RuntimeError(f"Nonfinite state at {t}") + poses.append(p) + post_penetrations.append(sampled_post_penetration(p, rest_lengths)) + speeds.append(np.sqrt(np.mean(np.sum(v[:, :3] ** 2, axis=1)))) + target, finger, current, turns = target_at(t) + hand_pose = ( + robot.get_link_pose("fr3_hand_tcp", to_matrix=True) + .detach() + .cpu() + .numpy()[0] + ) + actual_tcp.append(hand_pose[:3, 3]) + feed_point = hand_pose[:3, 3] + hand_pose[:3, :3] @ TOOL_POINT + actual_feed_point.append(feed_point) + material_s, grip_distance, grip_segment = material_at_gripper( + p, rest_lengths, feed_point + ) + grip_material_s.append(material_s) + grip_distances.append(grip_distance) + grip_segments.append(grip_segment) + working_line = np.vstack((p[: grip_segment + 1, :3, 3], feed_point)) + angles = [ + np.unwrap( + np.arctan2(working_line[:, 2] - c[2], working_line[:, 1] - c[1]) + ) + for c in POSTS + ] + net = [float((x[-1] - x[0]) / (2 * np.pi)) for x in angles] + prefix_windings.append(net) + free_tip_distances.append( + float(np.linalg.norm(p[-1, :3, 3] - hand_pose[:3, 3])) + ) + if current != phase or frame % 300 == 0: + print( + f"{t:.2f}s {current} material_s={material_s:.3f} grip_axis_distance={grip_distance:.4f} working_winding={net}", + flush=True, + ) + phase = current + if camera is not None and frame % 7 == 0: + with sim.render_frame(capture_camera_images=False): + sim.sync_render_state() + camera.update() + detail_camera.update() + rgb = camera.get_data()["color"][0, ..., :3].cpu().numpy() + bgr = cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR) + detail_rgb = detail_camera.get_data()["color"][0, ..., :3].cpu().numpy() + bgr[625:1185, 825:1585] = cv2.cvtColor(detail_rgb, cv2.COLOR_RGB2BGR) + cv2.rectangle(bgr, (823, 623), (1587, 1187), (190, 190, 190), 2) + cv2.putText( + bgr, + "CONTACT DETAIL", + (845, 652), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (255, 255, 255), + 1, + cv2.LINE_AA, + ) + cv2.rectangle(bgr, (0, 0), (1600, 124), (24, 29, 35), -1) + cv2.putText( + bgr, + "P-11 | FRANKA FR3 | SLIDING FEED | 3 TURNS / POST", + (25, 36), + cv2.FONT_HERSHEY_SIMPLEX, + 0.85, + (240, 245, 250), + 2, + cv2.LINE_AA, + ) + cv2.putText( + bgr, + f"{t:06.2f}s {current} | Commanded turns: {turns[0]:.2f} / {turns[1]:.2f} | Gravity: 9.81 m/s2 | {TIME_SCALE:g}x replay", + (25, 74), + cv2.FONT_HERSHEY_SIMPLEX, + 0.63, + (180, 207, 220), + 1, + cv2.LINE_AA, + ) + grasp_label = ( + f"MATERIAL AT GRIP: {material_s:.3f} m | TAIL: {CABLE_LENGTH-material_s:.3f} m | CONTACT ONLY / NO ATTACHMENT" + if t < 36 * TIME_SCALE + else "GRIP OPEN / FREE RELEASE | ROPE MOTION FROM GRAVITY AND CONTACT" + ) + cv2.putText( + bgr, + grasp_label, + (25, 106), + cv2.FONT_HERSHEY_SIMPLEX, + 0.63, + (110, 195, 255), + 1, + cv2.LINE_AA, + ) + video.write(bgr) + if frame % 420 == 0: + cv2.imwrite(str(out / f"frame_{frame:04d}.jpg"), bgr) + wp.synchronize() + poses = np.asarray(poses) + feeding_checks = summarize_feeding( + times, + poses, + np.asarray(actual_feed_point), + np.asarray(grip_material_s), + np.asarray(grip_distances), + np.asarray(grip_segments), + np.asarray(prefix_windings), + np.asarray(post_penetrations), + ) + (out / "feeding_checks.json").write_text( + json.dumps(feeding_checks, indent=2) + "\n" + ) + np.savez_compressed( + out / "states.npz", + times=times, + poses=poses, + speed_rms=speeds, + free_tip_distance_to_tcp=free_tip_distances, + actual_tcp=actual_tcp, + actual_feed_point=actual_feed_point, + grip_material_s=grip_material_s, + grip_distance=grip_distances, + grip_segment=grip_segments, + working_prefix_winding=prefix_windings, + sampled_post_penetration_m=post_penetrations, + ) + metrics = { + "demo_only": True, + "acceptance_status": "not_validated; commanded turns are not measured rope wraps", + "feeding_checks": feeding_checks, + "robot_asset": str(urdf), + "source_robot_asset": str(source_urdf), + "robot_sha256": hashlib.sha256(urdf.read_bytes()).hexdigest(), + "dexsim_commit": dexsim.__commit_id__, + "solver": sim.physics.solver_type, + "newton": newton.__version__, + "warp": wp.__version__, + "grasp": "sliding contact in visible grooved Franka pads; free material feed", + "diagnostic_endpoint_attachment": False, + "release_time_s": 36 * TIME_SCALE, + "base_position": BASE.tolist(), + "segments": args.segments, + "length_m": float(np.linalg.norm(np.diff(points, axis=0), axis=1).sum()), + "radius_m": RADIUS, + "bend_stiffness": BEND_STIFFNESS, + "initial_material_at_grip_m": grip_material_s[0], + "final_material_at_grip_m": grip_material_s[-1], + "max_grip_centerline_distance_before_release_m": float( + np.max(np.array(grip_distances)[times <= 36 * TIME_SCALE]) + ), + "working_prefix_winding_final": prefix_windings[-1], + "dt": 1 / FPS, + "substeps": args.substeps, + "iterations": args.iterations, + "duration_s": duration, + "wall_s": time.perf_counter() - start, + "final_free_tip_distance_to_tcp_m": float(free_tip_distances[-1]), + "final_speed_rms_m_s": float(speeds[-1]), + } + (out / "manifest.json").write_text(json.dumps(metrics, indent=2) + "\n") + print(json.dumps(metrics, indent=2), flush=True) + finally: + if video is not None: + video.release() + robot = cable = camera = detail_camera = None + sim.destroy(exit_process=False) + SimulationManager.flush_cleanup_queue() + if not args.no_video: + subprocess.run( + [ + "ffmpeg", + "-y", + "-loglevel", + "error", + "-i", + str(out / "preview_raw.mp4"), + "-c:v", + "libx264", + "-crf", + "19", + "-pix_fmt", + "yuv420p", + "-movflags", + "+faststart", + str(out / "cable_wall_franka.mp4"), + ], + check=True, + ) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--output", type=Path, required=True) + parser.add_argument("--robot", type=Path, required=True) + parser.add_argument("--trajectory", type=Path) + parser.add_argument("--duration", type=float, default=DURATION) + parser.add_argument("--segments", type=int, default=128) + parser.add_argument("--substeps", type=int, default=8) + parser.add_argument("--iterations", type=int, default=40) + parser.add_argument("--no-video", action="store_true") + parser.add_argument( + "--viewer", + action="store_true", + help="Open the native simulation viewer while running", + ) + parser.add_argument("--plan-only", action="store_true") + parser.add_argument("--ik-q7", type=float, default=0.75) + args = parser.parse_args() + if ( + args.duration <= 0 + or args.segments < 2 + or args.substeps < 1 + or args.iterations < 1 + ): + parser.error( + "duration, substeps and iterations must be positive; segments must be >= 2" + ) + run(args) diff --git a/examples/sim/demo/franka_rope_wrap/README.md b/examples/sim/demo/franka_rope_wrap/README.md new file mode 100644 index 0000000000..374bcd02e9 --- /dev/null +++ b/examples/sim/demo/franka_rope_wrap/README.md @@ -0,0 +1,154 @@ +# Franka floor pickup and rope wrapping + +A single Franka arm picks up a rope end directly from the floor, lifts it, +wraps it around one post, tightens it and holds it for two seconds. The default +run simulates 36 seconds with 96 cable segments in one CUDA environment. + +The joint targets are planned before execution. During physics, the arm uses +position/velocity drives and the fingers hold the rope through contact. There +is no gripper attachment and no per-frame rope state assignment. The final +hold keeps the gripper closed; release and self-locking are not demonstrated. +Finger geometry, friction and material values are demonstration settings, not +hardware calibration. The configured Task Program executes ten named phases through the Gym +controller. Each phase uses the same continuously seeded IK plan and position/velocity +drive samples that pass the offline motion checks. + +## Requirements and assets + +Use DexSim `0.6.0rc2` with USD cable import, `SpawnedCable` state access +and DexUni. See the parent +[Newton cable example](../../../README.md#newton-cable-demo) for SDK installation. +A version string alone does not establish compatibility; the example records +the actual SDK build commit. Full physics execution requires a CUDA GPU. + +The original robot is resolved through +`get_data_path("Franka/Panda/PandaWithHand.urdf")`. The existing `Franka` registry +uses `FrankaV4.zip` with MD5 `0c613d884798e8b084604381641cc8e1`. Although the +asset path contains `Panda`, this demo uses the model's `fr3_*` joint/link names. +Do not substitute an arbitrary Panda URDF without adapting the configuration. +Pre-download it for an offline run: + +```bash +python -m embodichain data download --name Franka +``` + +Set `EMBODICHAIN_DATA_ROOT` before starting Python to select an existing asset +cache. A complete replacement configuration may instead set `robot_urdf` to an +absolute local URDF path. Missing meshes or other referenced model files fail +before simulation. + +The example prepares a robot URDF with flat finger pads, preserving the source +joint definitions and inertias. It copies referenced meshes and material +resources into the run directory using relative paths. The original asset cache +is unchanged. `generated_assets/provenance.json` records the asset reference, +source and generated model hashes, resource hashes and finger modifications. +Original model notices remain part of their resources. + +The floor slab, fixed rope clamp and post are primitive shapes. The packaged +`cable.usda` supplies the 97 original centerline points, 8 mm diameter and +white material. The deployment imports `/World/cable` and sets the fixed first +segment, collision filter span, rod coefficients and contact parameters in +`env.yaml`. A custom geometry configuration writes a matching USD into the run's +`deployment` directory. +The default run uses `no-render` and does not load native rendering resources. +Native rendering helpers, if used separately, also need the existing +`SimResources` asset (`python -m embodichain data download --name SimResources`). +Video generation, browser visualization and report publishing are outside this +example's command-line interface. + +## Run + +Use the configured deployment to execute the Task Program or record a standard trajectory: + +```bash +python -m embodichain run-task \ + --gym_config extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task.franka.yaml \ + --headless --max_episodes 1 --record_trajectory --trajectory_save_dir /tmp/franka-wrap-episodes +``` + +The example below runs the same program and exports the per-frame measurements, +robot motion and 19 individual acceptance results. + +```bash +python -m embodichain run-example sim/demo/franka_rope_wrap \ + --output /tmp/franka-rope-wrap-run +``` + +The output directory must not already exist. The bundled `config.json` is the +default regardless of the working directory. To replace it with a complete +configuration: + +```bash +python -m embodichain run-example sim/demo/franka_rope_wrap \ + --config /path/to/config.json --output /tmp/franka-rope-wrap-custom +``` + +Generate assets and the joint plan without starting physics: + +```bash +python -m embodichain run-example sim/demo/franka_rope_wrap \ + --plan-only --output /tmp/franka-rope-wrap-plan +``` + +`--seconds` limits how many complete control intervals the diagnostic can execute. +At 60 Hz, `--seconds 35.999` allows 2,159 commands and records 2,160 states +including the settled initial frame. A truncated run writes `partial` and +`passed=false` and exits with code 1. A complete run exits with code 0 when +the Task Program and all physical checks pass. + +Changing `duration_s` rescales the motion stages and their observation +windows. For example, at 72 seconds the fingers close by 10 s, the lift ends +at 14 s, and the 465° orbit runs from 20 s through 56 s. The stability +measurement still uses the last two seconds of recorded states. +`--plan-only` exits with code 0 after a valid plan and records `planned`. + +## Results and acceptance + +The run directory contains: + +- `config.json`, `source_snapshot.py` and `manifest.json`: exact inputs, source + hash, optional repository commit, package/build versions and device metadata. +- `generated_assets/`: the modified URDF, copied resources and provenance. +- `robot_motion.npz` and `motion_validation.json`: planned targets and motion + checks before physics. +- `states.npz`, `metrics.json` and `settled_state.json`: recorded states and + measurements from physical execution. +- `validation.json`: the 19 acceptance checks, thresholds and measured summary. + Exceptions are recorded in `failure.txt` with a failed manifest. + +The acceptance checks cover completion, finite state, joint limits, direct floor +pickup, retained grasp, material-point slip, bilateral finger contact, winding, +contact arc, hold stability, rope height, post penetration, observed robot/post +clearance, anchor direction and exit bend, fixed anchor, segment gaps, dropped +contacts and floor penetration. They use actual simulated states and valid +contact geometry. DexUni contact geometry does not imply that normal or friction +impulses are available; consumers must inspect query capabilities. + +### Same-engine migration check + +The original example at `c911dc21` and the configured Task Program both passed +19/19 in a 36-second GPU episode on DexSim **0.6.0rc2** (build +`dbaca660471d5aaa7e750093e7fe1c207465c4af`), Newton 1.7.0 and an RTX 5090 D v2. +The program executed ten segments and 2,160 controls. Both runs used the same +97 cable points, derived robot model and control targets. + +| Measured quantity | Original example | Task Program | +| --- | ---: | ---: | +| Final rope winding | 524.8542° | 524.8543° | +| Maximum material slip | 0.1174 mm | 0.1176 mm | +| Bilateral contact after closing | 100% | 100% | +| Minimum contact arc over final hold | 360° | 360° | +| Rope/post penetration | 0 mm | 0 mm | +| Free-end drift over final hold | 0.0512 mm | 0.0429 mm | + +On 2026-11-08 the default 36-second NoRender run passed all 19 checks with +DexSim `0.6.0.dev11491` (dev commit +`505ee1b5600ca0b3d0a2c8235930e7684283d049`), Newton 1.7.0, MuJoCo/MuJoCo-Warp +3.15.0, Python 3.12 and an RTX 5090 D v2. Final winding was 524.86 degrees, +maximum material grasp slip was 0.123 mm, and hold winding drift was +0.00949 degrees. No post penetration or dropped contacts were reported. + +The included checks do not constitute an independent robot self-collision or +continuous swept-volume certification. One successful run also does not measure +multi-seed reliability, batched execution or reset stability. Recorded states +can support later replay, but the CLI does not generate a video. diff --git a/examples/sim/demo/franka_rope_wrap/__main__.py b/examples/sim/demo/franka_rope_wrap/__main__.py new file mode 100644 index 0000000000..fdc9e59b07 --- /dev/null +++ b/examples/sim/demo/franka_rope_wrap/__main__.py @@ -0,0 +1,411 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Dynamic Franka floor pickup and single-post rope wrapping. + +Only robot joint drive targets move during physics. The rope's first segment +is fixed at the visible floor clamp; its free end is grasped by finger contact. +No robot/rope attachment or rope state assignment is used during simulation. +Material and fingertip parameters are demonstration settings, not calibration. +""" + +from __future__ import annotations + +import argparse +from datetime import datetime, timezone +from decimal import Decimal +import hashlib +from importlib import metadata +import json +import os +from pathlib import Path +import platform +import shutil +import subprocess +import sys +from tempfile import TemporaryDirectory +import traceback +from typing import Any + +import numpy as np +import torch + +from embodichain_tasks.manipulation.cable.franka_rope_wrap import ( + load_config, + materialize_deployment, + prepare_models, + task_directory, +) +from embodichain_tasks.manipulation.cable.wrap_motion import plan_wrap_motion +from embodichain_tasks.manipulation.cable.wrap_evaluation import attach_wrap_monitor + +__all__ = ["main", "create_replay_scene", "restore_replay_frame", "replay_camera_views"] + + +def _json(path: Path, data: Any) -> None: + path.write_text(json.dumps(data, indent=2, ensure_ascii=False) + "\n") + + +def _hash(path: Path, algorithm: str = "sha256") -> str: + digest = hashlib.new(algorithm) + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _runtime_versions() -> dict[str, Any]: + """Collect reproducibility metadata without constructing a simulation world.""" + packages = {} + metadata_errors = {} + for name in ( + "embodichain", + "dexsim-engine", + "newton", + "warp-lang", + "torch", + "scipy", + "pytorch-kinematics", + ): + try: + packages[name] = metadata.version(name) + except Exception as error: + packages[name] = None + metadata_errors[name] = str(error) + result: dict[str, Any] = { + "python": platform.python_version(), + "python_executable": sys.executable, + "platform": platform.platform(), + "packages": packages, + "metadata_errors": metadata_errors, + "torch_cuda": torch.version.cuda, + "cuda_visible_devices": os.environ.get("CUDA_VISIBLE_DEVICES"), + "gpus": [], + } + try: + import dexsim + + result["dexsim_commit"] = getattr(dexsim, "__commit_id__", None) + except Exception as error: + result["dexsim_metadata_error"] = str(error) + try: + query = subprocess.run( + [ + "nvidia-smi", + "--query-gpu=index,name,uuid,driver_version", + "--format=csv,noheader", + ], + capture_output=True, + text=True, + check=False, + timeout=5, + ) + if query.returncode == 0: + for row in query.stdout.splitlines(): + values = [item.strip() for item in row.split(",")] + if len(values) == 4: + result["gpus"].append( + dict(zip(("index", "name", "uuid", "driver_version"), values)) + ) + except (OSError, subprocess.TimeoutExpired): + pass + return result + + +def _source_revision() -> str | None: + """Report the source checkout revision when available, including from any cwd.""" + source = Path(__file__).resolve() + for parent in source.parents: + if not (parent / "pyproject.toml").is_file(): + continue + if source.relative_to(parent).parts[0] != "examples": + continue + try: + return subprocess.check_output( + ["git", "-C", str(parent), "rev-parse", "HEAD"], + text=True, + stderr=subprocess.DEVNULL, + timeout=5, + ).strip() + except (OSError, subprocess.CalledProcessError, subprocess.TimeoutExpired): + return None + return None + + +def _deployment_config(deployment: Path) -> tuple: + """Resolve the same deployment and hooks used by the standard run-task CLI.""" + from embodichain.lab.scripts.run_task import _create_parser + from embodichain.lab.gym.utils.gym_utils import build_env_cfg_from_args + from embodichain.lab.gym.utils.registration import ( + discover_task_packages, + execute_init_hooks, + ) + + discover_task_packages() + execute_init_hooks() + args = _create_parser().parse_args(["--gym_config", str(deployment), "--headless"]) + return build_env_cfg_from_args(args) + + +def _run(deployment: Path, config: dict, out: Path, seconds: float | None) -> dict: + import gymnasium + from embodichain.lab.gym.envs.demo import execute_demo_episode + + cfg, gym_config, actions = _deployment_config(deployment) + env = gymnasium.make(gym_config["id"], cfg=cfg, **actions) + target = env.unwrapped + limit = None + if seconds is not None and seconds < config["duration_s"]: + limit = int(Decimal(str(seconds)) * config["control_hz"]) + + try: + env.reset(options={"save_data": False}) + result = execute_demo_episode(env, max_steps=limit) + _json(out / "task_program_episode.json", result.to_metadata()) + monitor = getattr(target, "_wrap_monitor", None) + if monitor is None and limit == 0: + # The collector stops before constructing its first segment. + path = Path(target.robot.cfg.fpath) + monitor = attach_wrap_monitor( + target, config, plan_wrap_motion(config, path), path + ) + if monitor is None: + raise RuntimeError( + f"Task Program stopped before the first phase: {result.terminal_reason}" + ) + monitor.motion.write(out) + report = monitor.write(out) + report["passed"] = bool( + report["passed"] + and result.completed + and result.all_success + and result.length == len(monitor.motion.times) - 1 + and limit is None + ) + _json(out / "validation.json", report) + return report + finally: + target.close(exit_process=False) + + +def create_replay_scene( + config: dict[str, Any], run_dir: Path, output_dir: Path +) -> dict[str, Any]: + """Load the same physical scene configuration and the recorded robot asset for rendering.""" + with TemporaryDirectory() as scratch: + deployment = materialize_deployment( + config, run_dir / "generated_assets/franka/franka_rope.urdf", Path(scratch) + ) + cfg, _, _ = _deployment_config(deployment) + return _prepare_replay_scene(cfg, config) + + +def _prepare_replay_scene(cfg, config: dict[str, Any]) -> dict[str, Any]: + """Prepare replay resources while the deployment USD is available.""" + from embodichain.lab.sim import SimulationManager + from embodichain.lab.sim.cfg import DenoisingCfg, LightCfg, RenderCfg + from embodichain.lab.sim.sensors import CameraCfg + + cfg.sim_cfg.render_cfg = RenderCfg( + renderer=config["render"].get("renderer", "hybrid"), + spp=config["render"]["spp"], + denoising=DenoisingCfg(window="off", offscreen="optix"), + tone_mapping_enabled=config["render"].get("tone_mapping_enabled", False), + tone_mapping_exposure=config["render"].get("exposure", 1.0), + min_bounces=config["render"].get("min_bounces", 4), + max_bounces=config["render"].get("max_bounces", 8), + ) + sim = SimulationManager(cfg.sim_cfg) + cameras = [] + try: + sim.set_ground_plane_visibility(False) + for rigid_cfg in (*cfg.background, *cfg.rigid_object): + sim.add_rigid_object(rigid_cfg) + robot = sim.add_robot(cfg.robot) + cable = sim.add_usd(cfg.usd_scene)["/World/cable"] + for name, view, fov in ( + ("overview", config["camera"], 48), + ("detail", config["detail_camera"], 42), + ): + width, height = (config["render"]["width"], config["render"]["height"]) + focal = height / (2 * np.tan(np.deg2rad(fov / 2))) + cameras.append( + sim.add_sensor( + CameraCfg( + uid=name, + width=width, + height=height, + near=0.01, + far=4, + intrinsics=(focal, focal, width / 2, height / 2), + enable_color=True, + enable_depth=False, + enable_mask=False, + extrinsics=CameraCfg.ExtrinsicsCfg( + eye=tuple(view["eye_m"]), target=tuple(view["target_m"]) + ), + ) + ) + ) + for uid, position, power in ( + ("key", (0.4, -0.6, 1.6), 14), + ("fill", (0.1, 0.7, 1.1), 9), + ): + sim.add_light( + LightCfg( + uid=uid, init_pos=position, light_type="point", intensity=power + ) + ) + sim.prepare() + return dict( + sim=sim, + robot=robot, + cable=cable, + contact=sim.add_sensor(cfg.sensor[0]), + cameras=cameras, + ) + except BaseException: + sim.destroy(exit_process=False) + raise + + +def restore_replay_frame( + scene: dict[str, Any], states: dict[str, np.ndarray], index: int +) -> None: + """Restore an exact recorded state; this function never steps physics.""" + device = scene["sim"].device + scene["robot"].set_qpos( + torch.as_tensor( + states["robot_qpos"][index : index + 1], dtype=torch.float32, device=device + ), + target=False, + ) + scene["cable"].write_segment_state_to_sim( + torch.as_tensor( + states["cable_states"][index : index + 1], + dtype=torch.float32, + device=device, + ).contiguous() + ) + + +def replay_camera_views( + t: float, config: dict[str, Any], states: dict[str, np.ndarray], index: int +) -> dict[str, tuple[np.ndarray, np.ndarray]]: + """Use stable overview and high oblique views to expose the actual winding.""" + return { + name: (np.asarray(view["eye_m"]), np.asarray(view["target_m"])) + for name, view in ( + ("overview", config["camera"]), + ("detail", config["detail_camera"]), + ) + } + + +def main() -> int: + """Run a reproducible contact-driven floor pickup and rope wrapping demo.""" + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument( + "--config", + type=Path, + default=task_directory() / "config.json", + help="Scene configuration; defaults to the config shipped beside this example", + ) + parser.add_argument( + "--output", type=Path, required=True, help="New output directory" + ) + parser.add_argument( + "--seconds", + type=float, + help="Stop early for diagnostics; an incomplete run does not pass acceptance", + ) + parser.add_argument( + "--plan-only", + action="store_true", + help="Prepare assets and validate the motion without stepping physics", + ) + args = parser.parse_args() + if args.seconds is not None and ( + not np.isfinite(args.seconds) or args.seconds <= 0 + ): + parser.error("--seconds must be a finite positive number") + args.config = args.config.expanduser().resolve() + try: + config = load_config(args.config) + except (OSError, ValueError, TypeError, KeyError) as error: + parser.error(f"Invalid configuration: {error}") + args.output = args.output.resolve() + args.output.mkdir(parents=True, exist_ok=False) + shutil.copyfile(args.config, args.output / "config.json") + shutil.copyfile(__file__, args.output / "source_snapshot.py") + manifest = { + "run_id": args.output.name, + "status": "running", + "created_at": datetime.now(timezone.utc).isoformat(), + "framework": "EmbodiChain", + "robot": "Franka FR3 / Panda hand with flat rope pads", + "physics": "Newton / DexUni", + "rendered": False, + "control": "Dynamic position/velocity joint drives; direct finger contact on the rope; no rope state writes or gripper attachment during physics", + "floor": { + "default_ground_hidden": True, + "surface_height_m": config["floor_height_m"], + "pickup_stand": False, + }, + "source_sha256": _hash(Path(__file__)), + "config_sha256": _hash(args.config), + "git_head": _source_revision(), + "runtime": _runtime_versions(), + } + _json(args.output / "manifest.json", manifest) + try: + from dexsim.kit.usd import parse_usd + + paths = prepare_models(config, args.output) + deployment = materialize_deployment(config, paths["robot"], args.output) + asset = deployment.with_name("cable.usda") + (cable,) = parse_usd(str(asset)).cables + manifest["cable_usd_sha256"] = _hash(asset) + np.savez_compressed( + args.output / "scene_inputs.npz", + points=np.asarray(cable.points, dtype=np.float32), + anchor=np.asarray(config["cable_waypoints_m"][0]), + post_center=config["post_center_xy_m"], + ) + if args.plan_only: + plan_wrap_motion(config, paths["robot"]).write(args.output) + manifest["status"] = "planned" + return 0 + report = _run(deployment, config, args.output, args.seconds) + print( + ("DEMO_COMPLETE " if report["checks"]["completed"] else "DEMO_PARTIAL ") + + json.dumps(report), + flush=True, + ) + manifest.update( + status="completed" if report["checks"]["completed"] else "partial", + passed=report["passed"], + ) + return 0 if report["passed"] else 1 + except Exception: + manifest["status"] = "failed" + (args.output / "failure.txt").write_text(traceback.format_exc()) + raise + finally: + _json(args.output / "manifest.json", manifest) + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/examples/sim/demo/franka_rope_wrap/config.json b/examples/sim/demo/franka_rope_wrap/config.json new file mode 120000 index 0000000000..8686481a96 --- /dev/null +++ b/examples/sim/demo/franka_rope_wrap/config.json @@ -0,0 +1 @@ +../../../../extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/config.json \ No newline at end of file diff --git a/extensions/embodichain_robots/_asset_catalog.py b/extensions/embodichain_robots/_asset_catalog.py index 6ea9d7bcd9..57eef7a3f5 100644 --- a/extensions/embodichain_robots/_asset_catalog.py +++ b/extensions/embodichain_robots/_asset_catalog.py @@ -83,6 +83,10 @@ "class_path": "embodichain_robots.franka.assets:FrankaPandaArm", "category": "robot", }, + "FrankaRopePads": { + "class_path": "embodichain_robots.franka.assets:FrankaRopePads", + "category": "robot", + }, "Franka": { "class_path": "embodichain_robots.franka.assets:Franka", "category": "robot", diff --git a/extensions/embodichain_robots/franka/assets.py b/extensions/embodichain_robots/franka/assets.py index 974cf61b52..2566ac4419 100644 --- a/extensions/embodichain_robots/franka/assets.py +++ b/extensions/embodichain_robots/franka/assets.py @@ -33,17 +33,26 @@ EMBODICHAIN_DEFAULT_DATA_ROOT, ) -__all__ = ["Franka", "FrankaPandaArm"] +__all__ = ["Franka", "FrankaPandaArm", "FrankaRopePads"] robot_assets = "robot_assets" +_FR3_WITH_HAND_SHA256 = ( + "a4a2d3265dadd69c4c7d8ed9238ae44494c217db6f66f586fcc40e8754ce2315" +) + + +def _franka_v4_archive() -> o3d.data.DataDescriptor: + return o3d.data.DataDescriptor( + os.path.join(EMBODICHAIN_DOWNLOAD_PREFIX, robot_assets, "FrankaV4.zip"), + "0c613d884798e8b084604381641cc8e1", + ) def _materialize_hand_component(extract_dir: str, data_root: str) -> None: """Split the authored hand subtree without modifying its source URDF.""" source = Path(extract_dir) / "Panda/PandaWithHand.urdf" content = source.read_bytes() - expected = "a4a2d3265dadd69c4c7d8ed9238ae44494c217db6f66f586fcc40e8754ce2315" - if hashlib.sha256(content).hexdigest() != expected: + if hashlib.sha256(content).hexdigest() != _FR3_WITH_HAND_SHA256: raise ValueError("pinned FR3/hand source changed in the asset cache") original = ElementTree.fromstring(content) links = ("fr3_hand", "fr3_hand_tcp", "fr3_leftfinger", "fr3_rightfinger") @@ -59,7 +68,7 @@ def _materialize_hand_component(extract_dir: str, data_root: str) -> None: report = json.dumps( dict( schema="fr3-hand-component/v1", - source_sha256=expected, + source_sha256=_FR3_WITH_HAND_SHA256, derived_sha256=hashlib.sha256(output).hexdigest(), links=links, joints=joints, @@ -67,15 +76,22 @@ def _materialize_hand_component(extract_dir: str, data_root: str) -> None: sort_keys=True, indent=2, ).encode() + _write_derivatives( + source, + data_root, + {"Fr3HandV1.urdf": output, "Fr3HandV1.derivation.json": report}, + ) + + +def _write_derivatives(source: Path, data_root: str, files: dict[str, bytes]) -> None: with _dataset_download_lock(data_root, "Franka"): - for name, value in ( - ("Fr3HandV1.urdf", output), - ("Fr3HandV1.derivation.json", report), - ): + for name, value in files.items(): target = source.parent / name if target.exists(): if target.read_bytes() != value: - raise ValueError("derived FR3 hand differs from its pinned source") + raise ValueError( + f"derived FR3 asset {name} differs from its pinned source" + ) continue temporary = None try: @@ -114,10 +130,7 @@ class Franka(EmbodiChainDataset): """ def __init__(self, data_root: str = None): - data_descriptor = o3d.data.DataDescriptor( - os.path.join(EMBODICHAIN_DOWNLOAD_PREFIX, robot_assets, "FrankaV4.zip"), - "0c613d884798e8b084604381641cc8e1", - ) + data_descriptor = _franka_v4_archive() prefix = type(self).__name__ path = EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root @@ -125,6 +138,42 @@ def __init__(self, data_root: str = None): _materialize_hand_component(self.extract_dir, path) +class FrankaRopePads(EmbodiChainDataset): + """FR3 with flat rope-grasping pads, derived on demand in the shared V4 cache.""" + + def __init__(self, data_root: str | None = None) -> None: + from .rope_pads import apply_rope_pads + + root = EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root + super().__init__("Franka", _franka_v4_archive(), root) + source = Path(self.extract_dir) / "Panda/PandaWithHand.urdf" + content = source.read_bytes() + if hashlib.sha256(content).hexdigest() != _FR3_WITH_HAND_SHA256: + raise ValueError("pinned FR3/hand source changed in the asset cache") + padded = ElementTree.fromstring(content) + apply_rope_pads(padded) + model = ElementTree.tostring(padded, encoding="utf-8", xml_declaration=True) + report = json.dumps( + { + "schema": "fr3-rope-pads/v1", + "source": source.name, + "source_sha256": _FR3_WITH_HAND_SHA256, + "derived_sha256": hashlib.sha256(model).hexdigest(), + "modified_links": ["fr3_leftfinger", "fr3_rightfinger"], + }, + sort_keys=True, + indent=2, + ).encode() + _write_derivatives( + source, + root, + { + "Fr3RopePadsV1.urdf": model, + "Fr3RopePadsV1.derivation.json": report, + }, + ) + + class FrankaPandaArm(EmbodiChainDataset): """Pinned original Panda arm with Panda limits, distinct from FR3 assets. diff --git a/extensions/embodichain_robots/franka/rope_pads.py b/extensions/embodichain_robots/franka/rope_pads.py new file mode 100644 index 0000000000..59c846cf55 --- /dev/null +++ b/extensions/embodichain_robots/franka/rope_pads.py @@ -0,0 +1,67 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Construct flat FR3 fingertip pads for direct floor cable pickup.""" + +from __future__ import annotations +import xml.etree.ElementTree as ET +from typing import Sequence + +__all__ = ["apply_rope_pads"] + + +def apply_rope_pads( + root: ET.Element, + *, + center: Sequence[float] = (0, 0.002, 0.04175), + size: Sequence[float] = (0.028, 0.004, 0.0135), +) -> None: + """Replace finger geometry, preserving the original inertias and joint frames.""" + for name in ("fr3_leftfinger", "fr3_rightfinger"): + link = root.find(f"link[@name='{name}']") + if link is None: + raise ValueError(f"FR3 model is missing its finger link {name!r}") + for child in list(link): + if child.tag in ("visual", "collision"): + link.remove(child) + _box( + link, + name + "_stem", + (0, 0.004, 0.02), + (0.018, 0.008, 0.04), + ".08 .09 .11 1", + ) + _box(link, name + "_tip", center, size, ".16 .18 .21 1") + + +def _box( + link: ET.Element, + name: str, + xyz: Sequence[float], + size: Sequence[float], + color: str, +) -> None: + for kind in ("visual", "collision"): + element = ET.SubElement(link, kind, name=name + "_" + kind) + ET.SubElement(element, "origin", xyz=" ".join(map(str, xyz)), rpy="0 0 0") + ET.SubElement( + ET.SubElement(element, "geometry"), "box", size=" ".join(map(str, size)) + ) + if kind == "visual": + ET.SubElement( + ET.SubElement(element, "material", name=name + "_material"), + "color", + rgba=color, + ) diff --git a/extensions/embodichain_tasks/configs/components/embodiments/franka_flat_pads.yaml b/extensions/embodichain_tasks/configs/components/embodiments/franka_flat_pads.yaml new file mode 100644 index 0000000000..df2f6a0274 --- /dev/null +++ b/extensions/embodichain_tasks/configs/components/embodiments/franka_flat_pads.yaml @@ -0,0 +1,66 @@ +embodiment_id: franka_flat_pads +simulation: + uid: franka + fpath: FrankaRopePads/Panda/Fr3RopePadsV1.urdf + init_pos: [0, 0, 0.002] + init_qpos: [0.0, -0.569, 0.0, -2.81, 0.0, 3.037, 0.741, 0.0395, 0.0395] + control_parts: + arm: ["fr3_joint[1-7]"] + eef: ["fr3_finger_joint[1-2]"] + arm_and_hand: ["fr3_joint[1-7]", "fr3_finger_joint[1-2]"] + solver_cfg: + arm: + class_type: PytorchSolver + root_link_name: fr3_link0 + end_link_name: fr3_hand_tcp + joint_drive_props: + # Preserve the source URDF joint armature and friction as in the calibrated scene. + drive_type: null + armature: null + friction: null + stiffness: {arm: 6000.0, eef: 6000.0} + damping: {arm: 150.0, eef: 35.0} + max_effort: {arm: 87.0, eef: 20.0} + target_mode: position_velocity + root_props: + newton_gravity_compensation: 1.0 + attrs: + collision_props: {backend: newton, margin: 0.0, gap: 0.00005} + material_props: {backend: newton, dynamic_friction: 1.2, ke: 20000.0, kd: 1.0} +sensor: +- sensor_type: ContactSensor + uid: contacts + rigid_uid_list: [post] + articulation_cfg_list: + - articulation_uid: franka + link_name_list: [] + filter_need_both_actor: false + max_contacts_per_env: 4096 +skill_profile: + contract_id: single_arm_parallel_gripper + profile_id: franka_rope_wrap + evaluation_links: + first_grasp_finger: fr3_leftfinger + second_grasp_finger: fr3_rightfinger + resources: + - resource_id: primary_manipulator + endpoints: + - endpoint_id: motion + control_part: arm + capabilities: [kinematics.batch_inverse, motion.cartesian_pose, kinematics.forward, motion.joint_position] + - endpoint_id: grasp + control_part: eef + capabilities: [interaction.grasp, motion.joint_position] + command_preset: flat_pads + - resource_id: arm_and_hand + endpoints: + - endpoint_id: motion + control_part: arm_and_hand + capabilities: [motion.joint_position] + members: [primary_manipulator] + command_presets: + - preset_id: flat_pads + control_part: eef + commands: + open: [0.0395, 0.0395] + grasp: [0.0018, 0.0018] diff --git a/extensions/embodichain_tasks/configs/components/embodiments/yam_i2rt.yaml b/extensions/embodichain_tasks/configs/components/embodiments/yam_i2rt.yaml new file mode 100644 index 0000000000..ca423d557a --- /dev/null +++ b/extensions/embodichain_tasks/configs/components/embodiments/yam_i2rt.yaml @@ -0,0 +1,104 @@ +embodiment_id: yam_i2rt +simulation: + class_type: YamI2rt + uid: yam + root_props: + newton_gravity_compensation: 1.0 + joint_drive_props: + stiffness: + right_joint[1-6]: 300.0 + left_joint[1-6]: 2000.0 + .*_joint[78]: 6000.0 + damping: + right_joint[1-6]: 30.0 + left_joint[1-6]: 90.0 + .*_joint[78]: 120.0 + link_attrs: + pads: + link_names_expr: + - (right|left)_tip_(left|right) + attrs: + material_props: + backend: newton + dynamic_friction: 60.0 + ke: 10000.0 + kd: 0.1 + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 +sensor: [] +skill_profile: + contract_id: dual_arm_parallel_gripper + profile_id: yam_i2rt + resources: + - resource_id: right_manipulator + endpoints: + - endpoint_id: motion + control_part: right_arm + capabilities: + - kinematics.batch_inverse + - motion.cartesian_pose + - kinematics.forward + - motion.joint_position + - endpoint_id: grasp + control_part: right_eef + capabilities: + - interaction.grasp + - motion.joint_position + command_preset: right_jaw + - resource_id: right_hand + endpoints: + - endpoint_id: motion + control_part: right_eef + capabilities: + - motion.joint_position + members: + - right_manipulator + - resource_id: left_manipulator + endpoints: + - endpoint_id: motion + control_part: left_arm + capabilities: + - kinematics.batch_inverse + - motion.cartesian_pose + - kinematics.forward + - motion.joint_position + - endpoint_id: grasp + control_part: left_eef + capabilities: + - interaction.grasp + - motion.joint_position + command_preset: left_jaw + - resource_id: left_hand + endpoints: + - endpoint_id: motion + control_part: left_eef + capabilities: + - motion.joint_position + members: + - left_manipulator + command_presets: + - preset_id: right_jaw + control_part: right_eef + commands: + open: + - 0.0475 + - 0.0475 + grasp: + - 0.003 + - 0.003 + - preset_id: left_jaw + control_part: left_eef + commands: + open: + - 0.0475 + - 0.0475 + grasp: + - 0.003 + - 0.003 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/cable.usda b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/cable.usda new file mode 100644 index 0000000000..bd910fcf36 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/cable.usda @@ -0,0 +1,134 @@ +#usda 1.0 +( + defaultPrim = "World" + metersPerUnit = 1 + kilogramsPerUnit = 1 + upAxis = "Z" +) + +def Xform "World" +{ + def BasisCurves "cable" ( + prepend apiSchemas = ["PhysicsCurvesDeformableSimAPI", "PhysicsCollisionAPI", "MaterialBindingAPI"] + ) + { + uniform token type = "linear" + uniform token wrap = "nonperiodic" + int[] curveVertexCounts = [97] + point3f[] points = [ + (0.209999993, -0.200000003, 0.00600000005), + (0.217188239, -0.194675386, 0.00600000005), + (0.224370629, -0.189342961, 0.00600000005), + (0.231592745, -0.184064388, 0.00600000005), + (0.238798141, -0.178763345, 0.00600000005), + (0.245842054, -0.173249841, 0.00600000005), + (0.252806395, -0.167638332, 0.00600000005), + (0.260527343, -0.163148969, 0.00600000005), + (0.268971115, -0.160236403, 0.00600000005), + (0.277802795, -0.158881187, 0.00600000005), + (0.286741167, -0.15888761, 0.00600000005), + (0.295613945, -0.159991324, 0.00600000005), + (0.304298371, -0.162111297, 0.00600000005), + (0.312590569, -0.165442377, 0.00600000005), + (0.320187509, -0.170141339, 0.00600000005), + (0.326739371, -0.176210329, 0.00600000005), + (0.331986338, -0.183438838, 0.00600000005), + (0.335870743, -0.191485748, 0.00600000005), + (0.338513196, -0.200025022, 0.00600000005), + (0.340143234, -0.208817571, 0.00600000005), + (0.341180056, -0.217701465, 0.00600000005), + (0.34166351, -0.226632357, 0.00600000005), + (0.341549665, -0.235575378, 0.00600000005), + (0.340800524, -0.244487479, 0.00600000005), + (0.339387804, -0.253318548, 0.00600000005), + (0.337296724, -0.262013882, 0.00600000005), + (0.334528506, -0.270517886, 0.00600000005), + (0.331101269, -0.278778374, 0.00600000005), + (0.327048212, -0.286750615, 0.00600000005), + (0.322414666, -0.294400305, 0.00600000005), + (0.31725359, -0.301704705, 0.00600000005), + (0.31162113, -0.308652431, 0.00600000005), + (0.305574983, -0.315243661, 0.00600000005), + (0.299626291, -0.321922034, 0.00600000005), + (0.294217348, -0.329043716, 0.00600000005), + (0.289473921, -0.336623937, 0.00600000005), + (0.285538852, -0.344652444, 0.00600000005), + (0.282557517, -0.35308075, 0.00600000005), + (0.280652642, -0.361814797, 0.00600000005), + (0.279893577, -0.370721728, 0.00600000005), + (0.280277133, -0.379653186, 0.00600000005), + (0.281730533, -0.388474852, 0.00600000005), + (0.284135133, -0.397087067, 0.00600000005), + (0.287354887, -0.40542984, 0.00600000005), + (0.291258216, -0.413476348, 0.00600000005), + (0.295769274, -0.421198219, 0.00600000005), + (0.300944388, -0.428490847, 0.00600000005), + (0.306810826, -0.435238898, 0.00600000005), + (0.313363224, -0.441322267, 0.00600000005), + (0.32055679, -0.446631521, 0.00600000005), + (0.328308403, -0.45108673, 0.00600000005), + (0.336507916, -0.454651982, 0.00600000005), + (0.345036805, -0.457337528, 0.00600000005), + (0.353784978, -0.459191352, 0.00600000005), + (0.362660974, -0.460284531, 0.00600000005), + (0.371595025, -0.460697651, 0.00600000005), + (0.380533993, -0.46043101, 0.00600000005), + (0.3894113, -0.459357917, 0.00600000005), + (0.39812398, -0.457351744, 0.00600000005), + (0.406526804, -0.454301566, 0.00600000005), + (0.414439827, -0.450145304, 0.00600000005), + (0.421678364, -0.444902539, 0.00600000005), + (0.428097844, -0.438683033, 0.00600000005), + (0.43362835, -0.431659728, 0.00600000005), + (0.438277245, -0.424022585, 0.00600000005), + (0.442106277, -0.415941805, 0.00600000005), + (0.445231766, -0.407562286, 0.00600000005), + (0.447679847, -0.398961097, 0.00600000005), + (0.449328154, -0.390172422, 0.00600000005), + (0.450047165, -0.381260455, 0.00600000005), + (0.449721098, -0.372326463, 0.00600000005), + (0.448275059, -0.363504887, 0.00600000005), + (0.445699722, -0.354944617, 0.00600000005), + (0.442058802, -0.346779853, 0.00600000005), + (0.437480152, -0.339100063, 0.00600000005), + (0.432886571, -0.331426263, 0.00600000005), + (0.428828001, -0.323456198, 0.00600000005), + (0.425270736, -0.315249801, 0.00600000005), + (0.422177345, -0.306857258, 0.00600000005), + (0.419509768, -0.298319608, 0.00600000005), + (0.417231083, -0.289669901, 0.00600000005), + (0.415306807, -0.280934364, 0.00600000005), + (0.41370517, -0.272133857, 0.00600000005), + (0.412789553, -0.263247252, 0.00600000005), + (0.414676487, -0.25459832, 0.00600000005), + (0.421680957, -0.249475315, 0.00600000005), + (0.430575877, -0.249301776, 0.00600000005), + (0.439495057, -0.249975488, 0.00600000005), + (0.448437244, -0.250168413, 0.00600000005), + (0.457381606, -0.250026435, 0.00600000005), + (0.466326892, -0.249970526, 0.00600000005), + (0.475272387, -0.24999088, 0.00600000005), + (0.484217912, -0.250004321, 0.00600000005), + (0.493163437, -0.250002652, 0.00600000005), + (0.502108932, -0.249999449, 0.00600000005), + (0.511054456, -0.249998569, 0.00600000005), + (0.519999981, -0.25, 0.00600000005) + ] + float[] physics:thicknesses = [0.008] + token physics:thicknesses:elementType = "constant" + rel material:binding = + } + + def Material "white_rope" + { + token outputs:surface.connect = + def Shader "Surface" + { + uniform token info:id = "UsdPreviewSurface" + # Scene-linear equivalent of the original (0.93, 0.94, 0.92) sRGB. + color3f inputs:diffuseColor = (0.84808826, 0.86889939, 0.82757083) + float inputs:roughness = 0.6 + token outputs:surface + } + } +} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/catalog.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/catalog.yaml new file mode 100644 index 0000000000..71eaa05ac4 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/catalog.yaml @@ -0,0 +1,8 @@ +task_key: franka_rope_wrap +title: Franka Rope Wrap +summary: Pick up a rope from the floor, wrap it around a post and keep hold of the free end. +tags: [cable, deformable, manipulation, task_program_expert] +default_deployment: franka +deployments: + franka: + config: task.franka.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/config.json b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/config.json new file mode 100644 index 0000000000..a2cf72ea2f --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/config.json @@ -0,0 +1,210 @@ +{ + "schema_version": 1, + "robot_urdf": "Franka/Panda/PandaWithHand.urdf", + "home_qpos": [ + 0.0, + -0.569, + 0.0, + -2.81, + 0.0, + 3.037, + 0.741 + ], + "floor_height_m": 0.002, + "post_center_xy_m": [ + 0.48, + 0 + ], + "post_radius_m": 0.045, + "post_height_m": 0.12, + "cable_waypoints_m": [ + [ + 0.21, + -0.2, + 0.006 + ], + [ + 0.22607114386689492, + -0.18809544898748526, + 0.006 + ], + [ + 0.24214228773378982, + -0.1761908979749705, + 0.006 + ], + [ + 0.2582134316006847, + -0.16428634696245575, + 0.006 + ], + [ + 0.32, + -0.17, + 0.006 + ], + [ + 0.34, + -0.25, + 0.006 + ], + [ + 0.28, + -0.37626441961695295, + 0.006 + ], + [ + 0.32, + -0.44626441961695296, + 0.006 + ], + [ + 0.42, + -0.44626441961695296, + 0.006 + ], + [ + 0.45, + -0.37626441961695295, + 0.006 + ], + [ + 0.42, + -0.3, + 0.006 + ], + [ + 0.42, + -0.25, + 0.006 + ], + [ + 0.44, + -0.25, + 0.006 + ], + [ + 0.46, + -0.25, + 0.006 + ], + [ + 0.48, + -0.25, + 0.006 + ], + [ + 0.5, + -0.25, + 0.006 + ], + [ + 0.52, + -0.25, + 0.006 + ] + ], + "cable_segments": 96, + "cable_radius_m": 0.004, + "cable_density_kg_m3": 800.0, + "bend_stiffness_Nm_rad": 0.0001, + "twist_stiffness_Nm_rad": 0.0001, + "bend_damping_Nms_rad": 0.0001, + "linear_stiffness_N_m": 100000.0, + "control_hz": 60, + "substeps": 32, + "iterations": 48, + "pre_roll_s": 2.0, + "duration_s": 36.0, + "grasp_complete_s": 5.0, + "lift_complete_s": 7.0, + "finger_pad_width_m": 0.028, + "grip_half_gap_m": 0.0018, + "grasp_inset_m": 0.025, + "post_friction": 0.3, + "rope_friction": 0.8, + "floor_friction": 0.2, + "contact_ke_N_m": 20000.0, + "contact_kd_Ns_m": 1.0, + "contact_gap_m": 5e-05, + "contact_match_distance_m": 5e-05, + "rigid_contact_hard": false, + "rigid_contact_history": false, + "friction_epsilon": 1e-05, + "camera": { + "eye_m": [ + 1.24, + -1.16, + 0.93 + ], + "target_m": [ + 0.35, + -0.05, + 0.3 + ] + }, + "detail_camera": { + "eye_m": [ + 0.55, + -0.24, + 1.05 + ], + "target_m": [ + 0.46, + -0.04, + 0.05 + ] + }, + "render": { + "width": 3840, + "height": 2160, + "fps": 30, + "spp": 16, + "renderer": "rt", + "tone_mapping_enabled": true, + "exposure": 0.8, + "min_bounces": 4, + "max_bounces": 8, + "crf": 15, + "preset": "slow" + }, + "validation": { + "tail_tcp_max_m": 0.045, + "winding_min_deg": 360.0, + "contact_arc_min_deg": 300.0, + "contact_band_m": 0.003, + "hold_winding_drift_max_deg": 3.0, + "hold_end_drift_max_m": 0.003, + "post_penetration_max_m": 0.0008, + "robot_post_penetration_max_m": 0.0001, + "joint_gap_max_m": 0.0008, + "floor_penetration_max_m": 0.0008, + "grasp_slip_max_m": 0.008, + "anchor_exit_bend_max_deg": 20.0 + }, + "grasp_offset_world_m": [ + -0.025, + 0, + 0 + ], + "orbit_radius_m": 0.16, + "orbit_degrees": 465, + "orbit_start_z_m": 0.065, + "orbit_end_z_m": 0.085, + "orbit_start_deg": -90.0, + "orbit_direction": 1, + "grasp_yaw_deg": 0, + "tighten_radial_m": 0.018, + "tighten_tangent_m": 0.02, + "open_half_gap_m": 0.0395, + "finger_pad_center_m": [ + 0, + 0.002, + 0.04175 + ], + "finger_pad_size_m": [ + 0.028, + 0.004, + 0.0135 + ] +} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/env.yaml new file mode 100644 index 0000000000..5ea9153d2d --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/env.yaml @@ -0,0 +1,131 @@ +environment_id: franka_rope_wrap +enable_ground_plane: true +physics: newton +physics_config: + physics_dt: 0.0005208333333333333 + num_substeps: 1 + collision_cfg: null + sync_to_renderer: false + use_cuda_graph: true + solver_cfg: + solver_type: dexuni + joint_mode: dynamic + coupling: two_way + mujoco_articulation_paths: + - arena_0/franka + vbd_options: + iterations: 48 + rigid_contact_history: false + rigid_contact_hard: false + rigid_joint_linear_ke: 100000.0 + rigid_joint_angular_ke: 10.0 + friction_epsilon: 1.0e-05 + collision_options: + contact_matching: sticky + contact_matching_pos_threshold: 5.0e-05 + rigid_contact_max: 4096 +max_episode_steps: 2161 +num_envs: 1 +simulation: + background: + - uid: table + body_type: static + init_pos: [0.35, 0, -0.023] + shape: + shape_type: Cube + size: [1.2, 1.05, 0.05] + visual_material: + uid: table_mat + base_color: [0.33, 0.37, 0.41, 1] + roughness: 0.65 + attrs: + collision_props: + backend: newton + margin: 0.0 + gap: 5.0e-05 + material_props: + backend: newton + dynamic_friction: 0.2 + ke: 20000.0 + kd: 1.0 + - uid: anchor_clamp + body_type: static + init_pos: [0.20517865683993153, -0.20357136530375444, 0.002] + shape: + shape_type: Cube + size: [0.028, 0.018, 0.008] + visual_material: + uid: anchor_clamp_mat + base_color: [0.08, 0.11, 0.15, 1] + roughness: 0.65 + attrs: + collision_props: + backend: newton + margin: 0.0 + gap: 5.0e-05 + material_props: + backend: newton + dynamic_friction: 0.2 + ke: 20000.0 + kd: 1.0 + init_rot: [0, 0, 36.52885536698517] + - uid: post + body_type: static + init_pos: [0.48, 0, 0.062] + shape: + shape_type: Cylinder + radius: 0.045 + half_height: 0.06 + visual_material: + uid: post_mat + base_color: [0.025, 0.45, 0.39, 1] + roughness: 0.38 + attrs: + collision_props: + backend: newton + margin: 0.0 + gap: 5.0e-05 + material_props: + backend: newton + dynamic_friction: 0.3 + ke: 20000.0 + kd: 1.0 + usd_scene: + name: cable_scene + source: + file_path: ./cable.usda + asset_cfgs: + /World/cable: + uid: cable + fixed_segments: [0] + collision_filter_span: 2 + attrs: + density: 800.0 + stretch_stiffness: 100000.0 + shear_stiffness: 100000.0 + bend_stiffness: 0.0001 + twist_stiffness: 0.0001 + stretch_damping: 0.03 + shear_damping: 0.03 + bend_damping: 0.0001 + twist_damping: 0.0001 + collision_props: + backend: newton + margin: 0.0 + gap: 5.0e-05 + material_props: + backend: newton + dynamic_friction: 0.8 + ke: 20000.0 + kd: 1.0 +env: + sim_steps_per_control: 32 + ignore_terminations: true + expert_trajectory: + joint_command_mode: position_velocity + task_instruction: Pick up the rope, wrap it around the post, tighten and hold it. + events: + settle_rope: + func: settle_franka_rope + mode: reset + params: {} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task.franka.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task.franka.yaml new file mode 100644 index 0000000000..e98ade67f3 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task.franka.yaml @@ -0,0 +1,22 @@ +id: FrankaRopeWrap-v1 +environment: + component: env.yaml +embodiment: + component: ../../../../components/embodiments/franka_flat_pads.yaml +headless: true +render_cfg: + renderer: no-render +ground_plane_visible: false +enable_entity_gizmo: false +robot_ik_gizmo: null +visualization: + allow_commands: false +collection: + target_episodes: 1 + max_attempts: 1 +task_program: + program: task_program/program.yaml + integration: task_program/integration.yaml + execution_policy: ../../../../components/execution_policies/trajectory_open_loop.yaml +evaluation: + component: task_program/evaluation.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/evaluation.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/evaluation.yaml new file mode 100644 index 0000000000..74a73699de --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/evaluation.yaml @@ -0,0 +1,102 @@ +graph: + roots: [wrap_success] + rules: + completed: + kind: scalar_range + bindings: {value: cables/cable/wrap_completed} + params: {minimum: 1, maximum: 1} + finite: + kind: scalar_range + bindings: {value: cables/cable/wrap_finite} + params: {minimum: 1, maximum: 1} + joint_limits: + kind: scalar_range + bindings: {value: cables/cable/wrap_joint_limits} + params: {minimum: 1, maximum: 1} + direct_floor_pickup: + kind: scalar_range + bindings: {value: cables/cable/wrap_direct_floor_pickup} + params: {minimum: 1, maximum: 1} + grasp_retained: + kind: scalar_range + bindings: {value: cables/cable/wrap_grasp_retained} + params: {minimum: 1, maximum: 1} + material_grasp_slip: + kind: scalar_range + bindings: {value: cables/cable/wrap_material_grasp_slip} + params: {minimum: 1, maximum: 1} + bilateral_finger_contact: + kind: scalar_range + bindings: {value: cables/cable/wrap_bilateral_finger_contact} + params: {minimum: 1, maximum: 1} + actual_winding: + kind: scalar_range + bindings: {value: cables/cable/wrap_actual_winding} + params: {minimum: 1, maximum: 1} + post_contact_arc: + kind: scalar_range + bindings: {value: cables/cable/wrap_post_contact_arc} + params: {minimum: 1, maximum: 1} + hold_stable: + kind: scalar_range + bindings: {value: cables/cable/wrap_hold_stable} + params: {minimum: 1, maximum: 1} + rope_below_post_top: + kind: scalar_range + bindings: {value: cables/cable/wrap_rope_below_post_top} + params: {minimum: 1, maximum: 1} + post_nonpenetration: + kind: scalar_range + bindings: {value: cables/cable/wrap_post_nonpenetration} + params: {minimum: 1, maximum: 1} + robot_post_clearance: + kind: scalar_range + bindings: {value: cables/cable/wrap_robot_post_clearance} + params: {minimum: 1, maximum: 1} + anchor_points_toward_post: + kind: scalar_range + bindings: {value: cables/cable/wrap_anchor_points_toward_post} + params: {minimum: 1, maximum: 1} + smooth_anchor_exit: + kind: scalar_range + bindings: {value: cables/cable/wrap_smooth_anchor_exit} + params: {minimum: 1, maximum: 1} + anchor_fixed: + kind: scalar_range + bindings: {value: cables/cable/wrap_anchor_fixed} + params: {minimum: 1, maximum: 1} + length_preserved: + kind: scalar_range + bindings: {value: cables/cable/wrap_length_preserved} + params: {minimum: 1, maximum: 1} + contacts_not_dropped: + kind: scalar_range + bindings: {value: cables/cable/wrap_contacts_not_dropped} + params: {minimum: 1, maximum: 1} + floor_nonpenetration: + kind: scalar_range + bindings: {value: cables/cable/wrap_floor_nonpenetration} + params: {minimum: 1, maximum: 1} + wrap_success: + kind: all + refs: + - completed + - finite + - joint_limits + - direct_floor_pickup + - grasp_retained + - material_grasp_slip + - bilateral_finger_contact + - actual_winding + - post_contact_arc + - hold_stable + - rope_below_post_top + - post_nonpenetration + - robot_post_clearance + - anchor_points_toward_post + - smooth_anchor_exit + - anchor_fixed + - length_preserved + - contacts_not_dropped + - floor_nonpenetration +final_acceptance: [wrap_success] diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/integration.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/integration.yaml new file mode 100644 index 0000000000..93fc48cd5c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/integration.yaml @@ -0,0 +1,23 @@ +integration_id: franka_rope_wrap_v1 +program_id: franka_rope_wrap +requires: + scene_contract: cable_post_v1 + embodiment_contract: single_arm_parallel_gripper +scene_binding: + contract_id: cable_post_v1 + registry_id: cable_post + cables: + - entity_id: cable + simulation_uid: cable +profile: + defaults: + move_joints: + primary: arm_and_hand + skill_options: + simulation.cable_wrap_phase: + kind: move_joints + preserve_velocity_samples: true + effect_monitors: {} +runtime_services: + registered_semantic_lowerers: + - kind: cable_wrap_phase diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/program.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/program.yaml new file mode 100644 index 0000000000..2afa4e7ee0 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/franka_rope_wrap/task_program/program.yaml @@ -0,0 +1,87 @@ +program_id: franka_rope_wrap +instruction: Grasp the free end of the floor cable, wrap it around the post and hold the tightened wrap. +targets: {} +program: +- segment: home + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: home +- segment: approach + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: approach +- segment: descend + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: descend +- segment: close + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: close +- segment: lift + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: lift +- segment: approach_post + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: approach_post +- segment: orbit + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: orbit +- segment: tighten + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: tighten +- segment: settle + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: settle +- segment: hold_closed + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_wrap_phase + arguments: + phase: hold_closed + validators: + - kind: condition + ref: wrap_success diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md new file mode 100644 index 0000000000..90a2922c07 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/README.md @@ -0,0 +1,299 @@ +# Route the Cable (YAM) + +This configured Task Program routes a 600 mm anchored cable around two spools and +places its free end in a 10 mm channel. It adapts the +[robosimstudio example](https://github.com/robosimstudio/robosimstudio/tree/73739abe212f963e8a9f5862b27bca370df091a7/examples/08_demos/route_the_cable). +The task uses the common `EmbodiedEnv`, configuration-owned registration, +Semantic Call lowering to existing Atomic Skills, the standard `DemoSegment` +bridge and trajectory recorder. Only the proximal cable segment +is fixed; grasps use collision and friction, with no runtime attachment or +in-episode state writes. + +## Requirements and entry point + +Use CUDA and the pinned DexSim `0.6.0rc2` wheel. The packaged +[`cable.usda`](cable.usda) defines the original 51 centerline points, the 7 mm +diameter and the initial pose. The `usd_scene.asset_cfgs` entry in +[`env.yaml`](env.yaml) binds the curve as `cable` and supplies the fixed first +segment, the four per-joint stiffness and damping pairs, density and contact +parameters. The environment config also selects Newton/DexUni, dynamic MuJoCo +joints and two-way VBD cable contact. + +From the EmbodiChain repository: + +```bash +python -m embodichain data download --name YamI2rt +python -m embodichain list-task --category manipulation/cable +python -m embodichain run-task \ + --gym_config extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml \ + --max_episodes 1 --seed 73 --headless --record_trajectory \ + --trajectory_save_dir ./output/route_the_cable +``` + +`YamI2rtCfg` resolves `get_data_path("YamI2rt/yam_i2rt.urdf")` through the same +asset registry and cache used by Franka. The predownload command above is optional: +constructing the robot downloads and extracts the ready +`robot_assets/YamI2rtV1.zip` bundle when needed. Downloading the bundle does not +require CoACD. `EMBODICHAIN_DOWNLOAD_PREFIX` selects a mirror; the official +`DexForceAI/embodichain_data` Hugging Face dataset is the fallback. + +The bundle was +[published on 2026-11-05](https://huggingface.co/datasets/DexForceAI/embodichain_data/commit/8c0b7dcc8a80ab5a1f177c7384ad9d6950281a68) +and is publicly downloadable without a Hugging Face token. Official HF download +into an empty cache, archive checksums, extraction, cache reuse and robot +configuration/FK-chain loading were verified. This asset validation does not +qualify a complete routing episode. + +For optional reconstruction from the pinned source in a checkout, use: + +```bash +python -m pip install coacd==1.0.14 trimesh==4.12.2 scipy==1.18.1 +python scripts/tools/prepare_yam_i2rt_assets.py +``` + +The preparation script downloads the 9 STL meshes, source MJCF and MIT license +from robosimstudio commit `73739abe212f963e8a9f5862b27bca370df091a7`. It converts +the dual-arm MJCF to URDF and generates 24 left-tip and 15 right-tip collision +hulls with CoACD 1.0.14, threshold 0.06 and seed 73. Every source and generated +file must match the SHA-256 of the qualified robot before the staged directory +is installed. A valid existing bundle is verified and reused; an incomplete or +modified bundle is rejected without being overwritten. This preparation runs +on CPU and does not require the simulation renderer or CUDA. + +The download registry extracts under `/extract/YamI2rt/`; the source +script defaults to `/YamI2rt/`. Both are recognized by `get_data_path`. +The default data root is `~/.cache/embodichain_data/`. Set +`EMBODICHAIN_DATA_ROOT` before both preparation and demo execution to use another +cache root. For offline preparation, pass `--source-dir` pointing to the pinned +upstream `assets/embodiments/yam_i2rt/` directory. `--output` selects an explicit +destination; configure the robot's `fpath` to its `yam_i2rt.urdf` if this differs +from the default cache. The raw meshes, generated URDF and collision hulls are +local assets, excluded from Git and the Python wheel. + +For asset publication, `--archive /path/to/YamI2rtV1.zip` additionally builds a +deterministic archive from the verified bundle and includes its MIT license, +NOTICE and provenance. It retains the qualified geometry and all 52 original +file hashes. Archive MD5: `23601ea820ff3e6260336c4962ef01a5`; SHA-256: +`9125380a6240847f42730b79025aa61cd17b665f90a2586b3c6f061d769c503a`. +The repository's `NOTICE` and `licenses/YamI2rt-MIT.txt` record the external +asset attribution even though the binary assets are distributed separately. + +The collector accepts complete successful episodes only. A failed intermediate +validator stops its episode. A launched expert is not evidence of task success. +This expert deployment sets `visualization.allow_commands: false`: Viser is an +observer, with camera orbit/pan still available. Dragging robot controls while +the expert writes joint targets introduces a competing controller and can +invalidate grasps. The launcher preserves this read-only policy with `--viser`. +Use a separately configured, stepping scene without the expert for manual IK +experiments. Failed motion validators log planning status, TCP position error +and maximum measured grasp slip with their limits. +Every motion is followed by an explicit bounded hold and a measured TCP +position/orientation check. Insertion corrections read the latest material +geometry when grounded; the finite correction slots retain the commanded pose +once the release condition is reached. They do not run an unbounded Python loop. + +## Stages and reusable boundaries + +The program contains 51 segments and 129 Semantic Call invocations. + +The right arm places capsule centres at material distances 0.162, 0.282 and +0.438 m; these are the nearest capsule centres to the upstream nominal grasp +locations. The left arm grasps at 0.57 m, leaving a 30 mm free span. All four +pinches use a 0.7605 m TCP height. `PickUp` closes and settles with +`lift_height: 0`; a separate `material_lift` target lowers to `MoveEndEffector` +and raises the held material while retracting opposite its observed tangent +projected onto XY. This supplies upstream slack as the cable lifts from the +bench. The lift preserves the measured tool orientation and keeps the original +post-closure slip reference. + +| Grasp material distance | Retraction | Lift TCP height | +| --- | --- | --- | +| Right, 0.162 m | 1.5 mm | 0.774 m | +| Right, 0.282 m | 3 mm | 0.780 m | +| Right, 0.438 m | 8 mm | 0.800 m | +| Left, 0.570 m | 3 mm | 0.780 m | + +The left arm selects a wrist branch that can reach the staging TCP position +`[0.586, 0.055, 0.805]` m above the open channel. The staging target constructs a +complete orthonormal rotation, including when the measured tool has pitch or +roll. Four bounded XY corrections aim the observed free end at +`[0.586, 0.077]` m. Subsequent TCP height stages are 0.795, 0.7875, 0.780 and +0.7655 m, each followed by two correction slots. Each correction is limited to +5 mm and keeps TCP Y at or below 0.05 m so the hand stays outside the channel +mouth while lowering. Final vertical corrections are limited to 3 mm and retain +a 0.7655 m TCP height floor. Corrections accumulate on the commanded pose to +compensate for contact-load bias. + +Release requires the observed endpoint to be inside the strict channel with +`y >= 0.072` m. The configured release region is +`[0.5845, 0.072, 0.753]` to `[0.5875, 0.083, 0.758]` m, entirely within the +channel; readiness also accepts an endpoint already seated in the strict final +region at the same minimum depth. The X bounds account for the 3.5 mm cable +radius between the channel's inner walls. Merely crossing the channel mouth +does not stop the remaining corrections. Release also requires sampled cable +penetration no greater than 1 mm. Once ready, subsequent correction slots retain +the commanded pose and its contact load. Ordinary measured TCP tracking remains +required until the endpoint is seated inside the strict channel. + +The left hand first releases with an 8 mm joint target per finger, then retreats +to TCP `[same X, 0.03, 0.82]` m. `open_clear_of_port` opens the hand fully only +after that retreat, avoiding a full finger sweep through the upstream cable at +the insertion pose. The final validator requires the actual end inside the +strict channel after release, retreat and settling. Each stage is planned +lazily from the latest cable state. Both hands return home before the final +settling period. + +The YAM IK uses 10 μm position and 1e-5 rad rotation convergence so finite- +difference joint acceleration is not dominated by numerical jitter during +Cartesian retiming. Task moves accept the planner's possibly longer control +grid; grasp-slip and physical placement thresholds are unchanged. + +At each spool, the right hand releases with an 8 mm joint target per finger, +retreats to 0.82 m TCP height, and only then opens fully. Opening through the full +finger stroke beside the first spool sweeps a finger through its rim. These explicit +waypoints are task geometry: joint interpolation does not provide obstacle +avoidance, and contact response is not a substitute for a clear commanded path. + +- `PickUp` accepts `ContactGraspGoal` for the observed pinch pose. It shares + approach/close/lift command assembly with rigid pickup and requests physical + contact verification. The routing host verifies material-point lift and slip. + `PickUpOptions` retains the 48-frame close, 0.35-second closed hold and + zero lift height. The following `material_lift` invocation performs the + retraction and lift before the material-point lift validator runs. +- `compute.geometry.cable` owns simulator-independent capsule endpoints, + material-point observations and TCP-relative slip measurement. +- `task_program/program.yaml` owns the ordered routing and insertion stages. + `integration.yaml` binds declared material targets and the board geometry. + `execution_policy.yaml` owns the adaptive Cartesian sampling policy. + `evaluation.yaml` requires both spool seats, the strict final channel, + clearance, speed, penetration and the 12 mm episode slip limit. +- The allowlisted `cable_material` service resolves material-coordinate TCP + targets. Calls lower to `PickUp(ContactGraspGoal)`, `MoveEndEffector` and + `MoveJoints`; no cable-specific Atomic Skill or environment subclass exists. + References are captured after the acknowledged close frame, before lift, + and belong to one bridge/episode and explicit environment rows. +- The deployment still uses the original `RouteTheCable-YAM-v1` ID. Loading + `task.yam.yaml` registers it as the common `EmbodiedEnv`; importing the task + package alone does not register a configuration-owned deployment. +- `YamI2rtCfg` resolves the versioned robot bundle from the data registry/cache or an explicit + `fpath`. Its bundle retains the original MIT license and pinned provenance. + Simulation and FK use + the same URDF. Fingertips retain separate convex pieces instead of one hull + spanning their concave grasp region. + +The scene follows the pinned upstream `RodSpec` coefficients, including density, +stretch/shear, bend/twist and contact damping. Shape friction combines by a +geometric mean: cable/pad are 60, bench 0.02, and other scenery 2. The task uses +`rigid_contact_history: true` with `contact_matching: latest`. Both +`rigid_contact_stick_freeze_translation_eps` and +`rigid_contact_stick_freeze_angular_eps` are set to `0.0`. Contact history still +retains the contact multipliers and local sticking anchors. The zero thresholds +disable VBD's body-level world-space motion freeze for dynamic sticking contacts +without a static/kinematic anchor contact. For sufficiently small motion, that +operation would otherwise restore the previous body pose before computing +velocity. Disabling it allows slowly moving gripper proxies and cable segments +to retain their motion while frictional sticking remains active. +These are task-specific solver settings; the strict final channel bounds and +12 mm slip limit remain unchanged. + +The routing component increases left-arm drive stiffness for insertion tracking. +The grasp profile specifies finger joint positions, not the physical gap between the +collision pads. At the grasp depth, two 3 mm joint targets give an unloaded +inner pad gap of approximately 4.45 mm for the 7 mm diameter cable; the nominal +target therefore has about 2.55 mm of interference. Actual finger positions +depend on the drive and contact response. The first and third right-hand +grasps and the left-hand grasp use zero-position firm commands through the +existing `SkillControlOverrides` interface; the second right-hand grasp uses +the shared 3 mm targets. +Each pinch holds its grasp command for 0.35 s on the control grid before +lifting. This dwell is part of the Atomic Skill's `close` phase; the material +reference is sampled after closing/settling, before any lift, and is not rebased +during the grasp. + +## Validation and recordings + +Every grasp must lift the observed material point by more than 8 mm and remain +within 12 mm of its post-closure TCP-relative position. Final metrics include +both spool-seat counts, free-end location, hand clearance, speed and sampled +cable/bench, cable/spool and cable/port penetration. `reference_routed` +reproduces the upstream broad regions. Final `success` additionally requires +the free end inside the strict region +`[0.5845, 0.064, 0.753]` to `[0.5875, 0.0896, 0.759]` m and penetration no greater +than 1 mm. These final bounds and the original lift/slip limits are unchanged. +Sampled SDF diagnostics do not constitute an exact continuous collision-distance +query. + +These final cable predicates do not certify the robot's swept path. The +first-spool CUDA regression samples backend robot/spool contacts throughout +approach, placement, release and retreat, rejects penetration over 1 mm, and +also requires the cable to remain seated and the hand to end open above the +spool: + +```bash +python -m pytest tests/gym/envs/tasks/test_route_the_cable_cuda.py --run-gpu -m gpu -q +``` + +The standard headless collector completed one episode with the original point +list and one with the packaged USD on DexSim `0.6.0rc2` (build `dbaca660`), +using the same RTX 5090 D v2 and seed 73. Both passed all 51 segments and final +physical acceptance after two seconds of settling. The original and USD runs +took 5,981 and 5,994 control steps, respectively. The final tip positions differed +by 1.184 mm, both inside the strict channel. Maximum grasp slip was 0.316 mm +and 0.307 mm, respectively, and both runs reported zero final sampled cable +penetration against the bench, spools and port. + +The earlier three NoRender CUDA regressions ran on DexSim development build +`b040478a`, using seed 73 and a fresh process per case. The full-route case naturally +exhausts all Task Program segments, validates every stage, then requires +`task_success=True` after release, retreat and two seconds of physical settling. + +| Case | Maximum material-point displacement | Reported robot/obstacle penetration | +| --- | --- | --- | +| First-spool release | 0.080 mm | 0 mm | +| Third carry through release and retreat | 0.379 mm | 0 mm | +| Complete route after the main integration | 0.306 mm | 0 mm | + +The full route was repeated successfully after integrating main `1955c640` +and finished at endpoint `[0.58459103, 0.07624141, 0.75500649]` m, +inside the unchanged strict channel. Sampled cable/bench, cable/spool and +cable/port penetration were all zero. The complete-route regression checks raw +robot/bench, robot/spool and robot/port contacts after every control step; it also +checks the final bounds independently of the configured evaluator. The original +12 mm displacement and 1 mm penetration limits remain active. + +The production deployment inspector and 45 focused CPU cases cover the new +material lift, staging orientation, commanded hold, release-depth gating and +acceptance behavior. Diagnostic suffix fixtures are excluded from complete-route +qualification. These individual fresh-process cases do not establish multi-seed +success rates or repeated-reset reliability. + +Recordings contain full robot commands and +`states.cables.cable.segment_state_w`, shaped `(environments, time, 50, 13)` in +world XYZ + XYZW + COM linear/angular velocity order. Segment outcomes retain +planning status and material-coordinate identities. A cable observation is not +a solver-history checkpoint. Construct the replay environment with the same +physical configuration. New recordings retain source Arena origins so a selected +row can replay in a fresh single-environment scene. Older files without origin +metadata require matching Arena placement. `ReplayWrapper(mode="dynamic")` verifies +matching full-reset recorded observations and environment counts; it replays +commands without overwriting cable states. Both `position` and +`position_velocity` expert command modes use the environment control grid; the +latter records derived joint velocities and zero velocities during holds. Cable kinematic scrubbing +and partial replay resets are rejected. The deployment defaults to one episode and +one attempt per process. Reset/episode repeatability with contact history +needs separate physical qualification; API state/reset tests alone do +not prove repeatable frictional routing. Use a fresh process for each trial until +that reset path is qualified. Raw trajectory recordings may include failed +attempts; select training data using the segment and final outcomes. + +## Task Program migration qualification + +The migration preserves the Newton/DexUni two-way cable solver, scene and robot +contact coefficients. The shared execution policy applies its joint acceleration +budget to all Cartesian moves, so stage durations may be longer than the former +handwritten direct-motion path. The episode budget is 12,000 control steps. + +Static composition/catalog preflight and intermediate physical qualification do +not certify a complete cable task. Full acceptance remains strict after release, +retreat and settling. Failed samples retain their measured stopping stage; +changing orchestration does not by itself establish physical insertion success. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/cable.usda b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/cable.usda new file mode 100644 index 0000000000..711b15d23d --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/cable.usda @@ -0,0 +1,36 @@ +#usda 1.0 +( + defaultPrim = "World" + metersPerUnit = 0.001 + kilogramsPerUnit = 1 + upAxis = "Z" +) + +def Xform "World" +{ + def BasisCurves "cable" ( + prepend apiSchemas = ["PhysicsCurvesDeformableSimAPI", "PhysicsCollisionAPI"] + ) + { + uniform token type = "linear" + uniform token wrap = "nonperiodic" + int[] curveVertexCounts = [51] + point3f[] points = [ + (0, 0, 0), (0, 12, 0), (0, 24, 0), (0, 36, 0), (0, 48, 0), + (0, 60, 0), (0, 72, 0), (0, 84, 0), (0, 96, 0), (0, 108, 0), + (0, 120, 0), (0, 132, 0), (0, 144, 0), (0, 156, 0), (0, 168, 0), + (0, 180, 0), (0, 192, 0), (0, 204, 0), (0, 216, 0), (0, 228, 0), + (0, 240, 0), (0, 252, 0), (0, 264, 0), (0, 276, 0), (0, 288, 0), + (0, 300, 0), (0, 312, 0), (0, 324, 0), (0, 336, 0), (0, 348, 0), + (0, 360, 0), (0, 372, 0), (0, 384, 0), (0, 396, 0), (0, 408, 0), + (0, 420, 0), (0, 432, 0), (0, 444, 0), (0, 456, 0), (0, 468, 0), + (0, 480, 0), (0, 492, 0), (0, 504, 0), (0, 516, 0), (0, 528, 0), + (0, 540, 0), (0, 552, 0), (0, 564, 0), (0, 576, 0), (0, 588, 0), + (0, 600, 0) + ] + float[] physics:thicknesses = [7] + token physics:thicknesses:elementType = "constant" + double3 xformOp:translate = (420, -360, 753.5) + uniform token[] xformOpOrder = ["xformOp:translate"] + } +} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/catalog.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/catalog.yaml new file mode 100644 index 0000000000..4711589548 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/catalog.yaml @@ -0,0 +1,13 @@ +task_key: route_the_cable +title: Route the Cable +summary: Task Program with material-coordinate pinches, measured slip and bounded + routing feedback. +tags: +- cable +- deformable +- manipulation +- task_program_expert +default_deployment: yam +deployments: + yam: + config: task.yam.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml new file mode 100644 index 0000000000..d4b80fd063 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/env.yaml @@ -0,0 +1,278 @@ +# Material coefficients follow the pinned upstream RodSpec and its scene-friction pass. +environment_id: route_the_cable +enable_ground_plane: true +physics: newton +physics_config: + physics_dt: 0.004166666666666667 + num_substeps: 4 + collision_cfg: null + solver_cfg: + solver_type: dexuni + joint_mode: dynamic + coupling: two_way + contact_mode: full + vbd_options: + iterations: 20 + rigid_contact_history: true + # Keep moving cable/pad anchors without snapping slow bodies in world space. + rigid_contact_stick_freeze_translation_eps: 0.0 + rigid_contact_stick_freeze_angular_eps: 0.0 + rigid_body_contact_buffer_size: 256 + collision_options: + contact_matching: latest +max_episode_steps: 12000 +num_envs: 1 +arena_space: 2.0 +simulation: + background: + - uid: bench + shape: + shape_type: Cube + size: + - 0.595 + - 1.3 + - 0.1 + init_pos: + - 0.6 + - 0 + - 0.7 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 0.02 + ke: 10000.0 + kd: 0.1 + - uid: spool_0_foot + shape: + shape_type: Cylinder + radius: 0.0125 + half_height: 0.00125 + init_pos: + - 0.525 + - -0.26 + - 0.75125 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: spool_0_shaft + shape: + shape_type: Cylinder + radius: 0.00475 + half_height: 0.0085895 + init_pos: + - 0.525 + - -0.26 + - 0.7610895 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: spool_0_rim + shape: + shape_type: Cylinder + radius: 0.0125 + half_height: 0.0019105000000000007 + init_pos: + - 0.525 + - -0.26 + - 0.7715895 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: spool_1_foot + shape: + shape_type: Cylinder + radius: 0.0125 + half_height: 0.00125 + init_pos: + - 0.525 + - -0.02 + - 0.75125 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: spool_1_shaft + shape: + shape_type: Cylinder + radius: 0.00475 + half_height: 0.0085895 + init_pos: + - 0.525 + - -0.02 + - 0.7610895 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: spool_1_rim + shape: + shape_type: Cylinder + radius: 0.0125 + half_height: 0.0019105000000000007 + init_pos: + - 0.525 + - -0.02 + - 0.7715895 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: port_wall_a + shape: + shape_type: Cube + size: + - 0.011 + - 0.032 + - 0.0125 + init_pos: + - 0.5755 + - 0.08 + - 0.75625 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: port_wall_b + shape: + shape_type: Cube + size: + - 0.011 + - 0.032 + - 0.0125 + init_pos: + - 0.5965 + - 0.08 + - 0.75625 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: port_back + shape: + shape_type: Cube + size: + - 0.01 + - 0.0064 + - 0.0125 + init_pos: + - 0.586 + - 0.0928 + - 0.75625 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + - uid: anchor + shape: + shape_type: Cube + size: + - 0.032 + - 0.032 + - 0.006 + init_pos: + - 0.42 + - -0.352 + - 0.76 + body_type: static + attrs: + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 2.0 + ke: 10000.0 + kd: 0.1 + usd_scene: + name: cable_scene + source: + file_path: ./cable.usda + asset_cfgs: + /World/cable: + uid: cable + fixed_segments: + - 0 + attrs: + density: 350.0 + bend_stiffness: 0.00111572809234342 + bend_damping: 0.00167359213851513 + twist_stiffness: 0.00111572809234342 + twist_damping: 0.00167359213851513 + collision_props: + contact_offset: 0.01 + rest_offset: 0.0 + material_props: + backend: newton + dynamic_friction: 60.0 + ke: 10000.0 + kd: 0.1 + stretch_stiffness: 200000.0 + stretch_damping: 1.0 + shear_stiffness: 200000.0 + shear_damping: 1.0 +env: + sim_steps_per_control: 4 + ignore_terminations: true + task_instruction: Route the anchored cable around both spools and place its free + end in the port. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml new file mode 100644 index 0000000000..34dad4c17c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml @@ -0,0 +1,20 @@ +id: RouteTheCable-YAM-v1 +environment: + component: env.yaml +embodiment: + component: ../../../../components/embodiments/yam_i2rt.yaml +headless: true +render_cfg: + renderer: no-render +enable_entity_gizmo: false +visualization: + allow_commands: false +collection: + target_episodes: 1 + max_attempts: 1 +task_program: + program: task_program/program.yaml + integration: task_program/integration.yaml + execution_policy: task_program/execution_policy.yaml +evaluation: + component: task_program/evaluation.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/evaluation.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/evaluation.yaml new file mode 100644 index 0000000000..6d47a2728d --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/evaluation.yaml @@ -0,0 +1,104 @@ +graph: + roots: + - route_success + rules: + seat_0: + kind: scalar_range + bindings: + value: cables/cable/seat_0 + params: + minimum: 2 + maximum: 50 + seat_1: + kind: scalar_range + bindings: + value: cables/cable/seat_1 + params: + minimum: 2 + maximum: 50 + port_count: + kind: scalar_range + bindings: + value: cables/cable/port_count + params: + minimum: 3 + maximum: 50 + end_in_port: + kind: scalar_range + bindings: + value: cables/cable/end_in_port + params: + minimum: 1 + maximum: 1 + hands_clearance: + kind: scalar_range + bindings: + value: cables/cable/hands_clearance + params: + minimum: 0.05 + maximum: 100 + minimum_inclusive: false + max_speed: + kind: scalar_range + bindings: + value: cables/cable/max_speed + params: + minimum: 0 + maximum: 0.05 + bench_penetration: + kind: scalar_range + bindings: + value: cables/cable/bench_penetration + params: + minimum: 0 + maximum: 0.001 + spool_penetration: + kind: scalar_range + bindings: + value: cables/cable/spool_penetration + params: + minimum: 0 + maximum: 0.001 + port_penetration: + kind: scalar_range + bindings: + value: cables/cable/port_penetration + params: + minimum: 0 + maximum: 0.001 + max_grasp_slip: + kind: scalar_range + bindings: + value: cables/cable/max_grasp_slip + params: + minimum: 0 + maximum: 0.012 + strict_port: + kind: inside_region + bindings: + position: cables/cable/end_position + params: + lower: + - 0.5845 + - 0.064 + - 0.753 + upper: + - 0.5875 + - 0.0896 + - 0.759 + route_success: + kind: all + refs: + - seat_0 + - seat_1 + - port_count + - end_in_port + - hands_clearance + - max_speed + - bench_penetration + - spool_penetration + - port_penetration + - max_grasp_slip + - strict_port +final_acceptance: +- route_success diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.yaml new file mode 100644 index 0000000000..38c6d2bc7a --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/execution_policy.yaml @@ -0,0 +1,24 @@ +policy_id: cable_sampled_contact_v1 +preset_id: cable +requires: + embodiment_contract: dual_arm_parallel_gripper +planner: + type: trapezoidal +motion: + strategy: ik_interp + velocity_targets: auto + sample_count: 73 + interpolate_position_step: 0.0008333333333333334 + dynamic_collision_mode: 'off' +tracking: + kind: timed +recovery: + max_replans: 0 + max_skill_retries: 0 +workflow_recovery: + max_recovery_attempts: 0 +runner: + minimum_cycle_time: 0.0 + hold_on_completion: false + hold_during_effect_verification: false +effect_assurance: projected diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml new file mode 100644 index 0000000000..452524ba31 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml @@ -0,0 +1,765 @@ +integration_id: route_the_cable_v1 +program_id: route_the_cable +requires: + scene_contract: cable_board_v1 + embodiment_contract: dual_arm_parallel_gripper +scene_binding: + contract_id: cable_board_v1 + registry_id: cable_board + cables: + - entity_id: cable + simulation_uid: cable +profile: + defaults: + pick_up: + primary: right_manipulator + move_end_effector: + primary: right_manipulator + move_joints: + primary: right_manipulator + skill_options: + simulation.cable_pick: + kind: pick_up + hand_interp_steps: 48 + grasp_settle_steps: 21 + lift_height: 0.0 + pre_grasp_distance: 0.0 + simulation.cable_move: + kind: move_end_effector + preserve_cartesian_samples: true + simulation.cable_joint: + kind: move_joints + preserve_samples: true + simulation.cable_check: + kind: move_joints + preserve_samples: true + effect_monitors: {} +runtime_services: + cable_material: + entity_id: cable + simulation_uid: cable + motion_parts: + - right_arm + - left_arm + slip_limit: 0.012 + geometry: + bench_height: 0.75 + port_center: + - 0.586 + - 0.08 + port_stage: + - 0.586 + - 0.055 + - 0.805 + port_mouth: + - 0.586 + - 0.064 + seats: + - - 0.525 + - -0.26 + - -1 + - - 0.525 + - -0.02 + - 1 + cylinders: + - - 0.525 + - -0.26 + - 0.0125 + - 0 + - 0.0025 + - - 0.525 + - -0.26 + - 0.00475 + - 0.0025 + - 0.019679 + - - 0.525 + - -0.26 + - 0.0125 + - 0.019679 + - 0.0235 + - - 0.525 + - -0.02 + - 0.0125 + - 0 + - 0.0025 + - - 0.525 + - -0.02 + - 0.00475 + - 0.0025 + - 0.019679 + - - 0.525 + - -0.02 + - 0.0125 + - 0.019679 + - 0.0235 + boxes: + - - - 0.5755 + - 0.08 + - 0.75625 + - - 0.0055 + - 0.016 + - 0.00625 + - - - 0.5965 + - 0.08 + - 0.75625 + - - 0.0055 + - 0.016 + - 0.00625 + - - - 0.586 + - 0.0928 + - 0.75625 + - - 0.005 + - 0.0032 + - 0.00625 + strict_region: + - - 0.5845 + - 0.064 + - 0.753 + - - 0.5875 + - 0.0896 + - 0.759 + release_region: + - - 0.5845 + - 0.072 + - 0.753 + - - 0.5875 + - 0.083 + - 0.758 + targets: + right_approach_0.162: + kind: material + material_distance: 0.162 + position: + - null + - null + - 0.81 + speed: 0.12 + port_screen: false + settle_motion: + kind: hold + samples: 15 + right_approach_0.162_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + right_grasp_0.162: + kind: material + material_distance: 0.162 + position: + - null + - null + - 0.7605 + firm: true + port_screen: false + right_lifted_0.162: + kind: check + condition: lift + raise_0: + kind: material_lift + position: + - null + - null + - 0.774 + material_distance: 0.162 + retract_distance: 0.0015 + raise_0_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + carry_0: + kind: command + position: + - 0.5 + - -0.255 + - null + carry_0_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + lower_0: + kind: command + position: + - null + - null + - 0.7645 + lower_0_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + release_0: + kind: open + samples: 36 + finger_position: 0.008 + release_part: right_arm + retreat_0: + kind: command + position: + - null + - null + - 0.82 + speed: 0.08 + retreat_0_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + open_clear_of_spool_0: + kind: open + samples: 36 + release_part: right_arm + right_approach_0.282: + kind: material + material_distance: 0.282 + position: + - null + - null + - 0.81 + speed: 0.12 + port_screen: false + right_approach_0.282_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + right_grasp_0.282: + kind: material + material_distance: 0.282 + position: + - null + - null + - 0.7605 + firm: false + port_screen: false + right_lifted_0.282: + kind: check + condition: lift + raise_1: + kind: material_lift + position: + - null + - null + - 0.78 + material_distance: 0.282 + retract_distance: 0.003 + raise_1_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + carry_1: + kind: command + position: + - 0.528 + - -0.14 + - null + carry_1_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + lower_1: + kind: command + position: + - null + - null + - 0.7645 + lower_1_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + release_1: + kind: open + samples: 36 + finger_position: 0.008 + release_part: right_arm + retreat_1: + kind: command + position: + - null + - null + - 0.82 + speed: 0.08 + retreat_1_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + open_clear_of_spool_1: + kind: open + samples: 36 + release_part: right_arm + right_approach_0.438: + kind: material + material_distance: 0.438 + position: + - null + - null + - 0.81 + speed: 0.12 + port_screen: false + right_approach_0.438_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + right_grasp_0.438: + kind: material + material_distance: 0.438 + position: + - null + - null + - 0.7605 + firm: true + port_screen: false + right_lifted_0.438: + kind: check + condition: lift + raise_2: + kind: material_lift + position: + - null + - null + - 0.8 + material_distance: 0.438 + retract_distance: 0.008 + raise_2_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + carry_2: + kind: command + position: + - 0.565 + - 0.008 + - null + carry_2_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + lower_2: + kind: command + position: + - null + - null + - 0.7645 + lower_2_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + release_2: + kind: open + samples: 36 + finger_position: 0.008 + release_part: right_arm + retreat_2: + kind: command + position: + - null + - null + - 0.82 + speed: 0.08 + retreat_2_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + open_clear_of_spool_2: + kind: open + samples: 36 + release_part: right_arm + home_right: + kind: home + speed: 0.15 + home_right_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + left_approach_0.57: + kind: material + material_distance: 0.57 + position: + - null + - null + - 0.81 + speed: 0.12 + port_screen: true + left_approach_0.57_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + left_grasp_0.57: + kind: material + material_distance: 0.57 + position: + - null + - null + - 0.7605 + firm: true + port_screen: true + left_lifted_0.57: + kind: check + condition: lift + stage_free_end: + kind: port_stage + position: + - 0.586 + - 0.055 + - 0.805 + stage_free_end_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + lower_channel_0: + kind: command + position: + - null + - null + - 0.795 + speed: 0.025 + tolerance: 0.006 + stop_when_seated: true + lower_channel_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_0_0: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.05 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_0_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_0_1: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.05 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_0_1_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + lower_channel_1: + kind: command + position: + - null + - null + - 0.7875 + speed: 0.025 + tolerance: 0.006 + stop_when_seated: true + lower_channel_1_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_1_0: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.05 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_1_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_1_1: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.05 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_1_1_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + lower_channel_2: + kind: command + position: + - null + - null + - 0.78 + speed: 0.025 + tolerance: 0.006 + stop_when_seated: true + lower_channel_2_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_2_0: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.05 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_2_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_2_1: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.05 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_2_1_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + lower_channel_3: + kind: command + position: + - null + - null + - 0.7655 + speed: 0.025 + tolerance: 0.006 + stop_when_seated: true + lower_channel_3_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_3_0: + kind: tip_align + position: + - 0.586 + - 0.077 + - 0.754 + maximum_correction: 0.005 + maximum_y: 0.05 + minimum_z: 0.7655 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_3_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_tip_3_1: + kind: tip_align + position: + - 0.586 + - 0.077 + - 0.754 + maximum_correction: 0.005 + maximum_y: 0.05 + minimum_z: 0.7655 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + maximum_z_correction: 0.003 + align_tip_3_1_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + release_alignment: + kind: check + condition: seated + release_free_end: + kind: open + samples: 36 + release_part: left_arm + finger_position: 0.008 + retreat_free_end: + kind: command + position: + - null + - 0.03 + - 0.82 + speed: 0.08 + retreat_free_end_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + home_left: + kind: home + speed: 0.15 + home_left_tracking: + kind: check + condition: pose + tolerance: 0.02 + orientation_tolerance: 0.25 + stop_when_seated: false + settle_and_validate: + kind: hold + samples: 120 + align_over_port_0: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.05 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + align_over_port_0_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_over_port_1: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.05 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + align_over_port_1_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_over_port_2: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.05 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + align_over_port_2_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + align_over_port_3: + kind: tip_align + position: + - 0.586 + - 0.077 + - null + maximum_correction: 0.005 + maximum_y: 0.05 + speed: 0.02 + tolerance: 0.006 + stop_when_seated: true + align_over_port_3_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: true + raise_free_end: + kind: material_lift + material_distance: 0.57 + retract_distance: 0.003 + position: + - null + - null + - 0.78 + raise_free_end_tracking: + kind: check + condition: pose + tolerance: 0.006 + orientation_tolerance: 0.25 + stop_when_seated: false + open_clear_of_port: + kind: open + samples: 36 + release_part: left_arm diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.yaml new file mode 100644 index 0000000000..b1cd0dea7e --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/program.yaml @@ -0,0 +1,1142 @@ +program_id: route_the_cable +instruction: Route the anchored cable around two spools and seat its free end in the + port. +targets: {} +program: +- segment: right_approach_0.162 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: right_approach_0.162 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: right_approach_0.162_tracking + resources: + primary: right_manipulator +- segment: right_pinch_0.162 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pick + arguments: + target: right_grasp_0.162 + resources: + primary: right_manipulator +- segment: raise_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: raise_0 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: raise_0_tracking + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: right_lifted_0.162 + resources: + primary: right_manipulator +- segment: carry_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: carry_0 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: carry_0_tracking + resources: + primary: right_manipulator +- segment: lower_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lower_0 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: lower_0_tracking + resources: + primary: right_manipulator +- segment: release_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: release_0 + resources: + primary: right_hand +- segment: retreat_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: retreat_0 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: retreat_0_tracking + resources: + primary: right_manipulator +- segment: open_clear_of_spool_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: open_clear_of_spool_0 + resources: + primary: right_hand +- segment: right_approach_0.282 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: right_approach_0.282 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: right_approach_0.282_tracking + resources: + primary: right_manipulator +- segment: right_pinch_0.282 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pick + arguments: + target: right_grasp_0.282 + resources: + primary: right_manipulator +- segment: raise_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: raise_1 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: raise_1_tracking + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: right_lifted_0.282 + resources: + primary: right_manipulator +- segment: carry_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: carry_1 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: carry_1_tracking + resources: + primary: right_manipulator +- segment: lower_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lower_1 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: lower_1_tracking + resources: + primary: right_manipulator +- segment: release_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: release_1 + resources: + primary: right_hand +- segment: retreat_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: retreat_1 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: retreat_1_tracking + resources: + primary: right_manipulator +- segment: open_clear_of_spool_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: open_clear_of_spool_1 + resources: + primary: right_hand +- segment: right_approach_0.438 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: right_approach_0.438 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: right_approach_0.438_tracking + resources: + primary: right_manipulator +- segment: right_pinch_0.438 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pick + arguments: + target: right_grasp_0.438 + resources: + primary: right_manipulator +- segment: raise_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: raise_2 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: raise_2_tracking + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: right_lifted_0.438 + resources: + primary: right_manipulator +- segment: carry_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: carry_2 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: carry_2_tracking + resources: + primary: right_manipulator +- segment: lower_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lower_2 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: lower_2_tracking + resources: + primary: right_manipulator +- segment: release_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: release_2 + resources: + primary: right_hand +- segment: retreat_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: retreat_2 + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: retreat_2_tracking + resources: + primary: right_manipulator +- segment: open_clear_of_spool_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: open_clear_of_spool_2 + resources: + primary: right_hand +- segment: home_right + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: home_right + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: right_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: home_right_tracking + resources: + primary: right_manipulator +- segment: left_approach_0.57 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: left_approach_0.57 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: left_approach_0.57_tracking + resources: + primary: left_manipulator +- segment: left_pinch_0.57 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_pick + arguments: + target: left_grasp_0.57 + resources: + primary: left_manipulator +- segment: raise_free_end + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: raise_free_end + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: raise_free_end_tracking + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: left_lifted_0.57 + resources: + primary: left_manipulator +- segment: stage_free_end + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: stage_free_end + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: stage_free_end_tracking + resources: + primary: left_manipulator +- segment: align_over_port_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_over_port_0 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_over_port_0_tracking + resources: + primary: left_manipulator +- segment: align_over_port_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_over_port_1 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_over_port_1_tracking + resources: + primary: left_manipulator +- segment: align_over_port_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_over_port_2 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_over_port_2_tracking + resources: + primary: left_manipulator +- segment: align_over_port_3 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_over_port_3 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_over_port_3_tracking + resources: + primary: left_manipulator +- segment: lower_channel_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lower_channel_0 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: lower_channel_0_tracking + resources: + primary: left_manipulator +- segment: align_tip_0_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_0_0 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_0_0_tracking + resources: + primary: left_manipulator +- segment: align_tip_0_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_0_1 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_0_1_tracking + resources: + primary: left_manipulator +- segment: lower_channel_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lower_channel_1 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: lower_channel_1_tracking + resources: + primary: left_manipulator +- segment: align_tip_1_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_1_0 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_1_0_tracking + resources: + primary: left_manipulator +- segment: align_tip_1_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_1_1 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_1_1_tracking + resources: + primary: left_manipulator +- segment: lower_channel_2 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lower_channel_2 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: lower_channel_2_tracking + resources: + primary: left_manipulator +- segment: align_tip_2_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_2_0 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_2_0_tracking + resources: + primary: left_manipulator +- segment: align_tip_2_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_2_1 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_2_1_tracking + resources: + primary: left_manipulator +- segment: lower_channel_3 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: lower_channel_3 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: lower_channel_3_tracking + resources: + primary: left_manipulator +- segment: align_tip_3_0 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_3_0 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_3_0_tracking + resources: + primary: left_manipulator +- segment: align_tip_3_1 + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: align_tip_3_1 + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: align_tip_3_1_tracking + resources: + primary: left_manipulator +- segment: verify_release_alignment + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: release_alignment + resources: + primary: left_manipulator +- segment: release_free_end + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: release_free_end + resources: + primary: left_hand +- segment: retreat_free_end + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: retreat_free_end + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: retreat_free_end_tracking + resources: + primary: left_manipulator +- segment: open_clear_of_port + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: open_clear_of_port + resources: + primary: left_hand +- segment: home_left + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_move + arguments: + target: home_left + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_motion + resources: + primary: left_manipulator + - kind: invoke + call: + kind: registered + call_id: simulation.cable_check + arguments: + target: home_left_tracking + resources: + primary: left_manipulator +- segment: settle_and_validate + steps: + - kind: invoke + call: + kind: registered + call_id: simulation.cable_joint + arguments: + target: settle_and_validate + resources: + primary: left_manipulator + validators: + - kind: condition + ref: route_success diff --git a/extensions/embodichain_tasks/embodichain_tasks/__init__.py b/extensions/embodichain_tasks/embodichain_tasks/__init__.py index 785a804cce..b9ce8b7c31 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/__init__.py +++ b/extensions/embodichain_tasks/embodichain_tasks/__init__.py @@ -54,6 +54,7 @@ register_manager_modules( [ + "embodichain_tasks.manipulation.cable.franka_rope_wrap", "embodichain_tasks.locomotion.managers.observations", "embodichain_tasks.locomotion.managers.rewards", ] diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/__init__.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/__init__.py new file mode 100644 index 0000000000..3818cfd8e4 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/__init__.py @@ -0,0 +1,19 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +__all__ = [] diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py new file mode 100644 index 0000000000..ad18dc057c --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/franka_rope_wrap.py @@ -0,0 +1,678 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Scene, asset and reset handling for the Franka rope-wrapping task.""" + +from __future__ import annotations + +from importlib.resources import files +from pathlib import Path +from typing import TYPE_CHECKING, Any +from urllib.parse import unquote, urlsplit +import hashlib +import json +import shlex +import shutil +import xml.etree.ElementTree as ET +import numpy as np +from scipy.interpolate import make_interp_spline + +if TYPE_CHECKING: + import torch + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + +__all__ = [ + "cable_points", + "load_config", + "materialize_deployment", + "prepare_models", + "settle_franka_rope", + "task_directory", +] + + +def task_directory() -> Path: + """Locate the co-installed physical scene and task components.""" + local = Path(__file__).resolve().parents[3] / "configs" + root = local if local.is_dir() else Path(str(files("embodichain_tasks.configs"))) + return root / "tasks/manipulation/cable/franka_rope_wrap" + + +def load_config(path: Path | None = None) -> dict[str, Any]: + """Load scene settings and derive evaluation windows from the motion clock.""" + config = json.loads( + ( + Path(path) if path is not None else task_directory() / "config.json" + ).read_text() + ) + config.setdefault("open_half_gap_m", 0.04) + config.setdefault("grip_half_gap_m", config["cable_radius_m"] - 0.0005) + config.setdefault("floor_friction", 0.2) + scale = config["duration_s"] / 36.0 + config["grasp_complete_s"] = 5.0 * scale + config["lift_complete_s"] = 7.0 * scale + _validate_config(config) + return config + + +def _json(path: Path, data: Any) -> None: + path.write_text(json.dumps(data, indent=2, ensure_ascii=False) + "\n") + + +def _hash(path: Path, algorithm: str = "sha256") -> str: + digest = hashlib.new(algorithm) + with path.open("rb") as stream: + for chunk in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(chunk) + return digest.hexdigest() + + +def _texture_reference(tokens: list[str]) -> str: + """Remove standard MTL map options before resolving the texture filename.""" + one_value = { + "-blendu", + "-blendv", + "-cc", + "-clamp", + "-imfchan", + "-texres", + "-type", + "-bm", + "-boost", + } + index = 1 + while index < len(tokens) and tokens[index].startswith("-"): + option = tokens[index] + index += 1 + if option in one_value: + index += 1 + elif option == "-mm": + index += 2 + elif option in {"-o", "-s", "-t"}: + count = 0 + while index < len(tokens) and count < 3: + try: + float(tokens[index]) + except ValueError: + break + index += 1 + count += 1 + else: + raise ValueError(f"Unsupported MTL texture option: {option}") + if index >= len(tokens): + raise ValueError("MTL texture declaration has no filename") + return " ".join(tokens[index:]) + + +def _mesh_paths(root: ET.Element, source: Path) -> list[Path]: + """Resolve the generated model's local mesh and material dependency closure. + + The generated model preserves the source directory layout. References must + therefore stay beneath the source URDF directory; package and remote URIs + are not supported by this standalone example. + """ + directory = source.parent.resolve() + pending = [(directory, mesh.get("filename", "")) for mesh in root.iter("mesh")] + dependencies: set[Path] = set() + while pending: + parent, reference = pending.pop() + uri = urlsplit(reference) + if uri.scheme or uri.netloc or not uri.path: + raise ValueError( + f"Asset references must be local relative paths: {reference!r}" + ) + relative = Path(unquote(uri.path)) + if relative.is_absolute(): + raise ValueError(f"Asset references must be relative: {reference!r}") + path = (parent / relative).resolve() + if not path.is_relative_to(directory): + raise ValueError( + f"Asset dependency escapes the source directory: {reference!r}" + ) + if not path.is_file(): + raise FileNotFoundError(path) + if path in dependencies: + continue + dependencies.add(path) + if path.suffix.lower() in {".obj", ".mtl"}: + for line in path.read_text().splitlines(): + tokens = shlex.split(line, comments=True) + if not tokens: + continue + if path.suffix.lower() == ".obj" and tokens[0] == "mtllib": + pending.extend((path.parent, name) for name in tokens[1:]) + elif path.suffix.lower() == ".mtl" and ( + tokens[0].startswith("map_") + or tokens[0].lower() in {"bump", "disp", "decal", "norm", "refl"} + ): + pending.append((path.parent, _texture_reference(tokens))) + elif path.suffix.lower() == ".dae": + for image in ET.parse(path).findall(".//{*}image"): + for value in image.iter(): + if value.tag.rsplit("}", 1)[-1] in {"init_from", "ref"}: + reference = (value.text or "").strip() + if reference and not reference.startswith("#"): + pending.append((path.parent, reference)) + return sorted(dependencies) + + +def _asset_archives(reference: str) -> list[dict[str, str]]: + """Record available registry archive identities without initiating downloads.""" + if Path(reference).is_absolute(): + return [] + from embodichain.data.constants import EMBODICHAIN_DEFAULT_DATA_ROOT + + prefix = reference.split("/", 1)[0] + directory = Path(EMBODICHAIN_DEFAULT_DATA_ROOT) / "download" / prefix + archives = [] + try: + paths = sorted(directory.glob("*.zip")) + except OSError as error: + return [{"metadata_error": str(error)}] + for path in paths: + try: + if path.is_file(): + archives.append( + { + "filename": path.name, + "md5": _hash(path, "md5"), + "sha256": _hash(path), + } + ) + except OSError as error: + archives.append({"filename": path.name, "metadata_error": str(error)}) + return archives + + +def _validate_config(config: dict[str, Any]) -> None: + """Reject inputs that break the standalone control and geometry contracts.""" + + def finite_values(value: Any, name: str) -> None: + if isinstance(value, dict): + for key, item in value.items(): + finite_values(item, f"{name}.{key}") + elif isinstance(value, list): + for index, item in enumerate(value): + finite_values(item, f"{name}[{index}]") + elif isinstance(value, float) and not np.isfinite(value): + raise ValueError(f"{name} must be finite") + + finite_values(config, "config") + for name, minimum in ( + ("control_hz", 1), + ("substeps", 4), + ("iterations", 1), + ("cable_segments", 8), + ): + value = config[name] + if isinstance(value, bool) or not isinstance(value, int) or value < minimum: + raise ValueError(f"{name} must be an integer >= {minimum}") + if config["substeps"] % 4: + raise ValueError("substeps must be divisible by four for contact sampling") + for name in ( + "cable_radius_m", + "cable_density_kg_m3", + "post_radius_m", + "post_height_m", + "finger_pad_width_m", + "duration_s", + ): + value = config[name] + if isinstance(value, bool) or not isinstance(value, (int, float)) or value <= 0: + raise ValueError(f"{name} must be a positive number") + for name in ( + "pre_roll_s", + "linear_stiffness_N_m", + "bend_stiffness_Nm_rad", + "twist_stiffness_Nm_rad", + "bend_damping_Nms_rad", + "contact_ke_N_m", + "contact_kd_Ns_m", + "contact_gap_m", + "contact_match_distance_m", + ): + value = config[name] + if isinstance(value, bool) or not isinstance(value, (int, float)) or value < 0: + raise ValueError(f"{name} must be a nonnegative number") + if config["duration_s"] < 36: + raise ValueError("duration_s must include the full 36-second motion and hold") + if not np.isclose( + config["duration_s"] * config["control_hz"], + round(config["duration_s"] * config["control_hz"]), + atol=1e-9, + rtol=0, + ): + raise ValueError("duration_s must align to the control grid") + if ( + not 0 + <= config["grasp_complete_s"] + < config["lift_complete_s"] + <= config["duration_s"] - 2 + ): + raise ValueError("Grasp and lift checks must precede the final two-second hold") + for name, shape in ( + ("home_qpos", (7,)), + ("post_center_xy_m", (2,)), + ("finger_pad_center_m", (3,)), + ("finger_pad_size_m", (3,)), + ): + vector = np.asarray(config[name], dtype=float) + if vector.shape != shape: + raise ValueError(f"{name} must have shape {shape}") + if not np.isfinite(vector).all(): + raise ValueError(f"{name} must contain finite values") + if np.any(np.asarray(config["finger_pad_size_m"], dtype=float) <= 0): + raise ValueError("finger_pad_size_m must contain positive dimensions") + points = np.asarray(config["cable_waypoints_m"], dtype=float) + if points.ndim != 2 or points.shape[1] != 3 or len(points) < 3: + raise ValueError("cable_waypoints_m must contain at least three XYZ waypoints") + if not np.isfinite(points).all(): + raise ValueError("cable_waypoints_m must contain finite values") + if np.any(np.linalg.norm(np.diff(points, axis=0), axis=1) <= 0): + raise ValueError("Consecutive cable waypoints must be distinct") + if ( + np.linalg.norm( + points[0, :2] - np.asarray(config["post_center_xy_m"], dtype=float) + ) + <= 0 + ): + raise ValueError( + "The cable anchor and post center must have distinct XY coordinates" + ) + total_length = float( + np.linalg.norm(np.diff(cable_points(config), axis=0), axis=1).sum() + ) + inset = config["grasp_inset_m"] + if ( + isinstance(inset, bool) + or not isinstance(inset, (int, float)) + or not 0 < inset < total_length + ): + raise ValueError( + "grasp_inset_m must lie strictly inside the sampled cable length" + ) + thresholds = config["validation"] + if not isinstance(thresholds, dict): + raise ValueError("validation must be a mapping of acceptance thresholds") + for name, value in thresholds.items(): + if ( + isinstance(value, bool) + or not isinstance(value, (int, float)) + or not np.isfinite(value) + or value < 0 + ): + raise ValueError(f"validation.{name} must be a finite nonnegative number") + if not isinstance(config["robot_urdf"], str) or not config["robot_urdf"].strip(): + raise ValueError("robot_urdf must be an asset reference or absolute URDF path") + + +def prepare_models(config: dict[str, Any], out: Path) -> dict[str, Path]: + """Build a standalone robot and record the source and dependency hashes.""" + from embodichain.data import get_data_path + + generated = out / "generated_assets" + generated.mkdir() + robot_directory = generated / "franka" + robot_directory.mkdir() + reference = config["robot_urdf"] + source = Path(get_data_path(reference)).resolve() + if not source.is_file(): + raise FileNotFoundError(f"Robot URDF does not exist: {source}") + tree = ET.parse(source) + root = tree.getroot() + from embodichain_robots.franka.rope_pads import apply_rope_pads + + apply_rope_pads( + root, center=config["finger_pad_center_m"], size=config["finger_pad_size_m"] + ) + dependencies = [] + for path in _mesh_paths(root, source): + relative = path.relative_to(source.parent) + target = robot_directory / relative + target.parent.mkdir(parents=True, exist_ok=True) + shutil.copy2(path, target) + dependencies.append( + {"path": target.relative_to(out).as_posix(), "sha256": _hash(target)} + ) + paths = {"robot": robot_directory / "franka_rope.urdf"} + tree.write(paths["robot"], encoding="utf-8", xml_declaration=True) + _json( + generated / "provenance.json", + { + "asset_reference": reference, + "cached_registry_archives": _asset_archives(reference), + "robot_source": str(source.resolve()), + "robot_source_sha256": _hash(source), + "generated_robot": paths["robot"].relative_to(out).as_posix(), + "generated_robot_sha256": _hash(paths["robot"]), + "dependencies": dependencies, + "modifications": "Flat finger pads for direct floor rope grasp; original joint names, origins, axes, limits and masses retained. No moving rope attachment.", + "finger_pad_width_m": config["finger_pad_width_m"], + "finger_pad_center_m": config["finger_pad_center_m"], + "finger_pad_size_m": config["finger_pad_size_m"], + }, + ) + return paths + + +def _uses_packaged_geometry(config: dict[str, Any]) -> bool: + defaults = json.loads((task_directory() / "config.json").read_text()) + return all( + config[key] == defaults[key] + for key in ( + "cable_waypoints_m", + "post_center_xy_m", + "floor_height_m", + "cable_radius_m", + "cable_segments", + ) + ) + + +def cable_points(config: dict[str, Any]) -> np.ndarray: + """Read the packaged centerline or sample the user's custom geometry.""" + if _uses_packaged_geometry(config): + from pxr import Sdf + + layer = Sdf.Layer.FindOrOpen(str(task_directory() / "cable.usda")) + curve = layer.GetPrimAtPath("/World/cable") + return np.array(curve.attributes["points"].default, dtype=np.float32) + return _sample_custom_centerline(config) + + +def _sample_custom_centerline(config: dict[str, Any]) -> np.ndarray: + points = np.asarray(config["cable_waypoints_m"], float) + distances = np.r_[0.0, np.cumsum(np.linalg.norm(np.diff(points, axis=0), axis=1))] + curve = make_interp_spline(distances, points, k=2)( + np.linspace(0, distances[-1], 4000) + ) + curve[:, 2] = np.maximum( + curve[:, 2], config["floor_height_m"] + config["cable_radius_m"] + ) + arc = np.r_[0.0, np.cumsum(np.linalg.norm(np.diff(curve, axis=0), axis=1))] + result = np.column_stack( + [ + np.interp( + np.linspace(0, arc[-1], config["cable_segments"] + 1), arc, curve[:, i] + ) + for i in range(3) + ] + ) + # The fixed root capsule exits its clamp toward the post. Preserve the + # sampled first-segment length while removing spline endpoint tangent error. + direction = np.r_[np.asarray(config["post_center_xy_m"]) - result[0, :2], 0.0] + direction /= np.linalg.norm(direction) + result[1] = result[0] + direction * np.linalg.norm(result[1] - result[0]) + return result.astype(np.float32) + + +def _write_cable_asset(config: dict[str, Any], path: Path) -> None: + """Copy the packaged rope, updating its geometry for a custom scenario.""" + source = task_directory() / "cable.usda" + if _uses_packaged_geometry(config): + shutil.copyfile(source, path) + return + + from pxr import Sdf, Vt + + layer = Sdf.Layer.OpenAsAnonymous(str(source)) + curve = layer.GetPrimAtPath("/World/cable") + points = _sample_custom_centerline(config) + curve.attributes["points"].default = Vt.Vec3fArray.FromNumpy(points) + curve.attributes["curveVertexCounts"].default = Vt.IntArray([len(points)]) + curve.attributes["physics:thicknesses"].default = Vt.FloatArray( + [2.0 * config["cable_radius_m"]] + ) + layer.Export(str(path)) + + +def settle_franka_rope( + env: EmbodiedEnv, env_ids: torch.Tensor | None, scenario_path: str | None = None +) -> None: + """Settle the free cable against the slab before observing task time zero.""" + import torch + + if env_ids is not None and len(env_ids) != env.num_envs: + raise ValueError( + "Franka rope pre-roll advances the world and requires a full reset" + ) + from .wrap_motion import _JOINT_NAMES + + if tuple(env.robot.joint_names) != _JOINT_NAMES: + raise ValueError(f"Unexpected Franka joint order {env.robot.joint_names!r}.") + config = load_config(Path(scenario_path) if scenario_path is not None else None) + env.franka_rope_config = config + arm_home = [*config["home_qpos"], *([config["open_half_gap_m"]] * 2)] + qpos = torch.tensor(arm_home, dtype=torch.float32, device=env.device).expand( + env.num_envs, -1 + ) + env.robot.set_qpos(qpos, target=True) + steps = round(config["pre_roll_s"] * config["control_hz"] * config["substeps"]) + for _ in range(steps): + env.sim.update(step=1, render_final_step=False) + + +def _contact(config: dict[str, Any], friction: float) -> dict[str, Any]: + return { + "collision_props": { + "backend": "newton", + "margin": 0.0, + "gap": config["contact_gap_m"], + }, + "material_props": { + "backend": "newton", + "dynamic_friction": friction, + "ke": config["contact_ke_N_m"], + "kd": config["contact_kd_Ns_m"], + }, + } + + +def _environment( + config: dict[str, Any], scenario_path: Path | None = None +) -> dict[str, Any]: + floor = config["floor_height_m"] + center = config["post_center_xy_m"] + anchor = np.asarray(config["cable_waypoints_m"][0], dtype=float) + direction = np.r_[np.array(center) - anchor[:2], 0.0] + direction /= np.linalg.norm(direction) + + def rigid( + uid: str, + shape: dict, + position: list, + friction: float, + color: list[float], + roughness: float, + ) -> dict: + return { + "uid": uid, + "body_type": "static", + "init_pos": position, + "shape": { + **shape, + "visual_material": { + "uid": uid + "_mat", + "base_color": color, + "roughness": roughness, + }, + }, + "attrs": _contact(config, friction), + } + + slab = rigid( + "table", + {"shape_type": "Cube", "size": [1.2, 1.05, 0.05]}, + [0.35, 0, floor - 0.025], + config["floor_friction"], + [0.33, 0.37, 0.41, 1], + 0.65, + ) + clamp = rigid( + "anchor_clamp", + {"shape_type": "Cube", "size": [0.028, 0.018, 0.008]}, + (anchor - 0.006 * direction - [0, 0, 0.004]).tolist(), + config["floor_friction"], + [0.08, 0.11, 0.15, 1], + 0.65, + ) + clamp["init_rot"] = [ + 0, + 0, + float(np.rad2deg(np.arctan2(direction[1], direction[0]))), + ] + post = rigid( + "post", + { + "shape_type": "Cylinder", + "radius": config["post_radius_m"], + "half_height": config["post_height_m"] / 2, + }, + [*center, floor + config["post_height_m"] / 2], + config["post_friction"], + [0.025, 0.45, 0.39, 1], + 0.38, + ) + cable = { + "uid": "cable", + "fixed_segments": [0], + "collision_filter_span": 2, + "attrs": { + "density": config["cable_density_kg_m3"], + "stretch_stiffness": config["linear_stiffness_N_m"], + "shear_stiffness": config["linear_stiffness_N_m"], + "bend_stiffness": config["bend_stiffness_Nm_rad"], + "twist_stiffness": config["twist_stiffness_Nm_rad"], + "stretch_damping": 0.03, + "shear_damping": 0.03, + "bend_damping": config["bend_damping_Nms_rad"], + "twist_damping": config["bend_damping_Nms_rad"], + **_contact(config, config["rope_friction"]), + }, + } + return { + "environment_id": "franka_rope_wrap", + "enable_ground_plane": True, + "physics": "newton", + "physics_config": { + "physics_dt": 1.0 / (config["control_hz"] * config["substeps"]), + "num_substeps": 1, + "collision_cfg": None, + "sync_to_renderer": False, + "use_cuda_graph": True, + "solver_cfg": { + "solver_type": "dexuni", + "joint_mode": "dynamic", + "coupling": "two_way", + "mujoco_articulation_paths": ["arena_0/franka"], + "vbd_options": { + "iterations": config["iterations"], + "rigid_contact_history": config.get("rigid_contact_history", True), + "rigid_contact_hard": config.get("rigid_contact_hard", False), + "rigid_joint_linear_ke": config.get( + "rigid_joint_linear_ke", 100000.0 + ), + "rigid_joint_angular_ke": config.get( + "rigid_joint_angular_ke", 10.0 + ), + "friction_epsilon": config.get("friction_epsilon", 1e-5), + }, + "collision_options": { + "contact_matching": "sticky", + "contact_matching_pos_threshold": config[ + "contact_match_distance_m" + ], + "rigid_contact_max": 4096, + }, + }, + }, + "max_episode_steps": round(config["duration_s"] * config["control_hz"]) + 1, + "num_envs": 1, + "simulation": { + "background": [slab, clamp, post], + "usd_scene": { + "name": "cable_scene", + "source": {"file_path": "./cable.usda"}, + "asset_cfgs": {"/World/cable": cable}, + }, + }, + "env": { + "sim_steps_per_control": config["substeps"], + "ignore_terminations": True, + "expert_trajectory": {"joint_command_mode": "position_velocity"}, + "task_instruction": "Pick up the rope, wrap it around the post, tighten and hold it.", + "events": { + "settle_rope": { + "func": "settle_franka_rope", + "mode": "reset", + "params": ( + {"scenario_path": str(scenario_path)} if scenario_path else {} + ), + } + }, + }, + } + + +def materialize_deployment(config: dict[str, Any], robot_path: Path, out: Path) -> Path: + """Write an equivalent configured Gym deployment for a user-supplied scene.""" + import yaml + + _validate_config(config) + folder = out.resolve() / "deployment" + folder.mkdir() + _write_cable_asset(config, folder / "cable.usda") + reference = task_directory() + deployment = yaml.safe_load((reference / "task.franka.yaml").read_text()) + embodiment_path = reference / deployment["embodiment"]["component"] + embodiment = yaml.safe_load(embodiment_path.read_text()) + robot = embodiment["simulation"] + robot["fpath"] = str(robot_path.resolve()) + robot["init_pos"] = [0, 0, config["floor_height_m"]] + robot["init_qpos"] = [*config["home_qpos"], *([config["open_half_gap_m"]] * 2)] + robot["attrs"] = _contact(config, 1.2) + commands = embodiment["skill_profile"]["command_presets"][0]["commands"] + commands.update( + open=[config["open_half_gap_m"]] * 2, grasp=[config["grip_half_gap_m"]] * 2 + ) + scenario_path = folder / "config.json" + _json(scenario_path, config) + environment = _environment(config, scenario_path) + for section in ("task_program", "evaluation"): + for key, path in deployment[section].items(): + deployment[section][key] = str((reference / path).resolve()) + # The orbit is one atomic action. Its timeout uses simulation seconds and + # must cover all its samples, including the boundary acknowledgement. + from embodichain.lab.sim.atomic_skills.policies import RecoveryPolicy + from .wrap_motion import PHASE_END_S + + ends = np.asarray([0.0, *PHASE_END_S.values()]) + longest_phase_s = float(np.diff(ends).max()) * config["duration_s"] / ends[-1] + required_timeout = float(longest_phase_s + 2 / config["control_hz"]) + policy_path = deployment["task_program"]["execution_policy"] + recovery = yaml.safe_load(Path(policy_path).read_text())["recovery"] + if required_timeout > recovery.get("skill_timeout", RecoveryPolicy().skill_timeout): + deployment["task_program"]["execution_policy"] = { + "component": policy_path, + "overrides": {"recovery": {"skill_timeout": required_timeout}}, + } + deployment["embodiment"]["component"] = "embodiment.yaml" + for name, values in [ + ("env.yaml", environment), + ("embodiment.yaml", embodiment), + ("task.franka.yaml", deployment), + ]: + (folder / name).write_text(yaml.safe_dump(values, sort_keys=False)) + return folder / "task.franka.yaml" diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_evaluation.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_evaluation.py new file mode 100644 index 0000000000..4635c57825 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_evaluation.py @@ -0,0 +1,286 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Observe the Franka wrap using the Gym-owned contact sampling clock.""" + +from __future__ import annotations + +from collections.abc import Sequence +import json +from pathlib import Path +from typing import Any, TYPE_CHECKING +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.spatial.transform import Rotation + +from .wrap_measurements import _capsule_pad_gap, _rope_geometry, evaluate_wrap + +if TYPE_CHECKING: + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + from .wrap_motion import WrapMotion + +__all__ = ["attach_wrap_monitor"] + + +class _WrapMonitor: + def __init__( + self, + env: EmbodiedEnv, + config: dict[str, Any], + motion: WrapMotion, + robot_path: Path, + ) -> None: + if env.num_envs != 1: + raise ValueError("The Franka rope wrap deployment requires one arena.") + self.config = config + self.robot = env.robot + self.cable = env.sim.get_cable_object("cable") + self.sensor = env.sim.get_sensor("contacts") + self.motion = motion + self.lengths = np.linalg.norm(np.diff(self.cable.cfg.points, axis=0), axis=1) + material_arc = self.lengths.sum() - config["grasp_inset_m"] + self.material_index = int( + np.searchsorted(np.cumsum(self.lengths), material_arc) + ) + self.material_offset = ( + material_arc + - self.lengths[: self.material_index].sum() + - self.lengths[self.material_index] / 2 + ) + tree = ET.parse(robot_path) + limits = [ + tree.find(f".//joint[@name='{name}']/limit") + for name in self.robot.joint_names + ] + self.lo = np.array([float(limit.get("lower")) for limit in limits]) + self.hi = np.array([float(limit.get("upper")) for limit in limits]) + self.actor_paths = {} + self.active = True + self.rows, self.states, self.joint_states, self.tcp_states = [], [], [], [] + self.baseline = self.cable.data.segment_state_w[0, 0, :3].cpu().numpy().copy() + self.material_baseline = None + self.terminal_report = None + self.begin_control_step() + self._capture() + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + if env_ids is None or 0 in env_ids: + self.active = False + + def begin_control_step(self) -> None: + self.substep = 0 + self.finger_contacts = [False, False] + self.bilateral_contact = False + self.robot_post_peak = self.penetration = 0.0 + self.dropped = 0 + self.worst = None + + def before_sample(self) -> None: + return None + + def after_sample(self, dt: float, token: object) -> None: + if not self.active or self.terminal_report is not None: + return + self.substep += 1 + if self.substep % (self.config["substeps"] // 4) == 0: + self._contacts() + if self.substep == self.config["substeps"]: + self._capture() + + def _contacts(self) -> None: + contact = self.sensor + robot, cable = self.robot, self.cable + config, lengths = self.config, self.lengths + actor_paths = self.actor_paths + data = contact.get_data() + valid = data["is_valid"] + candidate_positions = [[], []] + for ids, separation, position in zip( + data["user_ids"][valid].cpu().tolist(), + data["distance"][valid].cpu().tolist(), + data["position"][valid].cpu().tolist(), + ): + pair = [] + for actor_id in ids: + if actor_id < 0: + continue + if actor_id not in actor_paths: + info = contact.get_actor_info(actor_id) + actor_paths[actor_id] = f"{info.path}/{info.link_name or ''}" + pair.append(actor_paths[actor_id]) + # DexSim currently registers rigid/articulation actors, + # leaving cable counterparts anonymous. Unknown identity + # is only a candidate: match the live cable capsules and + # finger gap below before accepting contact evidence. + is_rope = any(actor_id < 0 for actor_id in ids) or any( + "/cable/" in p for p in pair + ) + for k, name in enumerate(("fr3_leftfinger", "fr3_rightfinger")): + if is_rope and any(name in p for p in pair) and separation <= 1e-5: + candidate_positions[k].append(position) + if any("/post/" in p for p in pair) and any("/franka/" in p for p in pair): + self.robot_post_peak = max(self.robot_post_peak, -separation) + if -separation > self.penetration: + self.penetration = -separation + self.worst = pair + matched = [False, False] + if any(candidate_positions): + sampled_state = cable.data.segment_state_w[0, -8:].cpu().numpy() + axes = Rotation.from_quat(sampled_state[:, 3:7]).as_matrix()[:, :, 2] + starts = sampled_state[:, :3] - axes * lengths[-8:, None] / 2 + delta = axes * lengths[-8:, None] + for k, name in enumerate(("fr3_leftfinger", "fr3_rightfinger")): + if not candidate_positions[k]: + continue + contact_points = np.asarray(candidate_positions[k]) + fraction = np.clip( + np.sum( + (contact_points[:, None] - starts) * delta, + axis=-1, + ) + / np.sum(delta * delta, axis=-1), + 0.0, + 1.0, + ) + closest = starts + fraction[..., None] * delta + point_gap = ( + np.linalg.norm(contact_points[:, None] - closest, axis=-1).min() + - config["cable_radius_m"] + ) + pose = robot.get_link_pose(name, to_matrix=True)[0].cpu().numpy() + pad_gap = _capsule_pad_gap(sampled_state, lengths[-8:], pose, config) + matched[k] = bool(point_gap <= 0.0003 and pad_gap <= 0.0002) + self.finger_contacts[k] |= matched[k] + self.bilateral_contact |= all(matched) + self.dropped += contact.latest_sample_dropped_contacts + + def _capture(self) -> None: + config, lengths = self.config, self.lengths + robot, cable = self.robot, self.cable + frame = len(self.rows) + t = frame / config["control_hz"] + targets, labels = self.motion.poses, self.motion.labels + material_index, material_offset = self.material_index, self.material_offset + lo, hi, baseline = self.lo, self.hi, self.baseline + finger_contacts = self.finger_contacts + state = cable.data.segment_state_w[0].cpu().numpy() + qpos = robot.get_qpos()[0].cpu().numpy() + tcp = robot.get_link_pose("fr3_hand_tcp", to_matrix=True)[0].cpu().numpy() + pad_gaps = [] + for name in ("fr3_leftfinger", "fr3_rightfinger"): + pose = robot.get_link_pose(name, to_matrix=True)[0].cpu().numpy() + pad_gaps.append(_capsule_pad_gap(state[-8:], lengths[-8:], pose, config)) + finger_contacts = [ + present and gap <= 0.0002 for present, gap in zip(finger_contacts, pad_gaps) + ] + material_axis = Rotation.from_quat(state[material_index, 3:7]).as_matrix()[:, 2] + material_world = state[material_index, :3] + material_offset * material_axis + material_local = tcp[:3, :3].T @ (material_world - tcp[:3, 3]) + if ( + self.material_baseline is None + and t >= config["grasp_complete_s"] + and self.bilateral_contact + ): + self.material_baseline = material_local.copy() + material_slip = ( + float(np.linalg.norm(material_local - self.material_baseline)) + if self.material_baseline is not None + else 0.0 + ) + geometry = _rope_geometry(state, lengths, config) + nodes = geometry.pop("nodes") + row = { + "time_s": t, + "phase": labels[frame], + **geometry, + "actual_tcp_pose": tcp.tolist(), + "command_tcp_pose": targets[frame].tolist(), + "tcp_error_m": float(np.linalg.norm(tcp[:3, 3] - targets[frame, :3, 3])), + "tail_tcp_distance_m": float(np.linalg.norm(nodes[-1] - tcp[:3, 3])), + "grasp_region_tcp_distance_m": float( + np.linalg.norm(state[-6:, :3] - tcp[:3, 3], axis=1).min() + ), + "grasp_confirmed": self.material_baseline is not None, + "bilateral_rope_contact": self.bilateral_contact, + "material_point_tcp_m": material_local.tolist(), + "material_slip_m": material_slip, + "finger_pad_gaps_m": pad_gaps, + "left_rope_contact": finger_contacts[0], + "right_rope_contact": finger_contacts[1], + "finger_gap_m": float(qpos[7] + qpos[8]), + "anchor_error_m": float(np.linalg.norm(state[0, :3] - baseline)), + "arm_joint_limit_violation_rad": float( + max(0.0, (lo[:7] - qpos[:7]).max(), (qpos[:7] - hi[:7]).max()) + ), + "finger_joint_limit_violation_m": float( + max(0.0, (lo[7:] - qpos[7:]).max(), (qpos[7:] - hi[7:]).max()) + ), + "robot_post_penetration_m": self.robot_post_peak, + "penetration_m": self.penetration, + "worst_contact": self.worst, + "dropped_contacts": self.dropped, + "finite": bool(np.isfinite(state).all() and np.isfinite(qpos).all()), + } + self.rows.append(row) + self.states.append(state.copy()) + self.joint_states.append(qpos.copy()) + self.tcp_states.append(tcp.copy()) + if not row["finite"]: + raise RuntimeError(f"Nonfinite physical state at {t}") + + if len(self.rows) == len(self.motion.times): + self.terminal_report = evaluate_wrap(self.rows, self.config) + + def write(self, out: Path) -> dict[str, Any]: + """Export measured frames for the existing offline renderer and report.""" + report = self.terminal_report or evaluate_wrap(self.rows, self.config) + for name, value in { + "validation": report, + "settled_state": { + "robot_qpos": self.joint_states[0].tolist(), + "cable_state": self.states[0].tolist(), + }, + "metrics": { + "rows": self.rows, + "actor_paths": self.actor_paths, + "contact_sample_hz": self.config["control_hz"] * 4, + }, + }.items(): + (out / f"{name}.json").write_text(json.dumps(value, indent=2) + "\n") + np.savez_compressed( + out / "states.npz", + times=[row["time_s"] for row in self.rows], + cable_states=self.states, + robot_qpos=self.joint_states, + tcp_poses=self.tcp_states, + joint_names=self.robot.joint_names, + ) + return report + + +def attach_wrap_monitor( + env: EmbodiedEnv, config: dict[str, Any], motion: WrapMotion, robot_path: Path +) -> _WrapMonitor: + """Start the episode at the settled state before its first command.""" + previous = getattr(env, "_wrap_monitor", None) + if previous is not None: + if previous.active: + return previous + previous.sensor.remove_sampling_observer("franka_rope_wrap", expected=previous) + monitor = _WrapMonitor(env, config, motion, robot_path) + monitor.sensor.add_sampling_observer("franka_rope_wrap", monitor) + env._wrap_monitor = monitor + return monitor diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_integration.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_integration.py new file mode 100644 index 0000000000..4688a3c9fd --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_integration.py @@ -0,0 +1,161 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Bind a checked, single-post cable wrap plan to the standard MoveJoints skill.""" + +from __future__ import annotations + +from dataclasses import dataclass +import math +from pathlib import Path +from typing import ClassVar, TYPE_CHECKING +from weakref import proxy + +import torch + +from embodichain.lab.sim.atomic_skills import ( + AtomicSkillEngine, + JointPositionGoal, + MoveJoints, + PlanningContext, + SkillDescriptor, + SkillOptions, + TimedTrajectory, +) +from embodichain.lab.task_program.compiler.lowering import ( + RegisteredSemanticLowerer, + SemanticLowering, +) +from embodichain.lab.task_program.language.semantic_contracts import ( + SemanticCallContract, +) +from embodichain.lab.task_program.semantics import ( + BoundSemanticCall, + RegisteredSemanticCall, + SceneRegistry, +) +from .wrap_motion import PHASE_END_S, WrapMotion, plan_wrap_motion + +if TYPE_CHECKING: + from embodichain.lab.gym.envs.embodied_env import EmbodiedEnv + from embodichain.lab.sim.objects.robot import Robot + +__all__ = ["CableWrapPhaseFactory"] + + +@dataclass(frozen=True, slots=True) +class CableWrapPhaseFactory: + """Create a phase lowerer with the physical scene's robot model and control grid.""" + + call_id: ClassVar[str] = "simulation.cable_wrap_phase" + revision: ClassVar[str] = "1" + target_descriptor: ClassVar[SkillDescriptor] = MoveJoints.descriptor() + + @property + def argument_contract(self) -> SemanticCallContract: + """Expose the supported semantic phases during static preflight.""" + return SemanticCallContract(required={"phase": tuple(PHASE_END_S)}, optional={}) + + def create( + self, + *, + simulation: object, + robot: object, + scene_registry: SceneRegistry, + engine: AtomicSkillEngine, + ) -> RegisteredSemanticLowerer: + raise ValueError( + "Cable wrap planning requires its environment's scene configuration." + ) + + def create_for_environment( + self, + *, + environment: EmbodiedEnv, + simulation: object, + robot: Robot, + scene_registry: SceneRegistry, + engine: AtomicSkillEngine, + ) -> RegisteredSemanticLowerer: + from .wrap_evaluation import attach_wrap_monitor + + if ( + engine.robot is not robot + or environment.robot is not robot + or environment.num_envs != 1 + ): + raise ValueError( + "The Franka wrap deployment requires its bound, single-arena robot." + ) + config = environment.franka_rope_config + monitor = getattr(environment, "_wrap_monitor", None) + if monitor is None or not monitor.active: + robot_path = Path(robot.cfg.fpath) + motion = plan_wrap_motion(config, robot_path) + monitor = attach_wrap_monitor(environment, config, motion, robot_path) + motion = monitor.motion + # The registration retains its lowerer identities. The episode monitor owns + # the full trajectory so expired episodes can release their command arrays. + return _CableWrapPhaseLowerer(proxy(motion), int(config["control_hz"])) + + +class _CableWrapPhaseLowerer(RegisteredSemanticLowerer): + call_id: ClassVar[str] = CableWrapPhaseFactory.call_id + target_descriptor: ClassVar[SkillDescriptor] = MoveJoints.descriptor() + preserves_symbolic_state: ClassVar[bool] = True + + def __init__(self, motion: WrapMotion, hz: int) -> None: + self._motion = motion + self._hz = hz + + def lower( + self, + call: RegisteredSemanticCall, + *, + context: PlanningContext, + bound: BoundSemanticCall, + option_template: SkillOptions, + ) -> SemanticLowering: + if set(call.arguments) != {"phase"} or not isinstance( + call.arguments["phase"], str + ): + raise ValueError("A cable wrap call requires one named phase.") + dt = context.require_control_dt() + if context.batch_size != 1 or not math.isclose( + dt, 1.0 / self._hz, rel_tol=1e-8 + ): + raise ValueError( + "Franka wrap commands must run at the scene's configured control rate." + ) + phase = self._motion.phase_slice(call.arguments["phase"]) + # Include the last command of the preceding phase so its incoming + # finite difference is preserved at every segment boundary. + samples = self._motion.positions[phase.start - 1 : phase.stop] + positions = torch.as_tensor( + samples.copy(), device=context.robot.qpos.device + ).unsqueeze(0) + trajectory = TimedTrajectory( + positions=positions[:, 1:], + velocities=positions.diff(dim=1) * self._hz, + accelerations=None, + dt=torch.full( + (1, positions.shape[1] - 1), + dt, + dtype=torch.float64, + device=positions.device, + ), + env_ids=context.env_ids, + ) + return SemanticLowering(goal=JointPositionGoal(trajectory)) diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_measurements.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_measurements.py new file mode 100644 index 0000000000..ad6af641f5 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_measurements.py @@ -0,0 +1,318 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Physical measurements and episode acceptance for wrapping a cable around a post.""" + +from __future__ import annotations + +from typing import Any +import numpy as np +from scipy.spatial.transform import Rotation + +__all__ = ["evaluate_wrap"] + +WRAP_CHECKS = ( + "completed", + "finite", + "joint_limits", + "direct_floor_pickup", + "grasp_retained", + "material_grasp_slip", + "bilateral_finger_contact", + "actual_winding", + "post_contact_arc", + "hold_stable", + "rope_below_post_top", + "post_nonpenetration", + "robot_post_clearance", + "anchor_points_toward_post", + "smooth_anchor_exit", + "anchor_fixed", + "length_preserved", + "contacts_not_dropped", + "floor_nonpenetration", +) + + +def _capsule_pad_gap( + state: np.ndarray, + lengths: np.ndarray, + finger_pose: np.ndarray, + config: dict[str, Any], +) -> float: + """Exact capsule-axis / finger-box distance, less the physical rope radius.""" + axes = Rotation.from_quat(state[:, 3:7]).as_matrix()[:, :, 2] + starts = state[:, :3] - axes * lengths[:, None] / 2 + ends = state[:, :3] + axes * lengths[:, None] / 2 + half = np.asarray(config["finger_pad_size_m"]) / 2 + a = (starts - finger_pose[:3, 3]) @ finger_pose[:3, :3] - config[ + "finger_pad_center_m" + ] + b = (ends - finger_pose[:3, 3]) @ finger_pose[:3, :3] - config[ + "finger_pad_center_m" + ] + best = np.inf + for origin, delta in zip(a, b - a): + breaks = [0.0, 1.0] + for k in range(3): + if abs(delta[k]) > 1e-12: + breaks.extend( + float(v) + for v in ( + (-half[k] - origin[k]) / delta[k], + (half[k] - origin[k]) / delta[k], + ) + if 0 < v < 1 + ) + breaks = sorted(breaks) + for left, right in zip(breaks[:-1], breaks[1:]): + midpoint = origin + delta * ((left + right) / 2) + active = np.abs(midpoint) > half + offset = origin[active] - np.sign(midpoint[active]) * half[active] + velocity = delta[active] + denominator = float(velocity @ velocity) + t = ( + float(np.clip(-(offset @ velocity) / denominator, left, right)) + if denominator > 1e-20 + else left + ) + distance = np.linalg.norm(np.maximum(np.abs(origin + t * delta) - half, 0)) + best = min(best, float(distance)) + return best - config["cable_radius_m"] + + +def _rope_geometry( + state: np.ndarray, lengths: np.ndarray, config: dict[str, Any] +) -> dict[str, Any]: + """Measure actual capsule centerline geometry, independently of commanded orbit.""" + axis = Rotation.from_quat(state[:, 3:7]).as_matrix()[:, :, 2] + starts = state[:, :3] - axis * lengths[:, None] / 2 + ends = state[:, :3] + axis * lengths[:, None] / 2 + nodes = np.vstack([starts[:1], (ends[:-1] + starts[1:]) / 2, ends[-1:]]) + center = np.asarray(config["post_center_xy_m"]) + theta = np.unwrap(np.arctan2(nodes[:, 1] - center[1], nodes[:, 0] - center[0])) + samples = ( + starts[:, None] + + (ends - starts)[:, None] * np.linspace(0, 1, 11)[None, :, None] + ) + radial = np.linalg.norm(samples[:, :, :2] - center, axis=-1) + gaps = radial - config["post_radius_m"] - config["cable_radius_m"] + in_height = ( + samples[:, :, 2] > config["floor_height_m"] - config["cable_radius_m"] + ) & ( + samples[:, :, 2] + < config["floor_height_m"] + config["post_height_m"] + config["cable_radius_m"] + ) + valid_gaps = gaps[in_height] + near = ( + np.min(np.where(in_height, gaps, np.inf), axis=1) + <= config["validation"]["contact_band_m"] + ) + # Union on the circle: revisiting one sector must not inflate coverage. + intervals = [] + for low, high, include in zip( + np.minimum(theta[:-1], theta[1:]), np.maximum(theta[:-1], theta[1:]), near + ): + if not include: + continue + width = high - low + start = low % (2 * np.pi) + end = start + width + intervals.append((start, min(end, 2 * np.pi))) + if end > 2 * np.pi: + intervals.append((0.0, end - 2 * np.pi)) + coverage, right = 0.0, 0.0 + for low, high in sorted(intervals): + coverage += max(0.0, high - max(low, right)) + right = max(right, high) + joint_gap = np.linalg.norm(ends[:-1] - starts[1:], axis=1) + anchor_direction = np.r_[center - starts[0, :2], 0.0] + anchor_direction /= np.linalg.norm(anchor_direction) + anchor_angle = np.rad2deg(np.arccos(np.clip(axis[0] @ anchor_direction, -1.0, 1.0))) + anchor_bends = np.rad2deg( + np.arccos(np.clip(np.sum(axis[:4] * axis[1:5], axis=-1), -1.0, 1.0)) + ) + return { + "nodes": nodes, + "anchor_direction_error_deg": float(anchor_angle), + "anchor_exit_bend_max_deg": float(anchor_bends.max()), + "winding_deg": float(np.rad2deg(theta[-1] - theta[0])), + "contact_arc_deg": float(np.rad2deg(coverage)), + "post_contact_segments": int(near.sum()), + "post_penetration_m": ( + float(max(0.0, -valid_gaps.min())) if valid_gaps.size else 0.0 + ), + "joint_gap_max_m": float(joint_gap.max()), + "centerline_length_m": float( + np.linalg.norm(np.diff(nodes, axis=0), axis=1).sum() + ), + "free_end_m": ends[-1].tolist(), + "rope_min_z_m": float(samples[:, :, 2].min() - config["cable_radius_m"]), + "rope_max_z_m": float(samples[:, :, 2].max() + config["cable_radius_m"]), + } + + +def evaluate_wrap(rows: list[dict[str, Any]], config: dict[str, Any]) -> dict[str, Any]: + thresholds = config["validation"] + hold = [r for r in rows if r["time_s"] >= config["duration_s"] - 2 - 1e-8] + grasped = [r for r in rows if r["time_s"] >= config["grasp_complete_s"]] + lifted = [ + r + for r in rows + if config["lift_complete_s"] <= r["time_s"] <= config["lift_complete_s"] + 0.5 + ] + checks = { + "completed": bool( + rows + and abs(rows[-1]["time_s"] - config["duration_s"]) < 1e-8 + and len(hold) >= round(2 * config["control_hz"]) + 1 + ), + "finite": bool(rows and all(r["finite"] for r in rows)), + "joint_limits": bool( + rows + and max(r["arm_joint_limit_violation_rad"] for r in rows) < 1e-4 + and max(r["finger_joint_limit_violation_m"] for r in rows) < 1e-4 + ), + "direct_floor_pickup": bool( + lifted + and rows[0]["free_end_m"][2] + <= config["floor_height_m"] + 2 * config["cable_radius_m"] + and min(r["free_end_m"][2] for r in lifted) + > config["floor_height_m"] + 0.035 + ), + "grasp_retained": bool( + grasped + and all(r["grasp_confirmed"] for r in grasped) + and max(r["tail_tcp_distance_m"] for r in grasped) + <= thresholds["tail_tcp_max_m"] + ), + "material_grasp_slip": bool( + grasped + and max(r["material_slip_m"] for r in grasped) + <= thresholds.get("grasp_slip_max_m", 0.008) + ), + "bilateral_finger_contact": bool( + grasped and np.mean([r["bilateral_rope_contact"] for r in grasped]) >= 0.95 + ), + "actual_winding": bool( + hold + and min(abs(r["winding_deg"]) for r in hold) + >= thresholds["winding_min_deg"] + ), + "post_contact_arc": bool( + hold + and min(r["contact_arc_deg"] for r in hold) + >= thresholds["contact_arc_min_deg"] + ), + "hold_stable": bool( + hold + and np.ptp([r["winding_deg"] for r in hold]) + <= thresholds["hold_winding_drift_max_deg"] + and np.max( + np.linalg.norm( + np.asarray([r["free_end_m"] for r in hold]) - hold[0]["free_end_m"], + axis=1, + ) + ) + <= thresholds["hold_end_drift_max_m"] + ), + "rope_below_post_top": bool( + rows + and max(r["rope_max_z_m"] for r in rows) + < config["floor_height_m"] + config["post_height_m"] + ), + "post_nonpenetration": bool( + rows + and max(r["post_penetration_m"] for r in rows) + <= thresholds["post_penetration_max_m"] + ), + "robot_post_clearance": bool( + rows + and max(r["robot_post_penetration_m"] for r in rows) + <= thresholds["robot_post_penetration_max_m"] + ), + "anchor_points_toward_post": bool( + rows and max(r["anchor_direction_error_deg"] for r in rows) < 0.1 + ), + "smooth_anchor_exit": bool( + hold + and max(r["anchor_exit_bend_max_deg"] for r in hold) + <= thresholds.get("anchor_exit_bend_max_deg", 20.0) + ), + "anchor_fixed": bool(rows and max(r["anchor_error_m"] for r in rows) < 1e-5), + "length_preserved": bool( + rows + and max(r["joint_gap_max_m"] for r in rows) <= thresholds["joint_gap_max_m"] + ), + "contacts_not_dropped": bool( + rows and max(r["dropped_contacts"] for r in rows) == 0 + ), + "floor_nonpenetration": bool( + rows + and min(r["rope_min_z_m"] for r in rows) + >= config["floor_height_m"] - thresholds["floor_penetration_max_m"] + ), + } + return { + "passed": all(checks.values()), + "checks": checks, + "summary": { + "hold_anchor_exit_bend_max_deg": max( + [r["anchor_exit_bend_max_deg"] for r in hold], default=None + ), + "material_grasp_slip_max_m": max( + [r["material_slip_m"] for r in grasped], default=None + ), + "final_winding_deg": rows[-1]["winding_deg"] if rows else None, + "hold_winding_min_deg": min( + [abs(r["winding_deg"]) for r in hold], default=None + ), + "hold_contact_arc_min_deg": min( + [r["contact_arc_deg"] for r in hold], default=None + ), + "tail_tcp_max_m": max( + [r["tail_tcp_distance_m"] for r in grasped], default=None + ), + "post_penetration_max_m": max( + [r["post_penetration_m"] for r in rows], default=None + ), + "robot_post_penetration_max_m": max( + [r["robot_post_penetration_m"] for r in rows], default=None + ), + "hold_winding_drift_deg": ( + float(np.ptp([r["winding_deg"] for r in hold])) if hold else None + ), + "hold_end_drift_m": ( + float( + np.max( + np.linalg.norm( + np.asarray([r["free_end_m"] for r in hold]) + - hold[0]["free_end_m"], + axis=1, + ) + ) + ) + if hold + else None + ), + "joint_gap_max_m": max([r["joint_gap_max_m"] for r in rows], default=None), + "bilateral_contact_fraction": ( + float(np.mean([r["bilateral_rope_contact"] for r in grasped])) + if grasped + else None + ), + }, + "thresholds": thresholds, + } diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_motion.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_motion.py new file mode 100644 index 0000000000..6408d6f556 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/cable/wrap_motion.py @@ -0,0 +1,334 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Plan a floor pickup and a continuous, fixed-wrist cable wrap around a post.""" + +from __future__ import annotations + +from dataclasses import dataclass +import json +from pathlib import Path +from typing import Any +import xml.etree.ElementTree as ET + +import numpy as np +from scipy.optimize import least_squares +from scipy.spatial.transform import Rotation, Slerp +import torch + +__all__ = ["PHASE_END_S", "WrapMotion", "plan_wrap_motion"] + +_JOINT_NAMES = tuple(f"fr3_joint{i}" for i in range(1, 8)) + ( + "fr3_finger_joint1", + "fr3_finger_joint2", +) + +PHASE_END_S = { + "home": 1.0, + "approach": 3.0, + "descend": 4.0, + "close": 5.0, + "lift": 7.0, + "approach_post": 10.0, + "orbit": 28.0, + "tighten": 32.0, + "settle": 34.0, + "hold_closed": 36.0, +} + + +@dataclass(frozen=True) +class WrapMotion: + """Time samples including the home reference at t=0, before the first action.""" + + positions: np.ndarray + poses: np.ndarray + labels: list[str] + times: np.ndarray + report: dict[str, Any] + joint_names: tuple[str, ...] + + def phase_slice(self, phase: str) -> slice: + """Select commands (start, stop] exactly once on the global control grid.""" + scale = self.report["duration_s"] / PHASE_END_S["hold_closed"] + previous_end = 0 + for name, nominal_end in PHASE_END_S.items(): + end = int( + np.searchsorted(self.times, nominal_end * scale + 1e-9, side="right") + ) + if phase == name: + return slice(max(1, previous_end), end) + previous_end = end + raise ValueError(f"Unknown wrap phase {phase!r}.") + + def write(self, out: Path) -> None: + """Export the checked plan for inspection or comparison against a recording.""" + out.mkdir(parents=True, exist_ok=True) + (out / "motion_validation.json").write_text( + json.dumps(self.report, indent=2) + "\n" + ) + np.savez_compressed( + out / "robot_motion.npz", + times=self.times, + joint_positions=self.positions, + joint_names=self.joint_names, + tcp_poses=self.poses, + phases=self.labels, + ) + + +def plan_wrap_motion(config: dict[str, Any], robot_path: Path) -> WrapMotion: + """Sample the complete TCP trajectory, solve seeded IK and verify FK and joint limits.""" + import pytorch_kinematics as pk + + arm_names = list(_JOINT_NAMES[:7]) + floor = float(config.get("floor_height_m", 0.002)) + fps = int(config.get("control_hz", 60)) + duration = float(config.get("duration_s", 36.0)) + if duration < 36.0: + raise ValueError( + "At least 36 seconds are required, including a two-second hold" + ) + scale = duration / PHASE_END_S["hold_closed"] + radius = float(config.get("orbit_radius_m", 0.12)) + wrap_degrees = float(config.get("orbit_degrees", 450.0)) + direction = int(config.get("orbit_direction", 1)) + if direction not in (-1, 1): + raise ValueError("orbit_direction must be +1 (CCW) or -1 (CW)") + theta_start = np.deg2rad(float(config.get("orbit_start_deg", -90.0))) + theta_end = theta_start + direction * np.deg2rad(wrap_degrees) + center = np.asarray(config["post_center_xy_m"], dtype=float) + pickup_end = np.asarray(config["cable_waypoints_m"][-1], dtype=float) + grasp_offset = np.asarray( + config.get("grasp_offset_world_m", [-0.020, 0, 0]), dtype=float + ) + z_start = float(config.get("orbit_start_z_m", floor + 0.055)) + z_end = float(config.get("orbit_end_z_m", floor + 0.075)) + lift_height = float(config.get("pickup_lift_m", 0.09)) + radial_pull = float(config.get("tighten_radial_m", 0.018)) + tangent_pull = float(config.get("tighten_tangent_m", 0.020)) + open_gap = float(config.get("open_half_gap_m", 0.04)) + closed_gap = float(config.get("grip_half_gap_m", config["cable_radius_m"] - 0.0005)) + if not 0 < closed_gap < open_gap <= 0.04: + raise ValueError("Invalid Franka gripper half gaps") + hand_r = Rotation.from_euler( + "z", float(config.get("grasp_yaw_deg", 0.0)), degrees=True + ).as_matrix() @ np.diag([1.0, -1.0, -1.0]) + chain = pk.build_serial_chain_from_urdf(robot_path.read_bytes(), "fr3_hand_tcp").to( + dtype=torch.float64, device="cpu" + ) + if chain.get_joint_parameter_names() != arm_names: + raise ValueError("Unexpected Franka arm joint order") + limits = np.asarray(chain.get_joint_limits(), dtype=float).T + lo, hi = limits[:, 0] + 1e-5, limits[:, 1] - 1e-5 + home = np.asarray(config["home_qpos"], dtype=float) + + def pose(p: np.ndarray, rotation: np.ndarray = hand_r) -> np.ndarray: + result = np.eye(4) + result[:3, :3], result[:3, 3] = rotation, p + return result + + def fk(q: np.ndarray) -> np.ndarray: + result = ( + chain.forward_kinematics(torch.tensor(q, dtype=torch.float64)) + .get_matrix()[0] + .numpy() + ) + result[2, 3] += floor + return result + + def ik(target: np.ndarray, seed: np.ndarray) -> np.ndarray: + q = seed.copy() + for _ in range(45): + current = fk(q) + dp = target[:3, 3] - current[:3, 3] + dr = Rotation.from_matrix(target[:3, :3] @ current[:3, :3].T).as_rotvec() + if np.linalg.norm(dp) < 2e-7 and np.linalg.norm(dr) < 2e-6: + return q + jac = chain.jacobian(torch.tensor(q, dtype=torch.float64))[0].numpy() + jac[3:] *= 0.15 + delta = jac.T @ np.linalg.solve( + jac @ jac.T + np.eye(6) * 1e-8, np.r_[dp, 0.15 * dr] + ) + q = np.clip( + q + delta * min(1.0, 0.1 / max(np.max(np.abs(delta)), 1e-9)), lo, hi + ) + + def residual(candidate: np.ndarray) -> np.ndarray: + current = fk(candidate) + return np.r_[ + current[:3, 3] - target[:3, 3], + 0.15 + * Rotation.from_matrix(target[:3, :3] @ current[:3, :3].T).as_rotvec(), + ] + + solved = least_squares( + residual, + q, + bounds=(lo, hi), + ftol=1e-12, + xtol=1e-12, + gtol=1e-12, + max_nfev=160, + ) + if np.linalg.norm(residual(solved.x)) > 1e-5: + raise RuntimeError( + f"Unreachable wrap TCP {target[:3, 3]}: {residual(solved.x)}" + ) + return solved.x + + def smooth(u: float) -> float: + u = float(np.clip(u, 0, 1)) + return u**3 * (10 - 15 * u + 6 * u * u) + + def end_at(theta: float, height: float) -> np.ndarray: + return np.r_[center + radius * np.array([np.cos(theta), np.sin(theta)]), height] + + home_pose = fk(home) + pickup = pose(pickup_end + grasp_offset) + above = pickup.copy() + above[2, 3] += lift_height + orbit_start = pose(end_at(theta_start, z_start) + grasp_offset) + orbit_end = pose(end_at(theta_end, z_end) + grasp_offset) + radial = np.array([np.cos(theta_end), np.sin(theta_end), 0.0]) + tangent = direction * np.array([-np.sin(theta_end), np.cos(theta_end), 0.0]) + tightened = orbit_end.copy() + tightened[:3, 3] += radial_pull * radial + tangent_pull * tangent + pickup_keys = [ + (0.0, home_pose, open_gap, "HOME"), + (PHASE_END_S["home"], home_pose, open_gap, "HOME"), + (PHASE_END_S["approach"], above, open_gap, "APPROACH FLOOR END"), + (PHASE_END_S["descend"], pickup, open_gap, "LOWER TO ROPE"), + (PHASE_END_S["close"], pickup, closed_gap, "GRASP ROPE"), + (PHASE_END_S["lift"], above, closed_gap, "LIFT AND ORGANIZE SLACK"), + (PHASE_END_S["approach_post"], orbit_start, closed_gap, "TANGENTIAL APPROACH"), + ] + times = np.arange(round(duration * fps) + 1) / fps + targets, labels, gaps = [], [], [] + orbit_start_clock = PHASE_END_S["approach_post"] + orbit_end_clock = PHASE_END_S["orbit"] + for t in times: + clock = t / scale + if clock <= PHASE_END_S["approach_post"]: + index = min( + max( + int( + np.searchsorted( + [k[0] for k in pickup_keys], clock, side="right" + ) + ) + - 1, + 0, + ), + len(pickup_keys) - 2, + ) + a, b = pickup_keys[index : index + 2] + u = smooth((clock - a[0]) / (b[0] - a[0])) + target = pose( + (1 - u) * a[1][:3, 3] + u * b[1][:3, 3], + Slerp([0, 1], Rotation.from_matrix([a[1][:3, :3], b[1][:3, :3]]))( + [u] + ).as_matrix()[0], + ) + gap, label = (1 - u) * a[2] + u * b[2], b[3] + elif clock <= PHASE_END_S["orbit"]: + u = smooth( + (clock - orbit_start_clock) / (orbit_end_clock - orbit_start_clock) + ) + theta = theta_start + (theta_end - theta_start) * u + target = pose(end_at(theta, (1 - u) * z_start + u * z_end) + grasp_offset) + gap, label = closed_gap, "WRAP ROPE AROUND PILLAR" + elif clock <= PHASE_END_S["tighten"]: + u = smooth( + (clock - orbit_end_clock) / (PHASE_END_S["tighten"] - orbit_end_clock) + ) + target = pose((1 - u) * orbit_end[:3, 3] + u * tightened[:3, 3]) + gap, label = closed_gap, "GENTLE TANGENTIAL TIGHTEN" + elif clock < PHASE_END_S["settle"]: + target, gap, label = tightened.copy(), closed_gap, "SETTLE WRAP" + else: + target, gap, label = tightened.copy(), closed_gap, "HOLD COMPLETED WRAP" + targets.append(target) + gaps.append(gap) + labels.append(label) + targets = np.asarray(targets) + qpath = np.zeros((len(times), 9), dtype=np.float32) + q = home.copy() + for i, target in enumerate(targets): + q = ik(target, q) + qpath[i, :7], qpath[i, 7:] = q, gaps[i] + actual = ( + chain.forward_kinematics(torch.tensor(qpath[:, :7], dtype=torch.float64)) + .get_matrix() + .numpy() + ) + actual[:, 2, 3] += floor + pe = np.linalg.norm(actual[:, :3, 3] - targets[:, :3, 3], axis=1) + re = Rotation.from_matrix( + targets[:, :3, :3] @ actual[:, :3, :3].transpose(0, 2, 1) + ).magnitude() + vmax = np.max(np.abs(np.diff(qpath[:, :7], axis=0) * fps), axis=0) + root = ET.parse(robot_path).getroot() + velocity_limits = np.array( + [ + float(root.find(f"joint[@name='{name}']/limit").get("velocity")) + for name in arm_names + ] + ) + joint_limits_ok = bool( + np.all(qpath[:, :7] >= lo - 1e-6) and np.all(qpath[:, :7] <= hi + 1e-6) + ) + passed = bool( + pe.max() < 1e-5 + and re.max() < 1e-4 + and np.all(vmax <= velocity_limits) + and joint_limits_ok + ) + report = { + "passed": passed, + "ik_position_max_m": float(pe.max()), + "ik_rotation_max_rad": float(re.max()), + "joint_velocity_max_rad_s": vmax.tolist(), + "joint_velocity_limits_rad_s": velocity_limits.tolist(), + "joint_limits_ok": joint_limits_ok, + "minimum_joint_limit_margin_rad": float( + np.min(np.minimum(qpath[:, :7] - lo, hi - qpath[:, :7])) + ), + "duration_s": duration, + "hold_start_s": PHASE_END_S["settle"] * scale, + "hold_duration_s": (PHASE_END_S["hold_closed"] - PHASE_END_S["settle"]) * scale, + "orbit_start_s": orbit_start_clock * scale, + "orbit_end_s": orbit_end_clock * scale, + "orbit_degrees": wrap_degrees, + "orbit_direction": direction, + "orbit_radius_m": radius, + "orbit_start_z_m": z_start, + "orbit_end_z_m": z_end, + "grasp_offset_world_m": grasp_offset.tolist(), + "grasp_yaw_deg": float(config.get("grasp_yaw_deg", 0.0)), + "grip_half_gap_m": closed_gap, + "final_end_target_m": (tightened[:3, 3] - grasp_offset).tolist(), + "physical_success_requires_actual_centerline_and_contacts": True, + } + if not passed: + raise RuntimeError(f"Wrap motion validation failed: {report}") + return WrapMotion( + qpath, + targets, + labels, + times, + report, + _JOINT_NAMES, + ) diff --git a/scripts/validation/accept_usd_import.py b/scripts/validation/accept_usd_import.py index dd7b09a757..37429f7eca 100644 --- a/scripts/validation/accept_usd_import.py +++ b/scripts/validation/accept_usd_import.py @@ -169,7 +169,10 @@ def _accept_case(case: dict[str, Any], output: Path) -> dict[str, Any]: "An imported entity was lost or a source path is duplicated." ) expected_assets = ( - len(raw.mesh_objects) + len(raw.articulations) + len(raw.particle_sets) + len(raw.mesh_objects) + + len(raw.articulations) + + len(raw.particle_sets) + + len(raw.cables) ) record["resources"] = dict(imported.resource_uids) record["resource_source_paths"] = dict(imported.resource_source_paths) @@ -286,6 +289,12 @@ def check_state() -> None: ) elif type(asset).__name__ in ("Articulation", "Robot"): value = asset.get_qpos() + elif type(asset).__name__ == "CableObject": + value = asset.data.segment_state_w + if value.numel() == 0: + raise AssertionError( + f"Cable {asset.uid} has no simulation segments." + ) else: value = asset.get_local_pose() if not torch.isfinite(value).all(): diff --git a/setup.py b/setup.py index d2b5b24d76..b543867a1f 100644 --- a/setup.py +++ b/setup.py @@ -172,6 +172,7 @@ def get_package_data() -> dict[str, list[str]]: **{name: list(extension_patterns) for name in _extension_package_dirs()}, "embodichain_tasks.configs": [ "tasks/manipulation/cloth_fold/task_program/trajectory.npz", + "**/*.usda", "**/*.json", "**/*.yaml", "**/*.yml", diff --git a/tests/compute/geometry/test_cable.py b/tests/compute/geometry/test_cable.py new file mode 100644 index 0000000000..9b0820cf04 --- /dev/null +++ b/tests/compute/geometry/test_cable.py @@ -0,0 +1,101 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from __future__ import annotations + +import math +import pytest +import torch + +from embodichain.compute.geometry.cable import ( + cable_endpoints, + sample_cable_material, + cable_grasp_slip, +) + +pytestmark = pytest.mark.no_sim + + +def test_material_coordinates_follow_rotated_capsule_axes_and_world_origins(): + state = torch.zeros(2, 2, 13) + state[..., 3] = -math.sqrt(0.5) + state[..., 6] = math.sqrt(0.5) + state[..., :3] = torch.tensor([[0, 0.1, 1], [0, 0.3, 1]]) + state[1, :, :3] += torch.tensor([3, 5, 0]) + lengths = torch.tensor([0.2, 0.2]) + point, tangent, index = sample_cable_material(state, lengths, 0.3) + assert index == 1 + torch.testing.assert_close(point, torch.tensor([[0, 0.3, 1], [3, 5.3, 1]])) + torch.testing.assert_close( + tangent, torch.tensor([[0, 1, 0], [0, 1, 0.0]]), atol=1e-6, rtol=0 + ) + first, last = cable_endpoints(state, lengths) + torch.testing.assert_close( + first[0, 0], torch.tensor([0, 0, 1.0]), atol=1e-6, rtol=0 + ) + torch.testing.assert_close( + last[0, -1], torch.tensor([0, 0.4, 1.0]), atol=1e-6, rtol=0 + ) + + +@pytest.mark.parametrize("distance", [-0.01, 0.41, float("nan")]) +def test_material_sampling_rejects_invalid_distance(distance): + s = torch.zeros(1, 2, 13) + s[..., 6] = 1 + with pytest.raises(ValueError): + sample_cable_material(s, torch.tensor([0.2, 0.2]), distance) + + +def test_slip_uses_the_tool_frame_material_point(): + initial = torch.tensor([[0.01, 0, 0], [0, 0, 0.01]]) + current = initial + torch.tensor([[0, 0.003, 0.004], [0, 0, 0]]) + torch.testing.assert_close( + cable_grasp_slip(current, initial), torch.tensor([0.005, 0]) + ) + + +@pytest.mark.parametrize( + "lengths", + [ + torch.tensor([]), + torch.tensor([-0.2]), + torch.tensor([0.0]), + torch.tensor([float("nan")]), + ], +) +def test_endpoints_reject_invalid_rest_geometry(lengths): + state = torch.zeros(2, lengths.numel(), 13) + state[..., 6] = 1 + with pytest.raises(ValueError, match="positive vector"): + cable_endpoints(state, lengths) + + +def test_material_sampling_preserves_endpoint_identity_for_unequal_segments(): + state = torch.zeros(1, 2, 13, dtype=torch.float64) + state[..., 6] = 1 + state[0, :, 2] = torch.tensor([0.05, 0.25]) + lengths = torch.tensor([0.1, 0.3], dtype=torch.float64) + for distance, expected_z, expected_index in [ + (0.0, 0.0, 0), + (0.1, 0.1, 0), + (0.4, 0.4, 1), + ]: + point, tangent, index = sample_cable_material(state, lengths, distance) + assert index == expected_index + torch.testing.assert_close(point, state.new_tensor([[0, 0, expected_z]])) + torch.testing.assert_close(tangent, state.new_tensor([[0, 0, 1]])) diff --git a/tests/gym/envs/task_program/test_bridge.py b/tests/gym/envs/task_program/test_bridge.py index 1746c21e81..dbf1db412f 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, @@ -2017,6 +2027,29 @@ def test_demo_executor_pre_step_stop_consumes_only_abort_hold() -> None: assert runtime.sink.pending_count == 0 +@pytest.mark.parametrize("max_steps", [0, 1]) +def test_simulation_step_limit_physically_truncates_and_does_not_advance_the_clock( + max_steps: int, +) -> None: + bridge, runtime, clock = _bridge(duration=2 * STEP_DT) + env = _BridgeExecutorEnv(bridge) + + result = execute_demo_episode(env, max_steps=max_steps) + + assert result.terminal_reason == "simulation_step_limit" + assert result.truncated == (True, True) + assert not result.completed + assert result.length == clock.step_index == max_steps + assert [action.metadata["bridge_action_kind"] for action in env.steps] == [ + "runtime_command" + ] * max_steps + assert runtime.cancel_count == (1 if max_steps else 0) + assert runtime.sink.pending_count == 0 + if max_steps: + assert result.segments[-1].metadata["runtime"]["status"] == "cancelled" + assert bridge.execution_finished + + def test_demo_executor_post_step_failure_acknowledges_then_safe_stops() -> None: bridge, runtime, clock = _bridge(duration=2 * STEP_DT) env = _BridgeExecutorEnv( diff --git a/tests/gym/envs/task_program/test_cable_material.py b/tests/gym/envs/task_program/test_cable_material.py new file mode 100644 index 0000000000..07e8a5f305 --- /dev/null +++ b/tests/gym/envs/task_program/test_cable_material.py @@ -0,0 +1,485 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from copy import deepcopy +from dataclasses import replace +from pathlib import Path +from types import SimpleNamespace +import math + +import pytest +import torch +import yaml + +from embodichain.lab.gym.utils.composition import resolve_gym_components +from embodichain.lab.task_program.integrations import ( + load_configured_task_program_deployment, +) +from embodichain.lab.task_program.integrations.simulation.cable import ( + _decode_cable_service, + _CableRuntime, + _CableCheckLowerer, + _CableMoveLowerer, + _CablePickFactory, +) +from embodichain.lab.task_program.semantics import RegisteredSemanticCall +from embodichain.lab.sim.atomic_skills import MoveEndEffectorOptions, MoveJointsOptions + +pytestmark = pytest.mark.no_sim +_ROOT = Path(__file__).resolve().parents[4] +_TASK = ( + _ROOT + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable" +) + + +def _declaration(): + return yaml.safe_load((_TASK / "task_program/integration.yaml").read_text())[ + "runtime_services" + ]["cable_material"] + + +def test_cable_deployment_preflights_all_calls_and_measured_final_acceptance(): + path = _TASK / "task.yam.yaml" + raw = yaml.safe_load(path.read_text()) + physical = resolve_gym_components(raw, base_dir=path.parent) + deployment = load_configured_task_program_deployment( + task_program=physical.config["task_program"], + skill_profile=physical.embodiment_skill_profile, + evaluation=physical.evaluation, + base_dir=path.parent, + ) + physical.validate_scene_bindings(deployment.scene_binding) + registration = deployment.integration.registration + compiled = registration.catalog.preflight(deployment.load_program()) + assert compiled.segment_count == 51 + assert registration.evaluation_final_refs == ("route_success",) + assert registration.scene_binding.declare().entries == () + factories = registration.registered_semantic_lowerer_factories + assert {factory.target_descriptor.skill_id for factory in factories} == { + "pick_up", + "move_end_effector", + "move_joints", + } + for factory in factories: + assert factory.argument_contract.required["target"] + + +@pytest.mark.parametrize( + "change", ["unknown", "negative_coordinate", "bad_geometry", "executable"] +) +def test_cable_configuration_rejects_invalid_closed_payload(change): + value = _declaration() + if change == "unknown": + value["teleport"] = True + elif change == "negative_coordinate": + value["targets"]["right_grasp_0.162"]["material_distance"] = -1 + elif change == "bad_geometry": + value["geometry"]["strict_region"] = [[0, 0, 0]] + else: + value["targets"]["right_grasp_0.162"]["class_type"] = "os.system" + with pytest.raises((ValueError, TypeError)): + _decode_cable_service(value) + + +@pytest.mark.parametrize( + "changes", + [ + {"material_distance": None}, + {"material_distance": -0.01}, + {"retract_distance": -0.001}, + {"retract_distance": float("inf")}, + {"position": [0.5, None, 0.78]}, + {"position": [None, None, None]}, + {"position": [None, None, float("nan")]}, + {"firm": True}, + {"kind": "material", "retract_distance": 0.005}, + ], +) +def test_material_lift_rejects_invalid_geometry_and_inapplicable_fields(changes): + value = _declaration() + target = { + "kind": "material_lift", + "material_distance": 0.162, + "position": [None, None, 0.78], + } + target.update(changes) + if target.get("material_distance") is None: + target.pop("material_distance") + value["targets"] = {"lift": target} + with pytest.raises((ValueError, TypeError)): + _decode_cable_service(value) + + +def test_cable_binding_rejects_missing_physical_uid_before_simulation(): + from embodichain.lab.gym.utils._component_composition import ( + _validate_scene_binding_targets, + ) + + with pytest.raises(ValueError, match="not declared"): + _validate_scene_binding_targets( + {"cables": [{"entity_id": "cable", "simulation_uid": "missing"}]}, + simulation={"cable_object": [{"uid": "cable"}]}, + ) + + +def _runtime(): + cfg = _decode_cable_service(_declaration()) + qpos = torch.zeros(4, 3) + origins = torch.tensor( + [[0, 0, 0], [10, 0, 0], [20, 0, 0], [30, 0, 0], [40, 0, 0]], dtype=torch.float32 + ) + state = torch.zeros(4, 50, 13) + state[..., 4] = math.sqrt(0.5) + state[..., 6] = math.sqrt(0.5) + state[..., 0] = (torch.arange(50) + 0.5) * 0.012 + state[..., :3] += origins[1:, None] + + class Arenas: + root_offsets = origins.numpy() + + def index(self, name): + return int(name[5:]) + + def fk(q, **_): + result = torch.eye(4).repeat(len(q), 1, 1) + result[:, :3, 3] = q + return result + + robot = SimpleNamespace( + body_data=SimpleNamespace( + entities=[SimpleNamespace(arena_name=f"arena{i}") for i in range(1, 5)] + ), + get_joint_ids=lambda **_: [0, 1, 2], + get_qpos=lambda **_: qpos, + compute_fk=fk, + ) + cable = SimpleNamespace( + cfg=SimpleNamespace( + points=[[i * 0.012, 0, 0] for i in range(51)], radius=0.0035 + ), + data=SimpleNamespace( + segment_state_w=state, batch=SimpleNamespace(arena_indices=(1, 2, 3, 4)) + ), + ) + sim = SimpleNamespace( + spawn_result=SimpleNamespace(arenas=Arenas()), get_cable_object=lambda _: cable + ) + service = _CableRuntime(cfg, sim, robot, torch.tensor([3, 1])) + return service, state, qpos + + +def test_material_observation_maps_selected_rows_and_arena_origins_once(): + service, _, _ = _runtime() + torch.testing.assert_close( + service.origins, torch.tensor([[40.0, 0, 0], [20.0, 0, 0]]) + ) + torch.testing.assert_close( + service.point(0.162)[0] - service.origins, + torch.tensor([[0.162, 0, 0], [0.162, 0, 0]]), + atol=4e-6, + rtol=0, + ) + + +@pytest.mark.parametrize("retract_distance", [0.0, 0.008]) +def test_material_lift_retracts_from_measured_tool_along_horizontal_material_tangent( + retract_distance, +): + service, state, measured = _runtime() + value = _declaration() + target = { + "kind": "material_lift", + "material_distance": 0.162, + "position": [None, None, 0.78], + } + if retract_distance: + target["retract_distance"] = retract_distance + value["targets"] = {"lift": target} + target = _decode_cable_service(value).target("lift") + measured[3] = torch.tensor([0.50, 0.02, 0.76]) + measured[1] = torch.tensor([0.60, 0.03, 0.77]) + commanded = measured + 0.01 + service.robot.get_qpos = lambda *, target=False: commanded if target else measured + original_fk = service.robot.compute_fk + measured_rotation = torch.tensor( + [[0.0, 1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, -1.0]] + ) + + def rotated_fk(qpos, **kwargs): + pose = original_fk(qpos, **kwargs) + pose[:, :3, :3] = measured_rotation + return pose + + service.robot.compute_fk = rotated_fk + # Native rows 3 and 1 belong to arenas at X=40 and X=20. The first + # tangent also tilts upward, so the XY projection must be normalized. + for row, direction in ((3, (0.0, 0.6, 0.8)), (1, (-0.6, 0.8, 0.0))): + x, y, z = direction + quaternion = torch.tensor([-y, x, 0.0, 1 + z]) + state[row, 13, 3:7] = quaternion / quaternion.norm() + pose = service.pose(target, "left_arm") + expected = torch.tensor([[0.50, 0.02, 0.78], [0.60, 0.03, 0.78]]) + expected[:, :2] -= retract_distance * torch.tensor([[0.0, 1.0], [-0.6, 0.8]]) + torch.testing.assert_close(pose[:, :3, 3], expected, atol=2e-6, rtol=0) + torch.testing.assert_close(pose[:, :3, :3], measured_rotation.repeat(2, 1, 1)) + + +def test_material_lift_rejects_vertical_tangent_without_horizontal_retraction_axis(): + service, state, _ = _runtime() + value = _declaration() + value["targets"] = { + "lift": { + "kind": "material_lift", + "material_distance": 0.162, + "position": [None, None, 0.78], + "retract_distance": 0.008, + } + } + state[1, 13, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0]) + with pytest.raises(ValueError, match="horizontal direction"): + service.pose(_decode_cable_service(value).target("lift"), "left_arm") + + +def test_material_reference_is_captured_after_close_and_tracks_slip_per_row(): + service, state, qpos = _runtime() + service.pending_pick = ("right_arm", service.cfg.target("right_grasp_0.162")) + action = SimpleNamespace(metadata={"bridge_action_kind": "runtime_command"}) + phase = ["close", False, 5] + runtime = SimpleNamespace(_last_sent_command_phase=lambda: phase) + service.after_action(action, runtime) + assert not service.grasps + # This represents the just-consumed close frame, before the first lift. + qpos[3, 0] = 0.004 + phase[1] = True + service.after_action(action, runtime) + baseline = service.grasps["right_arm"][1].clone() + torch.testing.assert_close( + baseline[:, 0], torch.tensor([0.158, 0.162]), atol=4e-6, rtol=0 + ) + state[3, :, 0] += 0.013 + rejected, _ = service.after_action(action, runtime) + assert rejected.tolist() == [True, False] + torch.testing.assert_close(service.grasps["right_arm"][1], baseline) + fresh, _, _ = _runtime() + assert not fresh.grasps and fresh.max_slip.tolist() == [0, 0] + + +def test_port_stage_replaces_measured_tilt_with_selected_downward_orientation(): + service, _, _ = _runtime() + original_fk = service.robot.compute_fk + cosine = math.sqrt(0.5) + # A physical tool tilted by 45 degrees in both pitch and roll. + measured_rotation = torch.tensor( + [[cosine, 0.5, 0.5], [0.0, cosine, -cosine], [-cosine, 0.5, 0.5]] + ) + + def tilted_fk(qpos, **kwargs): + pose = original_fk(qpos, **kwargs) + pose[:, :3, :3] = measured_rotation + return pose + + service.robot.compute_fk = tilted_fk + service.port_yaw["left_arm"] = torch.tensor([math.pi / 2, -math.pi / 2]) + target = replace( + service.cfg.target("stage_free_end"), position=(0.586, 0.047, 0.815) + ) + pose = service.pose(target, "left_arm") + rotation = pose[:, :3, :3] + # The two reachable wrist branches point down with opposite channel yaw. + expected_rotation = torch.tensor( + [ + [[0.0, 1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, -1.0]], + [[0.0, -1.0, 0.0], [-1.0, 0.0, 0.0], [0.0, 0.0, -1.0]], + ] + ) + torch.testing.assert_close(rotation, expected_rotation) + torch.testing.assert_close( + rotation.transpose(1, 2) @ rotation, torch.eye(3).repeat(2, 1, 1) + ) + torch.testing.assert_close(torch.linalg.det(rotation), torch.ones(2)) + torch.testing.assert_close( + pose[:, :3, 3], torch.tensor([[0.586, 0.047, 0.815]]).repeat(2, 1) + ) + + +def test_seated_rows_preserve_contact_loading_command_while_other_rows_move(): + service, _, measured = _runtime() + commanded = measured.clone() + measured[3] = torch.tensor([0.585, 0.034, 0.781]) + measured[1] = torch.tensor([0.585, 0.033, 0.785]) + commanded[3] = torch.tensor([0.586, 0.035, 0.775]) + commanded[1] = torch.tensor([0.587, 0.034, 0.780]) + service.robot.get_qpos = lambda *, target=False: commanded if target else measured + service.seated = lambda: torch.tensor([True, False]) + target = replace( + service.cfg.target("lower_channel_3"), position=(None, None, 0.760) + ) + + pose = service.pose(target, "left_arm") + + # The loaded command remains intact for the seated row; the other row + # retains its commanded XY and receives the next requested height. + torch.testing.assert_close( + pose[:, :3, 3], + torch.tensor([[0.586, 0.035, 0.775], [0.587, 0.034, 0.760]]), + ) + service.cfg = replace(service.cfg, targets=(target,)) + lowerer = _CableMoveLowerer(service.cfg) + lowerer.runtime = service + endpoint = SimpleNamespace(runtime_target=SimpleNamespace(control_part="left_arm")) + bound = SimpleNamespace( + binding=SimpleNamespace( + resources={"primary": SimpleNamespace(endpoints={"motion": endpoint})} + ) + ) + result = lowerer.lower( + RegisteredSemanticCall( + call_id="simulation.cable_move", arguments={"target": target.target_id} + ), + context=SimpleNamespace(require_control_dt=lambda: 1 / 60), + bound=bound, + option_template=MoveEndEffectorOptions(), + ) + path = result.goal.xpos + # Every command for the seated row must retain its loaded pose, including + # the first interpolation sample. Returning to the measured pose unloads + # the contact even if the final sample restores the original command. + torch.testing.assert_close(path[0], pose[0].expand_as(path[0])) + torch.testing.assert_close(path[1, -1], pose[1]) + assert bool((path[1, :, 2, 3].diff() < 0).all()) + assert path[1, 0, 2, 3] < measured[1, 2] + assert path[1, 0, 2, 3] > commanded[1, 2] + + +def test_cable_tracking_rejects_wrong_orientation_at_same_position(): + service, _, _ = _runtime() + target = service.tool("left_arm") + target[:, :2, :2] *= -1 + service.last_targets["left_arm"] = target + lowerer = _CableCheckLowerer(service.cfg) + lowerer.runtime = service + endpoint = SimpleNamespace(runtime_target=SimpleNamespace(control_part="left_arm")) + bound = SimpleNamespace( + binding=SimpleNamespace( + resources={"primary": SimpleNamespace(endpoints={"motion": endpoint})} + ) + ) + call = RegisteredSemanticCall( + call_id="simulation.cable_check", + arguments={"target": "stage_free_end_tracking"}, + ) + with pytest.raises(ValueError, match="orientation_error_rad"): + lowerer.lower( + call, + context=SimpleNamespace(env_ids=service.env_ids), + bound=bound, + option_template=MoveJointsOptions(), + ) + + +@pytest.mark.parametrize("tracking_error", ["position", "orientation"]) +def test_release_alignment_above_channel_does_not_bypass_tcp_tracking(tracking_error): + service, _, _ = _runtime() + service.last_targets["left_arm"] = service.tool("left_arm") + # Both rows are aligned for release, but only the first is physically + # inside the channel. Both tools miss the 6 mm or 0.25 rad tracking limit. + if tracking_error == "position": + service.last_targets["left_arm"][:, 0, 3] += 0.007 + else: + service.last_targets["left_arm"][:, :2, :2] = torch.tensor( + [[math.cos(0.3), -math.sin(0.3)], [math.sin(0.3), math.cos(0.3)]] + ) + service.seated = lambda: torch.tensor([True, True]) + service.metrics = lambda: {"strict_port": torch.tensor([True, False])} + lowerer = _CableCheckLowerer(service.cfg) + lowerer.runtime = service + endpoint = SimpleNamespace(runtime_target=SimpleNamespace(control_part="left_arm")) + bound = SimpleNamespace( + binding=SimpleNamespace( + resources={"primary": SimpleNamespace(endpoints={"motion": endpoint})} + ) + ) + call = RegisteredSemanticCall( + call_id="simulation.cable_check", + arguments={"target": "lower_channel_3_tracking"}, + ) + with pytest.raises(ValueError, match=r"failed in envs \[1\]"): + lowerer.lower( + call, + context=SimpleNamespace(env_ids=service.env_ids), + bound=bound, + option_template=MoveJointsOptions(), + ) + # The hovered row can proceed once it meets the original tracking limit; + # the inserted row may retain its contact-loaded command. + if tracking_error == "position": + service.last_targets["left_arm"][1, 0, 3] = 0.005 + else: + service.last_targets["left_arm"][1, :2, :2] = torch.tensor( + [[math.cos(0.2), -math.sin(0.2)], [math.sin(0.2), math.cos(0.2)]] + ) + lowerer.lower( + call, + context=SimpleNamespace(env_ids=service.env_ids), + bound=bound, + option_template=MoveJointsOptions(), + ) + + +def test_cable_observation_rejects_invalid_quaternion_per_row(): + service, state, _ = _runtime() + state[1, :, 3:7] = 0 + assert service.observe().tolist() == [False, True] + + +def test_cable_session_scope_is_checked_before_cached_measurements(): + service, _, _ = _runtime() + service.simulation.simulation_time = 1.0 + service.metrics() + + def invalid_scope(): + raise RuntimeError("episode changed") + + service._scope_guard = invalid_scope + with pytest.raises(RuntimeError, match="episode changed"): + service.metrics() + + +def test_cable_factories_return_fresh_ready_lowerers(): + service, _, _ = _runtime() + factory = _CablePickFactory(service.cfg) + engine = SimpleNamespace( + robot=service.robot, + planning_services=SimpleNamespace(env_ids=(3, 1)), + device=torch.device("cpu"), + ) + first = factory.create( + simulation=service.simulation, + robot=service.robot, + scene_registry=None, + engine=engine, + ) + second = factory.create( + simulation=service.simulation, + robot=service.robot, + scene_registry=None, + engine=engine, + ) + assert first is not second and first.runtime is not second.runtime + assert first.runtime.env_ids.tolist() == [3, 1] diff --git a/tests/gym/envs/task_program/test_configured_integration.py b/tests/gym/envs/task_program/test_configured_integration.py index 7734a91582..e40f2bb332 100644 --- a/tests/gym/envs/task_program/test_configured_integration.py +++ b/tests/gym/envs/task_program/test_configured_integration.py @@ -4643,22 +4643,22 @@ def test_invalid_evaluation_deployment_fails_before_registration( assert config["id"] not in gym_registry -def test_legacy_task_program_identities_keep_pre_evaluation_values(): +def test_unrelated_task_program_identities_keep_pre_cable_program_values(): from embodichain.lab.task_program.integrations import task_program_fingerprint deployment = _deployment_from_path(_config_path("hand_over")) integration = deployment.integration registration = integration.registration assert "evaluation" not in registration.fingerprint_payload() - # Independently recomputed on upstream main ac714ec8 after its robot/profile - # migrations. The evaluation-free payload and program are unchanged here. + # Independently recomputed at 694ddda7d before the cable Task Program + # migration. Contact-capable PickUp had already changed the old main golden. assert ( registration.fingerprint - == "61cf38ce9128dfcc7063cec3933d900674863facbc6b244f31dec07ea24ca730" + == "0b5253d265d1d88dc165138e915f56f3596f52525c9ae620943f68a3ecc1d79f" ) assert ( integration.integration_fingerprint - == "d8d328c33f7149a5ed76b861bead1155977ed7f4ef213c9ceae560b36c20034e" + == "400ee8ebb00e416a8c9b783f5a681b7f9566da92cef4cbc55ec966cd5c20b2c4" ) compiled = registration.catalog.preflight(deployment.load_program()) assert ( diff --git a/tests/gym/envs/tasks/test_franka_rope_wrap.py b/tests/gym/envs/tasks/test_franka_rope_wrap.py new file mode 100644 index 0000000000..64bf397a01 --- /dev/null +++ b/tests/gym/envs/tasks/test_franka_rope_wrap.py @@ -0,0 +1,143 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""CPU regressions for the Franka task's timed acceptance and contact evidence.""" + +from __future__ import annotations + +import json +from types import SimpleNamespace +from unittest.mock import Mock + +import numpy as np +import pytest +import torch + +from embodichain_tasks.manipulation.cable.franka_rope_wrap import load_config +from embodichain_tasks.manipulation.cable.wrap_evaluation import attach_wrap_monitor +from embodichain_tasks.manipulation.cable.wrap_measurements import evaluate_wrap + +pytestmark = pytest.mark.no_sim + + +def test_scaled_pickup_and_final_hold_are_evaluated_at_their_measured_times(tmp_path): + path = tmp_path / "72s.json" + path.write_text(json.dumps({**load_config(), "duration_s": 72.0})) + config = load_config(path) + # At 2x duration the grasp finishes at 10 s, the lift at 14 s, and + # the final two seconds of the measured hold start at 70 s. + rows = [] + for frame in range(72 * 60 + 1): + t = frame / 60 + rows.append( + dict( + time_s=t, + finite=True, + arm_joint_limit_violation_rad=0, + finger_joint_limit_violation_m=0, + free_end_m=[0, 0, 0.06 if t >= 14 else 0.006], + grasp_confirmed=t >= 10, + bilateral_rope_contact=t >= 10, + tail_tcp_distance_m=0.025, + material_slip_m=0, + winding_deg=420 if t >= 70 else 0, + contact_arc_deg=330 if t >= 70 else 0, + rope_max_z_m=0.08, + rope_min_z_m=0.002, + post_penetration_m=0, + robot_post_penetration_m=0, + anchor_direction_error_deg=0, + anchor_exit_bend_max_deg=0, + anchor_error_m=0, + joint_gap_max_m=0, + dropped_contacts=0, + ) + ) + + report = evaluate_wrap(rows, config) + assert len(report["checks"]) == 19 and report["passed"] + partial = evaluate_wrap(rows[:-1], config) + assert not partial["checks"]["completed"] and not partial["passed"] + + # A bad measurement at the first hold frame cannot be hidden by recovery. + rows[70 * 60]["contact_arc_deg"] = 295 + failed = evaluate_wrap(rows, config) + assert {name for name, ok in failed["checks"].items() if not ok} == { + "post_contact_arc" + } + + +def test_grasp_requires_both_fingers_on_rope_in_the_same_sample(tmp_path): + config = {**load_config(), "grasp_complete_s": 0, "finger_pad_center_m": [0, 0, 0]} + env = Mock(num_envs=1, _wrap_monitor=None) + cable = env.sim.get_cable_object.return_value + points = np.zeros((9, 3)) + points[:, 2] = np.arange(9) * 0.01 + cable.cfg.points = points + segments = torch.zeros(1, 8, 13, dtype=torch.float64) + segments[0, :, :3] = torch.from_numpy((points[1:] + points[:-1]) / 2) + segments[0, :, 6] = 1 # identity quaternion; capsule axes point up + cable.data.segment_state_w = segments + tcp = torch.eye(4, dtype=torch.float64)[None] + tcp[0, :3, 3] = segments[0, -1, :3] + env.robot.get_link_pose.return_value = tcp + env.robot.get_qpos.return_value = torch.zeros(1, 9) + env.robot.joint_names = [f"j{i}" for i in range(9)] + urdf = tmp_path / "limits.urdf" + urdf.write_text( + "" + + "".join( + f'' + for name in env.robot.joint_names + ) + + "" + ) + motion = SimpleNamespace( + poses=np.tile(np.eye(4), (4, 1, 1)), + labels=["grasp"] * 4, + times=np.arange(4) / 60, + ) + sensor = env.sim.get_sensor.return_value + sensor.latest_sample_dropped_contacts = 0 + names = ["fr3_leftfinger", "fr3_rightfinger"] + sensor.get_actor_info.side_effect = lambda i: SimpleNamespace( + path="/franka", link_name=names[i] + ) + monitor = attach_wrap_monitor(env, config, motion, urdf) + + def advance(contacts): + monitor.begin_control_step() + for substep in range(1, 33): + fingers = contacts.get(substep, []) + sensor.get_data.return_value = { + "user_ids": torch.tensor( + [[i, -1] for i in fingers], dtype=torch.long + ).reshape(-1, 2), + "distance": torch.zeros(len(fingers)), + "position": torch.tensor([[0.004, 0.0, 0.075]]).repeat(len(fingers), 1), + "is_valid": torch.ones(len(fingers), dtype=torch.bool), + } + monitor.after_sample(1 / 1920, None) + return monitor.rows[-1] + + # Between samples both touch, but on the 240 Hz sample grid their + # contacts alternate. The fingers must not jointly confirm this grasp. + split = advance({3: [0, 1], 8: [0], 16: [1]}) + assert split["left_rope_contact"] and split["right_rope_contact"] + assert not split["bilateral_rope_contact"] and not split["grasp_confirmed"] + together = advance({24: [0, 1]}) + assert together["bilateral_rope_contact"] and together["grasp_confirmed"] + assert sensor.get_data.call_count == 8 diff --git a/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py b/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py new file mode 100644 index 0000000000..dd264f1e9d --- /dev/null +++ b/tests/gym/envs/tasks/test_franka_rope_wrap_scene.py @@ -0,0 +1,108 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Check that the packaged scene and the custom-scene deployment share one geometry.""" + +from __future__ import annotations + +import json +from pathlib import Path + +import numpy as np +import yaml +from dexsim.kit.usd import parse_usd + +from embodichain_tasks.manipulation.cable.franka_rope_wrap import ( + cable_points, + load_config, + materialize_deployment, + task_directory, +) + + +def test_default_deployment_matches_materialized_scene(tmp_path: Path) -> None: + root = task_directory() + defaults = load_config() + urdf = tmp_path / "derived.urdf" + alternate = materialize_deployment(defaults, urdf, tmp_path) + stored = yaml.safe_load((root / "env.yaml").read_text()) + compiled = yaml.safe_load((alternate.parent / "env.yaml").read_text()) + event = compiled["env"]["events"]["settle_rope"] + assert load_config(Path(event["params"].pop("scenario_path"))) == defaults + assert compiled == stored + assert compiled["enable_ground_plane"] is True + assert compiled["env"]["sim_steps_per_control"] == 32 + assert compiled["physics_config"]["num_substeps"] == 1 + np.testing.assert_allclose(compiled["physics_config"]["physics_dt"] * 32, 1 / 60) + scene = compiled["simulation"]["usd_scene"] + cable = scene["asset_cfgs"]["/World/cable"] + asset = alternate.parent / scene["source"]["file_path"] + assert asset.read_bytes() == (root / "cable.usda").read_bytes() + (desc,) = parse_usd(str(asset)).cables + assert desc.usd.prim_path == "/World/cable" + assert desc.enable_collision + material = desc.render_material + assert material.color_space == "linear" + original_srgb = np.array([0.93, 0.94, 0.92]) + np.testing.assert_allclose( + material.base_color[:3], ((original_srgb + 0.055) / 1.055) ** 2.4 + ) + np.testing.assert_allclose(material.roughness, 0.6) + np.testing.assert_allclose(desc.radius, defaults["cable_radius_m"]) + points = np.asarray(desc.points) + np.testing.assert_array_equal(points, cable_points(defaults)) + assert points.shape == (97, 3) + np.testing.assert_allclose( + points[[0, -1]], np.asarray(defaults["cable_waypoints_m"])[[0, -1]], atol=1e-7 + ) + delta = np.r_[defaults["post_center_xy_m"], points[0, 2]] - points[0] + np.testing.assert_allclose( + (points[1] - points[0]) / np.linalg.norm(points[1] - points[0]), + delta / np.linalg.norm(delta), + atol=1e-6, + ) + assert cable["fixed_segments"] == [0] + assert cable["collision_filter_span"] == 2 + + +def test_custom_scene_rescales_windows_and_uses_requested_assets( + tmp_path: Path, +) -> None: + scenario = load_config() + scenario.update(duration_s=72.0, floor_height_m=0.013, cable_segments=64) + custom = tmp_path / "custom.json" + custom.write_text(json.dumps(scenario)) + effective = load_config(custom) + assert (effective["grasp_complete_s"], effective["lift_complete_s"]) == (10.0, 14.0) + urdf = tmp_path / "padded_franka.urdf" + deployment = materialize_deployment(effective, urdf, tmp_path) + policy = yaml.safe_load(deployment.read_text())["task_program"]["execution_policy"] + # 20–56 s is a single continuous orbit, plus two control ticks to acknowledge it. + np.testing.assert_allclose( + policy["overrides"]["recovery"]["skill_timeout"], 36 + 2 / 60 + ) + env = yaml.safe_load(deployment.with_name("env.yaml").read_text()) + cylinder = next( + body for body in env["simulation"]["background"] if body["uid"] == "post" + ) + assert cylinder["init_pos"] == [0.48, 0.0, 0.073] + scene = env["simulation"]["usd_scene"] + (desc,) = parse_usd(str(deployment.parent / scene["source"]["file_path"])).cables + np.testing.assert_array_equal(desc.points, cable_points(effective)) + assert len(desc.points) == 65 + assert np.min(desc.points[:, 2]) >= 0.017 - 1e-8 + embodiment = yaml.safe_load(deployment.with_name("embodiment.yaml").read_text()) + assert embodiment["simulation"]["fpath"] == str(urdf) + assert embodiment["simulation"]["init_pos"] == [0, 0, 0.013] diff --git a/tests/gym/envs/tasks/test_route_the_cable.py b/tests/gym/envs/tasks/test_route_the_cable.py new file mode 100644 index 0000000000..773b5f6459 --- /dev/null +++ b/tests/gym/envs/tasks/test_route_the_cable.py @@ -0,0 +1,364 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations +from pathlib import Path +import xml.etree.ElementTree as ET +import math +import yaml +import pytest +import torch +from dexsim.kit.usd import parse_usd +from embodichain.lab.gym.utils.gym_utils import ( + add_env_launcher_args_to_parser, + build_env_cfg_from_args, + config_to_cfg, +) +from embodichain.compute.geometry._cable_route import _measure_cable_route + + +from embodichain.lab.sim.spawn.descriptors import rigid_desc_from_cfg +from embodichain.lab.sim.cfg import RigidObjectCfg +from embodichain.lab.sim.shapes import CylinderCfg + +pytestmark = pytest.mark.no_sim + + +def _route_geometry(): + path = ( + Path(__file__).resolve().parents[4] + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task_program/integration.yaml" + ) + return yaml.safe_load(path.read_text())["runtime_services"]["cable_material"][ + "geometry" + ] + + +def measure_route(state, lengths, hands, origins): + return _measure_cable_route( + state, lengths, hands, origins, _route_geometry(), 0.0035 + ) + + +@pytest.fixture(autouse=True) +def local_yam_config_asset(tmp_path: Path, monkeypatch: pytest.MonkeyPatch) -> Path: + """Keep configuration tests independent of external model publication.""" + from embodichain_robots.i2rt.yam import cfg as yam_i2rt + + robot = ET.Element("robot", name="yam_config_fixture") + ET.SubElement(robot, "link", name="base_link") + for side in ("right", "left"): + parent = "base_link" + for child, number in [ + (f"{side}_base", None), + *[(f"{side}_link{i}", i) for i in range(1, 7)], + (f"{side}_tcp", None), + ]: + ET.SubElement(robot, "link", name=child) + joint = ET.SubElement( + robot, + "joint", + name=f"{side}_joint{number}" if number else f"{child}_fixed", + type="revolute" if number else "fixed", + ) + ET.SubElement(joint, "parent", link=parent) + ET.SubElement(joint, "child", link=child) + if number: + ET.SubElement(joint, "axis", xyz="0 0 1") + ET.SubElement( + joint, "limit", lower="-3", upper="3", effort="27", velocity="3" + ) + parent = child + path = tmp_path / "yam_i2rt.urdf" + ET.ElementTree(robot).write(path, encoding="unicode") + + def resolve(name: str) -> str: + assert name == "YamI2rt/yam_i2rt.urdf" + return str(path) + + monkeypatch.setattr(yam_i2rt, "get_data_path", resolve) + return path + + +def test_route_deployment_composes_registered_robot_cable_and_board(): + p = ( + Path(__file__).resolve().parents[4] + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml" + ) + cfg = config_to_cfg(yaml.safe_load(p.read_text()), source_path=p) + assert cfg.robot.root_props.newton_gravity_compensation == 1.0 + (curve,) = parse_usd(cfg.usd_scene.source.file_path).cables + override = cfg.usd_scene.asset_cfgs[curve.usd.prim_path] + assert not cfg.cable_object + assert cfg.sim_cfg.enable_ground_plane is True + assert curve.usd.prim_path == "/World/cable" + assert override["uid"] == "cable" and override["fixed_segments"] == [0] + assert curve.radius == pytest.approx(0.0035) + assert curve.enable_collision + points = torch.from_numpy(curve.points) + torch.from_numpy(curve.pose[:3, 3]) + torch.testing.assert_close(points[:, 2], torch.full((51,), 0.7535)) + assert bool( + ( + (points[:, 0] >= 0.3025) + & (points[:, 0] <= 0.8975) + & (points[:, 1].abs() <= 0.65) + ).all() + ) + lengths = points.diff(dim=0).norm(dim=-1) + cumulative = lengths.cumsum(0) + distance = cumulative[-1] - 0.030 + index = int(torch.searchsorted(cumulative, distance)) + grasp = torch.lerp( + points[index], + points[index + 1], + (distance - cumulative[index - 1]) / lengths[index], + ) + torch.testing.assert_close( + grasp[:2], torch.tensor([0.42, 0.21]), atol=0.002, rtol=0 + ) + assert lengths.max() <= 0.0121 + assert lengths.min() > 0.0115 + assert len(cfg.background) == 11 + attrs = override["attrs"] + assert attrs["density"] == 350.0 + assert attrs["stretch_stiffness"] == 2e5 + assert attrs["material_props"]["kd"] == 0.1 + assert attrs["material_props"]["dynamic_friction"] == 60.0 + assert cfg.robot.link_attrs["pads"].attrs.material_props.dynamic_friction == 60.0 + assert all( + obj.attrs.material_props.dynamic_friction == 2.0 + for obj in cfg.background + if obj.uid.startswith("spool_") + ) + assert cfg.sim_cfg.physics_cfg.solver_cfg["coupling"] == "two_way" + assert cfg.sim_cfg.physics_cfg.solver_cfg["vbd_options"]["rigid_contact_history"] + assert cfg.robot.joint_drive_props.stiffness["right_joint[1-6]"] == 300.0 + assert cfg.robot.joint_drive_props.stiffness[".*_joint[78]"] == 6000.0 + assert cfg.robot.joint_drive_props.damping[".*_joint[78]"] == 120.0 + assert cfg.sim_cfg.render_cfg.renderer == "no-render" + assert not cfg.sim_cfg.visualization.allow_commands + assert cfg.robot.fpath is None + assert Path(cfg.robot.urdf_cfg.assemble_urdf()).is_file() + for side, chain in cfg.robot.build_pk_serial_chain().items(): + assert ( + len(chain.get_joint_parameter_names()) + == len(cfg.robot.control_parts[side]) + == 6 + ) + + +def test_route_viser_launcher_keeps_expert_deployment_read_only(): + import argparse + + path = ( + Path(__file__).resolve().parents[4] + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml" + ) + parser = argparse.ArgumentParser() + add_env_launcher_args_to_parser(parser, require_gym_config=True) + args = parser.parse_args(["--gym_config", str(path), "--viser"]) + + cfg, _, _ = build_env_cfg_from_args(args) + + assert cfg.sim_cfg.visualization.backend == "viser" + assert not cfg.sim_cfg.visualization.allow_commands + + +def _goal_state(): + state = torch.zeros(1, 50, 13) + state[..., 3] = -math.sqrt(0.5) + state[..., 6] = math.sqrt(0.5) + state[..., :3] = torch.tensor([0.42, 0, 0.7535]) + state[0, :2, :3] = torch.tensor([0.50, -0.26, 0.7535]) + state[0, 2:4, :3] = torch.tensor([0.55, -0.02, 0.7535]) + state[0, -3:, :3] = torch.tensor([0.586, 0.078, 0.7535]) + return state + + +def test_reference_goal_is_measured_and_rejects_moving_or_held_cable(): + state = _goal_state() + lengths = torch.full((50,), 0.012) + hands = torch.tensor([[[0, 0, 1], [0, 1, 1.0]]]) + origin = torch.zeros(1, 3) + assert measure_route(state, lengths, hands, origin)["reference_routed"].item() + moving = state.clone() + moving[0, 20, 7] = 0.051 + assert not measure_route(moving, lengths, hands, origin)["reference_routed"].item() + hands[:, 0] = state[:, 0, :3] + assert not measure_route(state, lengths, hands, origin)["reference_routed"].item() + + +def test_route_metrics_remove_arena_offsets_once(): + state = _goal_state().expand(2, -1, -1).clone() + origin = torch.tensor([[0, 0, 0], [3, 5, 0.0]]) + state[..., :3] += origin[:, None, :] + hands = ( + torch.tensor([[[0, 0, 1], [0, 1, 1.0]]]).expand(2, -1, -1) + origin[:, None, :] + ) + out = measure_route(state, torch.full((50,), 0.012), hands, origin) + for value in out.values(): + torch.testing.assert_close(value[0], value[1]) + + +def test_cylinder_uses_native_collision_geometry(): + desc, _ = rigid_desc_from_cfg( + RigidObjectCfg( + uid="spool", + shape=CylinderCfg(radius=0.0125, half_height=0.00125), + body_type="static", + ), + newton_solver_type="dexuni", + ) + assert desc.collisions[0].radius == 0.0125 + assert desc.collisions[0].half_height == 0.00125 + + +def test_cable_deployment_rejects_duplicate_component_ownership(): + p = ( + Path(__file__).resolve().parents[4] + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml" + ) + data = yaml.safe_load(p.read_text()) + data["usd_scene"] = {} + with pytest.raises(ValueError, match="usd_scene"): + config_to_cfg(data, source_path=p) + + +def test_strict_channel_rejects_end_that_only_meets_broad_region(): + state = _goal_state() + lengths = torch.full((50,), 0.012) + hands = torch.tensor([[[0, 0, 1], [0, 1, 1.0]]]) + origin = torch.zeros(1, 3) + assert measure_route(state, lengths, hands, origin)["strict_port"].item() + state[0, -1, 0] += 0.014 + metrics = measure_route(state, lengths, hands, origin) + assert metrics["reference_routed"].item() + assert not metrics["strict_port"].item() + + +def test_release_requires_seating_depth_even_when_tip_is_inside_strict_channel(): + geometry = _route_geometry() + geometry["release_region"] = [[0.585, 0.072, 0.753], [0.587, 0.083, 0.772]] + state = _goal_state().repeat(2, 1, 1) + # The last capsule points along +Y and has a 6 mm half length. Both tips + # are in the final channel. The shallow tip must not stop corrections; + # the deeper tip is seated near a sidewall, outside the hover corridor. + state[:, -1, :3] = torch.tensor([[0.586, 0.060, 0.755], [0.5846, 0.067, 0.757]]) + metrics = _measure_cable_route( + state, + torch.full((50,), 0.012), + torch.ones(2, 2, 3), + torch.zeros(2, 3), + geometry, + 0.0035, + ) + assert metrics["strict_port"].tolist() == [True, True] + assert metrics["release_ready"].tolist() == [False, True] + + +def test_above_channel_release_region_is_independent_of_final_strict_region(): + geometry = _route_geometry() + # An open-top placement may release from above the channel. This is an + # action precondition; the final endpoint must still be at or below .759 m. + geometry["release_region"] = [[0.585, 0.072, 0.753], [0.587, 0.083, 0.772]] + state = _goal_state().repeat(3, 1, 1) + state[:, -1, :3] = torch.tensor( + [[0.586, 0.071, 0.766], [0.586, 0.071, 0.773], [0.5846, 0.071, 0.766]] + ) + metrics = _measure_cable_route( + state, + torch.full((50,), 0.012), + torch.ones(3, 2, 3), + torch.zeros(3, 3), + geometry, + 0.0035, + ) + assert metrics["release_ready"].tolist() == [True, False, False] + assert metrics["strict_port"].tolist() == [False, False, False] + + +@pytest.mark.parametrize("invalid", ["nonfinite", "zero_quaternion"]) +def test_strict_seating_does_not_allow_release_with_invalid_cable_state(invalid): + geometry = _route_geometry() + geometry["release_region"] = [[0.585, 0.072, 0.753], [0.587, 0.083, 0.772]] + state = _goal_state() + state[0, -1, :3] = torch.tensor([0.5846, 0.067, 0.757]) + if invalid == "nonfinite": + state[0, 0, 0] = float("nan") + else: + state[0, 0, 3:7] = 0 + metrics = _measure_cable_route( + state, + torch.full((50,), 0.012), + torch.ones(1, 2, 3), + torch.zeros(1, 3), + geometry, + 0.0035, + ) + assert metrics["strict_port"].item() + assert not metrics["release_ready"].item() + + +@pytest.mark.parametrize("invalid", ["motion", "nonfinite", "held", "empty_seat"]) +def test_reference_metrics_reject_invalid_or_incomplete_observations(invalid): + state = _goal_state() + hands = torch.ones(1, 2, 3) + if invalid == "motion": + state[0, 0, 7] = 0.051 + elif invalid == "nonfinite": + state[0, 0, 0] = float("nan") + elif invalid == "held": + hands[0, 0] = state[0, -1, :3] + else: + state[0, :2, 0] = 0.42 + assert not measure_route(state, torch.full((50,), 0.012), hands, torch.zeros(1, 3))[ + "reference_routed" + ].item() + + +def test_contact_penetration_uses_the_configured_native_back_wall(): + p = ( + Path(__file__).resolve().parents[4] + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml" + ) + cfg = config_to_cfg(yaml.safe_load(p.read_text()), source_path=p) + wall = next(obj for obj in cfg.background if obj.uid == "port_back") + inner_y = wall.init_pos[1] - wall.shape.size[1] / 2 + (curve,) = parse_usd(cfg.usd_scene.source.file_path).cables + state = torch.zeros(1, 1, 13) + state[..., 6] = 1 + state[..., :3] = torch.tensor([0.586, inner_y - curve.radius + 0.002, 0.755]) + metrics = measure_route( + state, torch.tensor([0.001]), torch.ones(1, 2, 3), torch.zeros(1, 3) + ) + assert metrics["port_penetration"].item() == pytest.approx(0.002, abs=2e-7) + + +def test_unknown_cable_fields_are_rejected_by_production_loader(tmp_path): + p = ( + Path(__file__).resolve().parents[4] + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml" + ) + data = yaml.safe_load(p.read_text()) + physical = yaml.safe_load((p.parent / "env.yaml").read_text()) + scene = physical["simulation"]["usd_scene"] + scene["source"]["file_path"] = str(p.parent / "cable.usda") + scene["asset_cfgs"]["/World/cable"]["unknown_cable_option"] = True + component = tmp_path / "env.yaml" + component.write_text(yaml.safe_dump(physical)) + data["environment"]["component"] = str(component) + with pytest.raises((TypeError, ValueError), match="unknown_cable_option"): + config_to_cfg(data, source_path=p) diff --git a/tests/gym/envs/tasks/test_route_the_cable_cuda.py b/tests/gym/envs/tasks/test_route_the_cable_cuda.py new file mode 100644 index 0000000000..286962f8a0 --- /dev/null +++ b/tests/gym/envs/tasks/test_route_the_cable_cuda.py @@ -0,0 +1,410 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from collections.abc import Callable +import json +from pathlib import Path +import subprocess +import sys + +import pytest +import torch +import yaml + +from embodichain.lab.gym.utils.gym_utils import config_to_cfg +from embodichain.lab.sim import SimulationManager +import gymnasium as gym +import embodichain_tasks + +pytestmark = [ + pytest.mark.subprocess_sim, + pytest.mark.gpu, + pytest.mark.slow, + pytest.mark.skipif( + not torch.cuda.is_available(), reason="Cable routing requires CUDA" + ), + pytest.mark.usefixtures("requires_dexuni_two_way", "requires_dexuni_contact_query"), +] + + +def _material_runtime(env): + return env._active_task_program_bridge.post_policy_port._cable_runtime + + +class _SpoolReleaseCUDAScenario: + """Qualify the actual jaw sweep, rather than only cable goal predicates.""" + + def setup_method(self) -> None: + if not torch.cuda.is_available(): + pytest.skip("Cable routing requires CUDA") + path = ( + Path(__file__).resolve().parents[4] + / "extensions/embodichain_tasks/configs/tasks/manipulation/cable/route_the_cable/task.yam.yaml" + ) + cfg = config_to_cfg(yaml.safe_load(path.read_text()), source_path=path) + cfg.seed = 73 + cfg.sim_cfg.render_cfg.renderer = "no-render" + cfg.sim_cfg.robot_ik_gizmo = None + self.env = gym.make("RouteTheCable-YAM-v1", cfg=cfg).unwrapped + + def teardown_method(self) -> None: + if hasattr(self, "env"): + self.env.close(exit_process=False) + SimulationManager.flush_cleanup_queue() + + def test_first_spool_release_preserves_seating_without_gripper_penetration( + self, + record_property: Callable[[str, float], None], + ) -> None: + env = self.env + scene = env.sim.spawn_result + handles = scene.handles + robot_path = next(path for path in handles if path.endswith("/yam")) + obstacle_paths = [ + path for path in handles if "/spool_" in path or path.endswith("/bench") + ] + query = scene.create_contact_query( + [handles[robot_path], *(handles[path] for path in obstacle_paths)], + match="all", + capacity=4096, + device=env.device, + ) + obstacle_ids = torch.tensor( + [actor.actor_id for actor in query.actors if actor.path in obstacle_paths], + device=env.device, + ) + robot_ids = torch.tensor( + [actor.actor_id for actor in query.actors if actor.path == robot_path], + device=env.device, + ) + worst_distance = 0.0 + worst_segment = "" + env.reset() + for segment in env.create_demo_segments(): + for action in segment.actions: + env.step(action) + contacts = query.fetch_solver_report() + assert contacts.dropped_count == 0 + pairs = contacts.actor_ids[: contacts.count] + robot_obstacle = torch.isin(pairs, robot_ids).any(-1) & torch.isin( + pairs, obstacle_ids + ).any(-1) + distances = contacts.distance[: contacts.count][robot_obstacle] + if distances.numel(): + distance = float(distances.min()) + if distance < worst_distance: + worst_distance, worst_segment = distance, segment.name + assert bool(segment.validator().all()), segment.name + if segment.name == "open_clear_of_spool_0": + break + + # Finite cable states and seated cable alone previously accepted a jaw + # sweep through the rim with about 16 mm reported penetration. + # Include the bench when qualifying the deeper initial pinch. + record_property("first_spool_robot_obstacle_distance_m", worst_distance) + assert worst_distance >= -0.001, (worst_segment, worst_distance) + max_slip = float(_material_runtime(env).max_slip.max()) + record_property("max_grasp_slip_m", max_slip) + assert max_slip <= 0.012 + assert bool((_material_runtime(env).metrics()["seat_0"] >= 2).all()) + assert bool((_material_runtime(env).tool("right_arm")[:, 2, 3] > 0.81).all()) + finger_ids = env.robot.get_joint_ids("right_eef") + assert bool((env.robot.get_qpos()[:, finger_ids] > 0.045).all()) + + def test_third_carry_preserves_grasp_and_clears_spools( + self, record_property: Callable[[str, float], None] + ) -> None: + env = self.env + scene = env.sim.spawn_result + paths = [ + path for path in scene.handles if path.endswith("/yam") or "/spool_" in path + ] + query = scene.create_contact_query( + [scene.handles[path] for path in paths], + match="all", + capacity=4096, + device=env.device, + ) + robot_ids = torch.tensor( + [actor.actor_id for actor in query.actors if actor.path.endswith("/yam")], + device=env.device, + ) + spool_ids = torch.tensor( + [actor.actor_id for actor in query.actors if "/spool_" in actor.path], + device=env.device, + ) + third_segments = { + "raise_2", + "carry_2", + "lower_2", + "release_2", + "retreat_2", + "open_clear_of_spool_2", + } + worst_distance = 0.0 + max_slip = 0.0 + raised = False + env.reset() + for segment in env.create_demo_segments(): + for action in segment.actions: + env.step(action) + if segment.name in third_segments: + max_slip = max( + max_slip, + float(_material_runtime(env).max_slip.max()), + ) + contacts = query.fetch_solver_report() + assert contacts.dropped_count == 0 + pairs = contacts.actor_ids[: contacts.count] + robot_spool = torch.isin(pairs, robot_ids).any(-1) & torch.isin( + pairs, spool_ids + ).any(-1) + distances = contacts.distance[: contacts.count][robot_spool] + if distances.numel(): + worst_distance = min(worst_distance, float(distances.min())) + assert bool(segment.validator().all()), segment.name + if segment.name == "raise_2": + raised = True + assert _material_runtime(env).grasps["right_arm"][0] == 0.438 + # The gripped centre must clear the 23.5 mm rim plus cable radius. + clearance_height = ( + 0.75 + 0.0235 + env.sim.get_cable_object("cable").cfg.radius + ) + assert bool( + ( + _material_runtime(env).point(0.438)[0][:, 2] > clearance_height + ).all() + ) + if segment.name == "open_clear_of_spool_2": + assert raised + record_property("third_carry_slip_m", max_slip) + record_property("third_carry_robot_spool_distance_m", worst_distance) + assert max_slip <= 0.012 + assert worst_distance >= -0.001 + assert bool((_material_runtime(env).metrics()["seat_1"] >= 2).all()) + return + pytest.fail("The expert did not complete the third carry") + + def test_full_route_keeps_released_endpoint_in_channel( + self, record_property: Callable[[str, float], None] + ) -> None: + """Require the measured route to survive opening, retreat and settling.""" + env = self.env + assert env.num_envs == 1 + scene = env.sim.spawn_result + handles = scene.handles + robot_path = next(path for path in handles if path.endswith("/yam")) + obstacle_paths = [ + path + for path in handles + if path.endswith("/bench") + or path.rsplit("/", 1)[-1].startswith(("spool_", "port_")) + ] + assert any(path.endswith("/bench") for path in obstacle_paths) + assert any("/spool_" in path for path in obstacle_paths) + assert any("/port_" in path for path in obstacle_paths) + query = scene.create_contact_query( + [handles[robot_path], *(handles[path] for path in obstacle_paths)], + match="all", + capacity=4096, + device=env.device, + ) + robot_ids = torch.tensor( + [actor.actor_id for actor in query.actors if actor.path == robot_path], + device=env.device, + ) + obstacle_ids = torch.tensor( + [actor.actor_id for actor in query.actors if actor.path in obstacle_paths], + device=env.device, + ) + assert robot_ids.numel() and obstacle_ids.numel() + worst_distance = 0.0 + worst_segment = "" + required_boundaries = { + "release_free_end", + "retreat_free_end", + "settle_and_validate", + } + completed_boundaries: set[str] = set() + settling_steps = 0 + last_segment = None + env.reset() + for segment in env.create_demo_segments(): + last_segment = segment.name + segment_distance = 0.0 + for action in segment.actions: + env.step(action) + # The material goal cannot detect a finger penetrating the + # bench or port during the left-hand pinch and insertion. + contacts = query.fetch_solver_report() + assert contacts.dropped_count == 0, ( + segment.name, + contacts.dropped_count, + ) + pairs = contacts.actor_ids[: contacts.count] + robot_obstacle = torch.isin(pairs, robot_ids).any(-1) & torch.isin( + pairs, obstacle_ids + ).any(-1) + distances = contacts.distance[: contacts.count][robot_obstacle] + if distances.numel(): + segment_distance = min(segment_distance, float(distances.min())) + if segment_distance < worst_distance: + worst_distance, worst_segment = segment_distance, segment.name + record_property( + f"robot_obstacle_{segment.name}_distance_m", segment_distance + ) + record_property( + "full_route_robot_obstacle_distance_m", worst_distance + ) + assert worst_distance >= -0.001, (worst_segment, worst_distance) + if segment.name == "settle_and_validate": + settling_steps += 1 + measurements = _material_runtime(env).metrics() + endpoint = measurements["end_position"][0] + # Keep the most recent endpoint even when an earlier stage fails. + # Named boundaries distinguish release-induced escape from drift + # during retreat or the final settling interval. + for axis, value in zip("xyz", endpoint, strict=True): + record_property(f"last_endpoint_{axis}_m", float(value)) + if segment.name in required_boundaries: + record_property(f"{segment.name}_endpoint_{axis}_m", float(value)) + record_property( + "max_grasp_slip_m", float(measurements["max_grasp_slip"].max()) + ) + assert bool(segment.validator().all()), ( + segment.name, + { + key: value.detach().cpu().tolist() + for key, value in measurements.items() + }, + ) + if segment.name in required_boundaries: + completed_boundaries.add(segment.name) + + assert completed_boundaries == required_boundaries + assert last_segment == "settle_and_validate" + record_property("full_route_robot_obstacle_distance_m", worst_distance) + # The deployment's 120 control frames at 60 Hz must include a real + # two-second physical settling interval after release and retreat. + settling_seconds = settling_steps * env.step_dt + record_property("settling_seconds", settling_seconds) + assert settling_seconds >= 2.0 - 1e-9 + + measurements = _material_runtime(env).metrics() + endpoint = measurements["end_position"] + # Independent physical channel bounds from the task acceptance + # contract: loosening the configured evaluator must not pass this case. + lower = endpoint.new_tensor((0.5845, 0.064, 0.753)) + upper = endpoint.new_tensor((0.5875, 0.0896, 0.759)) + assert bool(((endpoint >= lower) & (endpoint <= upper)).all()), endpoint + assert bool((measurements["max_grasp_slip"] <= 0.012).all()) + for metric in ( + "bench_penetration", + "spool_penetration", + "port_penetration", + ): + record_property(metric + "_m", float(measurements[metric].max())) + assert bool((measurements[metric] <= 0.001).all()), ( + metric, + measurements[metric], + ) + success = env.is_task_success() + record_property("task_success", float(success.all())) + assert bool(success.all()), measurements + + +def _run_spool_case(case_name: str, metrics_path: Path) -> None: + """Execute one unchanged acceptance case in its own NoRender world.""" + scenario = _SpoolReleaseCUDAScenario() + properties: dict[str, float] = {} + try: + scenario.setup_method() + getattr(scenario, case_name)(properties.__setitem__) + finally: + try: + scenario.teardown_method() + finally: + metrics_path.write_text(json.dumps(properties), encoding="utf-8") + + +def _assert_spool_case_in_subprocess( + case_name: str, + tmp_path: Path, + record_property: Callable[[str, float], None], +) -> None: + """Preserve acceptance metrics without initializing pytest's native world.""" + metrics_path = tmp_path / "acceptance_metrics.json" + script = ( + "import runpy, sys; from pathlib import Path; " + "module = runpy.run_path(sys.argv[1]); " + "module['_run_spool_case'](sys.argv[2], Path(sys.argv[3]))" + ) + result = subprocess.run( + [ + sys.executable, + "-c", + script, + str(Path(__file__).resolve()), + case_name, + str(metrics_path), + ], + capture_output=True, + text=True, + timeout=1800, + ) + if metrics_path.exists(): + for name, value in json.loads(metrics_path.read_text(encoding="utf-8")).items(): + record_property(name, value) + assert result.returncode == 0, result.stdout + result.stderr + + +class TestSpoolReleaseCUDA: + """Qualify the physical jaw sweep in a fresh NoRender process per case.""" + + def test_first_spool_release_preserves_seating_without_gripper_penetration( + self, + tmp_path: Path, + record_property: Callable[[str, float], None], + ) -> None: + _assert_spool_case_in_subprocess( + "test_first_spool_release_preserves_seating_without_gripper_penetration", + tmp_path, + record_property, + ) + + def test_third_carry_preserves_grasp_and_clears_spools( + self, + tmp_path: Path, + record_property: Callable[[str, float], None], + ) -> None: + _assert_spool_case_in_subprocess( + "test_third_carry_preserves_grasp_and_clears_spools", + tmp_path, + record_property, + ) + + def test_full_route_keeps_released_endpoint_in_channel( + self, + tmp_path: Path, + record_property: Callable[[str, float], None], + ) -> None: + _assert_spool_case_in_subprocess( + "test_full_route_keeps_released_endpoint_in_channel", + tmp_path, + record_property, + ) diff --git a/tests/gym/envs/test_cable_recording_cuda.py b/tests/gym/envs/test_cable_recording_cuda.py index 81c39b5eae..41fd6750a0 100644 --- a/tests/gym/envs/test_cable_recording_cuda.py +++ b/tests/gym/envs/test_cable_recording_cuda.py @@ -30,7 +30,7 @@ pytestmark = [pytest.mark.subprocess_sim, pytest.mark.gpu] -def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( +def test_usd_and_procedural_cables_record_and_replay_across_arenas( tmp_path: Path, ) -> None: # A fresh process preserves NoRender without the shared test engine's @@ -44,8 +44,32 @@ def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( """, encoding="utf-8", ) + usd = tmp_path / "wire.usda" + usd.write_text( + """#usda 1.0 +( + defaultPrim = "World" + metersPerUnit = 1 + upAxis = "Z" +) + +def Xform "World" +{ + def BasisCurves "wire" ( + prepend apiSchemas = ["PhysicsCurvesDeformableSimAPI"] + ) + { + int[] curveVertexCounts = [3] + point3f[] points = [(0, 0, 1), (0.1, 0, 1), (0.2, 0, 1)] + uniform token type = "linear" + uniform token wrap = "nonperiodic" + } +} +""", + encoding="utf-8", + ) result = subprocess.run( - [sys.executable, str(Path(__file__).resolve()), str(urdf)], + [sys.executable, str(Path(__file__).resolve()), str(urdf), str(usd)], env=os.environ.copy(), capture_output=True, text=True, @@ -54,7 +78,7 @@ def test_cable_recording_keeps_t_plus_one_states_and_partial_reset( assert result.returncode == 0, result.stdout + result.stderr -def _run_recording(urdf: str) -> None: +def _run_recording(urdf: str, usd: str) -> None: import torch from embodichain.lab.gym.envs import EmbodiedEnv, EmbodiedEnvCfg @@ -66,10 +90,13 @@ def _run_recording(urdf: str) -> None: NewtonPhysicsCfg, RenderCfg, RobotCfg, + UsdImportCfg, + UsdSourceCfg, ) cfg = EmbodiedEnvCfg( num_envs=2, + enable_ground_plane=True, max_episode_steps=4, sim_steps_per_control=1, ignore_terminations=True, @@ -90,6 +117,11 @@ def _run_recording(urdf: str) -> None: solver_cfg={"solver_type": "dexuni"}, ), ), + usd_scene=UsdImportCfg( + name="scene", + source=UsdSourceCfg(file_path=usd), + asset_cfgs={"/World/wire": {"uid": "wire", "collision_filter_span": 2}}, + ), robot=RobotCfg( uid="slider", fpath=urdf, @@ -98,12 +130,11 @@ def _run_recording(urdf: str) -> None: ), cable_object=[ CableObjectCfg( - uid=uid, - points=[[0.1 * index, 0, 0] for index in range(segments + 1)], + uid="long_wire", + points=[[0.1 * index, 0, 0] for index in range(4)], radius=0.005, - init_pos=(0.0, y, 1.0), + init_pos=(0.0, 0.5, 1.0), ) - for uid, segments, y in (("wire", 2, 0.0), ("long_wire", 3, 0.5)) ], observations={ "wire": ObservationCfg( @@ -121,11 +152,16 @@ def _run_recording(urdf: str) -> None: env = None try: env = EmbodiedEnv(cfg) + cable = env.usd_scene_import["/World/wire"] + assert cable is env.sim.get_cable_object("wire") + assert cable.cfg.collision_filter_span == 2 assert not env.sim.has_native_renderer obs, _ = env.reset(seed=73) manager = env.observation_manager assert manager is not None and manager.has_history cables = {uid: env.sim.get_cable_object(uid) for uid in cfg.trajectory_uids} + assert cables["wire"].data.num_segments == 2 + assert cables["long_wire"].data.num_segments == 3 fields = {uid: f"cables/{uid}/segment_state_w" for uid in cables} episodes = env.episode_context.episode_ids expected = { @@ -225,6 +261,12 @@ def _run_recording(urdf: str) -> None: assert saved["meta"]["arena_origins_w"] == [source_origin.tolist()] for uid, field in fields.items(): assert saved["raw_observations"]["fields"][field]["values"].shape[1] == 4 + torch.testing.assert_close( + saved["terminal_states"]["cables", uid, "segment_state_w"].cpu(), + latest.values[field][[1]], + rtol=0, + atol=0, + ) env.close(exit_process=False) env = None @@ -258,4 +300,4 @@ def _run_recording(urdf: str) -> None: if __name__ == "__main__": faulthandler.enable() faulthandler.dump_traceback_later(120) - _run_recording(sys.argv[1]) + _run_recording(sys.argv[1], sys.argv[2]) diff --git a/tests/gym/envs/test_demo.py b/tests/gym/envs/test_demo.py index 9e31a9e5a9..e071b2a99d 100644 --- a/tests/gym/envs/test_demo.py +++ b/tests/gym/envs/test_demo.py @@ -578,6 +578,17 @@ def _end_demo_segment_recording(self, result) -> None: self.segment_results.append(result) +def test_exact_step_limit_keeps_naturally_completed_episode() -> None: + env = _SegmentedEnv() + + result = execute_demo_episode(env, max_steps=3) + + assert env.actions == [1, 2, 3] + assert result.completed and result.all_success + assert result.truncated == (False,) + assert result.terminal_reason == "success" + + def test_execute_demo_episode_runs_lazy_segments_as_one_episode() -> None: """A task can plan the second object after the first segment executes.""" env = _SegmentedEnv() diff --git a/tests/lab/evaluation/test_rule_graph.py b/tests/lab/evaluation/test_rule_graph.py index 297fe938fa..df3e2dbe49 100644 --- a/tests/lab/evaluation/test_rule_graph.py +++ b/tests/lab/evaluation/test_rule_graph.py @@ -659,3 +659,27 @@ def test_replay_preserves_row_ends_and_rejects_missing_endpoint(): assert beyond.step == 10 assert beyond.revisions.tolist() == [-1, -1] assert beyond.rules["closed"].status.tolist() == [Status.UNAVAILABLE] * 2 + + +def test_scalar_range_preserves_open_boundary_and_missing_evidence(): + graph = builtin_predicates().compile( + EvaluationSpec( + { + "clear": PredicateSpec( + "scalar_range", + {"value": "clearance"}, + {"minimum": 0.05, "maximum": 100, "minimum_inclusive": False}, + ) + }, + ("clear",), + ), + field_kinds={"clearance": "scalar"}, + ) + result = graph.evaluate( + {"clearance": torch.tensor([[0.05], [0.051], [float("nan")]])}, batch_size=3 + ) + assert result["clear"].status.tolist() == [ + Status.FAILED, + Status.SATISFIED, + Status.UNAVAILABLE, + ] diff --git a/tests/sim/atomic_skills/test_actions.py b/tests/sim/atomic_skills/test_actions.py index 9a6f62561c..d9f688b824 100644 --- a/tests/sim/atomic_skills/test_actions.py +++ b/tests/sim/atomic_skills/test_actions.py @@ -945,7 +945,9 @@ def _plan_segment_contract_case(case_id: str) -> SkillPlan: def test_builtin_descriptors_expose_strict_goal_types() -> None: assert MoveEndEffector.GoalType is EndEffectorPoseGoal assert MoveJoints.GoalType is JointPositionGoal - assert PickUp.GoalType is GraspGoal + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + + assert PickUp.GoalType == (GraspGoal, ContactGraspGoal) assert AxisAlign.GoalType is AxisAlignGoal assert MoveHeldObject.GoalType is HeldObjectPoseGoal assert Place.GoalType == (PlaceGoal, AssembleGoal) @@ -2333,7 +2335,12 @@ def test_pick_holds_fully_closed_before_lifting_when_configured() -> None: ) -def test_pick_combined_motion_gen_preserves_backend_samples_before_split() -> None: +@pytest.mark.parametrize("contact", [False, True]) +def test_pick_combined_motion_gen_preserves_backend_samples_before_split( + contact, +) -> None: + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + sample_count = 20 backend_sample_count = 6 generator = _motion_generator() @@ -2351,9 +2358,13 @@ def test_pick_combined_motion_gen_preserves_backend_samples_before_split() -> No action, SkillInvocation( skill_id="pick_up", - goal=GraspGoal( - semantics=_semantics(entity_id="target"), - grasp_xpos=torch.eye(4), + goal=( + ContactGraspGoal(torch.eye(4)) + if contact + else GraspGoal( + semantics=_semantics(entity_id="target"), + grasp_xpos=torch.eye(4), + ) ), binding=_binding(action), motion_policy=MotionPolicy( @@ -2370,6 +2381,10 @@ def test_pick_combined_motion_gen_preserves_backend_samples_before_split() -> No assert motion_options.plan_opts is None assert plan.commands.frame_count == sample_count + if contact: + assert plan.expected_effects.is_empty + assert plan.effect_verification.kind == "contact_grasp" + def test_pick_failed_candidate_does_not_delay_successful_grasp() -> None: positions = ( @@ -5393,7 +5408,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", @@ -9620,3 +9634,422 @@ def test_axis_align_rejects_invalid_alignment_offsets(offset: torch.Tensor) -> N def test_pickup_rejects_invalid_per_row_approach(direction: torch.Tensor) -> None: with pytest.raises(ValueError, match="approach_direction"): PickUpOptions(approach_direction=direction) + + +@pytest.mark.parametrize("settle_steps", [0, 3]) +def test_contact_pickup_preserves_other_arm_and_requires_physical_verification( + settle_steps, +): + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + + generator = _dual_motion_generator() + skill = _bind_action(generator, PickUp()) + context = _dual_context() + context.robot.qpos[:, ARM_DOF:DUAL_ARM_DOF] = 0.37 + before = context.robot.qpos.clone() + plan = _plan_action( + skill, + SkillInvocation( + skill_id="pick_up", + goal=ContactGraspGoal(torch.eye(4)), + binding=_binding(skill, motion="left_arm", grasp="left_hand"), + motion_policy=MotionPolicy( + sample_count=12, interpolate_position_step=0.05 * CONTROL_DT + ), + skill_options=PickUpOptions( + hand_interp_steps=4, + grasp_settle_steps=settle_steps, + lift_height=0.0185, + pre_grasp_distance=0, + ), + ), + context, + ) + assert plan.plan_success.all() + assert [segment.name for segment in plan.segments] == ["approach", "close", "lift"] + torch.testing.assert_close(context.robot.qpos, before) + positions = plan.joint_trajectory.positions + close = next(segment for segment in plan.segments if segment.name == "close") + assert close.stop - close.start == 4 + settle_steps + closed = positions[:, close.stop - settle_steps - 1 : close.stop] + torch.testing.assert_close(closed, closed[:, -1:].expand_as(closed)) + torch.testing.assert_close( + positions[:, :, ARM_DOF:DUAL_ARM_DOF], + before[:, None, ARM_DOF:DUAL_ARM_DOF].expand(-1, positions.shape[1], -1), + ) + assert plan.expected_effects.is_empty + assert plan.effect_verification.kind == "contact_grasp" + assert plan.requires_effect_verification + assert plan.scene_dependencies == () + projected = plan.expected_effects.apply(context.task, plan.plan_success) + assert projected.get_held_object("left_arm") is None + generator.robot.set_qpos.assert_not_called() + + +@pytest.mark.parametrize("pre_grasp_distance", [0.0, 0.03]) +def test_contact_pickup_initial_lift_precedes_approach( + pre_grasp_distance: float, +) -> None: + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + + generator = _dual_motion_generator() + generator.generate = Mock(wraps=generator.generate) + skill = _bind_action(generator, PickUp()) + plan = _plan_action( + skill, + SkillInvocation( + skill_id="pick_up", + goal=ContactGraspGoal(torch.eye(4)), + binding=_binding(skill, motion="left_arm", grasp="left_hand"), + motion_policy=MotionPolicy( + sample_count=12, interpolate_position_step=0.05 * CONTROL_DT + ), + skill_options=PickUpOptions( + hand_interp_steps=4, + initial_lift_height=0.08, + pre_grasp_distance=pre_grasp_distance, + lift_height=0.0185, + ), + ), + _dual_context(), + ) + + assert plan.plan_success.all() + calls = generator.generate.call_args_list + assert len(calls) == (4 if pre_grasp_distance else 3) + initial_lift_target = calls[0].args[0][-1].xpos + torch.testing.assert_close( + initial_lift_target[:, :3, 3], + torch.tensor([0.0, 0.0, 0.08]).expand(NUM_ENVS, -1), + ) + torch.testing.assert_close( + calls[-2].args[0][-1].xpos, torch.eye(4).repeat(NUM_ENVS, 1, 1) + ) + assert plan.requires_effect_verification + generator.robot.set_qpos.assert_not_called() + + +@pytest.mark.parametrize("occupied", ["held", "coordinated"]) +def test_contact_pickup_rejects_occupied_task_resource(occupied): + from embodichain.lab.sim.atomic_skills import ( + ContactGraspGoal, + CoordinatedHeldObjectState, + ) + + generator = _dual_motion_generator() + generator.generate = Mock(wraps=generator.generate) + skill = _bind_action(generator, PickUp()) + pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + task = TaskState( + batch_size=NUM_ENVS, + device="cpu", + held_objects=( + {"left_arm": HeldObjectState(_semantics(), pose, pose)} + if occupied == "held" + else {} + ), + coordinated_held_objects=( + { + ("left_arm", "right_arm"): CoordinatedHeldObjectState( + _semantics(), pose, pose, pose, pose + ) + } + if occupied == "coordinated" + else {} + ), + ) + with pytest.raises(ValueError, match="unoccupied"): + _plan_action( + skill, + SkillInvocation( + skill_id="pick_up", + goal=ContactGraspGoal(torch.eye(4)), + binding=_binding(skill, motion="left_arm", grasp="left_hand"), + ), + _dual_context(task), + ) + generator.generate.assert_not_called() + + +@pytest.mark.parametrize( + "bad_pose", [torch.zeros(3, 3), torch.full((4, 4), float("nan"))] +) +def test_contact_pickup_rejects_invalid_goal_pose(bad_pose): + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + + with pytest.raises(ValueError): + ContactGraspGoal(bad_pose) + + +@pytest.mark.parametrize("step", [0.0, -0.01, float("nan"), float("inf"), None, True]) +def test_motion_policy_rejects_invalid_cartesian_sampling_distance(step): + with pytest.raises(ValueError, match="interpolate_position_step"): + MotionPolicy(interpolate_position_step=step) + + +def test_contact_pickup_external_motion_uses_same_verification_contract(): + from embodichain.lab.sim.atomic_skills import ( + ContactGraspGoal, + InstantiatedSkillMotion, + RecoveryPolicy, + ) + + generator = _motion_generator() + generator.robot.num_instances = 1 + get_qpos = generator.robot.get_qpos.side_effect + generator.robot.get_qpos.side_effect = lambda name=None: get_qpos(name)[:1] + generator.robot.get_qpos_limits.return_value = torch.tensor([-2.0, 2.0]).repeat( + 1, ARM_DOF, 1 + ) + generator.robot.get_qvel_limits.side_effect = None + generator.robot.get_qvel_limits.return_value = torch.full((1, ARM_DOF), 10.0) + generator.generate = Mock(wraps=generator.generate) + skill = _bind_action(generator, PickUp()) + qpos = torch.zeros(1, ROBOT_DOF) + context = PlanningContext( + robot=RobotObservation(timestamp=0.0, qpos=qpos, qvel=qpos.clone()), + task=TaskState.empty(1, "cpu"), + scene=SceneSnapshot.empty(), + env_ids=torch.tensor([0]), + control_dt=CONTROL_DT, + ) + hand_ids = list(generator.robot.get_joint_ids("hand")) + + class Source: + def instantiate(self, request, current): + positions = current.robot.qpos[:, None].repeat(1, 6, 1) + positions[:, 3:, hand_ids] = 1 + return InstantiatedSkillMotion( + trajectory=TimedTrajectory.from_uniform_step( + positions, env_ids=current.env_ids, step_dt=CONTROL_DT + ), + success=torch.ones(1, dtype=torch.bool), + segment_lengths={"approach": 2, "close": 2, "lift": 2}, + initial_qpos=current.robot.qpos, + source_id="contact_fixture", + source_revision="1", + unit_id="pickup", + origin="test", + grasp_pose=torch.eye(4).repeat(1, 1, 1), + joint_names=tuple(generator.robot.joint_names), + ) + + invocation = SkillInvocation( + skill_id="pick_up", + goal=ContactGraspGoal(torch.eye(4)), + binding=_binding(skill), + skill_options=PickUpOptions(lift_height=0), + recovery_policy=RecoveryPolicy(max_replans=0, max_skill_retries=0), + ) + plan = _ACTION_ENGINES[id(skill)].plan(invocation, context, nominal_source=Source()) + assert plan.plan_success.all() + assert plan.expected_effects.is_empty + assert plan.effect_verification.kind == "contact_grasp" + assert plan.requires_effect_verification + generator.generate.assert_not_called() + + +def test_contact_pickup_keeps_failed_rows_at_observed_state(): + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + + generator = _dual_motion_generator() + original_ik = generator.robot.compute_ik.side_effect + + def fail_second_row(*args, **kwargs): + valid, qpos = original_ik(*args, **kwargs) + valid[1] = False + return valid, qpos + + generator.robot.compute_ik.side_effect = fail_second_row + skill = _bind_action(generator, PickUp()) + context = _dual_context() + plan = _plan_action( + skill, + SkillInvocation( + skill_id="pick_up", + goal=ContactGraspGoal(torch.eye(4).repeat(NUM_ENVS, 1, 1)), + binding=_binding(skill, motion="left_arm", grasp="left_hand"), + skill_options=PickUpOptions(pre_grasp_distance=0), + ), + context, + ) + assert plan.plan_success.tolist() == [True, False] + torch.testing.assert_close( + plan.joint_trajectory.positions[1], + context.robot.qpos[1].expand(plan.joint_trajectory.waypoint_count, -1), + ) + + +def test_contact_pickup_monitors_late_bound_pose_until_acquisition(): + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + + generator = _dual_motion_generator() + skill = _bind_action(generator, PickUp()) + pose = torch.eye(4).repeat(NUM_ENVS, 1, 1) + context = _dual_context(scene=_target_scene(pose, timestamp=0.0, version=0)) + plan = _plan_action( + skill, + SkillInvocation( + skill_id="pick_up", + goal=ContactGraspGoal(SceneEntityPose("target")), + binding=_binding(skill, motion="left_arm", grasp="left_hand"), + skill_options=PickUpOptions(pre_grasp_distance=0), + ), + context, + ) + assert plan.scene_dependencies == ("target",) + assert ( + plan.scene_dependency_monitor_until["target"] == plan.segment("approach").stop + ) + + +def test_contact_pickup_propagates_nonprefix_environment_selection(): + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + + generator = _dual_motion_generator() + original_fk = generator.robot.compute_fk.side_effect + + def selected_fk(qpos, *, name, to_matrix, env_ids=None): + assert tuple(env_ids) == (1,) + return original_fk(qpos, name=name, to_matrix=to_matrix) + + generator.robot.compute_fk.side_effect = selected_fk + skill = _bind_action(generator, PickUp(), env_ids=(1,)) + full_context = _dual_context() + qpos = full_context.robot.qpos[[1]] + context = PlanningContext( + robot=RobotObservation(0.0, qpos, torch.zeros_like(qpos)), + task=TaskState.empty(1, "cpu"), + scene=SceneSnapshot.empty(), + env_ids=torch.tensor([1]), + control_dt=CONTROL_DT, + ) + plan = _plan_action( + skill, + SkillInvocation( + skill_id="pick_up", + goal=ContactGraspGoal(torch.eye(4)), + binding=_binding(skill, motion="left_arm", grasp="left_hand"), + skill_options=PickUpOptions(pre_grasp_distance=0), + ), + context, + ) + assert plan.plan_success.tolist() == [True] + assert plan.joint_trajectory.env_ids.tolist() == [1] + assert plan.joint_trajectory.positions.shape[0] == 1 + + +def test_contact_pickup_rejects_rigid_options_before_scene_lookup(): + from embodichain.lab.sim.atomic_skills import ContactGraspGoal + + generator = _dual_motion_generator() + generator.generate = Mock(wraps=generator.generate) + skill = _bind_action(generator, PickUp()) + with pytest.raises(ValueError, match="without rigid-object options"): + _plan_action( + skill, + SkillInvocation( + skill_id="pick_up", + goal=ContactGraspGoal(torch.eye(4)), + binding=_binding(skill, motion="left_arm", grasp="left_hand"), + skill_options=PickUpOptions( + downstream_object_target_poses=(SceneEntityPose("unavailable"),) + ), + ), + _dual_context(), + ) + generator.generate.assert_not_called() + + +def test_move_joints_preserves_authored_samples_and_cadence() -> None: + generator = _motion_generator() + action = _bind_action(generator, MoveJoints()) + samples = ( + torch.tensor([0.0, 0.001, 0.004, 0.005]) + .view(1, 4, 1) + .expand(NUM_ENVS, -1, ARM_DOF) + .clone() + ) + invocation = _invocation(action, JointPositionGoal(samples), sample_count=90) + invocation = replace( + invocation, skill_options=MoveJointsOptions(preserve_samples=True) + ) + plan = _plan_action(action, invocation, _context()) + torch.testing.assert_close(_joint_command_positions(plan, "arm"), samples) + assert _joint_trajectory(plan).duration.tolist() == pytest.approx( + [3 * CONTROL_DT] * NUM_ENVS + ) + + +def test_move_end_effector_preserves_authored_cartesian_samples() -> None: + generator = _motion_generator() + calls = [] + generate = generator.generate + + def trace(*args, **kwargs): + calls.append(kwargs["options"]) + return generate(*args, **kwargs) + + generator.generate = trace + action = _bind_action(generator, MoveEndEffector()) + poses = torch.eye(4).repeat(NUM_ENVS, 5, 1, 1) + invocation = replace( + _invocation(action, EndEffectorPoseGoal(poses), sample_count=90), + skill_options=MoveEndEffectorOptions(preserve_cartesian_samples=True), + ) + _plan_action(action, invocation, _context()) + assert calls[0].preserve_cartesian_samples + assert calls[0].sample_count == 6 + + +def test_presampled_joint_commands_cannot_bypass_motion_gen_backend(): + action = _bind_action(_motion_generator(), MoveJoints()) + invocation = replace( + _invocation(action, JointPositionGoal(torch.zeros(NUM_ENVS, 2, ARM_DOF))), + motion_policy=MotionPolicy(strategy="motion_gen"), + skill_options=MoveJointsOptions(preserve_samples=True), + ) + with pytest.raises(ValueError, match="requires strategy"): + _plan_action(action, invocation, _context()) + + +@pytest.mark.parametrize("preserve", [False, True]) +def test_timed_joint_trajectory_can_preserve_explicit_phase_boundary_velocities( + preserve: bool, +): + from embodichain.lab.sim.atomic_skills.policies import DynamicCollisionMode + + generator = _motion_generator() + generator.robot.control_parts["whole_robot"] = object() + action = _bind_action(generator, MoveJoints()) + context = _context() + position = ( + torch.tensor([0.003, 0.006, 0.009]) + .view(1, 3, 1) + .expand(NUM_ENVS, -1, ROBOT_DOF) + ) + velocity = torch.full_like(position, 0.18) + goal = JointPositionGoal( + TimedTrajectory( + positions=position, + velocities=velocity, + accelerations=None, + dt=torch.full((NUM_ENVS, 3), CONTROL_DT), + env_ids=context.env_ids, + ) + ) + plan = _plan_action( + action, + SkillInvocation( + skill_id="move_joints", + binding=_binding(action, motion="whole_robot"), + goal=goal, + motion_policy=MotionPolicy(dynamic_collision_mode=DynamicCollisionMode.OFF), + skill_options=MoveJointsOptions(preserve_velocity_samples=preserve), + ), + context, + ) + expected = velocity.clone() + if not preserve: + expected[:, 0] = expected[:, -1] = 0 + torch.testing.assert_close(_joint_trajectory(plan).velocities, expected) diff --git a/tests/sim/objects/test_cable_object.py b/tests/sim/objects/test_cable_object.py index d8a4d98b36..8c78e4cc6c 100644 --- a/tests/sim/objects/test_cable_object.py +++ b/tests/sim/objects/test_cable_object.py @@ -390,11 +390,10 @@ def add_cable(): source=UsdSourceCfg(file_path=str(_write_cable_usd(tmp_path))), ) ) - assert dict(imported) == {} + cable = imported["/Cable"] assert list(imported.resource_types.values()) == ["cable"] assert len(builder.cables) == 1 - assert not sim._cable_objects and not scene._assets - return None + return cable if cable_first: first = add_cable() diff --git a/tests/sim/objects/test_cable_object_cuda.py b/tests/sim/objects/test_cable_object_cuda.py index 2a25e8594f..7904112f56 100644 --- a/tests/sim/objects/test_cable_object_cuda.py +++ b/tests/sim/objects/test_cable_object_cuda.py @@ -226,6 +226,92 @@ def test_imported_particles_survive_rejected_cable(self, tmp_path: Path) -> None torch.testing.assert_close(reset[0], moved[0], rtol=0, atol=0) torch.testing.assert_close(reset[1], initial[1]) + def test_usd_cable_state_selective_reset_and_viser(self, tmp_path: Path) -> None: + from embodichain.lab.visualization import SceneExporter, VisualizationCfg + + path = tmp_path / "wire.usda" + path.write_text( + """#usda 1.0 +( + defaultPrim = "World" + metersPerUnit = 1 + upAxis = "Z" +) +def Xform "World" { + def BasisCurves "Wire" ( + prepend apiSchemas = ["PhysicsCurvesDeformableSimAPI"] + ) { + int[] curveVertexCounts = [3] + point3f[] points = [(0, 0, 0), (0.1, 0, 0), (0.3, 0, 0)] + uniform token type = "linear" + uniform token wrap = "nonperiodic" + float[] physics:thicknesses = [0.02] + token physics:thicknesses:elementType = "constant" + double3 xformOp:translate = (0.25, 0.5, 1) + uniform token[] xformOpOrder = ["xformOp:translate"] + } +} +""", + encoding="utf-8", + ) + imported = self.sim.add_usd( + UsdImportCfg(name="wire", source=UsdSourceCfg(file_path=str(path))) + ) + key = "/World/Wire" + cable = imported[key] + uid = imported.resource_uids[key] + assert self.sim.get_asset(uid) is self.sim.get_cable_object(uid) is cable + assert cable.num_instances == 2 and cable.data is None + + self.sim.prepare() + initial = cable.data.default_segment_state_w + assert initial.shape == (2, 2, 13) + expected_centres = initial.new_tensor([[0.3, 0.5, 1], [0.45, 0.5, 1]]) + torch.testing.assert_close( + initial[..., :3] - self.sim.arena_offsets[:, None, :], + expected_centres.expand(2, 2, 3), + atol=1e-6, + rtol=0, + ) + + self.sim.update(step=3, render_final_step=False) + moved = cable.data.segment_state_w + assert torch.all(moved[..., 2] < initial[..., 2]) + self.sim.reset_objects_state(env_ids=[1]) + reset = cable.data.segment_state_w + torch.testing.assert_close(reset[0], moved[0], rtol=0, atol=0) + torch.testing.assert_close(reset[1], initial[1], rtol=0, atol=0) + + state = reset[[1]].clone() + state[..., 0] += 0.125 + cable.write_segment_state_to_sim(state, env_ids=[1]) + torch.testing.assert_close( + cable.data.segment_state_w[0], moved[0], rtol=0, atol=0 + ) + torch.testing.assert_close(cable.data.segment_state_w[[1]], state) + + exporter = SceneExporter( + self.sim, VisualizationCfg(backend="viser", env_ids=[1]) + ) + manifest = exporter.build_manifest() + frame = exporter.capture(sim_step=3, sim_time=self.sim.simulation_time).frame + nodes = [node for node in manifest.nodes if node.kind == "cable_segment"] + assert len(nodes) == 2 + geometries = { + geometry.geometry_id: geometry for geometry in manifest.geometries + } + for segment_index, node in enumerate(nodes): + assert node.env_id == 1 + assert exporter.resolve_node_target(node.node_id) == (uid, "cable") + index = frame.node_ids.index(node.node_id) + pose = state[0, segment_index].cpu().numpy() + np.testing.assert_allclose(frame.positions[index], pose[:3], atol=1e-6) + np.testing.assert_allclose(frame.wxyz[index], pose[[6, 3, 4, 5]], atol=1e-6) + vertices = geometries[node.geometry_id].vertices + np.testing.assert_allclose( + np.ptp(vertices, axis=0), [0.02, 0.02, (0.12, 0.22)[segment_index]] + ) + def test_attached_articulation_expert_trajectory_repeats_after_reset( self, tmp_path: Path ) -> None: @@ -451,6 +537,11 @@ def test_imported_particles_survive_rejected_cable( "test_imported_particles_survive_rejected_cable", volume_particle_usd.parent ) + def test_usd_cable_state_selective_reset_and_viser(self, tmp_path: Path) -> None: + _assert_cable_case_in_subprocess( + "test_usd_cable_state_selective_reset_and_viser", tmp_path + ) + @pytest.mark.usefixtures("requires_dexuni_two_way") def test_attached_articulation_expert_trajectory_repeats_after_reset( self, tmp_path: Path diff --git a/tests/sim/spawn/test_usd_scene_overrides.py b/tests/sim/spawn/test_usd_scene_overrides.py index 6df102db09..91b3610182 100644 --- a/tests/sim/spawn/test_usd_scene_overrides.py +++ b/tests/sim/spawn/test_usd_scene_overrides.py @@ -18,6 +18,7 @@ from __future__ import annotations +from copy import deepcopy from types import SimpleNamespace from unittest.mock import MagicMock @@ -27,6 +28,9 @@ from pxr import Sdf, Usd, UsdGeom, UsdPhysics from dexsim.spawn import ( + CableDesc, + CablePhysicsDesc, + NewtonCollisionDesc, MeshObjectDesc, ConstraintDesc, RigidBodyPhysicsDesc, @@ -43,6 +47,7 @@ from embodichain.lab.sim.sim_manager import SimulationManager from embodichain.lab.sim.spawn.scene import SpawnScene from embodichain.lab.sim.cfg import UsdImportCfg, UsdSourceCfg +from embodichain.lab.sim.objects import CableObject pytestmark = pytest.mark.no_sim @@ -61,6 +66,7 @@ def _manager(): "_rigid_object_groups", "_deformable_objects", "_fluid_objects", + "_cable_objects", "_articulations", ): setattr(manager, name, {}) @@ -898,6 +904,11 @@ def test_curve_prim_resources_keep_all_uids_and_stable_source_keys( ) for namespace in (None, "copy"): manager = _manager() + manager.device = torch.device("cuda:0") + manager.physics = SimpleNamespace( + name="newton", solver_type="dexuni", supports_cables=True + ) + manager.sim_config.physics_cfg = SimpleNamespace(requires_grad=False) result = manager.add_usd( UsdImportCfg( name="scene", @@ -913,11 +924,81 @@ def test_curve_prim_resources_keep_all_uids_and_stable_source_keys( assert dict(result.resource_source_paths) == { key: "/World/Cable" for key in expected_keys } - assert len(result) == 0 + assert list(result) == expected_keys + for key in expected_keys: + cable = result[key] + uid = result.resource_uids[key] + assert isinstance(cable, CableObject) + assert cable is manager.get_cable_object(uid) is manager.get_asset(uid) + assert cable.is_declared and cable.data is None with pytest.raises(TypeError): result.resource_source_paths[expected_keys[0]] = "/Other" +def test_usd_cable_sparse_overrides_preserve_source_geometry_and_material(monkeypatch): + import dexsim.kit.usd + + source = _source() + key = "/World/Wire" + points = np.array([[0, 0, 1], [0.2, 0, 1], [0.55, 0, 1]], dtype=np.float32) + source.cables.append( + CableDesc( + name="wire", + points=points, + radius=0.015, + usd=UsdSourceDesc(prim_path=key), + physics=CablePhysicsDesc(density=321, twist_stiffness=7, bend_damping=2), + collision=NewtonCollisionDesc(mu=0.77), + ) + ) + monkeypatch.setattr(dexsim.kit.usd, "parse_usd", lambda *a, **k: deepcopy(source)) + manager = _manager() + manager.device = torch.device("cuda") + manager.physics = SimpleNamespace( + name="newton", solver_type="dexuni", supports_cables=True + ) + manager.sim_config.physics_cfg = SimpleNamespace(requires_grad=False) + recipe = UsdImportCfg( + name="scene", + source=UsdSourceCfg(file_path="scene.usda"), + asset_cfgs={key: {"fixed_segments": [2]}}, + ) + with pytest.raises(ValueError, match="fixed_segments"): + manager.add_usd(recipe) + manager._spawn_scene.builder.add_scene.assert_not_called() + + recipe.asset_cfgs[key] = { + "uid": "cable", + "fixed_segments": [0], + "collision_filter_span": 2, + "attrs": { + "stretch_stiffness": 1e5, + "bend_damping": 0, + "collision_props": {"contact_offset": 0.01, "rest_offset": 0.0}, + "material_props": {"backend": "newton", "ke": 10000, "kd": 0.1}, + }, + } + imported = manager.add_usd(recipe) + native = manager._spawn_scene.builder.add_scene.call_args.args[0].cables[0] + assert native.name == imported.resource_uids[key] == "cable" + facade = manager.get_cable_object("cable") + assert facade is imported[key] + np.testing.assert_array_equal(native.points, points) + assert native.radius == pytest.approx(0.015) + assert native.fixed_segments == facade.cfg.fixed_segments == (0,) + assert native.collision_filter_span == facade.cfg.collision_filter_span == 2 + for physics in (native.physics, facade.cfg.attrs): + assert physics.stretch_stiffness == 1e5 + assert physics.bend_damping == 0 + assert physics.density == 321 + assert physics.twist_stiffness == 7 + assert native.collision.mu == 0.77 + assert native.collision.margin == 0 + assert native.collision.gap == 0.01 + assert native.collision.ke == 10000 + assert native.collision.kd == 0.1 + + def test_portable_sparse_articulation_pose_matrix_is_decoded(monkeypatch): import json import dexsim.kit.usd