From d1b9f929ae5c84b60403d5ccf6c20474480a7f32 Mon Sep 17 00:00:00 2001 From: Chris von Csefalvay Date: Thu, 16 Jul 2026 10:27:04 -0600 Subject: [PATCH 1/6] feat(assets): add dVRK PSM configuration --- .../feat-dvrk-needle-pass.minor.rst | 4 + .../isaaclab_assets/robots/__init__.pyi | 2 + .../isaaclab_assets/robots/dvrk.py | 128 ++++++++++++++++++ .../isaaclab_assets/robots/dvrk_asset.py | 15 ++ 4 files changed, 149 insertions(+) create mode 100644 source/isaaclab_assets/changelog.d/feat-dvrk-needle-pass.minor.rst create mode 100644 source/isaaclab_assets/isaaclab_assets/robots/dvrk.py create mode 100644 source/isaaclab_assets/isaaclab_assets/robots/dvrk_asset.py diff --git a/source/isaaclab_assets/changelog.d/feat-dvrk-needle-pass.minor.rst b/source/isaaclab_assets/changelog.d/feat-dvrk-needle-pass.minor.rst new file mode 100644 index 000000000000..86689c415849 --- /dev/null +++ b/source/isaaclab_assets/changelog.d/feat-dvrk-needle-pass.minor.rst @@ -0,0 +1,4 @@ +Added +^^^^^ + +* Added the dVRK Patient Side Manipulator asset configuration. diff --git a/source/isaaclab_assets/isaaclab_assets/robots/__init__.pyi b/source/isaaclab_assets/isaaclab_assets/robots/__init__.pyi index 246bfc05fb07..332fed814238 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/__init__.pyi +++ b/source/isaaclab_assets/isaaclab_assets/robots/__init__.pyi @@ -19,6 +19,7 @@ __all__ = [ "CART_DOUBLE_PENDULUM_CFG", "CARTPOLE_CFG", "CASSIE_CFG", + "DVRK_PSM_CFG", "GR1T2_CFG", "GR1T2_HIGH_PD_CFG", "FRANKA_PANDA_CFG", @@ -76,6 +77,7 @@ from .anymal import ( from .cart_double_pendulum import CART_DOUBLE_PENDULUM_CFG from .cartpole import CARTPOLE_CFG from .cassie import CASSIE_CFG +from .dvrk import DVRK_PSM_CFG from .fourier import GR1T2_CFG, GR1T2_HIGH_PD_CFG from .franka import FRANKA_PANDA_CFG, FRANKA_PANDA_HIGH_PD_CFG, FRANKA_ROBOTIQ_GRIPPER_CFG from .fourbar_pole import FOURBAR_POLE_CFG diff --git a/source/isaaclab_assets/isaaclab_assets/robots/dvrk.py b/source/isaaclab_assets/isaaclab_assets/robots/dvrk.py new file mode 100644 index 000000000000..f26dba90a9ec --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/dvrk.py @@ -0,0 +1,128 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the dVRK Patient Side Manipulator (PSM). + +The following configuration is available: + +* :obj:`DVRK_PSM_CFG`: The fixed-base dVRK PSM surgical robot. + +Asset provenance: + +* Catalogue: `Isaac for Healthcare asset catalogue `__ +* Catalogue release: ``v0.6.0`` (tag commit ``bee7e9314bb8f1c78f7e178a7840d708eda9ffb1``) +* Content revision: ``c189487`` +* Source: ``https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/Healthcare/0.6.0/c189487/Robots/dVRK/PSM/psm.usd`` +* Catalogue licence: `Apache License 2.0 `__ + +The USD has SHA-256 ``5730339c3b806f17a5228c69b97464d0b3469888002f62fb23d9621f746347c8`` +and default prim ``/psm``. +""" + +import math + +from isaaclab_physx.sim.schemas import PhysxArticulationRootPropertiesCfg + +import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets.articulation import ArticulationCfg + +from .dvrk_asset import DVRK_PSM_USD_PATH +from .dvrk_asset import DVRK_PSM_USD_SHA256 as DVRK_PSM_USD_SHA256 + +## +# Asset metadata and articulation names +## + +DVRK_PSM_DEFAULT_PRIM_PATH = "/psm" +"""Default prim authored by the pinned dVRK PSM USD.""" + +DVRK_PSM_ARM_JOINT_NAMES = [ + "psm_yaw_joint", + "psm_pitch_end_joint", + "psm_main_insertion_joint", + "psm_tool_roll_joint", + "psm_tool_pitch_joint", + "psm_tool_yaw_joint", +] +"""Ordered names of the six PSM arm joints.""" + +DVRK_PSM_JAW_JOINT_NAMES = ["psm_tool_gripper1_joint", "psm_tool_gripper2_joint"] +"""Ordered names of the two PSM jaw joints.""" + +DVRK_PSM_TOOL_TIP_BODY_NAME = "psm_tool_tip_link" +"""Name of the PSM end-effector body.""" + +DVRK_PSM_JAW_BODY_NAMES = ["psm_tool_gripper1_link", "psm_tool_gripper2_link"] +"""Ordered names of the two PSM jaw collision bodies.""" + +DVRK_PSM_JAW_JOINT_LIMITS = ((-math.pi / 6.0, 0.0), (0.0, math.pi / 6.0)) +"""Joint limits [rad] of the pinned PSM jaws, in :data:`DVRK_PSM_JAW_JOINT_NAMES` order.""" + +DVRK_PSM_JAW_OPEN_POS = (-math.pi / 6.0, math.pi / 6.0) +"""Fully-open jaw endpoint [rad], in :data:`DVRK_PSM_JAW_JOINT_NAMES` order.""" + +DVRK_PSM_JAW_CLOSED_POS = (0.0, 0.0) +"""Fully-closed jaw endpoint [rad], in :data:`DVRK_PSM_JAW_JOINT_NAMES` order.""" + + +def _validate_jaw_endpoint(endpoint: tuple[float, float], name: str) -> None: + """Check a jaw command endpoint against the limits authored in the pinned USD.""" + for joint_name, value, (lower, upper) in zip( + DVRK_PSM_JAW_JOINT_NAMES, endpoint, DVRK_PSM_JAW_JOINT_LIMITS, strict=True + ): + if not lower <= value <= upper: + raise ValueError(f"{name} value for {joint_name} ({value}) is outside [{lower}, {upper}].") + + +_validate_jaw_endpoint(DVRK_PSM_JAW_OPEN_POS, "DVRK_PSM_JAW_OPEN_POS") +_validate_jaw_endpoint(DVRK_PSM_JAW_CLOSED_POS, "DVRK_PSM_JAW_CLOSED_POS") + +## +# Configuration +## + +DVRK_PSM_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=DVRK_PSM_USD_PATH, + # Contact sensors in a task can filter this reporting to DVRK_PSM_JAW_BODY_NAMES. + activate_contact_sensors=True, + articulation_props=PhysxArticulationRootPropertiesCfg( + fix_root_link=True, + solver_velocity_iteration_count=4, + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + "psm_yaw_joint": 0.0, + "psm_pitch_end_joint": 0.0, + "psm_main_insertion_joint": 0.12, + "psm_tool_roll_joint": 0.0, + "psm_tool_pitch_joint": 0.0, + "psm_tool_yaw_joint": 0.0, + "psm_tool_gripper1_joint": DVRK_PSM_JAW_OPEN_POS[0], + "psm_tool_gripper2_joint": DVRK_PSM_JAW_OPEN_POS[1], + }, + joint_vel={".*": 0.0}, + ), + actuators={ + "arm": ImplicitActuatorCfg( + joint_names_expr=DVRK_PSM_ARM_JOINT_NAMES, + stiffness=None, + damping=None, + effort_limit_sim=None, + velocity_limit_sim=None, + ), + "jaws": ImplicitActuatorCfg( + joint_names_expr=DVRK_PSM_JAW_JOINT_NAMES, + stiffness=None, + damping=None, + effort_limit_sim=None, + velocity_limit_sim=None, + ), + }, + soft_joint_pos_limit_factor=1.0, +) +"""Configuration of the fixed-base dVRK PSM with USD-authored drives and joint limits.""" diff --git a/source/isaaclab_assets/isaaclab_assets/robots/dvrk_asset.py b/source/isaaclab_assets/isaaclab_assets/robots/dvrk_asset.py new file mode 100644 index 000000000000..880d89c71c1b --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/dvrk_asset.py @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Import-safe provenance metadata for the pinned dVRK PSM asset.""" + +DVRK_PSM_USD_PATH = ( + "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/Healthcare/0.6.0/" + "c189487/Robots/dVRK/PSM/psm.usd" +) +"""Pinned Isaac for Healthcare dVRK PSM USD path.""" + +DVRK_PSM_USD_SHA256 = "5730339c3b806f17a5228c69b97464d0b3469888002f62fb23d9621f746347c8" +"""SHA-256 digest of the pinned dVRK PSM USD.""" From ad5e586f38a8cce90f627188d3fe3ad166a706b8 Mon Sep 17 00:00:00 2001 From: Chris von Csefalvay Date: Thu, 16 Jul 2026 10:27:21 -0600 Subject: [PATCH 2/6] feat(tasks): add contrib dVRK needle-pass environment --- CONTRIBUTORS.md | 1 + .../preflight_dvrk_needle_pass_assets.py | 36 + .../feat-dvrk-needle-pass.minor.rst | 5 + .../contrib/needle_pass/__init__.py | 6 + .../contrib/needle_pass/assets.py | 183 ++ .../contrib/needle_pass/config/__init__.py | 6 + .../needle_pass/config/dvrk/__init__.py | 17 + .../needle_pass/config/dvrk/ik_abs_env_cfg.py | 571 ++++++ .../contrib/needle_pass/mdp/__init__.py | 10 + .../contrib/needle_pass/mdp/__init__.pyi | 111 + .../contrib/needle_pass/mdp/actions.py | 263 +++ .../contrib/needle_pass/mdp/events.py | 66 + .../contrib/needle_pass/mdp/grasp_solver.py | 426 ++++ .../contrib/needle_pass/mdp/observations.py | 118 ++ .../contrib/needle_pass/mdp/rewards.py | 34 + .../contrib/needle_pass/mdp/terminations.py | 510 +++++ .../needle_pass/needle_pass_env_cfg.py | 420 ++++ .../isaaclab_tasks/utils/parse_cfg.py | 11 + .../needle_pass/test_dvrk_needle_pass.py | 1050 ++++++++++ .../test_dvrk_needle_pass_physics.py | 1785 +++++++++++++++++ .../test_dvrk_needle_pass_teleop_pipeline.py | 142 ++ 21 files changed, 5771 insertions(+) create mode 100644 scripts/tools/preflight_dvrk_needle_pass_assets.py create mode 100644 source/isaaclab_tasks/changelog.d/feat-dvrk-needle-pass.minor.rst create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/__init__.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/assets.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/config/__init__.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/config/dvrk/__init__.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/config/dvrk/ik_abs_env_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/__init__.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/__init__.pyi create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/actions.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/events.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/grasp_solver.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/observations.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/rewards.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/terminations.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/needle_pass_env_cfg.py create mode 100644 source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass.py create mode 100644 source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_physics.py create mode 100644 source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_teleop_pipeline.py diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index 455b3dbf2aa7..4046cfabc31a 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -72,6 +72,7 @@ Guidelines for modifications: * Cathy Y. Li * Cheng-Rong Lai * Chenyu Yang +* Chris von Csefalvay * Connor Smith * CY (Chien-Ying) Chen * David Leon diff --git a/scripts/tools/preflight_dvrk_needle_pass_assets.py b/scripts/tools/preflight_dvrk_needle_pass_assets.py new file mode 100644 index 000000000000..95b2411d83d8 --- /dev/null +++ b/scripts/tools/preflight_dvrk_needle_pass_assets.py @@ -0,0 +1,36 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Verify the remote assets pinned by the dVRK needle-pass task.""" + +from pathlib import Path +from runpy import run_path + +REPOSITORY_ROOT = Path(__file__).resolve().parents[2] +needle_assets = run_path(REPOSITORY_ROOT / "source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/assets.py") +dvrk_asset = run_path(REPOSITORY_ROOT / "source/isaaclab_assets/isaaclab_assets/robots/dvrk_asset.py") + +NEEDLE_ASSET = needle_assets["NEEDLE_ASSET"] +SUTURE_PAD_ASSET = needle_assets["SUTURE_PAD_ASSET"] +verify_remote_asset_sha256 = needle_assets["verify_remote_asset_sha256"] +DVRK_PSM_USD_PATH = dvrk_asset["DVRK_PSM_USD_PATH"] +DVRK_PSM_USD_SHA256 = dvrk_asset["DVRK_PSM_USD_SHA256"] + + +def main() -> None: + """Download and hash each task asset as an explicit online preflight.""" + + assets = ( + ("needle", NEEDLE_ASSET.url, NEEDLE_ASSET.sha256), + ("suture pad", SUTURE_PAD_ASSET.url, SUTURE_PAD_ASSET.sha256), + ("dVRK PSM", DVRK_PSM_USD_PATH, DVRK_PSM_USD_SHA256), + ) + for name, url, sha256 in assets: + verify_remote_asset_sha256(url, sha256) + print(f"Verified {name}: {sha256}") + + +if __name__ == "__main__": + main() diff --git a/source/isaaclab_tasks/changelog.d/feat-dvrk-needle-pass.minor.rst b/source/isaaclab_tasks/changelog.d/feat-dvrk-needle-pass.minor.rst new file mode 100644 index 000000000000..b8b5f59e95f5 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/feat-dvrk-needle-pass.minor.rst @@ -0,0 +1,5 @@ +Added +^^^^^ + +* Added the ``IsaacContrib-NeedlePass-dVRK-IK-Abs`` bimanual surgical + needle-pass environment with XR teleoperation. diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/__init__.py new file mode 100644 index 000000000000..f09e482759c3 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Initially donor-held, contact-driven bimanual needle-pass environments.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/assets.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/assets.py new file mode 100644 index 000000000000..ec2bcb7f3f97 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/assets.py @@ -0,0 +1,183 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Pinned Isaac for Healthcare assets and physical constants for needle pass. + +The assets are referenced directly from the public Isaac for Healthcare 0.6.0 +catalogue under the Apache License 2.0. No USD or mesh content is redistributed +with Isaac Lab. +""" + +from __future__ import annotations + +import hashlib +from dataclasses import dataclass +from functools import cache +from urllib.error import URLError +from urllib.request import Request, urlopen + +I4H_CATALOGUE_RELEASE = "0.6.0" +"""Isaac for Healthcare asset catalogue release.""" + +I4H_CATALOGUE_COMMIT = "bee7e9314bb8f1c78f7e178a7840d708eda9ffb1" +"""Commit referenced by the ``v0.6.0`` catalogue tag.""" + +I4H_CATALOGUE_LICENCE = "Apache-2.0" +"""SPDX identifier of the pinned catalogue's licence.""" + +I4H_CATALOGUE_LICENCE_URL = "https://github.com/isaac-for-healthcare/i4h-asset-catalog/blob/v0.6.0/LICENSE" +"""Licence text for the pinned catalogue tag.""" + +I4H_CONTENT_REVISION = "c189487" +"""Immutable content revision embedded in the public asset URLs.""" + +I4H_CONTENT_ROOT = ( + "https://omniverse-content-production.s3-us-west-2.amazonaws.com/" + f"Assets/Isaac/Healthcare/{I4H_CATALOGUE_RELEASE}/{I4H_CONTENT_REVISION}" +) + + +@dataclass(frozen=True, slots=True) +class I4HAssetPin: + """Remote I4H asset with a digest for the explicit online preflight.""" + + key: str + sha256: str + + @property + def url(self) -> str: + """Return the revisioned public catalogue URL.""" + + return f"{I4H_CONTENT_ROOT}/{self.key}" + + +@cache +def verify_remote_asset_sha256(url: str, expected_sha256: str) -> None: + """Fail closed unless a remote USD response matches its declared SHA-256. + + The preflight is cached per process, so cloned environments do not repeat + the request. It verifies response bytes rather than trusting the revision + text embedded in the remote path. + """ + + expected_sha256 = expected_sha256.lower() + if len(expected_sha256) != 64 or any(character not in "0123456789abcdef" for character in expected_sha256): + raise ValueError("expected asset digest must be a lowercase SHA-256 hex string") + digest = hashlib.sha256() + try: + with urlopen(Request(url, headers={"User-Agent": "IsaacLab-dVRK-asset-preflight"}), timeout=30) as response: + for chunk in iter(lambda: response.read(1024 * 1024), b""): + digest.update(chunk) + except URLError as error: + raise RuntimeError(f"unable to preflight pinned dVRK asset {url!r}") from error + actual_sha256 = digest.hexdigest() + if actual_sha256 != expected_sha256: + raise RuntimeError( + f"pinned dVRK asset digest mismatch for {url!r}: expected {expected_sha256}, got {actual_sha256}" + ) + + +NEEDLE_ASSET = I4HAssetPin( + key="Props/SutureNeedle/needle_sdf.usd", + sha256="2b317a61f93631a7192e7ed2839ef20f7a75c05aa5f84a3905696134a64f36d7", +) +"""Preferred dynamic SDF needle used by I4H surgical tasks.""" + +NEEDLE_CONVEX_FALLBACK_ASSET = I4HAssetPin( + key="Props/SutureNeedle/needle.usd", + sha256="dd6910d4e3b8cede984e66d1772c2a16aa21c5cd2f41c5e6f7c4dd8f6d754620", +) +"""Pinned fallback; the task does not select it without measured simulator evidence.""" + +SUTURE_PAD_ASSET = I4HAssetPin( + key="Props/SuturePad/suture_pad.usd", + sha256="1c6e4624097fbf8ffc49131539e9eec72d96c5cf68916fbe04eefff1e9522a51", +) + +# The 0.4 scale is the scale used by the pinned I4H surgical handover scene. +NEEDLE_SCALE = (0.4, 0.4, 0.4) + +# The aligned source bounds include the nested +0.05 m X transform authored +# beneath the rigid-body root. They were computed from the exact pinned USD's +# extents and provide a reviewable size oracle independent of a live episode. +NEEDLE_SOURCE_AABB_MIN_M = (-0.002297283822972465, -0.049757134369845066, -0.001107099094351224) +NEEDLE_SOURCE_AABB_MAX_M = (0.04786571530379667, 0.0491908637664369, 0.0030219009137550075) +NEEDLE_BODY_LOCAL_AABB_MIN_M = tuple( + coordinate * scale for coordinate, scale in zip(NEEDLE_SOURCE_AABB_MIN_M, NEEDLE_SCALE, strict=True) +) +NEEDLE_BODY_LOCAL_AABB_MAX_M = tuple( + coordinate * scale for coordinate, scale in zip(NEEDLE_SOURCE_AABB_MAX_M, NEEDLE_SCALE, strict=True) +) +NEEDLE_BODY_LOCAL_EXTENT_M = tuple( + maximum - minimum + for minimum, maximum in zip(NEEDLE_BODY_LOCAL_AABB_MIN_M, NEEDLE_BODY_LOCAL_AABB_MAX_M, strict=True) +) + +# The source USD does not author these values. They are immutable task inputs, +# declared before the analytical grasp calculation and never adapted from a +# live episode. The source volume was computed from the exact pinned/hash- +# checked SDF USD mesh at /Needle/Needle/Needle: 934 consistently oriented +# triangles, the authored parent +0.05 m X transform, and the signed tetrahedron +# (divergence-theorem) sum. Uniform scale changes volume by the scale product. +# A 316L surgical-steel density of 8000 kg/m^3 is an explicit task reference +# assumption; it is not authored I4H metadata or a measured task value. +NEEDLE_SOURCE_VOLUME_M3 = 2.085934204311373e-6 +NEEDLE_REFERENCE_DENSITY_KG_M3 = 8000.0 +NEEDLE_MASS_KG = ( + NEEDLE_SOURCE_VOLUME_M3 * NEEDLE_SCALE[0] * NEEDLE_SCALE[1] * NEEDLE_SCALE[2] * (NEEDLE_REFERENCE_DENSITY_KG_M3) +) + +# The same pinned mesh integration gives the source centre of mass below. It +# is scaled with the asset, but is not tuned or updated from a running episode. +# The supported CUDA PhysX lane separately verifies the resolved runtime COM. +NEEDLE_SOURCE_CENTRE_OF_MASS_M = (0.016004362454017, 0.001034805027302, 0.000955963914748) +NEEDLE_CENTRE_OF_MASS_BODY_LOCAL_M = tuple( + coordinate * scale for coordinate, scale in zip(NEEDLE_SOURCE_CENTRE_OF_MASS_M, NEEDLE_SCALE, strict=True) +) + +# The dry steel/steel friction coefficients are likewise declared reference +# assumptions, not values authored by I4H and not tuned or measured against +# this task. Restitution zero encodes a non-bouncing surgical tool assumption. +NEEDLE_STATIC_FRICTION = 0.74 +NEEDLE_DYNAMIC_FRICTION = 0.57 +NEEDLE_RESTITUTION = 0.0 +# The pinned PSM jaw material contains an anomalous dynamic coefficient of +# 10.0. Resolving the pair with ``max`` would make retention depend on that +# value rather than on the declared dry steel/steel model above. ``min`` has +# higher PhysX precedence than the jaw material's unauthored/default +# ``average`` mode and therefore resolves the pair to 0.74 static / 0.57 +# dynamic friction without modifying the shared robot asset. +NEEDLE_FRICTION_COMBINE_MODE = "min" +NEEDLE_RESTITUTION_COMBINE_MODE = "min" + +__all__ = [ + "I4H_CATALOGUE_COMMIT", + "I4H_CATALOGUE_LICENCE", + "I4H_CATALOGUE_LICENCE_URL", + "I4H_CATALOGUE_RELEASE", + "I4H_CONTENT_REVISION", + "I4H_CONTENT_ROOT", + "I4HAssetPin", + "NEEDLE_ASSET", + "NEEDLE_BODY_LOCAL_AABB_MAX_M", + "NEEDLE_BODY_LOCAL_AABB_MIN_M", + "NEEDLE_BODY_LOCAL_EXTENT_M", + "NEEDLE_CENTRE_OF_MASS_BODY_LOCAL_M", + "NEEDLE_CONVEX_FALLBACK_ASSET", + "NEEDLE_DYNAMIC_FRICTION", + "NEEDLE_FRICTION_COMBINE_MODE", + "NEEDLE_MASS_KG", + "NEEDLE_REFERENCE_DENSITY_KG_M3", + "NEEDLE_RESTITUTION", + "NEEDLE_RESTITUTION_COMBINE_MODE", + "NEEDLE_SCALE", + "NEEDLE_STATIC_FRICTION", + "NEEDLE_SOURCE_AABB_MAX_M", + "NEEDLE_SOURCE_AABB_MIN_M", + "NEEDLE_SOURCE_CENTRE_OF_MASS_M", + "NEEDLE_SOURCE_VOLUME_M3", + "SUTURE_PAD_ASSET", + "verify_remote_asset_sha256", +] diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/config/__init__.py new file mode 100644 index 000000000000..f1f5b5f9cc92 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/config/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Robot-specific configurations for needle pass.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/config/dvrk/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/config/dvrk/__init__.py new file mode 100644 index 000000000000..2b9c186f345c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/config/dvrk/__init__.py @@ -0,0 +1,17 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Register the dVRK absolute-IK needle-pass environment.""" + +import gymnasium as gym + +gym.register( + id="IsaacContrib-NeedlePass-dVRK-IK-Abs", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + kwargs={ + "env_cfg_entry_point": f"{__name__}.ik_abs_env_cfg:DVRKNeedlePassEnvCfg", + }, + disable_env_checker=True, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/config/dvrk/ik_abs_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/config/dvrk/ik_abs_env_cfg.py new file mode 100644 index 000000000000..c0ef1de4ab24 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/config/dvrk/ik_abs_env_cfg.py @@ -0,0 +1,571 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Bimanual dVRK PSM configuration for absolute world-frame needle pass.""" + +import math + +import numpy as np +from isaaclab_teleop import IsaacTeleopCfg + +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg, RigidObjectCfg +from isaaclab.controllers import DifferentialIKControllerCfg +from isaaclab.utils.configclass import configclass + +from isaaclab_assets.robots.dvrk import ( + DVRK_PSM_ARM_JOINT_NAMES, + DVRK_PSM_CFG, + DVRK_PSM_JAW_CLOSED_POS, + DVRK_PSM_JAW_JOINT_NAMES, + DVRK_PSM_JAW_OPEN_POS, + DVRK_PSM_TOOL_TIP_BODY_NAME, +) + +from ... import mdp +from ...needle_pass_env_cfg import HANDOFF_PHASE_CFG, NeedlePassEnvCfg + +# ``isaaclab_teleop`` defers the optional ``isaacteleop`` import until session +# startup. This marker lets the environment test suite classify the task as a +# teleoperation environment without making ordinary task discovery depend on +# the external package. +_TELEOP_AVAILABLE = True + +LEFT_PSM_ROOT_POS = (-0.149189, -0.124189, 0.161400) +RIGHT_PSM_ROOT_POS = (0.149189, -0.124189, 0.161400) +PSM_ROOT_ROT_XYZW = (0.0, 0.0, 0.0, 1.0) + +# Symmetric homes place both tools in the shared hand-off region. These values +# are kept with the root placements so retargeter and articulation homes cannot +# drift independently. +LEFT_TOOL_HOME_POS_W = (-0.025, 0.0, 0.060) +RIGHT_TOOL_HOME_POS_W = (0.025, 0.0, 0.060) +LEFT_TOOL_HOME_ROT_XYZW = (0.29235514, -0.40562109, 0.13872189, 0.85484281) +RIGHT_TOOL_HOME_ROT_XYZW = (0.29235514, 0.40562109, -0.13872189, 0.85484281) + +LEFT_WORKSPACE_LOWER = (-0.18, -0.16, 0.015) +LEFT_WORKSPACE_UPPER = (0.08, 0.16, 0.20) +RIGHT_WORKSPACE_LOWER = (-0.08, -0.16, 0.015) +RIGHT_WORKSPACE_UPPER = (0.18, 0.16, 0.20) + +LEFT_ARM_HOME = (0.886077124, -0.659058036, 0.200, 0.0, 0.0, 0.0) +RIGHT_ARM_HOME = (-0.886077124, -0.659058036, 0.200, 0.0, 0.0, 0.0) + +# The two grasp poses below are ``needle_channel_*`` (``T_N_C``) rows emitted +# by Isaac Sim 5.1's native antipodal grasp generator, with quaternions +# reordered from the generator's wxyz output to Isaac Lab's xyzw convention. +# ``N`` is the scaled needle body and ``C`` is the generated channel frame, +# whose +Z axis is the jaw-gap axis. These are not the mesh-local +# ``native_*`` fields: the asset's nested authored transform separates those +# frames by about 19 mm. Candidate selection never interpolated or +# geometrically altered poses; the only component reordering is the convention +# conversion above. The donor was selected only after a fixed native candidate +# was physically closed, settled with gravity enabled, and retained through a +# fixed tool disturbance. +ISAAC_GRASP_GENERATOR_EXTENSION = "isaacsim.replicator.grasping" +ISAAC_GRASP_GENERATOR_EXTENSION_VERSION = "1.0.9" +ISAAC_GRASP_GENERATOR_API = "isaacsim.replicator.grasping.GraspingManager.generate_grasp_poses" +ISAAC_GRASP_GENERATOR_SIM_VERSION = "5.1.0" +ISAAC_GRASP_GENERATOR_SEED = 12 +ISAAC_GRASP_GENERATOR_CANDIDATE_COUNT = 8192 +ISAAC_GRASP_GENERATOR_ORIENTATIONS_PER_CENTRE = 32 +ISAAC_GRASP_GENERATOR_CENTRE_COUNT = 256 +ISAAC_GRASP_ASSET_SHA256 = "2b317a61f93631a7192e7ed2839ef20f7a75c05aa5f84a3905696134a64f36d7" +ISAAC_GRASP_WRAPPER_SHA256 = "01bc820d1777a1655a5c42b3ebac997c6281335a12d12f7636c3e25721f3a2d5" +ISAAC_GRASP_CONFIG_SHA256 = "b308ec31bf9bf425c686007e0dc0ad72f09ae7e1f67e1015ac53dc92a017e798" +ISAAC_GRASP_CANDIDATES_SHA256 = "7c601982d72759ca901fad9b59fa1df80a092221d1cb91eda88938b2b83bc374" +ISAAC_GRASP_MANIFEST_SHA256 = "13c72a5fb58db7c211619b72dcbdf27890a25a35a5aa8e3185ab4ae3139970ee" + +DONOR_GRASP_CANDIDATE_INDEX = 2321 +DONOR_GRASP_T_N_C_POS_M = ( + 0.0003148044879752905, + 0.0033030783449336226, + 0.0003504408852082775, +) +DONOR_GRASP_T_N_C_ROT_XYZW = ( + -0.06216530304406737, + -0.299876591345807, + 0.06093660132734946, + 0.9499980187763187, +) +DONOR_GRASP_CONTACT_POINTS_N_M = ( + (0.0008196471425340884, 0.0032317539769975326, -0.0003599608352924837), + (-0.00019003816658350742, 0.0033744027128697148, 0.0010608426057090398), +) +DONOR_GRASP_OUTWARD_NORMALS_N = ( + (0.5773406198878056, -0.08156690886849895, -0.812419010120518), + (-0.6585541989377149, 0.12276505562071868, 0.7424520915048636), +) + +# Exact emitted candidate selected for the receiver trial. It is not a pose +# perturbation or interpolation. +RECEIVER_GRASP_CANDIDATE_INDEX = 51 +RECEIVER_GRASP_T_N_C_POS_M = ( + 0.0023141994825170917, + -0.009712701723612324, + 0.0004488201068876367, +) +RECEIVER_GRASP_T_N_C_ROT_XYZW = ( + -0.01452245833926486, + 0.36639395824675103, + 0.38287722060063434, + 0.8479089570874903, +) + +# Fixed collision-channel calibration ``T_T_C`` in psm_tool_tip_link. Its +# origin is the measured midpoint of the two active jaw collision volumes, +# not the tool-tip-link origin. The reset and receiver targets are +# deterministic compositions of this calibration with generated ``T_N_C`` +# poses and the measured donor-held acquisition pose; focused tests reconstruct +# all identities. +DVRK_JAW_CHANNEL_T_T_C_POS_M = (0.0, 0.0, 0.004) +DVRK_JAW_CHANNEL_T_T_C_ROT_XYZW = ( + 0.0, + 0.7071067811865476, + 0.0, + 0.7071067811865476, +) + +# The native transform is the acquisition seed. The public reset must start +# held, so it uses the gravity-on physical equilibrium below instead of a +# collision-free geometric approximation. This state has no attachment: it +# was measured after real jaw closure and is requalified by the CUDA test. +DONOR_GRASP_NATIVE_SEED_POS = (-0.02676631338705744, -0.00554576569408158, 0.06096145185775791) +DONOR_GRASP_NATIVE_SEED_ROT_XYZW = ( + 0.04784599008904877, + 0.5946084191654624, + 0.24809474119226224, + 0.7632827686095777, +) +NEEDLE_RESET_POS = (-0.024928808212280273, -0.0031707286834716797, 0.05836881697177887) +NEEDLE_RESET_ROT_XYZW = ( + 0.028613094240427017, + 0.6356675028800964, + 0.2438839077949524, + 0.7318665981292725, +) +# Candidate 51 was acquired against this donor-held pose after the fixed +# gravity-on reset settling trace. Keeping the measured acquisition pose +# explicit makes the fixed controller target reproducible without moving the +# free needle or hiding a state write in the action path. +RECEIVER_ACQUISITION_NEEDLE_POS_W = (-0.0245427893868402, -0.0031316915911458786, 0.058761484034582104) +RECEIVER_ACQUISITION_NEEDLE_ROT_XYZW = ( + 0.0413061008969846, + 0.613496097694513, + 0.24468171703916028, + 0.7496980735529877, +) +# Measured candidate-51 receiver-frame equilibrium after a guarded release +# from the donor. It is an acceptance target only and is never written into +# the simulation. +RECEIVER_NEEDLE_TARGET_POS_T = (0.0018065175972878933, 0.008040599524974823, -0.0016274424269795418) +RECEIVER_NEEDLE_TARGET_ROT_XYZW = ( + -0.25603383779525757, + 0.3513960838317871, + -0.31044724583625793, + 0.8453344702720642, +) +RECEIVER_TOOL_TARGET_POS_W = (-0.02371715009212494, -0.008205749094486237, 0.052002355456352234) +RECEIVER_TOOL_TARGET_ROT_XYZW = ( + 0.4864428639411926, + 0.3236384689807892, + 0.2469031661748886, + 0.7730914950370789, +) + +# The reset writes the observed equilibrium joint positions, then the normal +# action path drives both donor jaws fully closed. Separating state from drive +# target preserves the physically settled hold rather than forcing an +# interpenetrating fully-closed configuration at reset. +DONOR_HELD_RESET_JAW_POS = (-0.20328494906425476, 0.003166106529533863) +DONOR_GRASP_JAW_POS = DVRK_PSM_JAW_CLOSED_POS +DONOR_GRASP_CLOSEDNESS = 1.0 + +# The load-qualified donor acquisition used these bounded drives, derived from +# the Large Needle Driver reflected jaw inertia and specified torque/speed +# limits. The task uses the same drives for reset retention and teleoperation. +DVRK_NEEDLE_PASS_JAW_REFLECTED_INERTIA_KG_M2 = 3.32e-7 +# This setting is under deterministic CUDA end-to-end qualification. Torque +# and velocity limits remain fixed; 150 rad/s is the bounded midpoint between +# the stable-but-under-retained 120 rad/s setting and the unstable 200 rad/s +# setting. +DVRK_NEEDLE_PASS_JAW_NATURAL_FREQUENCY_RAD_S = 150.0 +DVRK_NEEDLE_PASS_JAW_DAMPING_RATIO = 1.0 +DVRK_NEEDLE_PASS_JAW_EFFORT_LIMIT_N_M = 0.16 +DVRK_NEEDLE_PASS_JAW_VELOCITY_LIMIT_RAD_S = 2.1 +DVRK_NEEDLE_PASS_JAW_ACTUATOR = ImplicitActuatorCfg( + joint_names_expr=list(DVRK_PSM_JAW_JOINT_NAMES), + stiffness=DVRK_NEEDLE_PASS_JAW_REFLECTED_INERTIA_KG_M2 * DVRK_NEEDLE_PASS_JAW_NATURAL_FREQUENCY_RAD_S**2, + damping=( + 2.0 + * DVRK_NEEDLE_PASS_JAW_DAMPING_RATIO + * DVRK_NEEDLE_PASS_JAW_REFLECTED_INERTIA_KG_M2 + * DVRK_NEEDLE_PASS_JAW_NATURAL_FREQUENCY_RAD_S + ), + effort_limit_sim=DVRK_NEEDLE_PASS_JAW_EFFORT_LIMIT_N_M, + velocity_limit_sim=DVRK_NEEDLE_PASS_JAW_VELOCITY_LIMIT_RAD_S, +) + +DVRK_HANDOFF_PHASE_CFG = HANDOFF_PHASE_CFG.replace( + # Candidate 51's measured receiver reaction axes remain 20.6 degrees from + # perfectly opposed during the guarded transfer. The 25-degree gate keeps + # 4.4 degrees of geometric margin while still requiring bilateral load and + # dwell before any donor opening command may pass. + opposed_normal_tolerance_rad=math.radians(25.0), + receiver_relative_position_target_m=RECEIVER_NEEDLE_TARGET_POS_T, + receiver_relative_orientation_target_xyzw=RECEIVER_NEEDLE_TARGET_ROT_XYZW, + receiver_relative_position_limit_m=0.003, + receiver_relative_orientation_limit_rad=math.radians(15.0), +) + + +def _joint_home(arm_home: tuple[float, ...], jaw_home: tuple[float, float]) -> dict[str, float]: + return { + **dict(zip(DVRK_PSM_ARM_JOINT_NAMES, arm_home, strict=True)), + **dict(zip(DVRK_PSM_JAW_JOINT_NAMES, jaw_home, strict=True)), + } + + +def _psm_cfg( + prim_path: str, + root_pos: tuple[float, float, float], + arm_home: tuple[float, ...], + jaw_home: tuple[float, float], +): + return DVRK_PSM_CFG.replace( + prim_path=prim_path, + init_state=ArticulationCfg.InitialStateCfg( + pos=root_pos, + rot=PSM_ROOT_ROT_XYZW, + joint_pos=_joint_home(arm_home, jaw_home), + joint_vel={".*": 0.0}, + ), + actuators={**DVRK_PSM_CFG.actuators, "jaws": DVRK_NEEDLE_PASS_JAW_ACTUATOR}, + ) + + +def _tool_home_transform( + position: tuple[float, float, float], orientation_xyzw: tuple[float, float, float, float] +) -> np.ndarray: + """Build the world-frame homogeneous tool-home transform.""" + quaternion = np.asarray(orientation_xyzw, dtype=np.float64) + norm = float(np.linalg.norm(quaternion)) + if not np.isfinite(norm) or norm <= np.finfo(np.float64).eps: + raise ValueError("dVRK tool-home orientation must be a finite non-zero quaternion") + x, y, z, w = quaternion / norm + + transform = np.eye(4, dtype=np.float64) + transform[:3, :3] = ( + (1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - z * w), 2.0 * (x * z + y * w)), + (2.0 * (x * y + z * w), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - x * w)), + (2.0 * (x * z - y * w), 2.0 * (y * z + x * w), 1.0 - 2.0 * (x * x + y * y)), + ) + transform[:3, 3] = position + return transform + + +def _build_dvrk_needle_pass_pipeline(): + """Build the bimanual world-frame dVRK IsaacTeleop pipeline. + + The flattened output follows the task action declaration exactly: + ``left pose (7), left jaws (2), right pose (7), right jaws (2)``. + IsaacTeleop supplies ``world_T_anchor`` at runtime, so both controller + streams, tool homes, and workspace bounds remain in the shared simulation + world frame expected by the task's world-frame differential-IK actions. + """ + try: + from isaacteleop.retargeters import ( + DVRKPSMClutchConfig, + DVRKPSMClutchRetargeter, + DVRKPSMGripperConfig, + DVRKPSMGripperRetargeter, + TensorReorderer, + ) + except ImportError as exc: + raise RuntimeError( + "dVRK needle-pass teleoperation requires the dVRK retargeters from NVIDIA/IsaacTeleop PR 769; " + "until they are released, run scripts/tools/install_isaacteleop_pr769_for_tests.sh" + ) from exc + from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource + from isaacteleop.retargeting_engine.interface import OutputCombiner, ValueInput + from isaacteleop.retargeting_engine.tensor_types import TransformMatrix + + side_configs = { + "left": { + "controller": ControllersSource.LEFT, + "home_position": LEFT_TOOL_HOME_POS_W, + "home_orientation": LEFT_TOOL_HOME_ROT_XYZW, + "workspace_lower": LEFT_WORKSPACE_LOWER, + "workspace_upper": LEFT_WORKSPACE_UPPER, + "initial_closedness": DONOR_GRASP_CLOSEDNESS, + }, + "right": { + "controller": ControllersSource.RIGHT, + "home_position": RIGHT_TOOL_HOME_POS_W, + "home_orientation": RIGHT_TOOL_HOME_ROT_XYZW, + "workspace_lower": RIGHT_WORKSPACE_LOWER, + "workspace_upper": RIGHT_WORKSPACE_UPPER, + "initial_closedness": 0.0, + }, + } + for side, side_cfg in side_configs.items(): + if not all( + lower <= home <= upper + for lower, home, upper in zip( + side_cfg["workspace_lower"], side_cfg["home_position"], side_cfg["workspace_upper"], strict=True + ) + ): + raise ValueError(f"{side} dVRK tool home must lie inside its world workspace") + + controllers = ControllersSource(name="controllers") + world_transform = ValueInput("world_T_anchor", TransformMatrix()) + world_controllers = controllers.transformed(world_transform.output(ValueInput.VALUE)) + + connected_outputs = {} + for side, side_cfg in side_configs.items(): + controller_key = side_cfg["controller"] + controller_output = world_controllers.output(controller_key) + + clutch = DVRKPSMClutchRetargeter( + DVRKPSMClutchConfig( + input_device=controller_key, + home_reference_T_ee=_tool_home_transform(side_cfg["home_position"], side_cfg["home_orientation"]), + workspace_lower=side_cfg["workspace_lower"], + workspace_upper=side_cfg["workspace_upper"], + translation_scale=1.0, + orientation_offset=(0.0, 0.0, 0.0, 1.0), + clutch_threshold=0.5, + ), + name=f"{side}_ee_pose", + ) + gripper = DVRKPSMGripperRetargeter( + DVRKPSMGripperConfig( + input_device=controller_key, + jaw_open=DVRK_PSM_JAW_OPEN_POS, + jaw_closed=DVRK_PSM_JAW_CLOSED_POS, + initial_closedness=side_cfg["initial_closedness"], + clutch_threshold=0.5, + trigger_deadband=0.05, + opening_intent_duration_s=0.12, + ), + name=f"{side}_jaws", + ) + connected_outputs[f"{side}_pose"] = clutch.connect({controller_key: controller_output}).output( + DVRKPSMClutchRetargeter.OUTPUT_POSE + ) + connected_outputs[f"{side}_jaws"] = gripper.connect({controller_key: controller_output}).output( + DVRKPSMGripperRetargeter.OUTPUT_JAW_TARGETS + ) + + left_pose_elements = [ + "left_pos_x", + "left_pos_y", + "left_pos_z", + "left_quat_x", + "left_quat_y", + "left_quat_z", + "left_quat_w", + ] + left_jaw_elements = ["left_jaw_1", "left_jaw_2"] + right_pose_elements = [ + "right_pos_x", + "right_pos_y", + "right_pos_z", + "right_quat_x", + "right_quat_y", + "right_quat_z", + "right_quat_w", + ] + right_jaw_elements = ["right_jaw_1", "right_jaw_2"] + reorderer = TensorReorderer( + input_config={ + "left_pose": left_pose_elements, + "left_jaws": left_jaw_elements, + "right_pose": right_pose_elements, + "right_jaws": right_jaw_elements, + }, + output_order=left_pose_elements + left_jaw_elements + right_pose_elements + right_jaw_elements, + name="action_reorderer", + input_types={ + "left_pose": "array", + "left_jaws": "array", + "right_pose": "array", + "right_jaws": "array", + }, + ) + connected_reorderer = reorderer.connect(connected_outputs) + return OutputCombiner({"action": connected_reorderer.output("output")}) + + +@configclass +class DVRKNeedlePassEnvCfg(NeedlePassEnvCfg): + """Donor-held dVRK needle hand-off with motion-controller input and an 18D action ABI.""" + + requires_cuda: bool = True + """The contact-qualified dVRK needle pass is supported only on CUDA PhysX.""" + + def __post_init__(self): + super().__post_init__() + + if not ( + DVRK_PSM_JAW_OPEN_POS[0] < DVRK_PSM_JAW_CLOSED_POS[0] <= 0.0 + and DVRK_PSM_JAW_OPEN_POS[1] > DVRK_PSM_JAW_CLOSED_POS[1] >= 0.0 + ): + raise ValueError("dVRK ordered jaw endpoints do not satisfy the paired-jaw contract") + if not ( + DVRK_PSM_JAW_OPEN_POS[0] <= DONOR_HELD_RESET_JAW_POS[0] <= DVRK_PSM_JAW_CLOSED_POS[0] + and DVRK_PSM_JAW_OPEN_POS[1] >= DONOR_HELD_RESET_JAW_POS[1] >= DVRK_PSM_JAW_CLOSED_POS[1] + and DONOR_GRASP_JAW_POS == DVRK_PSM_JAW_CLOSED_POS + and DONOR_GRASP_CLOSEDNESS == 1.0 + ): + raise ValueError("donor reset must use a valid held equilibrium and a fully closed jaw target") + + self.scene.left_psm = _psm_cfg( + "{ENV_REGEX_NS}/LeftPSM", + LEFT_PSM_ROOT_POS, + LEFT_ARM_HOME, + DONOR_HELD_RESET_JAW_POS, + ) + self.scene.right_psm = _psm_cfg( + "{ENV_REGEX_NS}/RightPSM", + RIGHT_PSM_ROOT_POS, + RIGHT_ARM_HOME, + DVRK_PSM_JAW_OPEN_POS, + ) + self.scene.needle.init_state = RigidObjectCfg.InitialStateCfg( + pos=NEEDLE_RESET_POS, + rot=NEEDLE_RESET_ROT_XYZW, + lin_vel=(0.0, 0.0, 0.0), + ang_vel=(0.0, 0.0, 0.0), + ) + + phase_terms = ( + self.events.reset_all, + self.observations.policy.handoff_phase, + self.observations.subtask_terms.donor_hold, + self.observations.subtask_terms.co_hold, + self.observations.subtask_terms.receiver_only_hold, + self.observations.subtask_terms.retained_lift, + self.rewards.phase_progress, + self.rewards.retained_lift, + self.terminations.success, + self.terminations.needle_dropped_or_out_of_bounds, + ) + for term in phase_terms: + term.params = {**term.params, "phase_cfg": DVRK_HANDOFF_PHASE_CFG} + + self.actions.left_arm_action = mdp.WorldFrameDifferentialInverseKinematicsActionCfg( + asset_name="left_psm", + joint_names=list(DVRK_PSM_ARM_JOINT_NAMES), + body_name=DVRK_PSM_TOOL_TIP_BODY_NAME, + controller=DifferentialIKControllerCfg( + command_type="pose", + use_relative_mode=False, + ik_method="dls", + ), + scale=1.0, + ) + self.actions.left_jaw_action = mdp.DonorReleaseGuardedPairedJawJointPositionActionCfg( + asset_name="left_psm", + joint_names=list(DVRK_PSM_JAW_JOINT_NAMES), + scale=1.0, + offset=0.0, + use_default_offset=False, + preserve_order=True, + phase_cfg=DVRK_HANDOFF_PHASE_CFG, + # A release is a genuine opening request. Before a measured + # co-hold it is clamped to the load-qualified donor grasp. + release_aperture_threshold_rad=0.0, + # Preserve the load-qualified donor-held reset. The interlock + # blocks any outward donor-jaw motion; deliberate further closing + # remains a normal actuator command. + hold_jaw_pos=DONOR_GRASP_JAW_POS, + ) + self.actions.right_arm_action = mdp.WorldFrameDifferentialInverseKinematicsActionCfg( + asset_name="right_psm", + joint_names=list(DVRK_PSM_ARM_JOINT_NAMES), + body_name=DVRK_PSM_TOOL_TIP_BODY_NAME, + controller=DifferentialIKControllerCfg( + command_type="pose", + use_relative_mode=False, + ik_method="dls", + # The recipient traverses a native grasp channel near the PSM + # wrist singularity. Lower damping preserves the bounded + # public differential-IK solve while allowing it to converge + # to the generated pose instead of stalling millimetres away. + ik_params={"lambda_val": 0.003}, + ), + scale=1.0, + ) + self.actions.right_jaw_action = mdp.PairedJawJointPositionActionCfg( + asset_name="right_psm", + joint_names=list(DVRK_PSM_JAW_JOINT_NAMES), + scale=1.0, + offset=0.0, + use_default_offset=False, + preserve_order=True, + ) + + self.isaac_teleop = IsaacTeleopCfg( + pipeline_builder=_build_dvrk_needle_pass_pipeline, + sim_device=self.sim.device, + xr_cfg=self.xr, + ) + + +__all__ = [ + "DVRKNeedlePassEnvCfg", + "DVRK_HANDOFF_PHASE_CFG", + "DVRK_JAW_CHANNEL_T_T_C_POS_M", + "DVRK_JAW_CHANNEL_T_T_C_ROT_XYZW", + "DONOR_GRASP_CANDIDATE_INDEX", + "DONOR_GRASP_CLOSEDNESS", + "DONOR_GRASP_CONTACT_POINTS_N_M", + "DONOR_GRASP_JAW_POS", + "DONOR_GRASP_NATIVE_SEED_POS", + "DONOR_GRASP_NATIVE_SEED_ROT_XYZW", + "DONOR_GRASP_OUTWARD_NORMALS_N", + "DONOR_GRASP_T_N_C_POS_M", + "DONOR_GRASP_T_N_C_ROT_XYZW", + "DONOR_HELD_RESET_JAW_POS", + "DVRK_NEEDLE_PASS_JAW_ACTUATOR", + "ISAAC_GRASP_ASSET_SHA256", + "ISAAC_GRASP_CANDIDATES_SHA256", + "ISAAC_GRASP_CONFIG_SHA256", + "ISAAC_GRASP_GENERATOR_API", + "ISAAC_GRASP_GENERATOR_CANDIDATE_COUNT", + "ISAAC_GRASP_GENERATOR_CENTRE_COUNT", + "ISAAC_GRASP_GENERATOR_EXTENSION", + "ISAAC_GRASP_GENERATOR_EXTENSION_VERSION", + "ISAAC_GRASP_GENERATOR_ORIENTATIONS_PER_CENTRE", + "ISAAC_GRASP_GENERATOR_SEED", + "ISAAC_GRASP_GENERATOR_SIM_VERSION", + "ISAAC_GRASP_MANIFEST_SHA256", + "ISAAC_GRASP_WRAPPER_SHA256", + "LEFT_ARM_HOME", + "LEFT_PSM_ROOT_POS", + "LEFT_TOOL_HOME_POS_W", + "LEFT_TOOL_HOME_ROT_XYZW", + "LEFT_WORKSPACE_LOWER", + "LEFT_WORKSPACE_UPPER", + "NEEDLE_RESET_POS", + "NEEDLE_RESET_ROT_XYZW", + "PSM_ROOT_ROT_XYZW", + "RIGHT_ARM_HOME", + "RIGHT_PSM_ROOT_POS", + "RIGHT_TOOL_HOME_POS_W", + "RIGHT_TOOL_HOME_ROT_XYZW", + "RIGHT_WORKSPACE_LOWER", + "RIGHT_WORKSPACE_UPPER", + "RECEIVER_GRASP_CANDIDATE_INDEX", + "RECEIVER_GRASP_T_N_C_POS_M", + "RECEIVER_GRASP_T_N_C_ROT_XYZW", + "RECEIVER_ACQUISITION_NEEDLE_POS_W", + "RECEIVER_ACQUISITION_NEEDLE_ROT_XYZW", + "RECEIVER_NEEDLE_TARGET_POS_T", + "RECEIVER_NEEDLE_TARGET_ROT_XYZW", + "RECEIVER_TOOL_TARGET_POS_W", + "RECEIVER_TOOL_TARGET_ROT_XYZW", +] diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/__init__.py new file mode 100644 index 000000000000..595443ad4a61 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""MDP terms for the manager-based dVRK needle-pass environment.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/__init__.pyi new file mode 100644 index 000000000000..cea11227eda3 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/__init__.pyi @@ -0,0 +1,111 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +__all__ = [ + "DonorReleaseGuardedPairedJawJointPositionAction", + "DonorReleaseGuardedPairedJawJointPositionActionCfg", + "EXACT_POINT_CONTACT_FORCE_RESIDUAL_TOLERANCE_N", + "EXACT_POINT_CONTACT_MOMENT_ARM_TOLERANCE_M", + "FORCE_CLOSURE_CONE_FACETS", + "FiniteContactAcceptance", + "ForceClosureProof", + "HandoffMeasurements", + "HandoffPhase", + "HandoffPhaseCfg", + "HandoffPhaseMachine", + "JAW_BODY_REACTION_NORMALS_LOCAL", + "JAW_CONTACT_SENSOR_NAMES", + "PSM_JAW_JOINT_ORDER", + "PairedJawJointPositionAction", + "PairedJawJointPositionActionCfg", + "RetentionLoad", + "WorldFrameDifferentialInverseKinematicsAction", + "WorldFrameDifferentialInverseKinematicsActionCfg", + "assess_finite_contact_acceptance", + "donor_opening_requested", + "donor_release_is_allowed", + "end_effector_pose_w", + "friction_cone_generators", + "get_handoff_phase_machine", + "grasp_matrix", + "handoff_phase", + "handoff_phase_progress", + "impedance_gains", + "jaw_needle_contact_force", + "jaw_needle_contact_measurements", + "joint_position", + "joint_velocity", + "needle_dropped_or_out_of_bounds", + "needle_pose_w", + "needle_velocity_w", + "phase_at_least", + "prove_two_contact_force_closure", + "required_retention_load", + "reset_handoff_phase", + "reset_needle_pass_to_default", + "retained_lift_bonus", + "solve_minimum_closing_target", + "success", + "two_contact_friction_wrench_generators", + "update_handoff_phase", + "world_pose_xyzw_to_root_pose_xyzw", +] + +from isaaclab.envs.mdp import * # noqa: F403 + +from .actions import ( + PSM_JAW_JOINT_ORDER, + DonorReleaseGuardedPairedJawJointPositionAction, + DonorReleaseGuardedPairedJawJointPositionActionCfg, + PairedJawJointPositionAction, + PairedJawJointPositionActionCfg, + WorldFrameDifferentialInverseKinematicsAction, + WorldFrameDifferentialInverseKinematicsActionCfg, + donor_opening_requested, + donor_release_is_allowed, + world_pose_xyzw_to_root_pose_xyzw, +) +from .events import reset_needle_pass_to_default +from .grasp_solver import ( + EXACT_POINT_CONTACT_FORCE_RESIDUAL_TOLERANCE_N, + EXACT_POINT_CONTACT_MOMENT_ARM_TOLERANCE_M, + FORCE_CLOSURE_CONE_FACETS, + FiniteContactAcceptance, + ForceClosureProof, + RetentionLoad, + assess_finite_contact_acceptance, + friction_cone_generators, + grasp_matrix, + impedance_gains, + prove_two_contact_force_closure, + required_retention_load, + solve_minimum_closing_target, + two_contact_friction_wrench_generators, +) +from .observations import ( + end_effector_pose_w, + handoff_phase, + jaw_needle_contact_force, + joint_position, + joint_velocity, + needle_pose_w, + needle_velocity_w, + phase_at_least, +) +from .rewards import handoff_phase_progress, retained_lift_bonus +from .terminations import ( + JAW_BODY_REACTION_NORMALS_LOCAL, + JAW_CONTACT_SENSOR_NAMES, + HandoffMeasurements, + HandoffPhase, + HandoffPhaseCfg, + HandoffPhaseMachine, + get_handoff_phase_machine, + jaw_needle_contact_measurements, + needle_dropped_or_out_of_bounds, + reset_handoff_phase, + success, + update_handoff_phase, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/actions.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/actions.py new file mode 100644 index 000000000000..8a7f45e2d2b9 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/actions.py @@ -0,0 +1,263 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Task-owned action terms for the dVRK bimanual 18D action ABI.""" + +from __future__ import annotations + +import math +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils +from isaaclab.envs.mdp.actions.actions_cfg import ( + DifferentialInverseKinematicsActionCfg, + JointPositionActionCfg, +) +from isaaclab.envs.mdp.actions.joint_actions import JointPositionAction +from isaaclab.envs.mdp.actions.task_space_actions import DifferentialInverseKinematicsAction +from isaaclab.managers.action_manager import ActionTerm +from isaaclab.utils.configclass import configclass + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + + from .terminations import HandoffPhaseCfg + +PSM_JAW_JOINT_ORDER = ("psm_tool_gripper1_joint", "psm_tool_gripper2_joint") + + +def donor_release_is_allowed(phase: torch.Tensor, receiver_grasp: torch.Tensor, co_hold_phase: int) -> torch.Tensor: + """Return whether a donor opening may proceed in each environment. + + A completed co-hold dwell is necessary but deliberately insufficient on + its own: the receiver must still have a bilateral, opposed measured + contact in the latest post-physics sensor sample. The caller passes the + live contact result rather than a phase-derived latch so a contact loss + cannot leave an opening request authorised. + """ + + if phase.shape != receiver_grasp.shape: + raise ValueError("phase and receiver_grasp must have identical batch shapes") + if receiver_grasp.dtype is not torch.bool: + raise ValueError("receiver_grasp must be a boolean tensor") + if not isinstance(co_hold_phase, int): + raise TypeError("co_hold_phase must be an integer phase value") + return (phase >= co_hold_phase) & receiver_grasp + + +def donor_opening_requested( + jaw_targets: torch.Tensor, hold_targets: torch.Tensor, aperture_threshold_rad: float +) -> torch.Tensor: + """Identify any donor-jaw opening relative to the held grasp. + + The ordered dVRK joints have opposite signs. A release therefore moves + joint one negative or joint two positive *from the measured-compatible + held target*. The public ABI exposes two joint targets, so guarding only a + simultaneous paired command would leave an unsafe one-jaw escape path. + Deliberate further closing remains an ordinary actuator command. + """ + + if jaw_targets.shape != hold_targets.shape or jaw_targets.ndim != 2 or jaw_targets.shape[1] != 2: + raise ValueError("jaw_targets and hold_targets must both have shape (N, 2)") + if not torch.isfinite(jaw_targets).all() or not torch.isfinite(hold_targets).all(): + raise ValueError("jaw targets must be finite") + if not math.isfinite(aperture_threshold_rad) or aperture_threshold_rad < 0.0: + raise ValueError("aperture threshold must be finite and non-negative") + return (jaw_targets[:, 0] < hold_targets[:, 0] - aperture_threshold_rad) | ( + jaw_targets[:, 1] > hold_targets[:, 1] + aperture_threshold_rad + ) + + +def world_pose_xyzw_to_root_pose_xyzw( + pose_w_xyzw: torch.Tensor, + root_pos_w: torch.Tensor, + root_quat_w_xyzw: torch.Tensor, +) -> torch.Tensor: + """Convert absolute world poses from the public xyzw ABI to root-frame xyzw. + + Each row is converted against its matching live root transform. The helper + deliberately accepts batched tensors so differently placed cloned PSMs can + never accidentally share one root transform. + """ + + if pose_w_xyzw.ndim != 2 or pose_w_xyzw.shape[1] != 7: + raise ValueError("pose_w_xyzw must have shape (N, 7)") + if root_pos_w.shape != pose_w_xyzw[:, :3].shape or root_quat_w_xyzw.shape != pose_w_xyzw[:, 3:].shape: + raise ValueError("root transforms must match the batched world poses") + if not torch.isfinite(pose_w_xyzw).all(): + raise ValueError("world-frame IK actions must be finite") + + target_quat_xyzw = pose_w_xyzw[:, 3:7] + target_quat_norm = torch.linalg.vector_norm(target_quat_xyzw, dim=-1, keepdim=True) + if torch.any(target_quat_norm <= 1.0e-9): + raise ValueError("world-frame IK action quaternions must be normalisable") + target_quat_xyzw = target_quat_xyzw / target_quat_norm + target_pos_b, target_quat_b = math_utils.subtract_frame_transforms( + root_pos_w, + root_quat_w_xyzw, + pose_w_xyzw[:, :3], + target_quat_xyzw, + ) + return torch.cat((target_pos_b, target_quat_b), dim=-1) + + +class WorldFrameDifferentialInverseKinematicsAction(DifferentialInverseKinematicsAction): + """Absolute IK action that converts the live world target for every solve. + + Input is ``[x, y, z, qx, qy, qz, qw]`` in the shared world/XR frame. + Immediately before each IK solve, the term reads this articulation's live + root pose and supplies the controller with a root-frame xyzw target. A + command-level hold therefore keeps a target fixed; it does not disable the + articulation's actuator drives or latch measured joint state. + """ + + cfg: WorldFrameDifferentialInverseKinematicsActionCfg + + def __init__(self, cfg: WorldFrameDifferentialInverseKinematicsActionCfg, env: ManagerBasedEnv): + if cfg.scale != 1.0: + raise ValueError("world-frame absolute IK must use scale=1.0") + super().__init__(cfg, env) + + def process_actions(self, actions: torch.Tensor) -> None: + """Cache the world target without applying a stale root transform.""" + + if actions.shape != self._raw_actions.shape: + raise ValueError(f"expected world-frame IK actions with shape {tuple(self._raw_actions.shape)}") + if not torch.isfinite(actions).all(): + raise ValueError("world-frame IK actions must be finite") + quaternion_norm = torch.linalg.vector_norm(actions[:, 3:7], dim=-1, keepdim=True) + if torch.any(quaternion_norm <= 1.0e-9): + raise ValueError("world-frame IK action quaternions must be normalisable") + self._raw_actions[:] = actions + self._processed_actions[:, :3] = actions[:, :3] + self._processed_actions[:, 3:7] = actions[:, 3:7] / quaternion_norm + + def apply_actions(self) -> None: + """Convert against the live root and solve the current articulation.""" + + target_pose_b = world_pose_xyzw_to_root_pose_xyzw( + self._processed_actions, + self._asset.data.root_pos_w.torch, + self._asset.data.root_quat_w.torch, + ) + ee_pos_b, ee_quat_b = self._compute_frame_pose() + self._ik_controller.set_command(target_pose_b, ee_pos_b, ee_quat_b) + joint_pos = self._asset.data.joint_pos.torch[:, self._joint_ids] + if torch.linalg.vector_norm(ee_quat_b, dim=-1).gt(0.0).all(): + joint_pos_des = self._ik_controller.compute( + ee_pos_b, + ee_quat_b, + self._compute_frame_jacobian(), + joint_pos, + ) + else: + joint_pos_des = joint_pos.clone() + self._asset.set_joint_position_target_index(target=joint_pos_des, joint_ids=self._joint_ids) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Clear cached raw values for the selected environments.""" + + super().reset(env_ids) + self._processed_actions[env_ids] = 0.0 + + +class PairedJawJointPositionAction(JointPositionAction): + """Ordered two-jaw position term with a start-up ABI assertion.""" + + cfg: PairedJawJointPositionActionCfg + + def __init__(self, cfg: PairedJawJointPositionActionCfg, env: ManagerBasedEnv): + super().__init__(cfg, env) + if tuple(self._joint_names) != PSM_JAW_JOINT_ORDER: + raise ValueError( + f"dVRK jaw action must resolve exactly {list(PSM_JAW_JOINT_ORDER)}, got {self._joint_names}" + ) + + +class DonorReleaseGuardedPairedJawJointPositionAction(PairedJawJointPositionAction): + """Keep both donor jaws closed until the receiver has a measured co-hold. + + The interlock only suppresses an opening command. It never creates a + contact, changes the free needle, or advances the hand-off phase machine: + those remain consequences of the measured PhysX state. Once the previous + post-physics sample has established ``CO_HOLD`` *and* the most recent + receiver contact remains bilateral and opposed, the commanded donor jaw + target passes through unchanged. Before then, any outward movement of + either jaw is clamped. Losing that measured receiver grasp re-clamps the + donor on the next control application. + """ + + cfg: DonorReleaseGuardedPairedJawJointPositionActionCfg + + def __init__(self, cfg: DonorReleaseGuardedPairedJawJointPositionActionCfg, env: ManagerBasedEnv): + super().__init__(cfg, env) + if cfg.phase_cfg is None: + raise ValueError("donor release guard requires the shared hand-off phase configuration") + if not math.isfinite(cfg.release_aperture_threshold_rad) or cfg.release_aperture_threshold_rad < 0.0: + raise ValueError("donor release aperture threshold must be finite and non-negative") + hold_target = torch.tensor(cfg.hold_jaw_pos, dtype=torch.float32, device=self.device) + if hold_target.shape != (2,) or not torch.isfinite(hold_target).all(): + raise ValueError("donor release guard requires two finite holding jaw positions") + self._hold_target = hold_target.repeat(self.num_envs, 1) + + def apply_actions(self) -> None: + # Import locally because this module is loaded before ``terminations`` + # by the public MDP namespace. + from .terminations import HandoffPhase, get_handoff_phase_machine, jaw_needle_contact_measurements + + machine = get_handoff_phase_machine(self._env, self.cfg.phase_cfg) + loads, normals, _ = jaw_needle_contact_measurements(self._env) + receiver_grasp = machine._bilateral_contact(loads[:, 2:4], normals[:, 2:4], machine._receiver_engaged) + release_requested = donor_opening_requested( + self.processed_actions, + self._hold_target, + self.cfg.release_aperture_threshold_rad, + ) + release_allowed = donor_release_is_allowed(machine.phase, receiver_grasp, int(HandoffPhase.CO_HOLD)) + command = torch.where( + (release_requested & ~release_allowed).unsqueeze(-1), self._hold_target, self.processed_actions + ) + self._asset.set_joint_position_target_index(target=command, joint_ids=self._joint_ids) + + +@configclass +class WorldFrameDifferentialInverseKinematicsActionCfg(DifferentialInverseKinematicsActionCfg): + """Configuration for live world-to-root absolute differential IK.""" + + class_type: type[ActionTerm] = WorldFrameDifferentialInverseKinematicsAction + + +@configclass +class PairedJawJointPositionActionCfg(JointPositionActionCfg): + """Configuration for the exact ordered paired-jaw action.""" + + class_type: type[ActionTerm] = PairedJawJointPositionAction + + +@configclass +class DonorReleaseGuardedPairedJawJointPositionActionCfg(PairedJawJointPositionActionCfg): + """Exact paired donor jaws with a measured receiver-grasp release interlock.""" + + class_type: type[ActionTerm] = DonorReleaseGuardedPairedJawJointPositionAction + phase_cfg: HandoffPhaseCfg | None = None + release_aperture_threshold_rad: float = 0.0 + hold_jaw_pos: tuple[float, float] = (0.0, 0.0) + + +__all__ = [ + "DonorReleaseGuardedPairedJawJointPositionAction", + "DonorReleaseGuardedPairedJawJointPositionActionCfg", + "PSM_JAW_JOINT_ORDER", + "PairedJawJointPositionAction", + "PairedJawJointPositionActionCfg", + "WorldFrameDifferentialInverseKinematicsAction", + "WorldFrameDifferentialInverseKinematicsActionCfg", + "donor_release_is_allowed", + "donor_opening_requested", + "world_pose_xyzw_to_root_pose_xyzw", +] diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/events.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/events.py new file mode 100644 index 000000000000..443e1e4b7053 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/events.py @@ -0,0 +1,66 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Reset-only direct state writes for dVRK needle pass.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import SceneEntityCfg + +from .terminations import HandoffPhaseCfg, reset_handoff_phase + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def reset_needle_pass_to_default( + env: ManagerBasedRLEnv, + env_ids: torch.Tensor, + phase_cfg: HandoffPhaseCfg, + left_psm_cfg: SceneEntityCfg = SceneEntityCfg("left_psm"), + right_psm_cfg: SceneEntityCfg = SceneEntityCfg("right_psm"), + needle_cfg: SceneEntityCfg = SceneEntityCfg("needle"), +) -> None: + """Perform the deterministic reset in the only permitted write order. + + Both PSMs are first written to their configured arm and jaw start states, + with matching position targets and zero velocity. The donor start state is + a closed, load-qualified grasp around the needle while the receiver starts + open. The free needle then receives exactly one pose write and one velocity + write. The event does not step or settle physics and never applies an + action. + """ + + if env_ids is None: + env_ids = torch.arange(env.num_envs, dtype=torch.long, device=env.device) + else: + env_ids = env_ids.to(device=env.device, dtype=torch.long) + + for asset_cfg in (left_psm_cfg, right_psm_cfg): + psm: Articulation = env.scene[asset_cfg.name] + joint_pos = psm.data.default_joint_pos.torch[env_ids].clone() + joint_vel = torch.zeros_like(joint_pos) + psm.write_joint_position_to_sim_index(position=joint_pos, env_ids=env_ids) + psm.write_joint_velocity_to_sim_index(velocity=joint_vel, env_ids=env_ids) + psm.set_joint_position_target_index(target=joint_pos, env_ids=env_ids) + psm.set_joint_velocity_target_index(target=joint_vel, env_ids=env_ids) + + needle: RigidObject = env.scene[needle_cfg.name] + needle_pose = needle.data.default_root_pose.torch[env_ids].clone() + needle_pose[:, :3] += env.scene.env_origins[env_ids] + needle.write_root_pose_to_sim_index(root_pose=needle_pose, env_ids=env_ids) + needle.write_root_velocity_to_sim_index( + root_velocity=torch.zeros((len(env_ids), 6), dtype=needle_pose.dtype, device=needle_pose.device), + env_ids=env_ids, + ) + reset_handoff_phase(env, env_ids, needle_pose[:, 2], phase_cfg) + + +__all__ = ["reset_needle_pass_to_default"] diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/grasp_solver.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/grasp_solver.py new file mode 100644 index 000000000000..a5f14c2c4f2c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/grasp_solver.py @@ -0,0 +1,426 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Deterministic retention helpers for the fixed native needle grasps. + +These routines consume only pinned generator output and declared physical +constants. They never inspect a live trajectory, and therefore cannot turn the +reset into an adaptive or scripted grasp. +""" + +from __future__ import annotations + +import math +from collections.abc import Callable +from dataclasses import dataclass +from itertools import combinations + +import numpy as np +import numpy.typing as npt + +ArrayLike = npt.ArrayLike + +FORCE_CLOSURE_CONE_FACETS = 8 +"""Fixed facet count used by the deterministic static-friction proof.""" + +EXACT_POINT_CONTACT_FORCE_RESIDUAL_TOLERANCE_N = 1.0e-10 +"""Fixed numerical force tolerance for exact point-contact feasibility.""" + +EXACT_POINT_CONTACT_MOMENT_ARM_TOLERANCE_M = 1.0e-10 +"""Fixed numerical moment-arm tolerance for exact point-contact feasibility.""" + + +def _vector3(value: ArrayLike, name: str) -> np.ndarray: + vector = np.asarray(value, dtype=np.float64) + if vector.shape != (3,) or not np.isfinite(vector).all(): + raise ValueError(f"{name} must be a finite three-vector") + return vector + + +def _unit(value: ArrayLike, name: str) -> np.ndarray: + vector = _vector3(value, name) + norm = float(np.linalg.norm(vector)) + if norm <= 1.0e-12: + raise ValueError(f"{name} must be non-zero") + return vector / norm + + +@dataclass(frozen=True, slots=True) +class RetentionLoad: + """Analytical two-contact load required by gravity and commanded motion.""" + + external_force_n: float + normal_force_per_jaw_n: float + friction_coefficient: float + safety_factor: float + + +@dataclass(frozen=True, slots=True) +class ForceClosureProof: + """Deterministic point-contact certificate for one required wrench. + + Force and torque residuals are reported separately because their units + cannot be combined into one Euclidean norm. The equivalent moment-arm + residual divides the torque residual by the required-force norm. It is + infinite when a non-zero torque residual has no required force against + which to normalise. + """ + + exact_point_contact_feasible: bool + coefficients: tuple[float, ...] + achieved_wrench: tuple[float, float, float, float, float, float] + residual_wrench: tuple[float, float, float, float, float, float] + required_force_norm_n: float + force_residual_norm_n: float + torque_residual_norm_n_m: float + equivalent_moment_arm_residual_m: float + force_residual_tolerance_n: float + moment_arm_residual_tolerance_m: float + active_generator_indices: tuple[int, ...] + + @property + def feasible(self) -> bool: + """Return the exact point-contact result for compatibility.""" + + return self.exact_point_contact_feasible + + +@dataclass(frozen=True, slots=True) +class FiniteContactAcceptance: + """Explicit finite-patch acceptance of an inexact point-contact proof. + + This result is a task-level soft-contact approximation, not exact force + closure. Both tolerances must be supplied by the caller. + """ + + accepted: bool + force_within_tolerance: bool + moment_arm_within_tolerance: bool + force_residual_tolerance_n: float + moment_arm_residual_tolerance_m: float + + +def required_retention_load( + *, + mass_kg: float, + gravity_m_s2: float, + maximum_commanded_acceleration_m_s2: float, + friction_coefficient: float, + safety_factor: float, +) -> RetentionLoad: + """Return the per-jaw normal load for an opposed two-contact grasp.""" + + values = ( + mass_kg, + gravity_m_s2, + maximum_commanded_acceleration_m_s2, + friction_coefficient, + safety_factor, + ) + if not all(math.isfinite(value) for value in values): + raise ValueError("retention-load inputs must be finite") + if mass_kg <= 0.0 or gravity_m_s2 < 0.0 or maximum_commanded_acceleration_m_s2 < 0.0: + raise ValueError("mass must be positive and accelerations non-negative") + if friction_coefficient <= 0.0 or safety_factor < 1.0: + raise ValueError("friction must be positive and safety_factor at least one") + external_force = mass_kg * (gravity_m_s2 + maximum_commanded_acceleration_m_s2) + normal_force = safety_factor * external_force / (2.0 * friction_coefficient) + return RetentionLoad(external_force, normal_force, friction_coefficient, safety_factor) + + +def friction_cone_generators( + inward_normal: ArrayLike, + friction_coefficient: float, + facets: int, +) -> np.ndarray: + """Return a fixed polygonal approximation of one Coulomb friction cone.""" + + normal = _unit(inward_normal, "inward normal") + if not math.isfinite(friction_coefficient) or friction_coefficient <= 0.0: + raise ValueError("friction_coefficient must be finite and positive") + if facets < 4: + raise ValueError("friction cone requires at least four facets") + reference = np.array((1.0, 0.0, 0.0)) + if abs(float(reference @ normal)) > 0.9: + reference = np.array((0.0, 1.0, 0.0)) + tangent_1 = _unit(np.cross(normal, reference), "friction tangent") + tangent_2 = np.cross(normal, tangent_1) + generators = [] + for index in range(facets): + angle = 2.0 * math.pi * index / facets + tangent = math.cos(angle) * tangent_1 + math.sin(angle) * tangent_2 + generators.append(normal + friction_coefficient * tangent) + return np.stack(generators) + + +def two_contact_friction_wrench_generators( + contact_points_m: ArrayLike, + inward_normals: ArrayLike, + static_friction_coefficient: float, +) -> np.ndarray: + """Return 16 row-wise point-contact wrench generators for two contacts. + + Contact points are expressed relative to the wrench origin. Each contact + contributes eight edges of a polygonal Coulomb cone using the declared + *static* friction coefficient. A generator is ordered as ``[force, + moment]``, with ``moment = point x force``. + """ + + points = np.asarray(contact_points_m, dtype=np.float64) + normals = np.asarray(inward_normals, dtype=np.float64) + if points.shape != (2, 3) or not np.isfinite(points).all(): + raise ValueError("contact_points_m must be a finite (2, 3) array") + if normals.shape != (2, 3) or not np.isfinite(normals).all(): + raise ValueError("inward_normals must be a finite (2, 3) array") + if not math.isfinite(static_friction_coefficient) or static_friction_coefficient <= 0.0: + raise ValueError("static_friction_coefficient must be finite and positive") + + contact_grasp_matrix = grasp_matrix(points) + wrenches = [] + for contact_index, normal in enumerate(normals): + forces = friction_cone_generators( + normal, + static_friction_coefficient, + FORCE_CLOSURE_CONE_FACETS, + ) + for force in forces: + padded_force = np.zeros(6, dtype=np.float64) + padded_force[3 * contact_index : 3 * (contact_index + 1)] = force + wrenches.append(contact_grasp_matrix @ padded_force) + return np.stack(wrenches) + + +def prove_two_contact_force_closure( + contact_points_m: ArrayLike, + inward_normals: ArrayLike, + static_friction_coefficient: float, + required_wrench: ArrayLike, +) -> ForceClosureProof: + """Prove whether two static-friction contacts can supply one 6D wrench. + + The proof enumerates generator subsets in stable lexicographic order and + solves only unconstrained least-squares systems whose coefficients are + nonnegative. Conic Caratheodory bounds a certificate to the generator + matrix rank, which is at most six here, so the search is finite and does + not require SciPy or an adaptive optimiser. Exact point-contact + feasibility requires both the force residual and its equivalent + moment-arm residual to meet their numerical tolerances. The conservative + fixed moment-arm tolerance is 0.1 nanometres, well below 10 micrometres; + it cannot be loosened into a finite-contact acceptance allowance. Use + :func:`assess_finite_contact_acceptance` for that separate judgement. + + Infeasible candidates are ranked by the maximum of their two dimensionless + tolerance ratios. No force value is ever added to a torque value. + """ + + target = np.asarray(required_wrench, dtype=np.float64) + if target.shape != (6,) or not np.isfinite(target).all(): + raise ValueError("required_wrench must be a finite six-vector") + force_residual_tolerance_n = EXACT_POINT_CONTACT_FORCE_RESIDUAL_TOLERANCE_N + moment_arm_residual_tolerance_m = EXACT_POINT_CONTACT_MOMENT_ARM_TOLERANCE_M + + required_force_norm_n = float(np.linalg.norm(target[:3])) + + def residual_metrics(residual_wrench: np.ndarray) -> tuple[float, float, float]: + force_residual_norm_n = float(np.linalg.norm(residual_wrench[:3])) + torque_residual_norm_n_m = float(np.linalg.norm(residual_wrench[3:])) + if required_force_norm_n > 0.0: + moment_arm_residual_m = torque_residual_norm_n_m / required_force_norm_n + elif torque_residual_norm_n_m == 0.0: + moment_arm_residual_m = 0.0 + else: + moment_arm_residual_m = math.inf + return force_residual_norm_n, torque_residual_norm_n_m, moment_arm_residual_m + + def make_proof( + candidate_coefficients: np.ndarray, + candidate_achieved: np.ndarray, + candidate_residual: np.ndarray, + active_indices: tuple[int, ...], + ) -> ForceClosureProof: + force_residual_n, torque_residual_n_m, moment_arm_residual_m = residual_metrics(candidate_residual) + exact = ( + force_residual_n <= force_residual_tolerance_n and moment_arm_residual_m <= moment_arm_residual_tolerance_m + ) + return ForceClosureProof( + exact_point_contact_feasible=exact, + coefficients=tuple(float(value) for value in candidate_coefficients), + achieved_wrench=tuple(float(value) for value in candidate_achieved), + residual_wrench=tuple(float(value) for value in candidate_residual), + required_force_norm_n=required_force_norm_n, + force_residual_norm_n=force_residual_n, + torque_residual_norm_n_m=torque_residual_n_m, + equivalent_moment_arm_residual_m=moment_arm_residual_m, + force_residual_tolerance_n=force_residual_tolerance_n, + moment_arm_residual_tolerance_m=moment_arm_residual_tolerance_m, + active_generator_indices=active_indices, + ) + + def residual_score(proof: ForceClosureProof) -> float: + return max( + proof.force_residual_norm_n / force_residual_tolerance_n, + proof.equivalent_moment_arm_residual_m / moment_arm_residual_tolerance_m, + ) + + generators = two_contact_friction_wrench_generators( + contact_points_m, + inward_normals, + static_friction_coefficient, + ) + coefficients = np.zeros(generators.shape[0], dtype=np.float64) + achieved = coefficients @ generators + residual = target - achieved + best = make_proof(coefficients, achieved, residual, ()) + best_score = residual_score(best) + if best.feasible: + return best + + generator_rank = int(np.linalg.matrix_rank(generators, tol=1.0e-12)) + maximum_subset_size = min(6, generator_rank) + nonnegative_tolerance = 1.0e-12 * max(1.0, float(np.linalg.norm(target))) + + for subset_size in range(1, maximum_subset_size + 1): + for indices in combinations(range(generators.shape[0]), subset_size): + subset = generators[np.asarray(indices)].T + subset_coefficients = np.linalg.lstsq(subset, target, rcond=1.0e-12)[0] + if np.any(subset_coefficients < -nonnegative_tolerance): + continue + subset_coefficients = np.maximum(subset_coefficients, 0.0) + candidate_coefficients = np.zeros_like(coefficients) + candidate_coefficients[np.asarray(indices)] = subset_coefficients + candidate_achieved = candidate_coefficients @ generators + candidate_residual = target - candidate_achieved + active = tuple(int(index) for index in np.flatnonzero(candidate_coefficients > nonnegative_tolerance)) + candidate = make_proof(candidate_coefficients, candidate_achieved, candidate_residual, active) + candidate_score = residual_score(candidate) + if candidate_score < best_score: + best = candidate + best_score = candidate_score + if best.feasible: + return best + return best + + +def assess_finite_contact_acceptance( + proof: ForceClosureProof, + *, + force_residual_tolerance_n: float, + moment_arm_residual_tolerance_m: float, +) -> FiniteContactAcceptance: + """Apply an explicit soft-contact rule to an inexact point-contact proof. + + This helper does not change ``proof.exact_point_contact_feasible``. It + models a separately justified finite contact patch or torsional compliance + and therefore requires both physical tolerances from the caller. A + conservative task should keep the moment-arm allowance at or below + 10 micrometres unless independent contact-patch evidence supports more. + """ + + for name, tolerance in ( + ("force_residual_tolerance_n", force_residual_tolerance_n), + ("moment_arm_residual_tolerance_m", moment_arm_residual_tolerance_m), + ): + if not math.isfinite(tolerance) or tolerance <= 0.0: + raise ValueError(f"{name} must be finite and positive") + force_within_tolerance = proof.force_residual_norm_n <= force_residual_tolerance_n + moment_arm_within_tolerance = proof.equivalent_moment_arm_residual_m <= moment_arm_residual_tolerance_m + return FiniteContactAcceptance( + accepted=force_within_tolerance and moment_arm_within_tolerance, + force_within_tolerance=force_within_tolerance, + moment_arm_within_tolerance=moment_arm_within_tolerance, + force_residual_tolerance_n=force_residual_tolerance_n, + moment_arm_residual_tolerance_m=moment_arm_residual_tolerance_m, + ) + + +def grasp_matrix(contact_points_m: ArrayLike) -> np.ndarray: + """Return the six-dimensional point-contact grasp matrix for two contacts.""" + + points = np.asarray(contact_points_m, dtype=np.float64) + if points.shape != (2, 3) or not np.isfinite(points).all(): + raise ValueError("contact_points_m must be a finite (2, 3) array") + blocks = [] + for point in points: + skew = np.array( + ((0.0, -point[2], point[1]), (point[2], 0.0, -point[0]), (-point[1], point[0], 0.0)), + dtype=np.float64, + ) + blocks.append(np.vstack((np.eye(3), skew))) + return np.hstack(blocks) + + +def impedance_gains( + reflected_inertia_kg_m2: float, + natural_frequency_rad_s: float, + damping_ratio: float, +) -> tuple[float, float]: + """Derive ``Kp`` and ``Kd`` from reflected inertia and pole placement.""" + + inputs = (reflected_inertia_kg_m2, natural_frequency_rad_s, damping_ratio) + if not all(math.isfinite(value) and value > 0.0 for value in inputs): + raise ValueError("impedance inputs must be finite and positive") + stiffness = reflected_inertia_kg_m2 * natural_frequency_rad_s**2 + damping = 2.0 * damping_ratio * reflected_inertia_kg_m2 * natural_frequency_rad_s + return stiffness, damping + + +def solve_minimum_closing_target( + normal_load_fn: Callable[[float], float], + *, + required_normal_load_n: float, + lower_closedness: float, + upper_closedness: float = 1.0, + tolerance: float = 1.0e-6, + max_iterations: int = 80, +) -> float: + """Find the smallest bounded closedness whose measured model meets the load.""" + + if not 0.0 <= lower_closedness <= upper_closedness <= 1.0: + raise ValueError("closedness bounds must be ordered inside [0, 1]") + if not math.isfinite(required_normal_load_n) or required_normal_load_n <= 0.0: + raise ValueError("required_normal_load_n must be finite and positive") + + def evaluate(value: float) -> float: + load = float(normal_load_fn(value)) + if not math.isfinite(load) or load < 0.0: + raise ValueError("normal_load_fn must return a finite non-negative load") + return load + + lower_load = evaluate(lower_closedness) + upper_load = evaluate(upper_closedness) + if upper_load < lower_load: + raise ValueError("normal-load model must be non-decreasing") + if upper_load < required_normal_load_n: + raise RuntimeError("authored jaw drive cannot meet the required retaining load") + if lower_load >= required_normal_load_n: + return lower_closedness + lower, upper = lower_closedness, upper_closedness + for _ in range(max_iterations): + if upper - lower <= tolerance: + break + midpoint = 0.5 * (lower + upper) + if evaluate(midpoint) >= required_normal_load_n: + upper = midpoint + else: + lower = midpoint + return upper + + +__all__ = [ + "EXACT_POINT_CONTACT_FORCE_RESIDUAL_TOLERANCE_N", + "EXACT_POINT_CONTACT_MOMENT_ARM_TOLERANCE_M", + "FiniteContactAcceptance", + "FORCE_CLOSURE_CONE_FACETS", + "ForceClosureProof", + "RetentionLoad", + "assess_finite_contact_acceptance", + "friction_cone_generators", + "grasp_matrix", + "impedance_gains", + "prove_two_contact_force_closure", + "required_retention_load", + "solve_minimum_closing_target", + "two_contact_friction_wrench_generators", +] diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/observations.py new file mode 100644 index 000000000000..f703129cffd3 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/observations.py @@ -0,0 +1,118 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Stable, recorder-friendly observations for dVRK needle pass.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import SceneEntityCfg + +from .terminations import ( + HandoffPhase, + HandoffPhaseCfg, + jaw_needle_contact_measurements, + update_handoff_phase, +) + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def _articulation(env: ManagerBasedRLEnv, cfg: SceneEntityCfg) -> Articulation: + return env.scene[cfg.name] + + +def joint_position( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg, +) -> torch.Tensor: + """Return all live articulation joint positions in native USD order.""" + + return _articulation(env, asset_cfg).data.joint_pos.torch + + +def joint_velocity( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg, +) -> torch.Tensor: + """Return all live articulation joint velocities in native USD order.""" + + return _articulation(env, asset_cfg).data.joint_vel.torch + + +def end_effector_pose_w( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg, + body_name: str = "psm_tool_tip_link", +) -> torch.Tensor: + """Return live tool-tip pose ``[xyz, qx, qy, qz, qw]`` in world frame.""" + + asset = _articulation(env, asset_cfg) + body_ids, body_names = asset.find_bodies(body_name) + if len(body_ids) != 1: + raise RuntimeError(f"expected one {body_name!r} body, found {body_names}") + body_id = body_ids[0] + return torch.cat((asset.data.body_pos_w.torch[:, body_id], asset.data.body_quat_w.torch[:, body_id]), dim=-1) + + +def needle_pose_w( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("needle"), +) -> torch.Tensor: + """Return simulated needle pose ``[xyz, qx, qy, qz, qw]`` in world frame.""" + + needle: RigidObject = env.scene[asset_cfg.name] + return torch.cat((needle.data.root_pos_w.torch, needle.data.root_quat_w.torch), dim=-1) + + +def needle_velocity_w( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("needle"), +) -> torch.Tensor: + """Return simulated needle linear then angular world velocity.""" + + needle: RigidObject = env.scene[asset_cfg.name] + return torch.cat((needle.data.root_lin_vel_w.torch, needle.data.root_ang_vel_w.torch), dim=-1) + + +def jaw_needle_contact_force(env: ManagerBasedRLEnv) -> torch.Tensor: + """Return four projected normal loads in left-jaw-1 through right-jaw-2 order.""" + + loads, _, _ = jaw_needle_contact_measurements(env) + return loads + + +def handoff_phase(env: ManagerBasedRLEnv, phase_cfg: HandoffPhaseCfg) -> torch.Tensor: + """Return one physical phase column; INITIAL is reset-held pending fresh contact.""" + + return update_handoff_phase(env, phase_cfg).phase.unsqueeze(-1) + + +def phase_at_least( + env: ManagerBasedRLEnv, + phase_cfg: HandoffPhaseCfg, + phase: HandoffPhase, +) -> torch.Tensor: + """Return a recorder subtask flag derived solely from measured phase state.""" + + current = update_handoff_phase(env, phase_cfg).phase + return (current >= int(phase)).to(dtype=torch.float32).unsqueeze(-1) + + +__all__ = [ + "end_effector_pose_w", + "handoff_phase", + "jaw_needle_contact_force", + "joint_position", + "joint_velocity", + "needle_pose_w", + "needle_velocity_w", + "phase_at_least", +] diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/rewards.py new file mode 100644 index 000000000000..966cdbbfdb1b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/rewards.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Progress rewards derived from the same measured physical phase state.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from .terminations import HandoffPhase, HandoffPhaseCfg, update_handoff_phase + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +def handoff_phase_progress(env: ManagerBasedRLEnv, phase_cfg: HandoffPhaseCfg) -> torch.Tensor: + """Return normalised ordered progress; this is not success evidence.""" + + phase = update_handoff_phase(env, phase_cfg).phase + return phase.to(dtype=torch.float32) / float(HandoffPhase.RETAINED_LIFT) + + +def retained_lift_bonus(env: ManagerBasedRLEnv, phase_cfg: HandoffPhaseCfg) -> torch.Tensor: + """Return a sparse bonus after the retained-lift dwell has completed.""" + + phase = update_handoff_phase(env, phase_cfg).phase + return (phase == int(HandoffPhase.RETAINED_LIFT)).to(dtype=torch.float32) + + +__all__ = ["handoff_phase_progress", "retained_lift_bonus"] diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/terminations.py new file mode 100644 index 000000000000..058ed5f397ef --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/terminations.py @@ -0,0 +1,510 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Measured-contact phase state and terminations for dVRK needle pass.""" + +from __future__ import annotations + +import math +from dataclasses import dataclass +from enum import IntEnum +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils +from isaaclab.assets import Articulation, RigidObject +from isaaclab.managers import SceneEntityCfg +from isaaclab.sensors import ContactSensor +from isaaclab.utils.configclass import configclass + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + +JAW_CONTACT_SENSOR_NAMES = ( + "left_jaw_1_needle_contact", + "left_jaw_2_needle_contact", + "right_jaw_1_needle_contact", + "right_jaw_2_needle_contact", +) +"""Stable left-to-right order used by observations, phases, and recordings.""" + +JAW_BODY_REACTION_NORMALS_LOCAL = ( + (1.0, 0.0, 0.0), + (-1.0, 0.0, 0.0), + (1.0, 0.0, 0.0), + (-1.0, 0.0, 0.0), +) +"""Link-local compressive reaction axes pointing from each jaw face into its solid. + +The active convex inner-face surface normals point out of the jaw solids and +into the channel: jaw 1 ``-X``, jaw 2 ``+X``. A needle contact force acting on +the sensor body has the opposite sign, so these body-reaction projection axes +are jaw 1 ``+X`` and jaw 2 ``-X``. This preserves the contract +``max(0, dot(F_w, n_w))`` for physical compression. +""" + + +class HandoffPhase(IntEnum): + """Ordered physical progress of one needle hand-off. + + ``INITIAL`` is the donor-held reset state awaiting a fresh measured-contact + dwell. It does not mean the needle is geometrically ungrasped. + """ + + INITIAL = 0 + DONOR_HOLD = 1 + CO_HOLD = 2 + RECEIVER_ONLY_HOLD = 3 + RETAINED_LIFT = 4 + + +@configclass +class HandoffPhaseCfg: + """Thresholds for the contact-driven hand-off state machine. + + Forces are in newtons, positions and lift distances in metres, angles in + radians, dwell periods in seconds, and velocity limits in SI units. + """ + + engage_force_n: float = 1.0e-4 + disengage_force_n: float = 5.0e-5 + opposed_normal_tolerance_rad: float = math.radians(20.0) + donor_dwell_s: float = 8.0 / 240.0 + co_hold_dwell_s: float = 8.0 / 240.0 + receiver_only_dwell_s: float = 8.0 / 240.0 + retained_lift_dwell_s: float = 10.0 / 240.0 + receiver_relative_position_target_m: tuple[float, float, float] = (0.0, 0.0, 0.0) + receiver_relative_orientation_target_xyzw: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0) + receiver_relative_position_limit_m: float = 0.035 + receiver_relative_orientation_limit_rad: float = math.radians(60.0) + maximum_linear_velocity_m_s: float = 0.10 + maximum_angular_velocity_rad_s: float = 5.0 + required_lift_delta_z_m: float = 0.015 + + def __post_init__(self) -> None: + if not 0.0 <= self.disengage_force_n < self.engage_force_n: + raise ValueError("contact hysteresis requires 0 <= disengage < engage") + if not 0.0 < self.opposed_normal_tolerance_rad < math.pi: + raise ValueError("opposed_normal_tolerance_rad must lie in (0, pi)") + receiver_position_target = torch.tensor(self.receiver_relative_position_target_m) + receiver_orientation_target = torch.tensor(self.receiver_relative_orientation_target_xyzw) + if receiver_position_target.shape != (3,) or not torch.isfinite(receiver_position_target).all(): + raise ValueError("receiver relative position target must be a finite three-vector") + if ( + receiver_orientation_target.shape != (4,) + or not torch.isfinite(receiver_orientation_target).all() + or torch.linalg.vector_norm(receiver_orientation_target) <= 1.0e-9 + ): + raise ValueError("receiver relative orientation target must be a normalisable xyzw quaternion") + positive_values = ( + self.donor_dwell_s, + self.co_hold_dwell_s, + self.receiver_only_dwell_s, + self.retained_lift_dwell_s, + self.receiver_relative_position_limit_m, + self.receiver_relative_orientation_limit_rad, + self.maximum_linear_velocity_m_s, + self.maximum_angular_velocity_rad_s, + self.required_lift_delta_z_m, + ) + if not all(math.isfinite(value) and value > 0.0 for value in positive_values): + raise ValueError("phase dwell, pose, velocity, and lift limits must be finite and positive") + + +@dataclass(slots=True) +class HandoffMeasurements: + """One post-physics batch consumed by :class:`HandoffPhaseMachine`.""" + + normal_forces_n: torch.Tensor + reaction_normals_w: torch.Tensor + needle_pose_w: torch.Tensor + needle_velocity_w: torch.Tensor + receiver_pose_w: torch.Tensor + + +class HandoffPhaseMachine: + """Vectorised, stateful, measured-contact hand-off evaluator. + + The machine reads filtered normal contact loads and simulated poses. It + never reads the commanded action. All counters and hysteresis state are + per environment and support partial resets. + """ + + def __init__(self, num_envs: int, device: str, step_dt: float, cfg: HandoffPhaseCfg): + if num_envs < 1: + raise ValueError("num_envs must be positive") + if not math.isfinite(step_dt) or step_dt <= 0.0: + raise ValueError("step_dt must be finite and positive") + self.num_envs = num_envs + self.device = device + self.step_dt = step_dt + self.cfg = cfg + self.phase = torch.zeros(num_envs, dtype=torch.long, device=device) + self._donor_engaged = torch.zeros(num_envs, dtype=torch.bool, device=device) + self._receiver_engaged = torch.zeros_like(self._donor_engaged) + self._donor_counter = torch.zeros(num_envs, dtype=torch.long, device=device) + self._co_hold_counter = torch.zeros_like(self._donor_counter) + self._receiver_only_counter = torch.zeros_like(self._donor_counter) + self._retained_lift_counter = torch.zeros_like(self._donor_counter) + self.reset_needle_z_w = torch.zeros(num_envs, dtype=torch.float32, device=device) + self._last_step_token = torch.full((num_envs,), -1, dtype=torch.long, device=device) + + def _required_steps(self, duration_s: float) -> int: + return max(1, math.ceil(duration_s / self.step_dt - 1.0e-12)) + + def reset( + self, + env_ids: torch.Tensor, + reset_needle_z_w: torch.Tensor, + step_token: int | None = None, + ) -> None: + """Reset to donor-held INITIAL and await a fresh post-action contact sample.""" + + env_ids = env_ids.to(device=self.device, dtype=torch.long) + reset_z = reset_needle_z_w.to(device=self.device, dtype=torch.float32).reshape(-1) + if reset_z.shape[0] != env_ids.shape[0] or not torch.isfinite(reset_z).all(): + raise ValueError("reset heights must be one finite value per environment") + self.phase[env_ids] = int(HandoffPhase.INITIAL) + self._donor_engaged[env_ids] = False + self._receiver_engaged[env_ids] = False + self._donor_counter[env_ids] = 0 + self._co_hold_counter[env_ids] = 0 + self._receiver_only_counter[env_ids] = 0 + self._retained_lift_counter[env_ids] = 0 + self.reset_needle_z_w[env_ids] = reset_z + self._last_step_token[env_ids] = -1 if step_token is None else int(step_token) + + def _bilateral_contact( + self, + loads: torch.Tensor, + normals: torch.Tensor, + engaged: torch.Tensor, + ) -> torch.Tensor: + threshold = torch.where( + engaged, + torch.full_like(loads[:, 0], self.cfg.disengage_force_n), + torch.full_like(loads[:, 0], self.cfg.engage_force_n), + ) + force_ok = torch.logical_and(loads[:, 0] >= threshold, loads[:, 1] >= threshold) + unit_normals = torch.nn.functional.normalize(normals, dim=-1, eps=1.0e-12) + normal_dot = torch.sum(unit_normals[:, 0] * unit_normals[:, 1], dim=-1) + opposed = normal_dot <= -math.cos(self.cfg.opposed_normal_tolerance_rad) + finite = torch.isfinite(loads).all(dim=-1) & torch.isfinite(normals).all(dim=(-2, -1)) + return finite & force_ok & opposed + + def _receiver_bounds(self, measurements: HandoffMeasurements) -> torch.Tensor: + needle_pos_r, needle_quat_r = math_utils.subtract_frame_transforms( + measurements.receiver_pose_w[:, :3], + measurements.receiver_pose_w[:, 3:7], + measurements.needle_pose_w[:, :3], + measurements.needle_pose_w[:, 3:7], + ) + position_target = torch.tensor( + self.cfg.receiver_relative_position_target_m, + dtype=needle_pos_r.dtype, + device=needle_pos_r.device, + ) + relative_position_ok = torch.linalg.vector_norm(needle_pos_r - position_target, dim=-1) <= ( + self.cfg.receiver_relative_position_limit_m + ) + unit_quat = torch.nn.functional.normalize(needle_quat_r, dim=-1, eps=1.0e-12) + orientation_target = torch.tensor( + self.cfg.receiver_relative_orientation_target_xyzw, + dtype=unit_quat.dtype, + device=unit_quat.device, + ).repeat(self.num_envs, 1) + orientation_target = torch.nn.functional.normalize(orientation_target, dim=-1, eps=1.0e-12) + relative_angle = math_utils.quat_error_magnitude(unit_quat, orientation_target) + relative_orientation_ok = relative_angle <= self.cfg.receiver_relative_orientation_limit_rad + linear_velocity_ok = torch.linalg.vector_norm(measurements.needle_velocity_w[:, :3], dim=-1) <= ( + self.cfg.maximum_linear_velocity_m_s + ) + angular_velocity_ok = torch.linalg.vector_norm(measurements.needle_velocity_w[:, 3:6], dim=-1) <= ( + self.cfg.maximum_angular_velocity_rad_s + ) + needle_quaternion_valid = torch.linalg.vector_norm(measurements.needle_pose_w[:, 3:7], dim=-1) > 1.0e-9 + receiver_quaternion_valid = torch.linalg.vector_norm(measurements.receiver_pose_w[:, 3:7], dim=-1) > 1.0e-9 + finite = torch.logical_and( + torch.isfinite(measurements.needle_pose_w).all(dim=-1), + torch.isfinite(measurements.needle_velocity_w).all(dim=-1), + ) + finite = finite & torch.isfinite(measurements.receiver_pose_w).all(dim=-1) + return ( + finite + & needle_quaternion_valid + & receiver_quaternion_valid + & relative_position_ok + & relative_orientation_ok + & linear_velocity_ok + & angular_velocity_ok + ) + + @staticmethod + def _count_consecutive(counter: torch.Tensor, condition: torch.Tensor, mask: torch.Tensor) -> None: + counter[mask] = torch.where(condition[mask], counter[mask] + 1, torch.zeros_like(counter[mask])) + + def _clear_progress(self, mask: torch.Tensor, *, after_phase: HandoffPhase) -> None: + """Clear counters downstream of a physical rollback. + + Completed dwell counters are retained only when their corresponding + measured phase remains established. This prevents a partial dwell + before contact loss from being combined with a later, disjoint dwell. + """ + + if after_phase < HandoffPhase.DONOR_HOLD: + self._donor_counter[mask] = 0 + if after_phase < HandoffPhase.CO_HOLD: + self._co_hold_counter[mask] = 0 + if after_phase < HandoffPhase.RECEIVER_ONLY_HOLD: + self._receiver_only_counter[mask] = 0 + if after_phase < HandoffPhase.RETAINED_LIFT: + self._retained_lift_counter[mask] = 0 + + def advance(self, measurements: HandoffMeasurements, step_token: int) -> torch.Tensor: + """Advance each environment at most once for one simulator step token.""" + + if measurements.normal_forces_n.shape != (self.num_envs, 4): + raise ValueError("normal_forces_n must have shape (num_envs, 4)") + if measurements.reaction_normals_w.shape != (self.num_envs, 4, 3): + raise ValueError("reaction_normals_w must have shape (num_envs, 4, 3)") + if measurements.needle_pose_w.shape != (self.num_envs, 7): + raise ValueError("needle_pose_w must have shape (num_envs, 7)") + if measurements.needle_velocity_w.shape != (self.num_envs, 6): + raise ValueError("needle_velocity_w must have shape (num_envs, 6)") + if measurements.receiver_pose_w.shape != (self.num_envs, 7): + raise ValueError("receiver_pose_w must have shape (num_envs, 7)") + active = self._last_step_token != int(step_token) + self._last_step_token[active] = int(step_token) + + donor = self._bilateral_contact( + measurements.normal_forces_n[:, 0:2], + measurements.reaction_normals_w[:, 0:2], + self._donor_engaged, + ) + receiver = self._bilateral_contact( + measurements.normal_forces_n[:, 2:4], + measurements.reaction_normals_w[:, 2:4], + self._receiver_engaged, + ) + self._donor_engaged[active] = donor[active] + self._receiver_engaged[active] = receiver[active] + receiver_bounds = self._receiver_bounds(measurements) + + initial = active & (self.phase == int(HandoffPhase.INITIAL)) + self._count_consecutive(self._donor_counter, donor, initial) + donor_complete = initial & (self._donor_counter >= self._required_steps(self.cfg.donor_dwell_s)) + self.phase[donor_complete] = int(HandoffPhase.DONOR_HOLD) + + donor_phase = active & (self.phase == int(HandoffPhase.DONOR_HOLD)) & ~donor_complete + donor_lost = donor_phase & ~donor + self.phase[donor_lost] = int(HandoffPhase.INITIAL) + self._clear_progress(donor_lost, after_phase=HandoffPhase.INITIAL) + donor_phase = donor_phase & donor + self._count_consecutive(self._co_hold_counter, donor & receiver, donor_phase) + co_hold_complete = donor_phase & (self._co_hold_counter >= self._required_steps(self.cfg.co_hold_dwell_s)) + self.phase[co_hold_complete] = int(HandoffPhase.CO_HOLD) + + co_hold_phase = active & (self.phase == int(HandoffPhase.CO_HOLD)) & ~co_hold_complete + receiver_lost = co_hold_phase & ~receiver + receiver_lost_to_donor = receiver_lost & donor + receiver_lost_to_initial = receiver_lost & ~donor + self.phase[receiver_lost_to_donor] = int(HandoffPhase.DONOR_HOLD) + self._clear_progress(receiver_lost_to_donor, after_phase=HandoffPhase.DONOR_HOLD) + self.phase[receiver_lost_to_initial] = int(HandoffPhase.INITIAL) + self._clear_progress(receiver_lost_to_initial, after_phase=HandoffPhase.INITIAL) + receiver_only_condition = ~donor & receiver & receiver_bounds + self._count_consecutive(self._receiver_only_counter, receiver_only_condition, co_hold_phase & receiver) + receiver_only_complete = co_hold_phase & ( + self._receiver_only_counter >= self._required_steps(self.cfg.receiver_only_dwell_s) + ) + self.phase[receiver_only_complete] = int(HandoffPhase.RECEIVER_ONLY_HOLD) + + receiver_phase = active & (self.phase == int(HandoffPhase.RECEIVER_ONLY_HOLD)) & ~receiver_only_complete + # Receiver-only ownership is a contact fact. A transient pose or + # velocity excursion while both recipient jaw loads remain bilateral + # must not reclassify the free needle as unheld: the bounds still gate + # retained-lift success below. Only a measured loss or reversal of + # physical ownership rolls this phase back. + receiver_regrasped_by_donor = receiver_phase & donor & receiver + receiver_lost_to_donor = receiver_phase & donor & ~receiver + receiver_lost_to_initial = receiver_phase & ~donor & ~receiver + self.phase[receiver_regrasped_by_donor] = int(HandoffPhase.CO_HOLD) + self._clear_progress(receiver_regrasped_by_donor, after_phase=HandoffPhase.CO_HOLD) + self.phase[receiver_lost_to_donor] = int(HandoffPhase.DONOR_HOLD) + self._clear_progress(receiver_lost_to_donor, after_phase=HandoffPhase.DONOR_HOLD) + self.phase[receiver_lost_to_initial] = int(HandoffPhase.INITIAL) + self._clear_progress(receiver_lost_to_initial, after_phase=HandoffPhase.INITIAL) + lifted = measurements.needle_pose_w[:, 2] - self.reset_needle_z_w >= self.cfg.required_lift_delta_z_m + retained_lift_condition = receiver_only_condition & lifted + self._count_consecutive(self._retained_lift_counter, retained_lift_condition, receiver_phase) + lift_complete = receiver_phase & ( + self._retained_lift_counter >= self._required_steps(self.cfg.retained_lift_dwell_s) + ) + self.phase[lift_complete] = int(HandoffPhase.RETAINED_LIFT) + return self.phase + + +def jaw_needle_contact_measurements( + env: ManagerBasedRLEnv, + sensor_names: tuple[str, str, str, str] = JAW_CONTACT_SENSOR_NAMES, +) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """Read four needle-filtered force matrices and body-reaction axes. + + Returns projected compressive loads, world-space reaction axes pointing + into the jaw solids, and the unmodified filtered world-force vectors. The + force matrix is the reaction acting on the jaw sensor body. Each sensor + must contain exactly one jaw body and exactly one needle filter. + """ + + if len(sensor_names) != 4: + raise ValueError("needle pass requires exactly four jaw contact sensors") + loads: list[torch.Tensor] = [] + normals: list[torch.Tensor] = [] + force_vectors: list[torch.Tensor] = [] + for sensor_name, local_normal in zip(sensor_names, JAW_BODY_REACTION_NORMALS_LOCAL, strict=True): + sensor: ContactSensor = env.scene.sensors[sensor_name] + force_matrix_proxy = sensor.data.force_matrix_w + if force_matrix_proxy is None: + actual_shape = None + force_matrix = None + else: + force_matrix = force_matrix_proxy.torch + actual_shape = tuple(force_matrix.shape) + if force_matrix is None or force_matrix.shape != (env.num_envs, 1, 1, 3): + raise RuntimeError( + f"contact sensor {sensor_name!r} must expose a (num_envs, 1, 1, 3) needle force matrix; " + f"got {actual_shape}" + ) + sensor_quat_proxy = sensor.data.quat_w + if sensor_quat_proxy is None or sensor_quat_proxy.torch.shape != (env.num_envs, 1, 4): + raise RuntimeError(f"contact sensor {sensor_name!r} must track its world pose") + sensor_quat_w = sensor_quat_proxy.torch + force_w = force_matrix[:, 0, 0, :] + local_normal_tensor = torch.tensor(local_normal, dtype=force_w.dtype, device=force_w.device).repeat( + env.num_envs, 1 + ) + body_reaction_normal_w = math_utils.quat_apply(sensor_quat_w[:, 0, :], local_normal_tensor) + # F_w acts on the jaw body. The body-reaction axis points from the + # channel face into the jaw solid, so physical compression is exactly + # F_n = max(0, dot(F_w, n_w)); J_n for one step is F_n * sim.dt. + compressive_load = torch.clamp(torch.sum(force_w * body_reaction_normal_w, dim=-1), min=0.0) + loads.append(compressive_load) + normals.append(body_reaction_normal_w) + force_vectors.append(force_w) + return torch.stack(loads, dim=-1), torch.stack(normals, dim=1), torch.stack(force_vectors, dim=1) + + +def _asset_pose_w(asset: Articulation, body_name: str) -> torch.Tensor: + body_ids, body_names = asset.find_bodies(body_name) + if len(body_ids) != 1: + raise RuntimeError(f"expected one {body_name!r} body, found {body_names}") + return torch.cat( + (asset.data.body_pos_w.torch[:, body_ids[0]], asset.data.body_quat_w.torch[:, body_ids[0]]), dim=-1 + ) + + +def get_handoff_phase_machine(env: ManagerBasedRLEnv, phase_cfg: HandoffPhaseCfg) -> HandoffPhaseMachine: + """Return the environment-owned phase machine, constructing it once.""" + + attribute_name = "_needle_pass_handoff_phase_machine" + machine = getattr(env, attribute_name, None) + if machine is None: + machine = HandoffPhaseMachine(env.num_envs, env.device, env.step_dt, phase_cfg) + setattr(env, attribute_name, machine) + elif machine.cfg != phase_cfg: + raise RuntimeError("needle-pass manager terms must share one HandoffPhaseCfg") + return machine + + +def update_handoff_phase( + env: ManagerBasedRLEnv, + phase_cfg: HandoffPhaseCfg, + needle_cfg: SceneEntityCfg = SceneEntityCfg("needle"), + receiver_cfg: SceneEntityCfg = SceneEntityCfg("right_psm"), + receiver_body_name: str = "psm_tool_tip_link", +) -> HandoffPhaseMachine: + """Update the shared phase machine idempotently from post-physics buffers.""" + + machine = get_handoff_phase_machine(env, phase_cfg) + step_token = int(env.common_step_counter) + cache_attribute = "_needle_pass_handoff_phase_sample_step_token" + if getattr(env, cache_attribute, None) == step_token: + return machine + loads, normals, _ = jaw_needle_contact_measurements(env) + needle: RigidObject = env.scene[needle_cfg.name] + receiver: Articulation = env.scene[receiver_cfg.name] + machine.advance( + HandoffMeasurements( + normal_forces_n=loads, + reaction_normals_w=normals, + needle_pose_w=torch.cat((needle.data.root_pos_w.torch, needle.data.root_quat_w.torch), dim=-1), + needle_velocity_w=torch.cat((needle.data.root_lin_vel_w.torch, needle.data.root_ang_vel_w.torch), dim=-1), + receiver_pose_w=_asset_pose_w(receiver, receiver_body_name), + ), + step_token=step_token, + ) + setattr(env, cache_attribute, step_token) + return machine + + +def reset_handoff_phase( + env: ManagerBasedRLEnv, + env_ids: torch.Tensor, + reset_needle_z_w: torch.Tensor, + phase_cfg: HandoffPhaseCfg, +) -> None: + """Partially reset state and the reset-relative height reference.""" + + get_handoff_phase_machine(env, phase_cfg).reset( + env_ids, + reset_needle_z_w, + step_token=env.common_step_counter, + ) + + +def success(env: ManagerBasedRLEnv, phase_cfg: HandoffPhaseCfg) -> torch.Tensor: + """Return true only after the measured retained-lift dwell completes.""" + + machine = update_handoff_phase(env, phase_cfg) + return machine.phase == int(HandoffPhase.RETAINED_LIFT) + + +def needle_dropped_or_out_of_bounds( + env: ManagerBasedRLEnv, + phase_cfg: HandoffPhaseCfg, + needle_cfg: SceneEntityCfg = SceneEntityCfg("needle"), + drop_distance_m: float = 0.12, + horizontal_distance_m: float = 0.45, +) -> torch.Tensor: + """Terminate a physically dropped needle separately from success.""" + + if drop_distance_m <= 0.0 or horizontal_distance_m <= 0.0: + raise ValueError("drop and horizontal bounds must be positive") + machine = update_handoff_phase(env, phase_cfg) + needle: RigidObject = env.scene[needle_cfg.name] + needle_root_pos_w = needle.data.root_pos_w.torch + dropped = needle_root_pos_w[:, 2] < machine.reset_needle_z_w - drop_distance_m + horizontal_offset = needle_root_pos_w[:, :2] - env.scene.env_origins[:, :2] + out_of_bounds = torch.linalg.vector_norm(horizontal_offset, dim=-1) > horizontal_distance_m + non_finite = ~torch.isfinite(needle.data.root_pose_w.torch).all(dim=-1) + non_finite |= ~torch.isfinite(needle.data.root_vel_w.torch).all(dim=-1) + return dropped | out_of_bounds | non_finite + + +__all__ = [ + "HandoffMeasurements", + "HandoffPhase", + "HandoffPhaseCfg", + "HandoffPhaseMachine", + "JAW_CONTACT_SENSOR_NAMES", + "JAW_BODY_REACTION_NORMALS_LOCAL", + "get_handoff_phase_machine", + "jaw_needle_contact_measurements", + "needle_dropped_or_out_of_bounds", + "reset_handoff_phase", + "success", + "update_handoff_phase", +] diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/needle_pass_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/needle_pass_env_cfg.py new file mode 100644 index 000000000000..5f121332cbec --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/needle_pass_env_cfg.py @@ -0,0 +1,420 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""General scene and measured task semantics for dVRK needle pass.""" + +from __future__ import annotations + +from collections.abc import Callable +from dataclasses import MISSING +from typing import Any + +from isaaclab_physx.physics import PhysxCfg +from isaaclab_physx.sim.schemas import PhysxCollisionPropertiesCfg, PhysxRigidBodyPropertiesCfg +from isaaclab_physx.sim.spawners.materials import PhysxRigidBodyMaterialCfg +from isaaclab_teleop import XrCfg + +from pxr import Usd, UsdPhysics + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import ContactSensorCfg +from isaaclab.sim.spawners.from_files import UsdFileCfg, spawn_from_usd +from isaaclab.sim.utils import bind_physics_material, find_matching_prim_paths, get_current_stage +from isaaclab.utils.configclass import configclass + +from isaaclab_tasks.utils import PresetCfg + +from . import mdp +from .assets import ( + NEEDLE_ASSET, + NEEDLE_DYNAMIC_FRICTION, + NEEDLE_FRICTION_COMBINE_MODE, + NEEDLE_MASS_KG, + NEEDLE_RESTITUTION, + NEEDLE_RESTITUTION_COMBINE_MODE, + NEEDLE_SCALE, + NEEDLE_STATIC_FRICTION, + SUTURE_PAD_ASSET, +) + +MAXIMUM_COMMANDED_ACCELERATION_M_S2 = 0.5 +RETENTION_LOAD_SAFETY_FACTOR = 2.0 +RETENTION_FRICTION_CONE_FACETS = mdp.FORCE_CLOSURE_CONE_FACETS +# The pinned PSM material at /psm/Looks/PhysicsMaterial authors these +# coefficients on both jaw collision bindings and no combine mode (PhysX's +# default is average). Its dynamic coefficient of 10.0 is not a defensible +# steel/steel task input. The needle material therefore declares PhysX's +# higher-priority ``min`` mode, resolving the pair to the task's dry +# steel/steel coefficients while leaving the shared PSM asset untouched. +# Static force closure uses the resolved static coefficient; the dynamic value +# is retained for runtime provenance. +DVRK_JAW_AUTHORED_STATIC_FRICTION = 1.0 +DVRK_JAW_AUTHORED_DYNAMIC_FRICTION = 10.0 +RESOLVED_JAW_NEEDLE_STATIC_FRICTION = min(NEEDLE_STATIC_FRICTION, DVRK_JAW_AUTHORED_STATIC_FRICTION) +RESOLVED_JAW_NEEDLE_DYNAMIC_FRICTION = min(NEEDLE_DYNAMIC_FRICTION, DVRK_JAW_AUTHORED_DYNAMIC_FRICTION) +REQUIRED_RETENTION_LOAD = mdp.required_retention_load( + mass_kg=NEEDLE_MASS_KG, + gravity_m_s2=9.81, + maximum_commanded_acceleration_m_s2=MAXIMUM_COMMANDED_ACCELERATION_M_S2, + friction_coefficient=RESOLVED_JAW_NEEDLE_STATIC_FRICTION, + safety_factor=RETENTION_LOAD_SAFETY_FACTOR, +) +"""Conservative per-jaw load implied by the declared mass/friction inputs.""" + +HANDOFF_PHASE_CFG = mdp.HandoffPhaseCfg( + engage_force_n=REQUIRED_RETENTION_LOAD.normal_force_per_jaw_n, + disengage_force_n=0.5 * REQUIRED_RETENTION_LOAD.normal_force_per_jaw_n, +) +"""One load-backed threshold object shared by every phase-dependent term.""" + + +@configclass +class NeedlePassPhysicsCfg(PresetCfg): + """PhysX presets for the contact-driven needle-pass task.""" + + default: PhysxCfg = PhysxCfg( + solver_type=1, + solve_articulation_contact_last=True, + enable_external_forces_every_iteration=True, + enable_enhanced_determinism=True, + min_position_iteration_count=4, + min_velocity_iteration_count=1, + bounce_threshold_velocity=0.01, + friction_correlation_distance=0.002, + ) + physx: PhysxCfg = default + + +def spawn_usd_with_rigid_material( + prim_path: str, + cfg: UsdFileWithRigidMaterialCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + **kwargs: Any, +) -> Usd.Prim: + """Spawn a USD and strongly bind one explicit rigid-body material. + + Current Isaac Lab ``UsdFileCfg`` does not expose a physics-material field. + The stock USD spawner first creates/clones the asset; this wrapper then + creates one material beneath every resolved clone and recursively binds it + to collision descendants. Binding happens during scene construction, not + during reset, and therefore cannot write or adapt needle state. + """ + + # The pinned needle authors its rigid body on a descendant without a + # ``MassAPI``. The stock USD spawner only modifies existing mass schemas, + # so applying ``mass_props`` at the referenced asset root is a no-op. Defer + # mass authoring until the unique rigid-body descendant has been resolved. + spawn_cfg = cfg.replace(mass_props=None) + prim = spawn_from_usd(prim_path, spawn_cfg, translation, orientation, **kwargs) + resolved_prim_paths = find_matching_prim_paths(prim_path) + if not resolved_prim_paths: + raise RuntimeError(f"USD material binding resolved no prims for {prim_path!r}") + stage = get_current_stage() + for resolved_prim_path in resolved_prim_paths: + material_path = f"{resolved_prim_path}/physicsMaterial" + cfg.physics_material.func(material_path, cfg.physics_material) + bind_physics_material( + resolved_prim_path, + material_path, + stronger_than_descendants=True, + ) + if cfg.mass_props is not None: + root_prim = stage.GetPrimAtPath(resolved_prim_path) + rigid_body_prims = [prim for prim in Usd.PrimRange(root_prim) if prim.HasAPI(UsdPhysics.RigidBodyAPI)] + if len(rigid_body_prims) != 1: + raise RuntimeError( + f"needle physical-property binding expected one rigid body beneath {resolved_prim_path!r}, " + f"found {[str(prim.GetPath()) for prim in rigid_body_prims]}" + ) + rigid_body_prim = rigid_body_prims[0] + sim_utils.define_mass_properties(str(rigid_body_prim.GetPath()), cfg.mass_props, stage=stage) + return prim + + +@configclass +class UsdFileWithRigidMaterialCfg(UsdFileCfg): + """Task-local USD spawner with an explicit rigid-body material binding.""" + + func: Callable = spawn_usd_with_rigid_material + physics_material: PhysxRigidBodyMaterialCfg = MISSING + + +@configclass +class NeedlePassSceneCfg(InteractiveSceneCfg): + """Two fixed PSMs, one free needle, and four filtered jaw sensors.""" + + left_psm: ArticulationCfg = MISSING + right_psm: ArticulationCfg = MISSING + + # Keep the spawned root name distinct from the pinned USD's nested + # ``Needle/Needle`` children. PhysX globs allow ``*`` to span path + # separators, so a repeated leaf name makes the environment wildcard + # resolve the root and both descendants as separate filter entries. + needle = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/SutureNeedle", + spawn=UsdFileWithRigidMaterialCfg( + usd_path=NEEDLE_ASSET.url, + scale=NEEDLE_SCALE, + activate_contact_sensors=True, + rigid_props=PhysxRigidBodyPropertiesCfg( + rigid_body_enabled=True, + kinematic_enabled=False, + disable_gravity=False, + linear_damping=0.01, + angular_damping=0.01, + max_depenetration_velocity=1.0, + solver_position_iteration_count=16, + solver_velocity_iteration_count=4, + ), + collision_props=PhysxCollisionPropertiesCfg(collision_enabled=True), + mass_props=sim_utils.MassPropertiesCfg(mass=NEEDLE_MASS_KG), + physics_material=PhysxRigidBodyMaterialCfg( + static_friction=NEEDLE_STATIC_FRICTION, + dynamic_friction=NEEDLE_DYNAMIC_FRICTION, + restitution=NEEDLE_RESTITUTION, + friction_combine_mode=NEEDLE_FRICTION_COMBINE_MODE, + restitution_combine_mode=NEEDLE_RESTITUTION_COMBINE_MODE, + ), + ), + # The dVRK-specific configuration replaces this with its pinned native + # grasp-generator pose inside the donor's closed jaws. + init_state=RigidObjectCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.10), + rot=(0.0, 0.0, 0.0, 1.0), + lin_vel=(0.0, 0.0, 0.0), + ang_vel=(0.0, 0.0, 0.0), + ), + ) + + # The pad is deliberately outside the reset, hand-off, and vertical drop + # regions. It cannot support an open-jaw counterfactual needle. + suture_pad = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/SuturePad", + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.45, 0.45, -0.20)), + spawn=sim_utils.UsdFileCfg(usd_path=SUTURE_PAD_ASSET.url), + ) + + ground = AssetBaseCfg( + prim_path="/World/GroundPlane", + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -0.50)), + spawn=sim_utils.GroundPlaneCfg(), + ) + + light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(color=(0.85, 0.85, 0.85), intensity=2500.0), + ) + key_light = AssetBaseCfg( + prim_path="/World/KeyLight", + spawn=sim_utils.DistantLightCfg(color=(0.75, 0.75, 0.75), intensity=1500.0), + init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, 500.0)), + ) + + left_jaw_1_needle_contact = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/LeftPSM/psm_tool_gripper1_link", + filter_prim_paths_expr=["{ENV_REGEX_NS}/SutureNeedle"], + track_pose=True, + update_period=0.0, + ) + left_jaw_2_needle_contact = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/LeftPSM/psm_tool_gripper2_link", + filter_prim_paths_expr=["{ENV_REGEX_NS}/SutureNeedle"], + track_pose=True, + update_period=0.0, + ) + right_jaw_1_needle_contact = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/RightPSM/psm_tool_gripper1_link", + filter_prim_paths_expr=["{ENV_REGEX_NS}/SutureNeedle"], + track_pose=True, + update_period=0.0, + ) + right_jaw_2_needle_contact = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/RightPSM/psm_tool_gripper2_link", + filter_prim_paths_expr=["{ENV_REGEX_NS}/SutureNeedle"], + track_pose=True, + update_period=0.0, + ) + + +@configclass +class ActionsCfg: + """Stable ``7 + 2 + 7 + 2`` dVRK bimanual action declaration.""" + + left_arm_action: mdp.WorldFrameDifferentialInverseKinematicsActionCfg = MISSING + left_jaw_action: mdp.PairedJawJointPositionActionCfg = MISSING + right_arm_action: mdp.WorldFrameDifferentialInverseKinematicsActionCfg = MISSING + right_jaw_action: mdp.PairedJawJointPositionActionCfg = MISSING + + +@configclass +class ObservationsCfg: + """Unconcatenated observations for policy recording and subtask display.""" + + @configclass + class PolicyCfg(ObsGroup): + left_joint_pos = ObsTerm( + func=mdp.joint_position, + params={"asset_cfg": SceneEntityCfg("left_psm")}, + ) + left_joint_vel = ObsTerm( + func=mdp.joint_velocity, + params={"asset_cfg": SceneEntityCfg("left_psm")}, + ) + right_joint_pos = ObsTerm( + func=mdp.joint_position, + params={"asset_cfg": SceneEntityCfg("right_psm")}, + ) + right_joint_vel = ObsTerm( + func=mdp.joint_velocity, + params={"asset_cfg": SceneEntityCfg("right_psm")}, + ) + left_ee_pose_w = ObsTerm( + func=mdp.end_effector_pose_w, + params={"asset_cfg": SceneEntityCfg("left_psm")}, + ) + right_ee_pose_w = ObsTerm( + func=mdp.end_effector_pose_w, + params={"asset_cfg": SceneEntityCfg("right_psm")}, + ) + needle_pose_w = ObsTerm(func=mdp.needle_pose_w) + needle_velocity_w = ObsTerm(func=mdp.needle_velocity_w) + jaw_needle_contact_force = ObsTerm(func=mdp.jaw_needle_contact_force) + handoff_phase = ObsTerm(func=mdp.handoff_phase, params={"phase_cfg": HANDOFF_PHASE_CFG}) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + @configclass + class SubtaskCfg(ObsGroup): + donor_hold = ObsTerm( + func=mdp.phase_at_least, + params={"phase_cfg": HANDOFF_PHASE_CFG, "phase": mdp.HandoffPhase.DONOR_HOLD}, + ) + co_hold = ObsTerm( + func=mdp.phase_at_least, + params={"phase_cfg": HANDOFF_PHASE_CFG, "phase": mdp.HandoffPhase.CO_HOLD}, + ) + receiver_only_hold = ObsTerm( + func=mdp.phase_at_least, + params={"phase_cfg": HANDOFF_PHASE_CFG, "phase": mdp.HandoffPhase.RECEIVER_ONLY_HOLD}, + ) + retained_lift = ObsTerm( + func=mdp.phase_at_least, + params={"phase_cfg": HANDOFF_PHASE_CFG, "phase": mdp.HandoffPhase.RETAINED_LIFT}, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = False + + policy: PolicyCfg = PolicyCfg() + subtask_terms: SubtaskCfg = SubtaskCfg() + + +@configclass +class EventCfg: + """Deterministic reset with no physics settling inside the event.""" + + reset_all = EventTerm( + func=mdp.reset_needle_pass_to_default, + mode="reset", + params={"phase_cfg": HANDOFF_PHASE_CFG}, + ) + + +@configclass +class RewardsCfg: + """Measured phase progress rewards; reward is not used to establish success.""" + + phase_progress = RewTerm( + func=mdp.handoff_phase_progress, + weight=1.0, + params={"phase_cfg": HANDOFF_PHASE_CFG}, + ) + retained_lift = RewTerm( + func=mdp.retained_lift_bonus, + weight=5.0, + params={"phase_cfg": HANDOFF_PHASE_CFG}, + ) + + +@configclass +class TerminationsCfg: + """Recorder-compatible success and separate physical failure terms.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + success = DoneTerm(func=mdp.success, params={"phase_cfg": HANDOFF_PHASE_CFG}) + needle_dropped_or_out_of_bounds = DoneTerm( + func=mdp.needle_dropped_or_out_of_bounds, + params={"phase_cfg": HANDOFF_PHASE_CFG}, + ) + + +@configclass +class NeedlePassEnvCfg(ManagerBasedRLEnvCfg): + """Manager-based needle pass that starts held by the donor and transfers by contact.""" + + scene: NeedlePassSceneCfg = NeedlePassSceneCfg( + num_envs=256, + env_spacing=1.25, + replicate_physics=True, + ) + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + events: EventCfg = EventCfg() + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + + commands = None + curriculum = None + + xr: XrCfg = XrCfg(anchor_pos=(0.0, -0.45, -0.10)) + + def __post_init__(self): + self.seed = 42 + self.decimation = 1 + self.episode_length_s = 30.0 + self.sim.dt = 1.0 / 240.0 + self.sim.render_interval = 1 + self.sim.physics = NeedlePassPhysicsCfg() + # A near-overhead surgical view keeps both PSM jaws and the free needle + # visible during the exchange; the previous oblique view let the right + # arm occlude the channel contact in recorded validation episodes. + self.viewer.eye = (0.0, -0.12, 0.45) + self.viewer.lookat = (0.0, 0.0, 0.055) + + +__all__ = [ + "ActionsCfg", + "EventCfg", + "HANDOFF_PHASE_CFG", + "DVRK_JAW_AUTHORED_DYNAMIC_FRICTION", + "DVRK_JAW_AUTHORED_STATIC_FRICTION", + "MAXIMUM_COMMANDED_ACCELERATION_M_S2", + "NeedlePassEnvCfg", + "NeedlePassPhysicsCfg", + "NeedlePassSceneCfg", + "ObservationsCfg", + "RewardsCfg", + "REQUIRED_RETENTION_LOAD", + "RETENTION_FRICTION_CONE_FACETS", + "RESOLVED_JAW_NEEDLE_DYNAMIC_FRICTION", + "RESOLVED_JAW_NEEDLE_STATIC_FRICTION", + "RETENTION_LOAD_SAFETY_FACTOR", + "TerminationsCfg", + "UsdFileWithRigidMaterialCfg", + "spawn_usd_with_rigid_material", +] diff --git a/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py index a5da603ae3f7..34c703a9b084 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py @@ -162,6 +162,7 @@ def parse_env_cfg( Raises: RuntimeError: If the configuration for the task is not a class. We assume users always use a class for the environment configuration. + ValueError: If the task requires CUDA and a non-CUDA simulation device is requested. """ # load the default configuration cfg = load_cfg_from_registry(task_name.split(":")[-1], "env_cfg_entry_point") @@ -178,8 +179,18 @@ def parse_env_cfg( # is replaced by its .default. cfg = resolve_presets(cfg) + # Some contact-rich tasks are qualified only against CUDA PhysX. Reject an + # unsupported override instead of silently changing their physical contract. + if getattr(cfg, "requires_cuda", False) and not device.startswith("cuda"): + raise ValueError(f"Task '{task_name}' requires a CUDA simulation device, got {device!r}") + # simulation device cfg.sim.device = device + # Keep unified IsaacTeleop output tensors on the same device as the + # environment when a caller overrides the configuration default. + isaac_teleop_cfg = getattr(cfg, "isaac_teleop", None) + if isaac_teleop_cfg is not None: + isaac_teleop_cfg.sim_device = device # disable fabric to read/write through USD if use_fabric is not None: cfg.sim.use_fabric = use_fabric diff --git a/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass.py b/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass.py new file mode 100644 index 000000000000..ad69aa02e973 --- /dev/null +++ b/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass.py @@ -0,0 +1,1050 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Focused contracts for the manager-based dVRK needle-pass task.""" + +from types import SimpleNamespace + +from isaaclab.app import AppLauncher + +app_launcher = AppLauncher(headless=True, enable_cameras=False) +simulation_app = app_launcher.app + +import gymnasium as gym +import numpy as np +import pytest +import torch + +from isaaclab.managers import ObservationTermCfg + +import isaaclab_tasks # noqa: F401 +from isaaclab_tasks.contrib.needle_pass import assets +from isaaclab_tasks.contrib.needle_pass.config.dvrk.ik_abs_env_cfg import ( + DONOR_GRASP_CANDIDATE_INDEX, + DONOR_GRASP_CLOSEDNESS, + DONOR_GRASP_CONTACT_POINTS_N_M, + DONOR_GRASP_JAW_POS, + DONOR_GRASP_NATIVE_SEED_POS, + DONOR_GRASP_NATIVE_SEED_ROT_XYZW, + DONOR_GRASP_OUTWARD_NORMALS_N, + DONOR_GRASP_T_N_C_POS_M, + DONOR_GRASP_T_N_C_ROT_XYZW, + DONOR_HELD_RESET_JAW_POS, + DVRK_HANDOFF_PHASE_CFG, + DVRK_JAW_CHANNEL_T_T_C_POS_M, + DVRK_JAW_CHANNEL_T_T_C_ROT_XYZW, + DVRK_NEEDLE_PASS_JAW_ACTUATOR, + ISAAC_GRASP_ASSET_SHA256, + ISAAC_GRASP_CANDIDATES_SHA256, + ISAAC_GRASP_CONFIG_SHA256, + ISAAC_GRASP_GENERATOR_API, + ISAAC_GRASP_GENERATOR_CANDIDATE_COUNT, + ISAAC_GRASP_GENERATOR_CENTRE_COUNT, + ISAAC_GRASP_GENERATOR_EXTENSION, + ISAAC_GRASP_GENERATOR_EXTENSION_VERSION, + ISAAC_GRASP_GENERATOR_ORIENTATIONS_PER_CENTRE, + ISAAC_GRASP_GENERATOR_SEED, + ISAAC_GRASP_GENERATOR_SIM_VERSION, + ISAAC_GRASP_MANIFEST_SHA256, + ISAAC_GRASP_WRAPPER_SHA256, + LEFT_TOOL_HOME_POS_W, + LEFT_TOOL_HOME_ROT_XYZW, + NEEDLE_RESET_POS, + NEEDLE_RESET_ROT_XYZW, + RECEIVER_ACQUISITION_NEEDLE_POS_W, + RECEIVER_ACQUISITION_NEEDLE_ROT_XYZW, + RECEIVER_GRASP_CANDIDATE_INDEX, + RECEIVER_GRASP_T_N_C_POS_M, + RECEIVER_GRASP_T_N_C_ROT_XYZW, + RECEIVER_NEEDLE_TARGET_POS_T, + RECEIVER_NEEDLE_TARGET_ROT_XYZW, + RECEIVER_TOOL_TARGET_POS_W, + RECEIVER_TOOL_TARGET_ROT_XYZW, + RIGHT_TOOL_HOME_POS_W, + DVRKNeedlePassEnvCfg, +) +from isaaclab_tasks.contrib.needle_pass.mdp.actions import ( + DonorReleaseGuardedPairedJawJointPositionAction, + DonorReleaseGuardedPairedJawJointPositionActionCfg, + donor_opening_requested, + donor_release_is_allowed, + world_pose_xyzw_to_root_pose_xyzw, +) +from isaaclab_tasks.contrib.needle_pass.mdp.events import reset_needle_pass_to_default +from isaaclab_tasks.contrib.needle_pass.mdp.grasp_solver import ( + EXACT_POINT_CONTACT_FORCE_RESIDUAL_TOLERANCE_N, + EXACT_POINT_CONTACT_MOMENT_ARM_TOLERANCE_M, + FORCE_CLOSURE_CONE_FACETS, + assess_finite_contact_acceptance, + friction_cone_generators, + impedance_gains, + prove_two_contact_force_closure, + required_retention_load, + two_contact_friction_wrench_generators, +) +from isaaclab_tasks.contrib.needle_pass.mdp.terminations import ( + JAW_BODY_REACTION_NORMALS_LOCAL, + JAW_CONTACT_SENSOR_NAMES, + HandoffMeasurements, + HandoffPhase, + HandoffPhaseCfg, + HandoffPhaseMachine, + jaw_needle_contact_measurements, + update_handoff_phase, +) +from isaaclab_tasks.contrib.needle_pass.needle_pass_env_cfg import ( + DVRK_JAW_AUTHORED_DYNAMIC_FRICTION, + HANDOFF_PHASE_CFG, + REQUIRED_RETENTION_LOAD, + RESOLVED_JAW_NEEDLE_DYNAMIC_FRICTION, + RESOLVED_JAW_NEEDLE_STATIC_FRICTION, + RETENTION_FRICTION_CONE_FACETS, + NeedlePassPhysicsCfg, + UsdFileWithRigidMaterialCfg, +) +from isaaclab_tasks.utils import parse_env_cfg + +TASK_ID = "IsaacContrib-NeedlePass-dVRK-IK-Abs" + + +def _proxy(tensor: torch.Tensor) -> SimpleNamespace: + """Model the develop-era ProxyArray interface in pure unit mocks.""" + + return SimpleNamespace(torch=tensor) + + +def _term_names(group) -> list[str]: + return [name for name, value in vars(group).items() if isinstance(value, ObservationTermCfg)] + + +def test_registration_action_order_and_observation_names(): + """Keep the task ID, 18D order, recorder names, and teleop config stable.""" + + assert gym.spec(TASK_ID).entry_point == "isaaclab.envs:ManagerBasedRLEnv" + cfg = DVRKNeedlePassEnvCfg() + assert isinstance(cfg.scene.needle.spawn, UsdFileWithRigidMaterialCfg) + assert cfg.scene.needle.spawn.func.__name__ == "spawn_usd_with_rigid_material" + assert list(vars(cfg.actions)) == [ + "left_arm_action", + "left_jaw_action", + "right_arm_action", + "right_jaw_action", + ] + assert cfg.actions.left_arm_action.controller.command_type == "pose" + assert cfg.actions.left_arm_action.controller.use_relative_mode is False + assert len(cfg.actions.left_jaw_action.joint_names) == 2 + assert cfg.actions.right_arm_action.controller.command_type == "pose" + assert cfg.actions.right_arm_action.controller.use_relative_mode is False + assert len(cfg.actions.right_jaw_action.joint_names) == 2 + assert cfg.actions.left_jaw_action.scale == 1.0 + assert cfg.actions.left_jaw_action.offset == 0.0 + assert cfg.actions.left_jaw_action.use_default_offset is False + assert cfg.actions.left_jaw_action.preserve_order is True + assert isinstance(cfg.actions.left_jaw_action, DonorReleaseGuardedPairedJawJointPositionActionCfg) + assert cfg.actions.left_jaw_action.phase_cfg == DVRK_HANDOFF_PHASE_CFG + assert DVRK_HANDOFF_PHASE_CFG.opposed_normal_tolerance_rad == pytest.approx(np.deg2rad(25.0)) + assert cfg.actions.left_jaw_action.release_aperture_threshold_rad == pytest.approx(0.0) + assert cfg.actions.left_jaw_action.hold_jaw_pos == DONOR_GRASP_JAW_POS + assert cfg.actions.right_jaw_action.preserve_order is True + assert cfg.isaac_teleop is not None + assert cfg.teleop_devices.devices == {} + + assert _term_names(cfg.observations.policy) == [ + "left_joint_pos", + "left_joint_vel", + "right_joint_pos", + "right_joint_vel", + "left_ee_pose_w", + "right_ee_pose_w", + "needle_pose_w", + "needle_velocity_w", + "jaw_needle_contact_force", + "handoff_phase", + ] + assert _term_names(cfg.observations.subtask_terms) == [ + "donor_hold", + "co_hold", + "receiver_only_hold", + "retained_lift", + ] + assert cfg.observations.policy.concatenate_terms is False + assert cfg.observations.subtask_terms.concatenate_terms is False + assert cfg.terminations.success.func.__name__ == "success" + assert cfg.terminations.success.params["phase_cfg"] == DVRK_HANDOFF_PHASE_CFG + assert cfg.terminations.success.params["phase_cfg"].receiver_relative_position_target_m == ( + RECEIVER_NEEDLE_TARGET_POS_T + ) + assert cfg.terminations.success.params["phase_cfg"].receiver_relative_orientation_target_xyzw == ( + RECEIVER_NEEDLE_TARGET_ROT_XYZW + ) + assert cfg.terminations.success.params["phase_cfg"].receiver_relative_position_limit_m == 0.003 + assert cfg.terminations.success.params["phase_cfg"].receiver_relative_orientation_limit_rad == pytest.approx( + np.deg2rad(15.0) + ) + + +def test_config_construction_does_not_contact_asset_host(monkeypatch): + """Config discovery and registry inspection must remain offline-safe.""" + + assets.verify_remote_asset_sha256.cache_clear() + monkeypatch.setattr( + assets, + "urlopen", + lambda *_args, **_kwargs: (_ for _ in ()).throw(AssertionError("config attempted a network request")), + ) + + cfg = DVRKNeedlePassEnvCfg() + + assert cfg.scene.needle.spawn.usd_path == assets.NEEDLE_ASSET.url + + +def test_cuda_configures_dvrk_teleoperation(): + """Keep the stock recorder's teleop action on the required CUDA device.""" + + device = "cuda:1" + cfg = parse_env_cfg(TASK_ID, device=device, num_envs=1) + + assert cfg.sim.device == device + assert cfg.isaac_teleop.sim_device == device + assert cfg.isaac_teleop.pipeline_builder is not None + assert cfg.teleop_devices.devices == {} + + +def test_dvrk_task_rejects_cpu_override(): + with pytest.raises(ValueError, match="requires a CUDA simulation device"): + parse_env_cfg(TASK_ID, device="cpu", num_envs=1) + + +def test_asset_pins_and_explicit_needle_physics(): + assert assets.I4H_CATALOGUE_LICENCE == "Apache-2.0" + assert assets.I4H_CATALOGUE_LICENCE_URL.endswith("/blob/v0.6.0/LICENSE") + assert assets.NEEDLE_ASSET.key.endswith("needle_sdf.usd") + assert assets.NEEDLE_ASSET.sha256 == "2b317a61f93631a7192e7ed2839ef20f7a75c05aa5f84a3905696134a64f36d7" + assert assets.SUTURE_PAD_ASSET.sha256 == "1c6e4624097fbf8ffc49131539e9eec72d96c5cf68916fbe04eefff1e9522a51" + assert assets.NEEDLE_SCALE == (0.4, 0.4, 0.4) + np.testing.assert_allclose( + assets.NEEDLE_BODY_LOCAL_AABB_MIN_M, + np.asarray(assets.NEEDLE_SOURCE_AABB_MIN_M) * np.asarray(assets.NEEDLE_SCALE), + atol=0.0, + rtol=0.0, + ) + np.testing.assert_allclose( + assets.NEEDLE_BODY_LOCAL_AABB_MAX_M, + np.asarray(assets.NEEDLE_SOURCE_AABB_MAX_M) * np.asarray(assets.NEEDLE_SCALE), + atol=0.0, + rtol=0.0, + ) + np.testing.assert_allclose( + assets.NEEDLE_BODY_LOCAL_EXTENT_M, + (0.020065199650707657, 0.03957919925451279, 0.001651600003242493), + atol=1.0e-15, + rtol=0.0, + ) + assert np.isclose(assets.NEEDLE_SOURCE_VOLUME_M3, 2.085934204311373e-6) + assert assets.NEEDLE_REFERENCE_DENSITY_KG_M3 == 8000.0 + assert np.isclose(assets.NEEDLE_MASS_KG, 0.0010679983126074231) + assert np.isclose( + assets.NEEDLE_MASS_KG, + assets.NEEDLE_SOURCE_VOLUME_M3 * np.prod(assets.NEEDLE_SCALE) * assets.NEEDLE_REFERENCE_DENSITY_KG_M3, + ) + np.testing.assert_allclose( + assets.NEEDLE_CENTRE_OF_MASS_BODY_LOCAL_M, + np.asarray(assets.NEEDLE_SOURCE_CENTRE_OF_MASS_M) * np.asarray(assets.NEEDLE_SCALE), + atol=0.0, + rtol=0.0, + ) + np.testing.assert_allclose( + assets.NEEDLE_CENTRE_OF_MASS_BODY_LOCAL_M, + (0.0064017449816068, 0.0004139220109208, 0.0003823855658992), + atol=1.0e-15, + rtol=0.0, + ) + assert 0.0 < assets.NEEDLE_DYNAMIC_FRICTION <= assets.NEEDLE_STATIC_FRICTION + + cfg = DVRKNeedlePassEnvCfg() + assert cfg.scene.needle.spawn.rigid_props.kinematic_enabled is False + assert cfg.scene.needle.spawn.rigid_props.disable_gravity is False + assert cfg.scene.needle.spawn.collision_props.collision_enabled is True + assert cfg.scene.needle.spawn.mass_props.mass == assets.NEEDLE_MASS_KG + assert cfg.scene.needle.spawn.physics_material.static_friction == assets.NEEDLE_STATIC_FRICTION + assert cfg.scene.needle.spawn.physics_material.dynamic_friction == assets.NEEDLE_DYNAMIC_FRICTION + assert cfg.scene.needle.prim_path == "{ENV_REGEX_NS}/SutureNeedle" + assert cfg.scene.suture_pad.init_state.pos[:2] == (0.45, 0.45) + for name in JAW_CONTACT_SENSOR_NAMES: + sensor_cfg = getattr(cfg.scene, name) + assert sensor_cfg.filter_prim_paths_expr == [cfg.scene.needle.prim_path] + assert sensor_cfg.track_pose is True + assert cfg.sim.dt == pytest.approx(1.0 / 240.0) + assert cfg.decimation == 1 + assert isinstance(cfg.sim.physics, NeedlePassPhysicsCfg) + for physx in (cfg.sim.physics.default, cfg.sim.physics.physx): + assert physx.solver_type == 1 + assert physx.solve_articulation_contact_last is True + assert physx.enable_external_forces_every_iteration is True + assert physx.enable_enhanced_determinism is True + assert physx.min_position_iteration_count == 4 + assert physx.min_velocity_iteration_count == 1 + assert physx.bounce_threshold_velocity == pytest.approx(0.01) + assert physx.friction_correlation_distance == pytest.approx(0.002) + assert cfg.scene.left_psm.spawn.articulation_props.solver_velocity_iteration_count == 4 + assert DVRK_JAW_AUTHORED_DYNAMIC_FRICTION == 10.0 + assert assets.NEEDLE_FRICTION_COMBINE_MODE == "min" + assert np.isclose(RESOLVED_JAW_NEEDLE_DYNAMIC_FRICTION, min(10.0, assets.NEEDLE_DYNAMIC_FRICTION)) + assert np.isclose(RESOLVED_JAW_NEEDLE_STATIC_FRICTION, min(1.0, assets.NEEDLE_STATIC_FRICTION)) + assert REQUIRED_RETENTION_LOAD.friction_coefficient == RESOLVED_JAW_NEEDLE_STATIC_FRICTION + assert HANDOFF_PHASE_CFG.engage_force_n == REQUIRED_RETENTION_LOAD.normal_force_per_jaw_n + assert HANDOFF_PHASE_CFG.engage_force_n > 0.011 + + +def _matrix_from_quat_xyzw(quaternion: np.ndarray) -> np.ndarray: + x, y, z, w = quaternion / np.linalg.norm(quaternion) + return np.array( + [ + [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], + [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], + [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], + ] + ) + + +def test_world_targets_use_each_live_psm_root(): + """Check two different rotated roots against independent homogeneous matrices.""" + + root_pos = np.array(((0.2, -0.1, 0.3), (-0.4, 0.2, 0.1)), dtype=np.float32) + root_quat_xyzw = np.array( + ((0.0, 0.0, 0.0, 1.0), (0.0, 0.0, np.sqrt(0.5), np.sqrt(0.5))), + dtype=np.float32, + ) + target_quat_xyzw = np.array( + ((np.sqrt(0.5), 0.0, 0.0, np.sqrt(0.5)), (0.5, 0.5, 0.5, 0.5)), + dtype=np.float32, + ) + target_pos = np.array(((0.3, 0.1, 0.5), (-0.1, 0.4, 0.2)), dtype=np.float32) + pose_xyzw = np.concatenate((target_pos, target_quat_xyzw), axis=1) + actual = world_pose_xyzw_to_root_pose_xyzw( + torch.tensor(pose_xyzw), + torch.tensor(root_pos), + torch.tensor(root_quat_xyzw), + ).numpy() + + for index in range(2): + root_transform = np.eye(4) + root_transform[:3, :3] = _matrix_from_quat_xyzw(root_quat_xyzw[index]) + root_transform[:3, 3] = root_pos[index] + target_transform = np.eye(4) + target_transform[:3, :3] = _matrix_from_quat_xyzw(target_quat_xyzw[index]) + target_transform[:3, 3] = target_pos[index] + expected = np.linalg.inv(root_transform) @ target_transform + np.testing.assert_allclose(actual[index, :3], expected[:3, 3], atol=1.0e-6) + np.testing.assert_allclose( + _matrix_from_quat_xyzw(actual[index, 3:7]), + expected[:3, :3], + atol=1.0e-6, + ) + assert not np.allclose(actual[0], actual[1]) + + +def test_grasp_solver_derives_loads_and_impedance_gains(): + load = required_retention_load( + mass_kg=assets.NEEDLE_MASS_KG, + gravity_m_s2=9.81, + maximum_commanded_acceleration_m_s2=0.5, + friction_coefficient=RESOLVED_JAW_NEEDLE_STATIC_FRICTION, + safety_factor=2.0, + ) + assert load.normal_force_per_jaw_n > 0.0 + cone = friction_cone_generators( + (1.0, 0.0, 0.0), + RESOLVED_JAW_NEEDLE_STATIC_FRICTION, + RETENTION_FRICTION_CONE_FACETS, + ) + assert cone.shape == (RETENTION_FRICTION_CONE_FACETS, 3) + np.testing.assert_allclose(cone[:, 0], 1.0) + np.testing.assert_allclose( + np.linalg.norm(cone[:, 1:], axis=1), + RESOLVED_JAW_NEEDLE_STATIC_FRICTION, + ) + stiffness, damping = impedance_gains(3.0e-7, 60.0, 1.0) + assert stiffness == pytest.approx(3.0e-7 * 60.0**2) + assert damping == pytest.approx(2.0 * 3.0e-7 * 60.0) + + +def test_two_contact_force_closure_proves_gravity_wrench_deterministically(): + contact_points_m = ((-0.01, 0.0, 0.0), (0.01, 0.0, 0.0)) + inward_normals = ((1.0, 0.0, 0.0), (-1.0, 0.0, 0.0)) + static_friction_coefficient = 0.5 + required_wrench = (0.0, 0.0, 2.0, 0.0, 0.0, 0.0) + + generators = two_contact_friction_wrench_generators( + contact_points_m, + inward_normals, + static_friction_coefficient, + ) + assert FORCE_CLOSURE_CONE_FACETS == RETENTION_FRICTION_CONE_FACETS == 8 + assert generators.shape == (2 * FORCE_CLOSURE_CONE_FACETS, 6) + + proof = prove_two_contact_force_closure( + contact_points_m, + inward_normals, + static_friction_coefficient, + required_wrench, + ) + repeated_proof = prove_two_contact_force_closure( + contact_points_m, + inward_normals, + static_friction_coefficient, + required_wrench, + ) + assert proof.feasible is True + assert proof.exact_point_contact_feasible is True + assert proof.active_generator_indices == (0, 12) + assert proof.coefficients == repeated_proof.coefficients + assert proof.force_residual_norm_n == repeated_proof.force_residual_norm_n + assert proof.torque_residual_norm_n_m == repeated_proof.torque_residual_norm_n_m + assert proof.equivalent_moment_arm_residual_m == repeated_proof.equivalent_moment_arm_residual_m + assert all(coefficient >= 0.0 for coefficient in proof.coefficients) + assert proof.coefficients[0] == pytest.approx(2.0) + assert proof.coefficients[12] == pytest.approx(2.0) + np.testing.assert_allclose(proof.achieved_wrench, required_wrench, atol=1.0e-12) + np.testing.assert_allclose(np.asarray(proof.coefficients) @ generators, required_wrench, atol=1.0e-12) + assert proof.required_force_norm_n == pytest.approx(2.0) + assert proof.force_residual_tolerance_n == EXACT_POINT_CONTACT_FORCE_RESIDUAL_TOLERANCE_N + assert proof.moment_arm_residual_tolerance_m == EXACT_POINT_CONTACT_MOMENT_ARM_TOLERANCE_M + assert proof.force_residual_norm_n <= 1.0e-12 + assert proof.torque_residual_norm_n_m <= 1.0e-12 + assert proof.equivalent_moment_arm_residual_m <= 1.0e-12 + + +def test_two_contact_force_closure_rejects_unresisted_contact_axis_torque(): + proof = prove_two_contact_force_closure( + ((-0.01, 0.0, 0.0), (0.01, 0.0, 0.0)), + ((1.0, 0.0, 0.0), (-1.0, 0.0, 0.0)), + 0.5, + (0.0, 0.0, 0.0, 1.0, 0.0, 0.0), + ) + assert proof.feasible is False + assert proof.exact_point_contact_feasible is False + assert proof.active_generator_indices == () + np.testing.assert_array_equal(proof.coefficients, np.zeros(2 * FORCE_CLOSURE_CONE_FACETS)) + np.testing.assert_allclose(proof.residual_wrench, (0.0, 0.0, 0.0, 1.0, 0.0, 0.0)) + assert proof.required_force_norm_n == 0.0 + assert proof.force_residual_norm_n == 0.0 + assert proof.torque_residual_norm_n_m == pytest.approx(1.0) + assert proof.equivalent_moment_arm_residual_m == np.inf + + +def test_finite_contact_acceptance_is_distinct_from_exact_point_contact_proof(): + """A documented soft-contact allowance must not relabel exact closure.""" + + proof = prove_two_contact_force_closure( + ((-0.01, 0.0, 0.0), (0.01, 0.0, 0.0)), + ((1.0, 0.0, 0.0), (-1.0, 0.0, 0.0)), + 0.5, + (0.0, 0.0, 2.0, 1.0e-5, 0.0, 0.0), + ) + assert proof.exact_point_contact_feasible is False + assert proof.force_residual_norm_n <= 1.0e-12 + assert proof.torque_residual_norm_n_m == pytest.approx(1.0e-5) + assert proof.equivalent_moment_arm_residual_m == pytest.approx(5.0e-6) + + accepted = assess_finite_contact_acceptance( + proof, + force_residual_tolerance_n=1.0e-9, + moment_arm_residual_tolerance_m=10.0e-6, + ) + assert accepted.accepted is True + assert accepted.force_within_tolerance is True + assert accepted.moment_arm_within_tolerance is True + assert proof.exact_point_contact_feasible is False + + rejected = assess_finite_contact_acceptance( + proof, + force_residual_tolerance_n=1.0e-9, + moment_arm_residual_tolerance_m=1.0e-6, + ) + assert rejected.accepted is False + assert rejected.force_within_tolerance is True + assert rejected.moment_arm_within_tolerance is False + + +def test_two_contact_force_closure_validates_shapes_and_finite_inputs(): + points = ((-0.01, 0.0, 0.0), (0.01, 0.0, 0.0)) + normals = ((1.0, 0.0, 0.0), (-1.0, 0.0, 0.0)) + wrench = (0.0, 0.0, 1.0, 0.0, 0.0, 0.0) + + with pytest.raises(ValueError, match=r"contact_points_m must be a finite \(2, 3\) array"): + two_contact_friction_wrench_generators(points[:1], normals, 0.5) + with pytest.raises(ValueError, match=r"inward_normals must be a finite \(2, 3\) array"): + two_contact_friction_wrench_generators(points, ((np.nan, 0.0, 0.0), normals[1]), 0.5) + with pytest.raises(ValueError, match="static_friction_coefficient must be finite and positive"): + two_contact_friction_wrench_generators(points, normals, np.inf) + with pytest.raises(ValueError, match="required_wrench must be a finite six-vector"): + prove_two_contact_force_closure(points, normals, 0.5, wrench[:5]) + with pytest.raises(ValueError, match="required_wrench must be a finite six-vector"): + prove_two_contact_force_closure(points, normals, 0.5, (*wrench[:5], np.nan)) + proof = prove_two_contact_force_closure(points, normals, 0.5, wrench) + with pytest.raises(ValueError, match="force_residual_tolerance_n must be finite and positive"): + assess_finite_contact_acceptance( + proof, + force_residual_tolerance_n=np.nan, + moment_arm_residual_tolerance_m=10.0e-6, + ) + with pytest.raises(ValueError, match="moment_arm_residual_tolerance_m must be finite and positive"): + assess_finite_contact_acceptance( + proof, + force_residual_tolerance_n=1.0e-9, + moment_arm_residual_tolerance_m=0.0, + ) + + +def _transform_from_pose(position, quaternion_xyzw): + transform = np.eye(4) + transform[:3, :3] = _matrix_from_quat_xyzw(np.asarray(quaternion_xyzw)) + transform[:3, 3] = position + return transform + + +def test_native_isaac_grasp_generator_provenance_and_held_reset(): + """Lock the native generator run and the physically held closed reset.""" + + assert ISAAC_GRASP_GENERATOR_EXTENSION == "isaacsim.replicator.grasping" + assert ISAAC_GRASP_GENERATOR_EXTENSION_VERSION == "1.0.9" + assert ISAAC_GRASP_GENERATOR_API.endswith("GraspingManager.generate_grasp_poses") + assert ISAAC_GRASP_GENERATOR_SIM_VERSION == "5.1.0" + assert ISAAC_GRASP_GENERATOR_SEED == 12 + assert ISAAC_GRASP_GENERATOR_CANDIDATE_COUNT == 8192 + assert ISAAC_GRASP_GENERATOR_ORIENTATIONS_PER_CENTRE == 32 + assert ISAAC_GRASP_GENERATOR_CENTRE_COUNT == 256 + assert ISAAC_GRASP_GENERATOR_CANDIDATE_COUNT == ( + ISAAC_GRASP_GENERATOR_CENTRE_COUNT * ISAAC_GRASP_GENERATOR_ORIENTATIONS_PER_CENTRE + ) + assert assets.NEEDLE_ASSET.sha256 == ISAAC_GRASP_ASSET_SHA256 + assert ISAAC_GRASP_WRAPPER_SHA256 == "01bc820d1777a1655a5c42b3ebac997c6281335a12d12f7636c3e25721f3a2d5" + assert ISAAC_GRASP_CONFIG_SHA256 == "b308ec31bf9bf425c686007e0dc0ad72f09ae7e1f67e1015ac53dc92a017e798" + assert ISAAC_GRASP_CANDIDATES_SHA256 == "7c601982d72759ca901fad9b59fa1df80a092221d1cb91eda88938b2b83bc374" + assert ISAAC_GRASP_MANIFEST_SHA256 == "13c72a5fb58db7c211619b72dcbdf27890a25a35a5aa8e3185ab4ae3139970ee" + + assert DONOR_GRASP_CANDIDATE_INDEX == 2321 + assert divmod(DONOR_GRASP_CANDIDATE_INDEX, ISAAC_GRASP_GENERATOR_ORIENTATIONS_PER_CENTRE) == (72, 17) + assert RECEIVER_GRASP_CANDIDATE_INDEX == 51 + assert divmod(RECEIVER_GRASP_CANDIDATE_INDEX, ISAAC_GRASP_GENERATOR_ORIENTATIONS_PER_CENTRE) == (1, 19) + for quaternion in (DONOR_GRASP_T_N_C_ROT_XYZW, RECEIVER_GRASP_T_N_C_ROT_XYZW): + assert np.linalg.norm(quaternion) == pytest.approx(1.0, abs=1.0e-15) + + np.testing.assert_allclose(DONOR_GRASP_JAW_POS, (0.0, 0.0), atol=0.0, rtol=0.0) + assert DONOR_GRASP_CLOSEDNESS == 1.0 + + cfg = DVRKNeedlePassEnvCfg() + for joint_name, position in zip(cfg.actions.left_jaw_action.joint_names, DONOR_HELD_RESET_JAW_POS, strict=True): + assert cfg.scene.left_psm.init_state.joint_pos[joint_name] == position + for joint_name, position in zip(cfg.actions.right_jaw_action.joint_names, (-np.pi / 6.0, np.pi / 6.0), strict=True): + assert cfg.scene.right_psm.init_state.joint_pos[joint_name] == pytest.approx(position) + assert cfg.isaac_teleop.pipeline_builder is not None + assert cfg.scene.left_psm.actuators["jaws"] == DVRK_NEEDLE_PASS_JAW_ACTUATOR + + +def test_native_grasp_transforms_reconstruct_seed_and_receiver_controller_target(): + """Rebuild native seeds while keeping physical holding equilibria explicit.""" + + transform_w_donor_tool = _transform_from_pose(LEFT_TOOL_HOME_POS_W, LEFT_TOOL_HOME_ROT_XYZW) + transform_tool_channel = _transform_from_pose( + DVRK_JAW_CHANNEL_T_T_C_POS_M, + DVRK_JAW_CHANNEL_T_T_C_ROT_XYZW, + ) + transform_needle_donor_channel = _transform_from_pose( + DONOR_GRASP_T_N_C_POS_M, + DONOR_GRASP_T_N_C_ROT_XYZW, + ) + transform_needle_receiver_channel = _transform_from_pose( + RECEIVER_GRASP_T_N_C_POS_M, + RECEIVER_GRASP_T_N_C_ROT_XYZW, + ) + + expected_needle_w = transform_w_donor_tool @ transform_tool_channel @ np.linalg.inv(transform_needle_donor_channel) + configured_native_seed_w = _transform_from_pose(DONOR_GRASP_NATIVE_SEED_POS, DONOR_GRASP_NATIVE_SEED_ROT_XYZW) + np.testing.assert_allclose(configured_native_seed_w, expected_needle_w, atol=1.0e-9) + configured_needle_w = _transform_from_pose(NEEDLE_RESET_POS, NEEDLE_RESET_ROT_XYZW) + assert not np.allclose(configured_needle_w, expected_needle_w, atol=1.0e-5) + + native_receiver_needle = transform_tool_channel @ np.linalg.inv(transform_needle_receiver_channel) + acceptance_receiver_needle = _transform_from_pose( + RECEIVER_NEEDLE_TARGET_POS_T, + RECEIVER_NEEDLE_TARGET_ROT_XYZW, + ) + assert not np.allclose(acceptance_receiver_needle, native_receiver_needle, atol=1.0e-5) + + acquisition_needle_w = _transform_from_pose( + RECEIVER_ACQUISITION_NEEDLE_POS_W, + RECEIVER_ACQUISITION_NEEDLE_ROT_XYZW, + ) + assert not np.allclose(acquisition_needle_w, configured_needle_w, atol=1.0e-5) + assert np.linalg.norm(acquisition_needle_w[:3, 3] - configured_needle_w[:3, 3]) < 1.0e-3 + + expected_receiver_tool_w = acquisition_needle_w @ np.linalg.inv(native_receiver_needle) + configured_receiver_tool_w = _transform_from_pose( + RECEIVER_TOOL_TARGET_POS_W, + RECEIVER_TOOL_TARGET_ROT_XYZW, + ) + np.testing.assert_allclose(configured_receiver_tool_w, expected_receiver_tool_w, atol=1.0e-9) + np.testing.assert_allclose(configured_receiver_tool_w @ native_receiver_needle, acquisition_needle_w, atol=1.0e-9) + + +def test_native_donor_grasp_does_not_claim_unproven_point_contact_closure(): + """Keep the analytical two-point model separate from physical retention.""" + + contact_points_from_com = np.asarray(DONOR_GRASP_CONTACT_POINTS_N_M) - np.asarray( + assets.NEEDLE_CENTRE_OF_MASS_BODY_LOCAL_M + ) + inward_normals = -np.asarray(DONOR_GRASP_OUTWARD_NORMALS_N) + required_force_w = np.array( + (0.0, 0.0, REQUIRED_RETENTION_LOAD.safety_factor * REQUIRED_RETENTION_LOAD.external_force_n) + ) + required_force_n = _matrix_from_quat_xyzw(np.asarray(NEEDLE_RESET_ROT_XYZW)).T @ required_force_w + proof = prove_two_contact_force_closure( + contact_points_from_com, + inward_normals, + RESOLVED_JAW_NEEDLE_STATIC_FRICTION, + np.concatenate((required_force_n, np.zeros(3))), + ) + assert proof.exact_point_contact_feasible is False + # The candidate is selected by its observed full collision-patch hold, not + # by a fictitious two-point proof. Its non-zero point-model moment error + # must remain visible so later changes cannot turn it into a false claim. + assert proof.force_residual_norm_n > EXACT_POINT_CONTACT_FORCE_RESIDUAL_TOLERANCE_N + assert proof.equivalent_moment_arm_residual_m > EXACT_POINT_CONTACT_MOMENT_ARM_TOLERANCE_M + + finite_patch = assess_finite_contact_acceptance( + proof, + force_residual_tolerance_n=3.0e-8, + moment_arm_residual_tolerance_m=10.0e-6, + ) + assert finite_patch.accepted is False + assert proof.exact_point_contact_feasible is False + + +def _measurements(num_envs: int, loads: torch.Tensor, needle_z: float = 0.0) -> HandoffMeasurements: + normals = torch.tensor(JAW_BODY_REACTION_NORMALS_LOCAL, dtype=torch.float32).repeat(num_envs, 1, 1) + needle_pose = torch.zeros((num_envs, 7), dtype=torch.float32) + needle_pose[:, 2] = needle_z + needle_pose[:, 6] = 1.0 + receiver_pose = needle_pose.clone() + return HandoffMeasurements( + normal_forces_n=loads, + reaction_normals_w=normals, + needle_pose_w=needle_pose, + needle_velocity_w=torch.zeros((num_envs, 6), dtype=torch.float32), + receiver_pose_w=receiver_pose, + ) + + +def test_reset_observation_does_not_advance_before_first_action(): + """A held reset remains INITIAL until a fresh measured-contact sample.""" + + cfg = HandoffPhaseCfg( + donor_dwell_s=0.02, + co_hold_dwell_s=0.02, + receiver_only_dwell_s=0.02, + retained_lift_dwell_s=0.02, + ) + machine = HandoffPhaseMachine(1, "cpu", 0.01, cfg) + machine.reset(torch.tensor([0]), torch.tensor([0.0]), step_token=7) + donor_loads = torch.tensor(((1.0, 1.0, 0.0, 0.0),)) + machine.advance(_measurements(1, donor_loads), step_token=7) + assert machine.phase.item() == HandoffPhase.INITIAL + assert machine._donor_counter.item() == 0 + machine.advance(_measurements(1, donor_loads), step_token=8) + assert machine._donor_counter.item() == 1 + + +@pytest.mark.parametrize( + ("loads", "normals"), + ( + (((float("inf"), 1.0),), (((1.0, 0.0, 0.0), (-1.0, 0.0, 0.0)),)), + (((1.0, 1.0),), (((float("nan"), 0.0, 0.0), (-1.0, 0.0, 0.0)),)), + ), +) +def test_bilateral_contact_rejects_nonfinite_measurements(loads, normals): + """Non-finite contact data must never qualify a physical grasp.""" + + machine = HandoffPhaseMachine(1, "cpu", 0.01, HandoffPhaseCfg()) + result = machine._bilateral_contact( + torch.tensor(loads, dtype=torch.float32), + torch.tensor(normals, dtype=torch.float32), + torch.zeros(1, dtype=torch.bool), + ) + assert result.tolist() == [False] + + +def test_handoff_update_reads_post_physics_buffers_once_per_step(monkeypatch): + """Manager terms sharing one phase machine must also share one sensor sample.""" + + from isaaclab_tasks.contrib.needle_pass.mdp import terminations + + calls = {"contacts": 0, "advance": 0} + + class _Machine: + def advance(self, measurements, step_token): + calls["advance"] += 1 + assert measurements.normal_forces_n.shape == (1, 4) + assert step_token in (12, 13) + + machine = _Machine() + + def _contacts(_env): + calls["contacts"] += 1 + return torch.zeros((1, 4)), torch.zeros((1, 4, 3)), torch.zeros((1, 4, 3)) + + needle = SimpleNamespace( + data=SimpleNamespace( + root_pos_w=_proxy(torch.zeros((1, 3))), + root_quat_w=_proxy(torch.tensor(((0.0, 0.0, 0.0, 1.0),))), + root_lin_vel_w=_proxy(torch.zeros((1, 3))), + root_ang_vel_w=_proxy(torch.zeros((1, 3))), + ) + ) + env = SimpleNamespace(common_step_counter=12, scene={"needle": needle, "right_psm": object()}) + monkeypatch.setattr(terminations, "get_handoff_phase_machine", lambda *_: machine) + monkeypatch.setattr(terminations, "jaw_needle_contact_measurements", _contacts) + monkeypatch.setattr( + terminations, + "_asset_pose_w", + lambda *_: torch.tensor(((0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0),)), + ) + + assert update_handoff_phase(env, HandoffPhaseCfg()) is machine + assert update_handoff_phase(env, HandoffPhaseCfg()) is machine + assert calls == {"contacts": 1, "advance": 1} + + env.common_step_counter = 13 + assert update_handoff_phase(env, HandoffPhaseCfg()) is machine + assert calls == {"contacts": 2, "advance": 2} + + +def test_donor_release_requires_a_current_receiver_grasp(): + """A completed co-hold dwell must not leave a stale release permission.""" + + phase = torch.tensor((int(HandoffPhase.CO_HOLD), int(HandoffPhase.RECEIVER_ONLY_HOLD), int(HandoffPhase.CO_HOLD))) + receiver_grasp = torch.tensor((True, False, False)) + allowed = donor_release_is_allowed(phase, receiver_grasp, int(HandoffPhase.CO_HOLD)) + assert allowed.tolist() == [True, False, False] + + with pytest.raises(ValueError, match="identical batch shapes"): + donor_release_is_allowed(phase, receiver_grasp[:2], int(HandoffPhase.CO_HOLD)) + + +def test_donor_release_blocks_any_outward_jaw_command(): + """A unilateral outward target must not bypass the donor-release interlock.""" + + held = torch.tensor(((-0.20, 0.01),)) + more_closed = torch.tensor(((-0.10, 0.0),)) + open_one_jaw = torch.tensor(((-0.40, 0.01),)) + paired_open = torch.tensor(((-0.40, 0.30),)) + assert donor_opening_requested(held, held, 0.01).tolist() == [False] + assert donor_opening_requested(more_closed, held, 0.01).tolist() == [False] + assert donor_opening_requested(open_one_jaw, held, 0.01).tolist() == [True] + assert donor_opening_requested(paired_open, held, 0.01).tolist() == [True] + + +def test_donor_release_guard_clamps_unqualified_jaw_targets_at_the_actuator(monkeypatch): + """Exercise the production action term rather than only its pure predicates.""" + + from isaaclab_tasks.contrib.needle_pass.mdp import terminations + + class _Asset: + def __init__(self): + self.calls = [] + + def set_joint_position_target_index(self, *, target, joint_ids): + self.calls.append((target.clone(), joint_ids)) + + hold = torch.tensor(((-0.20, 0.01),), dtype=torch.float32) + machine = SimpleNamespace( + phase=torch.tensor((int(HandoffPhase.CO_HOLD),) * 3), + _receiver_engaged=torch.ones(3, dtype=torch.bool), + _bilateral_contact=lambda loads, normals, engaged: torch.tensor((False, True, False)), + ) + env = SimpleNamespace() + action = object.__new__(DonorReleaseGuardedPairedJawJointPositionAction) + action._env = env + action._hold_target = hold.expand(3, -1).clone() + action._joint_ids = [6, 7] + action._asset = _Asset() + action._debug_vis_handle = None + action.cfg = SimpleNamespace(phase_cfg=object(), release_aperture_threshold_rad=0.01) + action._processed_actions = torch.tensor(((-0.40, 0.01), (-0.40, 0.30), (-0.40, 0.30)), dtype=torch.float32) + + monkeypatch.setattr(terminations, "get_handoff_phase_machine", lambda *_: machine) + monkeypatch.setattr( + terminations, + "jaw_needle_contact_measurements", + lambda _: (torch.zeros((3, 4)), torch.zeros((3, 4, 3)), torch.zeros((3, 4))), + ) + + action.apply_actions() + + command, joint_ids = action._asset.calls.pop() + torch.testing.assert_close(command, torch.tensor(((-0.20, 0.01), (-0.40, 0.30), (-0.20, 0.01)))) + assert joint_ids == [6, 7] + + +def test_receiver_bounds_use_configured_nonidentity_grasp_transform(): + half_angle = np.pi / 4.0 + cfg = HandoffPhaseCfg( + receiver_relative_position_target_m=(0.01, -0.02, 0.03), + receiver_relative_orientation_target_xyzw=(0.0, 0.0, np.sin(half_angle), np.cos(half_angle)), + receiver_relative_position_limit_m=1.0e-4, + receiver_relative_orientation_limit_rad=1.0e-4, + ) + machine = HandoffPhaseMachine(1, "cpu", 0.01, cfg) + receiver_pose = torch.tensor(((0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0),), dtype=torch.float32) + needle_pose = torch.tensor( + ((0.01, -0.02, 0.03, 0.0, 0.0, np.sin(half_angle), np.cos(half_angle)),), dtype=torch.float32 + ) + measurements = HandoffMeasurements( + normal_forces_n=torch.zeros((1, 4)), + reaction_normals_w=torch.tensor(JAW_BODY_REACTION_NORMALS_LOCAL).unsqueeze(0), + needle_pose_w=needle_pose, + needle_velocity_w=torch.zeros((1, 6)), + receiver_pose_w=receiver_pose, + ) + assert machine._receiver_bounds(measurements).item() is True + measurements.needle_pose_w[:, 0] += 0.001 + assert machine._receiver_bounds(measurements).item() is False + measurements.needle_pose_w[:, 0] -= 0.001 + measurements.receiver_pose_w[:, 0] = torch.nan + assert machine._receiver_bounds(measurements).item() is False + + +@pytest.mark.parametrize("pose_name", ("needle_pose_w", "receiver_pose_w")) +def test_receiver_bounds_reject_degenerate_input_quaternion(pose_name): + """A zero input quaternion cannot satisfy the receiver grasp bounds.""" + + machine = HandoffPhaseMachine(1, "cpu", 0.01, HandoffPhaseCfg()) + measurements = _measurements(1, torch.zeros((1, 4))) + getattr(measurements, pose_name)[:, 3:7] = 0.0 + assert machine._receiver_bounds(measurements).tolist() == [False] + + +def test_reset_write_order_and_single_needle_state_write(): + calls = [] + left_default = torch.tensor(((0.0, 1.0, 2.0, 3.0, 4.0, 5.0, *DONOR_GRASP_JAW_POS),), dtype=torch.float32) + right_default = torch.tensor( + ((10.0, 11.0, 12.0, 13.0, 14.0, 15.0, -np.pi / 6.0, np.pi / 6.0),), + dtype=torch.float32, + ) + + class MockPSM: + def __init__(self, name, default_joint_pos): + self.name = name + self.expected_joint_pos = default_joint_pos + self.data = SimpleNamespace(default_joint_pos=_proxy(default_joint_pos)) + + def write_joint_position_to_sim_index(self, *, position, env_ids): + calls.append((self.name, "position_state")) + torch.testing.assert_close(position, self.expected_joint_pos) + + def write_joint_velocity_to_sim_index(self, *, velocity, env_ids): + calls.append((self.name, "velocity_state")) + torch.testing.assert_close(velocity, torch.zeros_like(self.expected_joint_pos)) + + def set_joint_position_target_index(self, *, target, env_ids): + calls.append((self.name, "position_target")) + torch.testing.assert_close(target, self.expected_joint_pos) + + def set_joint_velocity_target_index(self, *, target, env_ids): + calls.append((self.name, "velocity_target")) + torch.testing.assert_close(target, torch.zeros_like(self.expected_joint_pos)) + + class MockNeedle: + def __init__(self): + self.data = SimpleNamespace( + default_root_pose=_proxy(torch.tensor(((0.1, 0.2, 0.3, 0.0, 0.0, 0.0, 1.0),))), + default_root_vel=_proxy(torch.full((1, 6), 5.0)), + ) + + def write_root_pose_to_sim_index(self, *, root_pose, env_ids): + calls.append(("needle", "pose")) + torch.testing.assert_close(root_pose[:, :3], torch.tensor(((1.1, 2.2, 3.3),))) + + def write_root_velocity_to_sim_index(self, *, root_velocity, env_ids): + calls.append(("needle", "velocity")) + torch.testing.assert_close(root_velocity, torch.zeros_like(root_velocity)) + + class MockScene(dict): + env_origins = torch.tensor(((1.0, 2.0, 3.0),)) + + env = SimpleNamespace( + num_envs=1, + device="cpu", + step_dt=0.01, + common_step_counter=4, + scene=MockScene( + left_psm=MockPSM("left", left_default), + right_psm=MockPSM("right", right_default), + needle=MockNeedle(), + ), + ) + reset_needle_pass_to_default(env, torch.tensor([0]), HandoffPhaseCfg()) + assert calls == [ + ("left", "position_state"), + ("left", "velocity_state"), + ("left", "position_target"), + ("left", "velocity_target"), + ("right", "position_state"), + ("right", "velocity_state"), + ("right", "position_target"), + ("right", "velocity_target"), + ("needle", "pose"), + ("needle", "velocity"), + ] + + +def test_contact_phase_sequence_counterfactual_and_partial_reset(): + cfg = HandoffPhaseCfg( + donor_dwell_s=0.02, + co_hold_dwell_s=0.02, + receiver_only_dwell_s=0.02, + retained_lift_dwell_s=0.02, + required_lift_delta_z_m=0.01, + ) + machine = HandoffPhaseMachine(2, "cpu", 0.01, cfg) + machine.reset(torch.tensor((0, 1)), torch.zeros(2), step_token=0) + donor = torch.tensor(((1.0, 1.0, 0.0, 0.0), (0.0, 0.0, 0.0, 0.0))) + for token in (1, 2): + machine.advance(_measurements(2, donor), token) + assert machine.phase.tolist() == [HandoffPhase.DONOR_HOLD, HandoffPhase.INITIAL] + for token in (3, 4): + machine.advance(_measurements(2, torch.tensor(((1.0, 1.0, 1.0, 1.0), (0.0, 0.0, 0.0, 0.0)))), token) + assert machine.phase.tolist() == [HandoffPhase.CO_HOLD, HandoffPhase.INITIAL] + for token in (5, 6): + machine.advance(_measurements(2, torch.tensor(((0.0, 0.0, 1.0, 1.0), (0.0, 0.0, 0.0, 0.0)))), token) + assert machine.phase.tolist() == [HandoffPhase.RECEIVER_ONLY_HOLD, HandoffPhase.INITIAL] + for token in (7, 8): + machine.advance( + _measurements(2, torch.tensor(((0.0, 0.0, 1.0, 1.0), (0.0, 0.0, 0.0, 0.0))), needle_z=0.02), + token, + ) + assert machine.phase.tolist() == [HandoffPhase.RETAINED_LIFT, HandoffPhase.INITIAL] + + machine.reset(torch.tensor([0]), torch.tensor([0.02]), step_token=8) + assert machine.phase.tolist() == [HandoffPhase.INITIAL, HandoffPhase.INITIAL] + machine.advance(_measurements(2, torch.zeros((2, 4))), step_token=8) + assert machine._donor_counter.tolist() == [0, 0] + + +def test_receiver_only_ownership_survives_a_transient_bounds_excursion(): + """A live bilateral recipient grasp must not roll back on transient motion.""" + + cfg = HandoffPhaseCfg( + donor_dwell_s=0.01, + co_hold_dwell_s=0.01, + receiver_only_dwell_s=0.01, + retained_lift_dwell_s=0.01, + required_lift_delta_z_m=0.01, + ) + machine = HandoffPhaseMachine(1, "cpu", 0.01, cfg) + machine.reset(torch.tensor([0]), torch.tensor([0.0]), step_token=0) + donor = torch.tensor(((1.0, 1.0, 0.0, 0.0),)) + cohold = torch.tensor(((1.0, 1.0, 1.0, 1.0),)) + receiver = torch.tensor(((0.0, 0.0, 1.0, 1.0),)) + machine.advance(_measurements(1, donor), step_token=1) + machine.advance(_measurements(1, cohold), step_token=2) + machine.advance(_measurements(1, receiver), step_token=3) + assert machine.phase.item() == HandoffPhase.RECEIVER_ONLY_HOLD + + moving_receiver = _measurements(1, receiver, needle_z=0.02) + moving_receiver.needle_velocity_w[:, 0] = cfg.maximum_linear_velocity_m_s * 2.0 + assert machine._receiver_bounds(moving_receiver).item() is False + machine.advance(moving_receiver, step_token=4) + assert machine.phase.item() == HandoffPhase.RECEIVER_ONLY_HOLD + assert machine._retained_lift_counter.item() == 0 + + machine.advance(_measurements(1, torch.zeros((1, 4))), step_token=5) + assert machine.phase.item() == HandoffPhase.INITIAL + + +def test_contact_dwell_is_consecutive_across_rollbacks(): + cfg = HandoffPhaseCfg( + donor_dwell_s=0.02, + co_hold_dwell_s=0.02, + receiver_only_dwell_s=0.02, + retained_lift_dwell_s=0.02, + ) + machine = HandoffPhaseMachine(1, "cpu", 0.01, cfg) + machine.reset(torch.tensor([0]), torch.tensor([0.0]), step_token=0) + donor = torch.tensor(((1.0, 1.0, 0.0, 0.0),)) + cohold = torch.tensor(((1.0, 1.0, 1.0, 1.0),)) + receiver = torch.tensor(((0.0, 0.0, 1.0, 1.0),)) + + machine.advance(_measurements(1, donor), 1) + machine.advance(_measurements(1, donor), 2) + machine.advance(_measurements(1, cohold), 3) + assert machine._co_hold_counter.item() == 1 + machine.advance(_measurements(1, receiver), 4) + assert machine.phase.item() == HandoffPhase.INITIAL + assert machine._co_hold_counter.item() == 0 + + machine.advance(_measurements(1, cohold), 5) + assert machine.phase.item() == HandoffPhase.INITIAL + machine.advance(_measurements(1, cohold), 6) + assert machine.phase.item() == HandoffPhase.DONOR_HOLD + machine.advance(_measurements(1, cohold), 7) + assert machine.phase.item() == HandoffPhase.DONOR_HOLD + machine.advance(_measurements(1, cohold), 8) + assert machine.phase.item() == HandoffPhase.CO_HOLD + + machine.advance(_measurements(1, donor), 9) + assert machine.phase.item() == HandoffPhase.DONOR_HOLD + assert machine._co_hold_counter.item() == 0 + machine.advance(_measurements(1, cohold), 10) + assert machine.phase.item() == HandoffPhase.DONOR_HOLD + machine.advance(_measurements(1, cohold), 11) + assert machine.phase.item() == HandoffPhase.CO_HOLD + + +def test_contact_projection_order_sign_and_pose_shape(): + assert JAW_BODY_REACTION_NORMALS_LOCAL == ( + (1.0, 0.0, 0.0), + (-1.0, 0.0, 0.0), + (1.0, 0.0, 0.0), + (-1.0, 0.0, 0.0), + ) + sensors = {} + expected_loads = torch.tensor((1.0, 2.0, 3.0, 4.0)) + identity_xyzw = (0.0, 0.0, 0.0, 1.0) + z_quarter_turn_xyzw = (0.0, 0.0, np.sqrt(0.5), np.sqrt(0.5)) + sensor_quaternions = (identity_xyzw, identity_xyzw, z_quarter_turn_xyzw, z_quarter_turn_xyzw) + expected_normals_w = torch.tensor(((1.0, 0.0, 0.0), (-1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, -1.0, 0.0))) + for name, normal_w, quaternion_xyzw, load in zip( + JAW_CONTACT_SENSOR_NAMES, + expected_normals_w, + sensor_quaternions, + expected_loads, + strict=True, + ): + force = normal_w * load + force_matrix_w = force.reshape(1, 1, 1, 3) + sensors[name] = SimpleNamespace( + data=SimpleNamespace( + force_matrix_w=_proxy(force_matrix_w), + quat_w=_proxy(torch.tensor(quaternion_xyzw, dtype=torch.float32).reshape(1, 1, 4)), + ) + ) + env = SimpleNamespace(num_envs=1, scene=SimpleNamespace(sensors=sensors)) + loads, normals, force_vectors = jaw_needle_contact_measurements(env) + torch.testing.assert_close(loads[0], expected_loads) + torch.testing.assert_close(normals[0], expected_normals_w) + torch.testing.assert_close(force_vectors[0], expected_normals_w * expected_loads[:, None]) + + for sensor in sensors.values(): + sensor.data.force_matrix_w.torch *= -1.0 + loads, _, _ = jaw_needle_contact_measurements(env) + torch.testing.assert_close(loads, torch.zeros_like(loads)) + + +def test_configured_device_homes_are_finite_and_distinct(): + assert np.isfinite(LEFT_TOOL_HOME_POS_W).all() + assert np.isfinite(RIGHT_TOOL_HOME_POS_W).all() + assert LEFT_TOOL_HOME_POS_W != RIGHT_TOOL_HOME_POS_W diff --git a/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_physics.py b/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_physics.py new file mode 100644 index 000000000000..87d59b38df8f --- /dev/null +++ b/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_physics.py @@ -0,0 +1,1785 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Supported-lane physics contracts for the dVRK needle-pass task. + +These tests require a fresh, load-qualified donor hold from the closed reset +state before exercising the runtime invariants and fixed hand-off traces. The +needle remains a free dynamic body throughout; no held state may rely on an +attachment or a post-reset state write. +""" + +from __future__ import annotations + +import csv +import hashlib +import math +import os +from collections.abc import Mapping +from contextlib import contextmanager +from pathlib import Path +from typing import Any +from uuid import uuid4 + +from isaaclab.app import AppLauncher + +# ``RecordVideo`` needs render products even when this module runs headless. +# Launch them only for an explicitly requested recording run so ordinary CUDA +# physics CI retains the renderer-free execution lane. +_RECORD_VIDEO = bool(os.environ.get("ISAACLAB_DVRK_NEEDLE_PASS_VIDEO_DIR")) +_app_launcher_kwargs = {"headless": True, "enable_cameras": _RECORD_VIDEO} +if _RECORD_VIDEO: + # Fixed off-screen dimensions avoid capture/swapchain mismatches in + # headless Isaac Sim, while RayTracedLighting is fast enough for this + # short physics-verification trace. + _video_renderer = os.environ.get("ISAACLAB_DVRK_NEEDLE_PASS_RENDERER", "RaytracedLighting") + if _video_renderer not in {"RaytracedLighting", "PathTracing", "HydraStorm"}: + raise ValueError("ISAACLAB_DVRK_NEEDLE_PASS_RENDERER must be RaytracedLighting, PathTracing, or HydraStorm") + _app_launcher_kwargs.update( + width=640, + height=480, + renderer=_video_renderer, + rendering_mode="performance", + anti_aliasing=0, + denoiser=False, + ) +app_launcher = AppLauncher(**_app_launcher_kwargs) +simulation_app = app_launcher.app + +import gymnasium as gym +import numpy as np +import pytest +import torch +import warp as wp + +import omni.usd +from pxr import PhysxSchema, Sdf, Tf, Usd, UsdGeom, UsdPhysics, UsdShade + +import isaaclab.utils.math as math_utils + +if _RECORD_VIDEO: + import carb + + _render_settings = carb.settings.get_settings() + _render_settings.set_int("/rtx/post/tonemap/op", 4) + _render_settings.set_float("/rtx/post/tonemap/filmIso", 200.0) + +import isaaclab_tasks # noqa: F401 +from isaaclab_tasks.contrib.needle_pass import assets +from isaaclab_tasks.contrib.needle_pass.config.dvrk.ik_abs_env_cfg import ( + DONOR_GRASP_JAW_POS, + DVRK_HANDOFF_PHASE_CFG, + DVRK_JAW_CHANNEL_T_T_C_POS_M, + DVRK_JAW_CHANNEL_T_T_C_ROT_XYZW, + ISAAC_GRASP_CANDIDATES_SHA256, + LEFT_TOOL_HOME_POS_W, + LEFT_TOOL_HOME_ROT_XYZW, + RECEIVER_TOOL_TARGET_POS_W, + RECEIVER_TOOL_TARGET_ROT_XYZW, + RIGHT_TOOL_HOME_POS_W, + RIGHT_TOOL_HOME_ROT_XYZW, +) +from isaaclab_tasks.contrib.needle_pass.mdp.terminations import ( + JAW_CONTACT_SENSOR_NAMES, + HandoffMeasurements, + HandoffPhase, + get_handoff_phase_machine, + jaw_needle_contact_measurements, +) +from isaaclab_tasks.utils import parse_env_cfg + +from isaaclab_assets.robots.dvrk import ( + DVRK_PSM_ARM_JOINT_NAMES, + DVRK_PSM_JAW_CLOSED_POS, + DVRK_PSM_JAW_JOINT_NAMES, + DVRK_PSM_JAW_OPEN_POS, + DVRK_PSM_TOOL_TIP_BODY_NAME, +) + +TASK_ID = "IsaacContrib-NeedlePass-dVRK-IK-Abs" +SEED = 42 +TRACE_STEPS = 100 +# Three seconds at the configured 240 Hz simulation rate. The normal CI lane +# remains short and renderer-free. +VIDEO_TRACE_STEPS = 720 +NATIVE_RECEIVER_PREGRASP_CLEARANCE_M = 0.05 +NATIVE_DONOR_HOLD_SETTLE_STEPS = 64 +NATIVE_RECEIVER_APPROACH_SEGMENT_STEPS = 160 +NATIVE_RECEIVER_CLOSE_SETTLE_STEPS = 480 +NATIVE_DONOR_RELEASE_SETTLE_STEPS = 480 +# The retained-lift motion is deliberately slower than the controller's 15 mm/s +# limit, while still requiring a 20 mm public-controller lift of the free body. +# The receiver also makes a 20 mm lateral escape from the static donor. This +# keeps an already-released needle clear of the donor jaws; it is a smooth +# controller trajectory, not a modification of Isaac's generated grasp pose. +NATIVE_RECEIVER_LIFT_STEPS = 960 +NATIVE_RECEIVER_LIFT_HEIGHT_M = 0.02 +NATIVE_RECEIVER_TRANSFER_ESCAPE_POS_M = (0.02, 0.0, 0.0) +NATIVE_HANDOFF_TRACE_STEPS = ( + NATIVE_DONOR_HOLD_SETTLE_STEPS + + 4 * NATIVE_RECEIVER_APPROACH_SEGMENT_STEPS + + NATIVE_RECEIVER_CLOSE_SETTLE_STEPS + + NATIVE_DONOR_RELEASE_SETTLE_STEPS + + NATIVE_RECEIVER_LIFT_STEPS +) +RESET_FLOAT_ATOL = 1.0e-7 + +_ARTICULATION_STATE_WRITERS = ( + "write_root_pose_to_sim_index", + "write_root_pose_to_sim_mask", + "write_root_link_pose_to_sim_index", + "write_root_link_pose_to_sim_mask", + "write_root_com_pose_to_sim_index", + "write_root_com_pose_to_sim_mask", + "write_root_velocity_to_sim_index", + "write_root_velocity_to_sim_mask", + "write_root_com_velocity_to_sim_index", + "write_root_com_velocity_to_sim_mask", + "write_root_link_velocity_to_sim_index", + "write_root_link_velocity_to_sim_mask", + "write_joint_state_to_sim_index", + "write_joint_state_to_sim_mask", + "write_joint_position_to_sim_index", + "write_joint_position_to_sim_mask", + "write_joint_velocity_to_sim_index", + "write_joint_velocity_to_sim_mask", +) +_RIGID_OBJECT_STATE_WRITERS = ( + "write_root_pose_to_sim_index", + "write_root_pose_to_sim_mask", + "write_root_link_pose_to_sim_index", + "write_root_link_pose_to_sim_mask", + "write_root_com_pose_to_sim_index", + "write_root_com_pose_to_sim_mask", + "write_root_velocity_to_sim_index", + "write_root_velocity_to_sim_mask", + "write_root_com_velocity_to_sim_index", + "write_root_com_velocity_to_sim_mask", + "write_root_link_velocity_to_sim_index", + "write_root_link_velocity_to_sim_mask", +) +_ARTICULATION_PHYSX_STATE_SETTERS = ( + "set_root_transforms", + "set_root_velocities", + "set_dof_positions", + "set_dof_velocities", +) +_ARTICULATION_PHYSX_CONTROL_SETTERS = ( + "set_dof_actuation_forces", + "set_dof_position_targets", + "set_dof_velocity_targets", +) +_RIGID_OBJECT_PHYSX_STATE_SETTERS = ( + "set_kinematic_targets", + "set_transforms", + "set_velocities", +) + +_EXPECTED_RESET_STATE_WRITE_SEQUENCE = ( + ("asset", "left_psm", "write_joint_position_to_sim_index"), + ("physx", "left_psm", "set_dof_positions"), + ("asset", "left_psm", "write_joint_velocity_to_sim_index"), + ("physx", "left_psm", "set_dof_velocities"), + ("asset", "right_psm", "write_joint_position_to_sim_index"), + ("physx", "right_psm", "set_dof_positions"), + ("asset", "right_psm", "write_joint_velocity_to_sim_index"), + ("physx", "right_psm", "set_dof_velocities"), + ("asset", "needle", "write_root_pose_to_sim_index"), + ("asset", "needle", "write_root_link_pose_to_sim_index"), + ("physx", "needle", "set_transforms"), + ("asset", "needle", "write_root_velocity_to_sim_index"), + ("asset", "needle", "write_root_com_velocity_to_sim_index"), + ("physx", "needle", "set_velocities"), +) +_RESET_STATE_WRITE_WHITELIST = frozenset(_EXPECTED_RESET_STATE_WRITE_SEQUENCE) + + +def _verification_video_dir() -> Path | None: + """Return an opt-in directory for a recorded CUDA verification trace.""" + + raw_path = os.environ.get("ISAACLAB_DVRK_NEEDLE_PASS_VIDEO_DIR") + if not raw_path: + return None + video_dir = Path(raw_path).expanduser().resolve() / f"runtime-contract-{uuid4().hex}" + video_dir.mkdir(parents=True, exist_ok=True) + return video_dir + + +@contextmanager +def _task_env( + num_envs: int, + *, + video_dir: Path | None = None, + video_length: int = VIDEO_TRACE_STEPS, + video_prefix: str = "dvrk-needle-pass-runtime-contract", +): + """Construct one isolated headless task environment and always close it.""" + + omni.usd.get_context().new_stage() + env = None + active_env = None + try: + env_cfg = parse_env_cfg(TASK_ID, device="cuda:0", num_envs=num_envs) + env_cfg.seed = SEED + if video_dir is None: + env = gym.make(TASK_ID, cfg=env_cfg) + else: + env = gym.make(TASK_ID, cfg=env_cfg, render_mode="rgb_array") + env.unwrapped.sim._app_control_on_stop_handle = None + active_env = env + if video_dir is not None: + active_env = gym.wrappers.RecordVideo( + env, + video_folder=str(video_dir), + episode_trigger=lambda _episode_index: True, + video_length=video_length, + name_prefix=video_prefix, + fps=round(1 / env.unwrapped.step_dt), + disable_logger=True, + ) + yield active_env if video_dir is not None else env.unwrapped + finally: + if env is not None: + audit = getattr(env.unwrapped, "_needle_pass_direct_state_write_audit", None) + if audit is not None: + audit.uninstall() + if active_env is not None: + active_env.close() + else: + env.close() + + +def _held_start_action(env) -> torch.Tensor: + """Hold the donor grasp and open receiver at their clone-local tool homes.""" + + origins = env.scene.env_origins + left_position_w = origins + torch.tensor(LEFT_TOOL_HOME_POS_W, device=env.device) + right_position_w = origins + torch.tensor(RIGHT_TOOL_HOME_POS_W, device=env.device) + left_orientation_xyzw = torch.tensor(LEFT_TOOL_HOME_ROT_XYZW, device=env.device).expand(env.num_envs, -1) + right_orientation_xyzw = torch.tensor(RIGHT_TOOL_HOME_ROT_XYZW, device=env.device).expand(env.num_envs, -1) + donor_held = torch.tensor(DONOR_GRASP_JAW_POS, device=env.device).expand(env.num_envs, -1) + receiver_open = torch.tensor(DVRK_PSM_JAW_OPEN_POS, device=env.device).expand(env.num_envs, -1) + action = torch.cat( + ( + left_position_w, + left_orientation_xyzw, + donor_held, + right_position_w, + right_orientation_xyzw, + receiver_open, + ), + dim=-1, + ) + assert action.shape == (env.num_envs, 18) + assert action.is_contiguous() + assert torch.isfinite(action).all() + return action + + +def _slerp_xyzw(start: torch.Tensor, end: torch.Tensor, fraction: float) -> torch.Tensor: + """Interpolate orientations along the shortest physical rotation.""" + + start = torch.nn.functional.normalize(start, dim=-1) + end = torch.nn.functional.normalize(end, dim=-1) + dot = torch.sum(start * end) + # Quaternion signs encode the same orientation. Flip the endpoint before + # interpolation so the controller never crosses the near-zero quaternion + # produced by a linear blend across a 171-degree rotation. + if dot < 0.0: + end = -end + dot = -dot + dot = torch.clamp(dot, min=-1.0, max=1.0) + angle = torch.arccos(dot) + if float(angle) < 1.0e-6: + return start + sine = torch.sin(angle) + weight_start = torch.sin((1.0 - fraction) * angle) / sine + weight_end = torch.sin(fraction * angle) / sine + return torch.nn.functional.normalize(weight_start * start + weight_end * end, dim=-1) + + +def _native_receiver_handoff_action( + env, + *, + approach_fraction: float, + receiver_jaw: tuple[float, float], + donor_jaw: tuple[float, float] = DONOR_GRASP_JAW_POS, + receiver_lift_offset_m: tuple[float, float, float] = (0.0, 0.0, 0.0), +) -> torch.Tensor: + """Command the fixed native receiver channel pose through the public ABI. + + ``RECEIVER_TOOL_TARGET_*`` is composed from the fixed Isaac grasp-generator + candidate. The only trajectory interpolation is the controller motion + from the receiver home to that generated pose; no grasp pose is searched, + perturbed, or written into the simulated state. + """ + + if not 0.0 <= approach_fraction <= 1.0: + raise ValueError("approach_fraction must lie in [0, 1]") + action = _held_start_action(env) + fraction = torch.tensor(approach_fraction, device=env.device) + home_position = torch.tensor(RIGHT_TOOL_HOME_POS_W, device=env.device) + target_position = torch.tensor(RECEIVER_TOOL_TARGET_POS_W, device=env.device) + target_position = target_position + torch.tensor(receiver_lift_offset_m, device=env.device) + action[:, 9:12] = home_position + fraction * (target_position - home_position) + + home_orientation = torch.tensor(RIGHT_TOOL_HOME_ROT_XYZW, device=env.device) + target_orientation = torch.tensor(RECEIVER_TOOL_TARGET_ROT_XYZW, device=env.device) + action[:, 12:16] = _slerp_xyzw(home_orientation, target_orientation, approach_fraction) + action[:, 7:9] = torch.tensor(donor_jaw, device=env.device) + action[:, 16:18] = torch.tensor(receiver_jaw, device=env.device) + return action + + +def _native_receiver_lift_offset(step: int) -> tuple[float, float, float]: + """Return a zero-velocity-endpoint public-controller escape and lift.""" + + if not 0 <= step < NATIVE_RECEIVER_LIFT_STEPS: + raise ValueError("lift step is outside the configured trace") + fraction = (step + 1) / NATIVE_RECEIVER_LIFT_STEPS + smooth_fraction = fraction * fraction * (3.0 - 2.0 * fraction) + return ( + NATIVE_RECEIVER_TRANSFER_ESCAPE_POS_M[0] * smooth_fraction, + NATIVE_RECEIVER_TRANSFER_ESCAPE_POS_M[1] * smooth_fraction, + NATIVE_RECEIVER_TRANSFER_ESCAPE_POS_M[2] * smooth_fraction + NATIVE_RECEIVER_LIFT_HEIGHT_M * smooth_fraction, + ) + + +def _native_donor_release_jaw(step: int) -> tuple[float, float]: + """Open the donor through the public jaw action without a contact impulse. + + The receiver is already in a measured co-hold before this starts. A + monotonic controller ramp gives the physical recipient grasp time to take + the full load; it does not alter either generated grasp pose or the needle + state. + """ + + if not 0 <= step < NATIVE_DONOR_RELEASE_SETTLE_STEPS: + raise ValueError("release step is outside the configured trace") + fraction = (step + 1) / NATIVE_DONOR_RELEASE_SETTLE_STEPS + return tuple( + held + fraction * (opened - held) + for held, opened in zip(DONOR_GRASP_JAW_POS, DVRK_PSM_JAW_OPEN_POS, strict=True) + ) + + +def _native_receiver_staged_approach_action(env, *, segment: int, fraction: float) -> torch.Tensor: + """Move to a generated channel via a fixed collision-free pre-grasp path. + + The end pose is exactly ``RECEIVER_TOOL_TARGET_*`` reconstructed from the + native candidate. The elevated waypoints only keep the public controller + out of the donor's occupied grasp volume while the recipient is open; they + neither perturb the generated grasp nor write rigid-body state. + """ + + if segment not in range(4): + raise ValueError("native receiver approach segment must be in [0, 3]") + if not 0.0 <= fraction <= 1.0: + raise ValueError("native receiver approach fraction must lie in [0, 1]") + + action = _held_start_action(env) + home_position = torch.tensor(RIGHT_TOOL_HOME_POS_W, device=env.device) + target_position = torch.tensor(RECEIVER_TOOL_TARGET_POS_W, device=env.device) + clearance = torch.tensor((0.0, 0.0, NATIVE_RECEIVER_PREGRASP_CLEARANCE_M), device=env.device) + home_high = home_position + clearance + target_high = target_position + clearance + home_orientation = torch.tensor(RIGHT_TOOL_HOME_ROT_XYZW, device=env.device) + target_orientation = torch.tensor(RECEIVER_TOOL_TARGET_ROT_XYZW, device=env.device) + + if segment == 0: + position = home_position + fraction * (home_high - home_position) + orientation = home_orientation + elif segment == 1: + position = home_high + orientation = _slerp_xyzw(home_orientation, target_orientation, fraction) + elif segment == 2: + position = home_high + fraction * (target_high - home_high) + orientation = target_orientation + else: + position = target_high + fraction * (target_position - target_high) + orientation = target_orientation + + action[:, 9:12] = position + action[:, 12:16] = orientation + return action + + +def _native_receiver_candidate_targets(env, candidate_poses_n: torch.Tensor) -> tuple[torch.Tensor, torch.Tensor]: + """Compose native ``T_N_C`` rows into physical world-frame tool targets.""" + + if candidate_poses_n.shape != (env.num_envs, 7): + raise ValueError("candidate poses must have shape (num_envs, 7)") + needle = env.scene["needle"] + needle_pos_w = needle.data.root_pos_w.torch + needle_quat_w = needle.data.root_quat_w.torch + channel_pos_t = torch.tensor(DVRK_JAW_CHANNEL_T_T_C_POS_M, device=env.device).expand(env.num_envs, -1) + channel_quat_t = torch.tensor(DVRK_JAW_CHANNEL_T_T_C_ROT_XYZW, device=env.device).expand(env.num_envs, -1) + channel_pos_n = candidate_poses_n[:, :3] + channel_quat_n = candidate_poses_n[:, 3:] + channel_quat_c_t = math_utils.quat_inv(channel_quat_t) + channel_pos_c_t = -math_utils.quat_apply(channel_quat_c_t, channel_pos_t) + tool_pos_n = channel_pos_n + math_utils.quat_apply(channel_quat_n, channel_pos_c_t) + tool_quat_n = math_utils.quat_mul(channel_quat_n, channel_quat_c_t) + tool_pos_w = needle_pos_w + math_utils.quat_apply(needle_quat_w, tool_pos_n) + tool_quat_w = math_utils.quat_mul(needle_quat_w, tool_quat_n) + return tool_pos_w, tool_quat_w + + +def _native_receiver_candidate_approach_action( + env, receiver_pos_w: torch.Tensor, receiver_quat_xyzw: torch.Tensor, *, segment: int, fraction: float +) -> torch.Tensor: + """Command a batched fixed pre-grasp path to unmodified native candidates.""" + + if receiver_pos_w.shape != (env.num_envs, 3) or receiver_quat_xyzw.shape != (env.num_envs, 4): + raise ValueError("batched receiver targets must match the environment count") + if segment not in range(4) or not 0.0 <= fraction <= 1.0: + raise ValueError("invalid native receiver approach segment or fraction") + action = _held_start_action(env) + home_pos_w = env.scene.env_origins + torch.tensor(RIGHT_TOOL_HOME_POS_W, device=env.device) + home_quat_xyzw = torch.tensor(RIGHT_TOOL_HOME_ROT_XYZW, device=env.device).expand(env.num_envs, -1) + clearance = torch.tensor((0.0, 0.0, NATIVE_RECEIVER_PREGRASP_CLEARANCE_M), device=env.device) + home_high = home_pos_w + clearance + target_high = receiver_pos_w + clearance + if segment == 0: + position = home_pos_w + fraction * (home_high - home_pos_w) + orientation = home_quat_xyzw + elif segment == 1: + position = home_high + orientation = torch.stack( + [_slerp_xyzw(home_quat_xyzw[index], receiver_quat_xyzw[index], fraction) for index in range(env.num_envs)] + ) + elif segment == 2: + position = home_high + fraction * (target_high - home_high) + orientation = receiver_quat_xyzw + else: + position = target_high + fraction * (receiver_pos_w - target_high) + orientation = receiver_quat_xyzw + action[:, 9:12] = position + action[:, 12:16] = orientation + return action + + +def _native_receiver_candidate_handoff_action( + env, + receiver_pos_w: torch.Tensor, + receiver_quat_xyzw: torch.Tensor, + *, + donor_jaw: tuple[float, float], + receiver_jaw: tuple[float, float], + receiver_lift_offset_m: tuple[float, float, float] = (0.0, 0.0, 0.0), +) -> torch.Tensor: + """Command exact generated channels for the batched physical screen.""" + + if receiver_pos_w.shape != (env.num_envs, 3) or receiver_quat_xyzw.shape != (env.num_envs, 4): + raise ValueError("batched receiver targets must match the environment count") + action = _held_start_action(env) + action[:, 9:12] = receiver_pos_w + torch.tensor(receiver_lift_offset_m, device=env.device) + action[:, 12:16] = receiver_quat_xyzw + action[:, 7:9] = torch.tensor(donor_jaw, device=env.device) + action[:, 16:18] = torch.tensor(receiver_jaw, device=env.device) + return action + + +def _pose_matrix(position: np.ndarray, quaternion_xyzw: np.ndarray) -> np.ndarray: + """Return a column-vector homogeneous transform from a PhysX body pose.""" + + x, y, z, w = quaternion_xyzw / np.linalg.norm(quaternion_xyzw) + transform = np.eye(4, dtype=np.float64) + transform[:3, :3] = np.asarray( + ( + (1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - z * w), 2.0 * (x * z + y * w)), + (2.0 * (x * y + z * w), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - x * w)), + (2.0 * (x * z - y * w), 2.0 * (y * z + x * w), 1.0 - 2.0 * (x * x + y * y)), + ), + dtype=np.float64, + ) + transform[:3, 3] = position + return transform + + +def _receiver_collision_channel_world(env, env_index: int) -> np.ndarray: + """Measure the midpoint of the two live receiver collision shapes. + + USD intentionally retains authoring transforms while PhysX integrates the + articulation. We therefore compose the static shape-within-link transform + with the current tensor body pose, rather than reading an authored world + transform as though it described the live collision geometry. + """ + + stage = omni.usd.get_context().get_stage() + cache = UsdGeom.XformCache() + receiver = env.scene["right_psm"] + body_ids, body_names = receiver.find_bodies(["psm_tool_gripper1_link", "psm_tool_gripper2_link"]) + assert len(body_ids) == 2, body_names + centres = [] + for body_id, link_name in zip(body_ids, body_names, strict=True): + link_path = f"{env.scene.env_prim_paths[env_index]}/RightPSM/{link_name}" + collision_path = f"{link_path}/collisions_xform/collisions" + authored_link_w = np.asarray(cache.GetLocalToWorldTransform(stage.GetPrimAtPath(link_path)), dtype=np.float64).T + authored_collision_w = np.asarray( + cache.GetLocalToWorldTransform(stage.GetPrimAtPath(collision_path)), dtype=np.float64 + ).T + link_to_collision = np.linalg.inv(authored_link_w) @ authored_collision_w + body_pose_w = _pose_matrix( + receiver.data.body_pos_w.torch[env_index, body_id].detach().cpu().numpy(), + receiver.data.body_quat_w.torch[env_index, body_id].detach().cpu().numpy(), + ) + centres.append((body_pose_w @ link_to_collision)[:3, 3]) + return np.mean(np.stack(centres), axis=0) + + +def _native_probe_candidate_poses() -> tuple[tuple[int, ...], torch.Tensor]: + """Load hash-pinned native rows as ``[xyz, qx, qy, qz, qw]`` poses.""" + + candidate_path = os.environ.get("ISAACLAB_DVRK_NATIVE_CANDIDATES_CSV") + indices_raw = os.environ.get("ISAACLAB_DVRK_NATIVE_RECEIVER_PROBE_INDICES") + if not candidate_path or not indices_raw: + pytest.skip("native receiver feasibility probe is opt-in") + candidate_file = Path(candidate_path) + digest = hashlib.sha256(candidate_file.read_bytes()).hexdigest() + assert digest == ISAAC_GRASP_CANDIDATES_SHA256 + indices = tuple(int(value) for value in indices_raw.split(",")) + assert indices and len(indices) == len(set(indices)) + with candidate_file.open(encoding="utf-8", newline="") as candidate_stream: + rows = {int(row["candidate_index"]): row for row in csv.DictReader(candidate_stream)} + assert set(indices) <= rows.keys() + # The pinned generator CSV stores each scalar-first quaternion in distinct + # qw/qx/qy/qz columns. Assemble those named fields explicitly in Isaac + # Lab's scalar-last xyzw order without altering the generated rotation. + poses = torch.tensor( + [ + [ + float(rows[index]["needle_channel_x_m"]), + float(rows[index]["needle_channel_y_m"]), + float(rows[index]["needle_channel_z_m"]), + float(rows[index]["needle_channel_qx"]), + float(rows[index]["needle_channel_qy"]), + float(rows[index]["needle_channel_qz"]), + float(rows[index]["needle_channel_qw"]), + ] + for index in indices + ], + dtype=torch.float32, + ) + assert torch.isfinite(poses).all() + torch.testing.assert_close( + torch.linalg.vector_norm(poses[:, 3:], dim=-1), + torch.ones(len(indices), dtype=poses.dtype), + rtol=1.0e-6, + atol=1.0e-6, + ) + return indices, poses + + +def _handoff_diagnostics(env, phase_machine) -> dict[str, object]: + """Return measured state when a physical hand-off assertion fails.""" + + loads, reaction_normals_w, raw_forces_w = jaw_needle_contact_measurements(env) + receiver = env.scene["right_psm"] + body_ids, body_names = receiver.find_bodies(DVRK_PSM_TOOL_TIP_BODY_NAME) + assert len(body_ids) == 1, body_names + needle_pos_r, needle_quat_r = math_utils.subtract_frame_transforms( + receiver.data.body_pos_w.torch[:, body_ids[0]], + receiver.data.body_quat_w.torch[:, body_ids[0]], + env.scene["needle"].data.root_pos_w.torch, + env.scene["needle"].data.root_quat_w.torch, + ) + target_pos_r = torch.tensor(DVRK_HANDOFF_PHASE_CFG.receiver_relative_position_target_m, device=env.device) + target_quat_r = torch.tensor( + DVRK_HANDOFF_PHASE_CFG.receiver_relative_orientation_target_xyzw, device=env.device + ).expand_as(needle_quat_r) + needle = env.scene["needle"] + receiver_pose_w = torch.cat( + (receiver.data.body_pos_w.torch[:, body_ids[0]], receiver.data.body_quat_w.torch[:, body_ids[0]]), dim=-1 + ) + measurements = HandoffMeasurements( + normal_forces_n=loads, + reaction_normals_w=reaction_normals_w, + needle_pose_w=torch.cat((needle.data.root_pos_w.torch, needle.data.root_quat_w.torch), dim=-1), + needle_velocity_w=torch.cat((needle.data.root_lin_vel_w.torch, needle.data.root_ang_vel_w.torch), dim=-1), + receiver_pose_w=receiver_pose_w, + ) + return { + "phase": int(phase_machine.phase.item()), + "loads_n": loads.detach().cpu().tolist(), + "reaction_normals_w": reaction_normals_w.detach().cpu().tolist(), + "raw_forces_w_n": raw_forces_w.detach().cpu().tolist(), + "jaw_normal_dots": torch.sum( + torch.nn.functional.normalize(reaction_normals_w[:, 0::2], dim=-1) + * torch.nn.functional.normalize(reaction_normals_w[:, 1::2], dim=-1), + dim=-1, + ) + .detach() + .cpu() + .tolist(), + "donor_bilateral": phase_machine._bilateral_contact( + loads[:, :2], reaction_normals_w[:, :2], phase_machine._donor_engaged + ) + .detach() + .cpu() + .tolist(), + "receiver_bilateral": phase_machine._bilateral_contact( + loads[:, 2:], reaction_normals_w[:, 2:], phase_machine._receiver_engaged + ) + .detach() + .cpu() + .tolist(), + "receiver_bounds": phase_machine._receiver_bounds(measurements).detach().cpu().tolist(), + "receiver_tool_pos_w": receiver.data.body_pos_w.torch[:, body_ids[0]].detach().cpu().tolist(), + "receiver_tool_quat_xyzw": receiver.data.body_quat_w.torch[:, body_ids[0]].detach().cpu().tolist(), + "receiver_joint_pos": receiver.data.joint_pos.torch.detach().cpu().tolist(), + "donor_joint_pos": env.scene["left_psm"].data.joint_pos.torch.detach().cpu().tolist(), + "needle_pos_w": env.scene["needle"].data.root_pos_w.torch.detach().cpu().tolist(), + "needle_quat_xyzw": env.scene["needle"].data.root_quat_w.torch.detach().cpu().tolist(), + "needle_velocity_w": torch.cat( + (env.scene["needle"].data.root_lin_vel_w.torch, env.scene["needle"].data.root_ang_vel_w.torch), dim=-1 + ) + .detach() + .cpu() + .tolist(), + "receiver_relative_pos_m": needle_pos_r.detach().cpu().tolist(), + "receiver_relative_position_error_m": torch.linalg.vector_norm(needle_pos_r - target_pos_r, dim=-1) + .detach() + .cpu() + .tolist(), + "receiver_relative_orientation_error_rad": math_utils.quat_error_magnitude(needle_quat_r, target_quat_r) + .detach() + .cpu() + .tolist(), + } + + +def _clone_tree(value: Any) -> Any: + """Clone a nested tensor tree for a reset determinism comparison.""" + + if isinstance(value, torch.Tensor): + return value.detach().clone() + if isinstance(value, Mapping): + return {key: _clone_tree(child) for key, child in value.items()} + if isinstance(value, tuple): + return tuple(_clone_tree(child) for child in value) + if isinstance(value, list): + return [_clone_tree(child) for child in value] + return value + + +def _assert_finite_tree(value: Any, path: str = "observations") -> None: + """Require every floating-point tensor in a nested observation tree to be finite.""" + + if isinstance(value, torch.Tensor): + if value.is_floating_point() or value.is_complex(): + assert torch.isfinite(value).all(), f"non-finite tensor at {path}" + return + if isinstance(value, Mapping): + for key, child in value.items(): + _assert_finite_tree(child, f"{path}.{key}") + return + if isinstance(value, (tuple, list)): + for index, child in enumerate(value): + _assert_finite_tree(child, f"{path}[{index}]") + return + raise AssertionError(f"unsupported observation leaf at {path}: {type(value).__name__}") + + +def _assert_same_tree(actual: Any, expected: Any, path: str = "state") -> None: + """Compare two reset snapshots with one declared floating-point tolerance.""" + + if isinstance(actual, torch.Tensor): + assert isinstance(expected, torch.Tensor), f"type mismatch at {path}" + if actual.is_floating_point() or actual.is_complex(): + torch.testing.assert_close( + actual, + expected, + rtol=0.0, + atol=RESET_FLOAT_ATOL, + msg=lambda message: f"{path}: {message}", + ) + else: + assert torch.equal(actual, expected), f"tensor mismatch at {path}" + return + if isinstance(actual, Mapping): + assert isinstance(expected, Mapping), f"type mismatch at {path}" + assert actual.keys() == expected.keys(), f"key mismatch at {path}" + for key in actual: + _assert_same_tree(actual[key], expected[key], f"{path}.{key}") + return + if isinstance(actual, (tuple, list)): + assert isinstance(expected, type(actual)), f"type mismatch at {path}" + assert len(actual) == len(expected), f"length mismatch at {path}" + for index, (actual_child, expected_child) in enumerate(zip(actual, expected, strict=True)): + _assert_same_tree(actual_child, expected_child, f"{path}[{index}]") + return + assert actual == expected, f"value mismatch at {path}" + + +def _reset_snapshot(env, observations: Mapping[str, Any]) -> dict[str, Any]: + """Capture reset-visible observations and all directly reset dynamic state.""" + + return _clone_tree( + { + "observations": observations, + "left_joint_pos": env.scene["left_psm"].data.joint_pos.torch, + "left_joint_vel": env.scene["left_psm"].data.joint_vel.torch, + "right_joint_pos": env.scene["right_psm"].data.joint_pos.torch, + "right_joint_vel": env.scene["right_psm"].data.joint_vel.torch, + "needle_root_link_pose_w": env.scene["needle"].data.root_link_pose_w.torch, + "needle_root_com_velocity_w": env.scene["needle"].data.root_com_vel_w.torch, + } + ) + + +class _DirectStateWriteAudit: + """Reject high- and low-level state writes outside a reset event. + + Articulation effort and drive-target setters are recorded separately and + deliberately remain usable: those are the physical control path exercised + by every environment step, not instantaneous state mutation. + """ + + def __init__(self, env): + self._env = env + self._reset_depth = 0 + self._active_reset_calls: list[tuple[str, str, str]] | None = None + self.reset_windows: list[tuple[tuple[str, str, str], ...]] = [] + self.asset_state_calls: list[tuple[str, str]] = [] + self.physx_state_calls: list[tuple[str, str]] = [] + self.physx_control_calls: list[tuple[str, str]] = [] + self.violations: list[tuple[str, str, str]] = [] + self.usd_mutations: list[tuple[str, str]] = [] + self._usd_notice_registration = None + + def install(self) -> None: + """Wrap the event dispatcher and primitive state-writer methods.""" + + if getattr(self._env, "_needle_pass_direct_state_write_audit", None) is not None: + raise RuntimeError("only one direct-state audit may be installed per environment") + self._env._needle_pass_direct_state_write_audit = self + + original_apply = self._env.event_manager.apply + + def audited_apply(mode: str, *args, **kwargs): + if mode != "reset": + return original_apply(mode, *args, **kwargs) + assert self._reset_depth == 0, "nested reset event windows are not permitted" + self._reset_depth += 1 + self._active_reset_calls = [] + try: + result = original_apply(mode, *args, **kwargs) + except BaseException: + self._active_reset_calls = None + raise + finally: + self._reset_depth -= 1 + reset_calls = tuple(self._active_reset_calls) + self._active_reset_calls = None + if reset_calls != _EXPECTED_RESET_STATE_WRITE_SEQUENCE: + self.violations.append(("reset", "window", "unexpected_write_sequence")) + raise AssertionError( + "reset state-write sequence differs from the documented whitelist:\n" + f"expected={_EXPECTED_RESET_STATE_WRITE_SEQUENCE!r}\nactual={reset_calls!r}" + ) + self.reset_windows.append(reset_calls) + return result + + self._env.event_manager.apply = audited_apply + for asset_name in ("left_psm", "right_psm"): + asset = self._env.scene[asset_name] + self._wrap_guarded_methods(asset, "asset", asset_name, _ARTICULATION_STATE_WRITERS) + self._wrap_guarded_methods( + asset.root_view, + "physx", + asset_name, + _ARTICULATION_PHYSX_STATE_SETTERS, + ) + self._wrap_control_methods(asset.root_view, asset_name, _ARTICULATION_PHYSX_CONTROL_SETTERS) + + needle = self._env.scene["needle"] + self._wrap_guarded_methods(needle, "asset", "needle", _RIGID_OBJECT_STATE_WRITERS) + self._wrap_guarded_methods( + needle.root_view, + "physx", + "needle", + _RIGID_OBJECT_PHYSX_STATE_SETTERS, + ) + stage = omni.usd.get_context().get_stage() + self._usd_notice_registration = Tf.Notice.Register(Usd.Notice.ObjectsChanged, self._record_usd_changes, stage) + + def uninstall(self) -> None: + """Revoke the stage notice before Isaac Lab tears the scene down.""" + + if self._usd_notice_registration is not None: + self._usd_notice_registration.Revoke() + self._usd_notice_registration = None + if getattr(self._env, "_needle_pass_direct_state_write_audit", None) is self: + del self._env._needle_pass_direct_state_write_audit + + def _wrap_guarded_methods( + self, + owner: Any, + layer: str, + asset_name: str, + method_names: tuple[str, ...], + ) -> None: + calls = self.asset_state_calls if layer == "asset" else self.physx_state_calls + for method_name in method_names: + original_method = getattr(owner, method_name) + + def guarded_writer( + *args, + _method=original_method, + _label=(asset_name, method_name), + _layer=layer, + **kwargs, + ): + calls.append(_label) + call = (_layer, *_label) + if self._active_reset_calls is not None: + self._active_reset_calls.append(call) + if self._reset_depth == 0 or call not in _RESET_STATE_WRITE_WHITELIST: + violation = call + self.violations.append(violation) + location = "outside reset" if self._reset_depth == 0 else "outside reset whitelist" + raise AssertionError(f"direct state write {location}: {_layer}:{_label[0]}.{_label[1]}") + return _method(*args, **kwargs) + + setattr(owner, method_name, guarded_writer) + + def _wrap_control_methods(self, owner: Any, asset_name: str, method_names: tuple[str, ...]) -> None: + for method_name in method_names: + original_method = getattr(owner, method_name) + + def allowed_control(*args, _method=original_method, _label=(asset_name, method_name), **kwargs): + self.physx_control_calls.append(_label) + return _method(*args, **kwargs) + + setattr(owner, method_name, allowed_control) + + def _record_usd_changes(self, notice: Usd.Notice.ObjectsChanged, stage: Usd.Stage) -> None: + """Record forbidden authored USD edits at any time after installation.""" + + needle_root_path = _needle_root_path(self._env) + for change_kind, paths in ( + ("resync", notice.GetResyncedPaths()), + ("info", notice.GetChangedInfoOnlyPaths()), + ): + for path in paths: + prim_path = path.GetPrimPath() + prim = stage.GetPrimAtPath(prim_path) + if prim_path.HasPrefix(Sdf.Path(needle_root_path)): + self.usd_mutations.append((change_kind, str(path))) + continue + if not prim.IsValid(): + continue + if UsdPhysics.Joint(prim) or _prim_is_attachment(prim): + self.usd_mutations.append((change_kind, str(path))) + continue + if PhysxSchema.PhysxPhysicsJointInstancer(prim) or any( + "Attachment" in schema_name for schema_name in prim.GetAppliedSchemas() + ): + self.usd_mutations.append((change_kind, str(path))) + continue + usd_property = stage.GetPropertyAtPath(path) + if isinstance(usd_property, Usd.Relationship) and any( + _path_is_in_needle(target, prim, needle_root_path) for target in _relationship_targets(usd_property) + ): + self.usd_mutations.append((change_kind, str(path))) + + +def _path_is_in_needle(target: Sdf.Path, owner_prim: Usd.Prim, needle_root_path: str) -> bool: + """Return whether a relationship target addresses the needle subtree.""" + + if not target.IsAbsolutePath(): + target = target.MakeAbsolutePath(owner_prim.GetPath()) + target_prim_path = target.GetPrimPath() + needle_root = Sdf.Path(needle_root_path) + return target_prim_path == needle_root or target_prim_path.HasPrefix(needle_root) + + +def _relationship_targets(relationship: Usd.Relationship) -> tuple[Sdf.Path, ...]: + """Return unique direct and forwarded relationship targets.""" + + targets: dict[str, Sdf.Path] = {} + for target in (*relationship.GetTargets(), *relationship.GetForwardedTargets()): + targets[str(target)] = target + return tuple(targets.values()) + + +def _needle_root_path(env, env_index: int = 0) -> str: + """Resolve one cloned needle root from the configured scene prim path.""" + + needle_prim_name = env.cfg.scene.needle.prim_path.rstrip("/").rsplit("/", maxsplit=1)[-1] + assert needle_prim_name and not any(token in needle_prim_name for token in ("*", "[", "]", "{", "}")) + return f"{env.scene.env_prim_paths[env_index]}/{needle_prim_name}" + + +def _prim_is_in_needle(prim: Usd.Prim, needle_root_path: str) -> bool: + prim_path = str(prim.GetPath()) + return prim_path == needle_root_path or prim_path.startswith(f"{needle_root_path}/") + + +def _prim_is_attachment(prim: Usd.Prim) -> bool: + """Return whether a prim type or applied schema represents an attachment.""" + + return "Attachment" in str(prim.GetTypeName()) or any( + "Attachment" in schema_name for schema_name in prim.GetAppliedSchemas() + ) + + +def _assert_live_needle_topology(env) -> None: + """Require one free dynamic body with no joint, attachment, or relationship constraint.""" + + stage = omni.usd.get_context().get_stage() + needle_root_path = _needle_root_path(env) + needle_root = stage.GetPrimAtPath(needle_root_path) + assert needle_root.IsValid() + assert str(needle_root.GetParent().GetPath()) == env.scene.env_prim_paths[0] + + needle_prims = list(Usd.PrimRange(needle_root)) + rigid_prims = [prim for prim in needle_prims if prim.HasAPI(UsdPhysics.RigidBodyAPI)] + assert len(rigid_prims) == 1 + assert not any(prim.HasAPI(UsdPhysics.ArticulationRootAPI) for prim in needle_prims) + + rigid_body = UsdPhysics.RigidBodyAPI(rigid_prims[0]) + assert rigid_body.GetRigidBodyEnabledAttr().Get() is True + assert rigid_body.GetKinematicEnabledAttr().Get() is False + + attached_joints: list[tuple[str, str, tuple[str, ...]]] = [] + attached_fixed_joints: list[str] = [] + physics_attachments: list[tuple[str, tuple[str, ...]]] = [] + physics_joint_instancers: list[tuple[str, tuple[str, ...]]] = [] + attachment_apis: list[tuple[str, str]] = [] + inbound_relationships: list[tuple[str, str, tuple[str, ...]]] = [] + for prim in stage.TraverseAll(): + joint = UsdPhysics.Joint(prim) + if joint: + targets = ( + *_relationship_targets(joint.GetBody0Rel()), + *_relationship_targets(joint.GetBody1Rel()), + ) + if _prim_is_in_needle(prim, needle_root_path) or any( + _path_is_in_needle(target, prim, needle_root_path) for target in targets + ): + attached_joints.append( + (str(prim.GetPath()), str(prim.GetTypeName()), tuple(str(target) for target in targets)) + ) + if UsdPhysics.FixedJoint(prim): + attached_fixed_joints.append(str(prim.GetPath())) + + if _prim_is_attachment(prim): + targets = tuple( + target for relationship in prim.GetRelationships() for target in _relationship_targets(relationship) + ) + if _prim_is_in_needle(prim, needle_root_path) or any( + _path_is_in_needle(target, prim, needle_root_path) for target in targets + ): + physics_attachments.append((str(prim.GetPath()), tuple(str(target) for target in targets))) + + joint_instancer = PhysxSchema.PhysxPhysicsJointInstancer(prim) + if joint_instancer: + targets = ( + *_relationship_targets(joint_instancer.GetPhysicsBody0sRel()), + *_relationship_targets(joint_instancer.GetPhysicsBody1sRel()), + ) + if _prim_is_in_needle(prim, needle_root_path) or any( + _path_is_in_needle(target, prim, needle_root_path) for target in targets + ): + physics_joint_instancers.append((str(prim.GetPath()), tuple(str(target) for target in targets))) + + if _prim_is_in_needle(prim, needle_root_path): + for schema_name in prim.GetAppliedSchemas(): + if "Attachment" in schema_name: + attachment_apis.append((str(prim.GetPath()), schema_name)) + + for relationship in prim.GetRelationships(): + targets = _relationship_targets(relationship) + if not any(_path_is_in_needle(target, prim, needle_root_path) for target in targets): + continue + relationship_name = str(relationship.GetName()) + if _prim_is_in_needle(prim, needle_root_path) and relationship_name.startswith("material:binding"): + continue + inbound_relationships.append( + (str(prim.GetPath()), relationship_name, tuple(str(target) for target in targets)) + ) + + assert attached_joints == [] + assert attached_fixed_joints == [] + assert physics_attachments == [] + assert physics_joint_instancers == [] + assert attachment_apis == [] + assert inbound_relationships == [] + + view_paths = tuple(str(path) for path in env.scene["needle"].root_view.prim_paths) + assert len(view_paths) == 1 + assert view_paths[0] == str(rigid_prims[0].GetPath()) + + +def _assert_live_needle_mass_and_material(env) -> None: + """Check PhysX-resolved values and every collision's strong task binding.""" + + needle = env.scene["needle"] + root_view = needle.root_view + coms = wp.to_torch(root_view.get_coms()) + assert coms.shape == (root_view.count, 7) + expected_com_position = torch.tensor( + assets.NEEDLE_CENTRE_OF_MASS_BODY_LOCAL_M, + device=coms.device, + dtype=coms.dtype, + ).expand(coms.shape[0], -1) + torch.testing.assert_close(coms[:, :3], expected_com_position, rtol=0.0, atol=5.0e-9) + + masses = wp.to_torch(root_view.get_masses()) + assert root_view.count == env.num_envs + assert masses.shape == (root_view.count, 1) + torch.testing.assert_close( + masses, + torch.full_like(masses, assets.NEEDLE_MASS_KG), + rtol=1.0e-6, + atol=1.0e-10, + ) + + live_materials = wp.to_torch(root_view.get_material_properties()) + assert live_materials.shape == ( + root_view.count, + root_view.max_shapes, + 3, + ) + assert root_view.max_shapes > 0 + expected_material = torch.tensor( + (assets.NEEDLE_STATIC_FRICTION, assets.NEEDLE_DYNAMIC_FRICTION, assets.NEEDLE_RESTITUTION), + device=live_materials.device, + dtype=live_materials.dtype, + ) + torch.testing.assert_close( + live_materials, + expected_material.expand_as(live_materials), + rtol=1.0e-6, + atol=1.0e-7, + ) + + stage = omni.usd.get_context().get_stage() + needle_root_path = _needle_root_path(env) + material_path = f"{needle_root_path}/physicsMaterial" + material_prim = stage.GetPrimAtPath(material_path) + assert material_prim.IsValid() + assert material_prim.GetAttribute("physics:staticFriction").Get() == pytest.approx(assets.NEEDLE_STATIC_FRICTION) + assert material_prim.GetAttribute("physics:dynamicFriction").Get() == pytest.approx(assets.NEEDLE_DYNAMIC_FRICTION) + assert material_prim.GetAttribute("physics:restitution").Get() == pytest.approx(assets.NEEDLE_RESTITUTION) + assert material_prim.GetAttribute("physxMaterial:frictionCombineMode").Get() == assets.NEEDLE_FRICTION_COMBINE_MODE + assert ( + material_prim.GetAttribute("physxMaterial:restitutionCombineMode").Get() + == assets.NEEDLE_RESTITUTION_COMBINE_MODE + ) + + needle_root = stage.GetPrimAtPath(needle_root_path) + collision_prims = [prim for prim in Usd.PrimRange(needle_root) if prim.HasAPI(UsdPhysics.CollisionAPI)] + assert collision_prims + for collision_prim in collision_prims: + binding_api = UsdShade.MaterialBindingAPI(collision_prim) + binding = binding_api.GetDirectBinding("physics") + assert binding.GetMaterialPath() == Sdf.Path(material_path) + assert binding.GetMaterialPurpose() == "physics" + bound_material, winning_relationship = binding_api.ComputeBoundMaterial("physics") + assert bound_material.GetPath() == Sdf.Path(material_path) + assert ( + UsdShade.MaterialBindingAPI.GetMaterialBindingStrength(winning_relationship) + == UsdShade.Tokens.strongerThanDescendants + ) + + +def _assert_contact_sensor_matrices(env) -> None: + """Require all four filtered jaw sensors to expose finite ``N x 1 x 1 x 3`` forces.""" + + assert len(JAW_CONTACT_SENSOR_NAMES) == 4 + for sensor_name in JAW_CONTACT_SENSOR_NAMES: + sensor = env.scene.sensors[sensor_name] + assert sensor.contact_view.filter_count == 1 + force_matrix_w = sensor.data.force_matrix_w.torch + assert force_matrix_w is not None + assert force_matrix_w.shape == (env.num_envs, 1, 1, 3) + assert torch.isfinite(force_matrix_w).all(), f"non-finite contact matrix for {sensor_name}" + + +def _assert_live_action_and_joint_order(env) -> None: + """Require the runtime action manager to preserve the public 18-D ABI.""" + + expected_terms = ["left_arm_action", "left_jaw_action", "right_arm_action", "right_jaw_action"] + assert env.action_manager.active_terms == expected_terms + assert env.action_manager.action_term_dim == [7, 2, 7, 2] + assert env.action_manager.total_action_dim == 18 + assert env.action_manager.action.shape == (env.num_envs, 18) + + for side in ("left", "right"): + arm_term = env.action_manager._terms[f"{side}_arm_action"] + jaw_term = env.action_manager._terms[f"{side}_jaw_action"] + assert tuple(arm_term._joint_names) == tuple(DVRK_PSM_ARM_JOINT_NAMES) + assert tuple(jaw_term._joint_names) == tuple(DVRK_PSM_JAW_JOINT_NAMES) + + +def _assert_live_psm_jaw_contract(env) -> None: + """Check resolved jaw limits and the pinned jaw collision material.""" + + stage = omni.usd.get_context().get_stage() + expected_material_values = { + "physics:staticFriction": 1.0, + "physics:dynamicFriction": 10.0, + "physics:restitution": 0.0, + } + for asset_name, prim_name in (("left_psm", "LeftPSM"), ("right_psm", "RightPSM")): + articulation = env.scene[asset_name] + jaw_ids, jaw_names = articulation.find_joints(list(DVRK_PSM_JAW_JOINT_NAMES), preserve_order=True) + assert tuple(jaw_names) == tuple(DVRK_PSM_JAW_JOINT_NAMES) + limits = articulation.data.joint_pos_limits.torch[:, jaw_ids, :] + expected_limits = torch.tensor( + ((-math.pi / 6.0, 0.0), (0.0, math.pi / 6.0)), + device=limits.device, + dtype=limits.dtype, + ).expand(env.num_envs, -1, -1) + torch.testing.assert_close(limits, expected_limits, rtol=0.0, atol=1.0e-6) + for endpoint in (DVRK_PSM_JAW_OPEN_POS, DVRK_PSM_JAW_CLOSED_POS): + endpoint_tensor = torch.tensor(endpoint, device=limits.device, dtype=limits.dtype) + assert torch.all(endpoint_tensor >= limits[0, :, 0]) + assert torch.all(endpoint_tensor <= limits[0, :, 1]) + + psm_root_path = f"{env.scene.env_prim_paths[0]}/{prim_name}" + material_path = f"{psm_root_path}/Looks/PhysicsMaterial" + material_prim = stage.GetPrimAtPath(material_path) + assert material_prim.IsValid() + for attribute_name, expected_value in expected_material_values.items(): + assert material_prim.GetAttribute(attribute_name).Get() == pytest.approx(expected_value) + # The pinned PSM does not author a combine mode. PhysX therefore uses + # its default average mode for the jaw material; the needle's explicit + # min mode wins the pair and yields the declared resolved coefficients. + assert not material_prim.GetAttribute("physxMaterial:frictionCombineMode").HasAuthoredValueOpinion() + + for jaw_link in ("psm_tool_gripper1_link", "psm_tool_gripper2_link"): + collision_path = f"{psm_root_path}/{jaw_link}/collisions_xform/collisions" + collision_prim = stage.GetPrimAtPath(collision_path) + assert collision_prim.IsValid() + assert collision_prim.HasAPI(UsdPhysics.CollisionAPI) + binding = UsdShade.MaterialBindingAPI(collision_prim).GetDirectBinding("physics") + assert binding.GetMaterialPath() == Sdf.Path(material_path) + + +def _assert_static_suture_pad(env) -> None: + """Require the remote pad asset to remain a static collider outside the proof region.""" + + stage = omni.usd.get_context().get_stage() + pad_path = f"{env.scene.env_prim_paths[0]}/SuturePad" + pad_root = stage.GetPrimAtPath(pad_path) + assert pad_root.IsValid() + pad_prims = list(Usd.PrimRange(pad_root)) + assert any(prim.HasAPI(UsdPhysics.CollisionAPI) for prim in pad_prims) + assert not any(prim.HasAPI(UsdPhysics.RigidBodyAPI) for prim in pad_prims) + + pad_position = env.cfg.scene.suture_pad.init_state.pos + needle_position = env.cfg.scene.needle.init_state.pos + assert pad_position[2] < needle_position[2] + assert torch.linalg.vector_norm(torch.tensor(pad_position[:2]) - torch.tensor(needle_position[:2])) > 0.25 + + +def _assert_configured_tool_homes(env) -> None: + """Compare both simulated tool tips with their shared configuration constants.""" + + for asset_name, position, orientation_xyzw in ( + ("left_psm", LEFT_TOOL_HOME_POS_W, LEFT_TOOL_HOME_ROT_XYZW), + ("right_psm", RIGHT_TOOL_HOME_POS_W, RIGHT_TOOL_HOME_ROT_XYZW), + ): + articulation = env.scene[asset_name] + body_ids, body_names = articulation.find_bodies(DVRK_PSM_TOOL_TIP_BODY_NAME) + assert len(body_ids) == 1, body_names + actual_position = articulation.data.body_pos_w.torch[:, body_ids[0], :] + expected_position = env.scene.env_origins + torch.tensor(position, device=env.device) + torch.testing.assert_close(actual_position, expected_position, rtol=0.0, atol=2.0e-6) + + expected_orientation = torch.tensor(orientation_xyzw, device=env.device).expand(env.num_envs, -1) + actual_orientation = torch.nn.functional.normalize( + articulation.data.body_quat_w.torch[:, body_ids[0], :], + dim=-1, + ) + quaternion_dot = torch.abs(torch.sum(actual_orientation * expected_orientation, dim=-1)).clamp(max=1.0) + orientation_error = 2.0 * torch.acos(quaternion_dot) + assert torch.all(orientation_error <= 1.0e-3), orientation_error + + +def _bounded_random_actions(env, steps: int, seed: int) -> torch.Tensor: + """Return deterministic, finite random commands close to the two homes.""" + + generator = torch.Generator(device="cpu").manual_seed(seed) + actions = _held_start_action(env).cpu().repeat(steps, 1, 1) + for position_start in (0, 9): + actions[:, :, position_start : position_start + 3] += 0.004 * ( + torch.rand((steps, env.num_envs, 3), generator=generator) - 0.5 + ) + for quaternion_start in (3, 12): + quaternion = actions[:, :, quaternion_start : quaternion_start + 4] + quaternion += 0.01 * (torch.rand(quaternion.shape, generator=generator) - 0.5) + actions[:, :, quaternion_start : quaternion_start + 4] = torch.nn.functional.normalize( + quaternion, + dim=-1, + ) + for jaw_start in (7, 16): + closedness = 0.25 * torch.rand((steps, env.num_envs, 1), generator=generator) + jaw_open = torch.tensor(DVRK_PSM_JAW_OPEN_POS).reshape(1, 1, 2) + actions[:, :, jaw_start : jaw_start + 2] = (1.0 - closedness) * jaw_open + actions = actions.to(env.device) + assert actions.shape == (steps, env.num_envs, 18) + assert actions.is_contiguous() + assert torch.isfinite(actions).all() + return actions + + +def _run_finite_action_trace(env, actions: torch.Tensor) -> None: + """Run a fixed action-only trace and check every transition tensor.""" + + with torch.inference_mode(): + for step, action in enumerate(actions): + assert torch.isfinite(action).all(), f"non-finite action at step {step}" + observations, rewards, terminated, truncated, _ = env.step(action) + _assert_finite_tree(observations) + assert rewards.shape == (env.num_envs,) + assert torch.isfinite(rewards).all(), f"non-finite reward at step {step}" + assert terminated.shape == (env.num_envs,) + assert truncated.shape == (env.num_envs,) + assert terminated.dtype == torch.bool + assert truncated.dtype == torch.bool + + +@pytest.mark.isaacsim_ci +def test_one_env_donor_held_reset_is_physically_retained(): + """Require the free needle to retain bilateral donor contact from reset.""" + + with _task_env(num_envs=1) as env: + env.reset(seed=SEED) + initial_needle_position = env.scene["needle"].data.root_pos_w.torch.clone() + initial_needle_orientation = env.scene["needle"].data.root_quat_w.torch.clone() + action = _held_start_action(env) + for _ in range(64): + _, _, terminated, truncated, _ = env.step(action) + assert not terminated.any() + assert not truncated.any() + + loads, _, raw_forces_w = jaw_needle_contact_measurements(env) + machine = get_handoff_phase_machine(env, DVRK_HANDOFF_PHASE_CFG) + assert torch.all(loads[:, :2] >= DVRK_HANDOFF_PHASE_CFG.engage_force_n), loads + assert torch.equal(machine.phase, torch.full_like(machine.phase, int(HandoffPhase.DONOR_HOLD))) + final_needle_position = env.scene["needle"].data.root_pos_w.torch + position_drift = final_needle_position - initial_needle_position + # Gravity-on seating is expected for a free rigid body between driven + # jaws. Bilateral load and the measured DONOR_HOLD phase prove + # retention; this sub-millimetre bound catches material slip or loss. + assert torch.max(torch.abs(position_drift)) <= 5.0e-4, { + "initial_needle_position_w": initial_needle_position.detach().cpu().tolist(), + "final_needle_position_w": final_needle_position.detach().cpu().tolist(), + "initial_needle_orientation_xyzw": initial_needle_orientation.detach().cpu().tolist(), + "final_needle_orientation_xyzw": env.scene["needle"].data.root_quat_w.torch.detach().cpu().tolist(), + "final_needle_velocity_w": torch.cat( + ( + env.scene["needle"].data.root_lin_vel_w.torch, + env.scene["needle"].data.root_ang_vel_w.torch, + ), + dim=-1, + ) + .detach() + .cpu() + .tolist(), + "position_drift_m": position_drift.detach().cpu().tolist(), + "loads_n": loads.detach().cpu().tolist(), + "raw_forces_w_n": raw_forces_w.detach().cpu().tolist(), + } + + +def _run_native_grasp_handoff(runner, env) -> None: + """Run the fixed native trace through measured hand-off and retained lift.""" + + phase_machine = get_handoff_phase_machine(env, DVRK_HANDOFF_PHASE_CFG) + debug_phase_transitions = bool(os.environ.get("ISAACLAB_DVRK_NEEDLE_PASS_DEBUG_PHASES")) + previous_phase = int(phase_machine.phase.item()) + + def report_phase_transition(segment: str, step: int) -> None: + nonlocal previous_phase + current_phase = int(phase_machine.phase.item()) + if debug_phase_transitions and current_phase != previous_phase: + diagnostic = _handoff_diagnostics(env, phase_machine) + print( + "NATIVE_HANDOFF_PHASE_TRANSITION=" + + repr( + { + "segment": segment, + "step": step, + "from": previous_phase, + "to": current_phase, + "loads_n": diagnostic["loads_n"], + "donor_bilateral": diagnostic["donor_bilateral"], + "receiver_bilateral": diagnostic["receiver_bilateral"], + "receiver_bounds": diagnostic["receiver_bounds"], + } + ) + ) + previous_phase = current_phase + + for _ in range(NATIVE_DONOR_HOLD_SETTLE_STEPS): + _, _, terminated, truncated, _ = runner.step(_held_start_action(env)) + report_phase_transition("donor_hold", _) + assert not terminated.any() + assert not truncated.any() + assert int(phase_machine.phase.item()) == int(HandoffPhase.DONOR_HOLD) + + for segment in range(4): + for step in range(NATIVE_RECEIVER_APPROACH_SEGMENT_STEPS): + _, _, terminated, truncated, _ = runner.step( + _native_receiver_staged_approach_action( + env, segment=segment, fraction=(step + 1) / NATIVE_RECEIVER_APPROACH_SEGMENT_STEPS + ) + ) + report_phase_transition(f"approach_{segment}", step) + assert not terminated.any(), _handoff_diagnostics(env, phase_machine) + assert not truncated.any(), _handoff_diagnostics(env, phase_machine) + assert int(phase_machine.phase.item()) == int(HandoffPhase.DONOR_HOLD), _handoff_diagnostics( + env, phase_machine + ) + for _ in range(NATIVE_RECEIVER_CLOSE_SETTLE_STEPS): + _, _, terminated, truncated, _ = runner.step( + _native_receiver_handoff_action(env, approach_fraction=1.0, receiver_jaw=DVRK_PSM_JAW_CLOSED_POS) + ) + report_phase_transition("receiver_close", _) + assert not terminated.any(), _handoff_diagnostics(env, phase_machine) + assert not truncated.any(), _handoff_diagnostics(env, phase_machine) + assert int(phase_machine.phase.item()) == int(HandoffPhase.CO_HOLD), _handoff_diagnostics(env, phase_machine) + + # The public donor command opens only after the measured receiver co-hold. + for step in range(NATIVE_DONOR_RELEASE_SETTLE_STEPS): + _, _, terminated, truncated, _ = runner.step( + _native_receiver_handoff_action( + env, + approach_fraction=1.0, + donor_jaw=_native_donor_release_jaw(step), + receiver_jaw=DVRK_PSM_JAW_CLOSED_POS, + ) + ) + report_phase_transition("donor_release", step) + assert not terminated.any(), _handoff_diagnostics(env, phase_machine) + assert not truncated.any(), _handoff_diagnostics(env, phase_machine) + if int(phase_machine.phase.item()) != int(HandoffPhase.RECEIVER_ONLY_HOLD): + print("NATIVE_HANDOFF_RELEASE_FAILURE=" + repr(_handoff_diagnostics(env, phase_machine))) + assert int(phase_machine.phase.item()) == int(HandoffPhase.RECEIVER_ONLY_HOLD), _handoff_diagnostics( + env, phase_machine + ) + + for step in range(NATIVE_RECEIVER_LIFT_STEPS): + before_step = _handoff_diagnostics(env, phase_machine) + # A non-terminal lift frame must remain receiver-only. This catches a + # donor re-grasp immediately rather than accepting a later recovery. + assert int(phase_machine.phase.item()) == int(HandoffPhase.RECEIVER_ONLY_HOLD), before_step + loads_before_step, _, _ = jaw_needle_contact_measurements(env) + assert torch.all(loads_before_step[:, :2] < DVRK_HANDOFF_PHASE_CFG.disengage_force_n), before_step + assert torch.all(loads_before_step[:, 2:] >= DVRK_HANDOFF_PHASE_CFG.engage_force_n), before_step + _, _, terminated, truncated, _ = runner.step( + _native_receiver_handoff_action( + env, + approach_fraction=1.0, + donor_jaw=DVRK_PSM_JAW_OPEN_POS, + receiver_jaw=DVRK_PSM_JAW_CLOSED_POS, + receiver_lift_offset_m=_native_receiver_lift_offset(step), + ) + ) + report_phase_transition("receiver_lift", step) + assert not truncated.any(), _handoff_diagnostics(env, phase_machine) + if terminated.any(): + success = env.termination_manager.get_term("success") + dropped = env.termination_manager.get_term("needle_dropped_or_out_of_bounds") + if not torch.all(success) or torch.any(dropped): + print( + "NATIVE_HANDOFF_LIFT_FAILURE=" + + repr( + { + "step": step, + "before_step": before_step, + "success": success.detach().cpu().tolist(), + "dropped": dropped.detach().cpu().tolist(), + "after_reset": _handoff_diagnostics(env, phase_machine), + } + ) + ) + assert torch.all(success), _handoff_diagnostics(env, phase_machine) + assert not torch.any(dropped), _handoff_diagnostics(env, phase_machine) + # ManagerBasedRLEnv resets a terminal environment before returning + # from ``step``. ``success`` is therefore the authoritative + # pre-reset retained-lift result; inspecting phase afterwards would + # incorrectly observe the next episode's INITIAL state. + return + assert int(phase_machine.phase.item()) == int(HandoffPhase.RECEIVER_ONLY_HOLD), _handoff_diagnostics( + env, phase_machine + ) + raise AssertionError("native receiver never completed the measured retained-lift termination") + + +def _assert_native_handoff_audit(audit: _DirectStateWriteAudit) -> None: + """Require only the explicit and terminal-reset writes, plus drives.""" + + # The first window is the explicit seeded reset. The second is Isaac + # Lab's automatic reset after the measured success termination. Both must + # exactly match the documented reset whitelist; no transfer-step state + # write is permitted. + assert audit.reset_windows == [ + _EXPECTED_RESET_STATE_WRITE_SEQUENCE, + _EXPECTED_RESET_STATE_WRITE_SEQUENCE, + ] + assert audit.violations == [] + assert audit.usd_mutations == [] + + +@pytest.mark.isaacsim_ci +def test_one_env_native_grasp_generator_handoff_is_physically_qualified(): + """Qualify the full native transfer on CUDA PhysX without state mutation.""" + + with _task_env(num_envs=1) as env: + audit = _DirectStateWriteAudit(env) + audit.install() + env.reset(seed=SEED) + _assert_live_needle_topology(env) + _run_native_grasp_handoff(env, env) + _assert_live_needle_topology(env) + _assert_native_handoff_audit(audit) + + +@pytest.mark.isaacsim_ci +def test_one_env_native_grasp_generator_handoff_video(): + """Record the same full CUDA qualification trace when recording is requested.""" + + video_dir = _verification_video_dir() + if video_dir is None: + pytest.skip("set ISAACLAB_DVRK_NEEDLE_PASS_VIDEO_DIR to record the qualified handoff") + with _task_env( + num_envs=1, + video_dir=video_dir, + video_length=NATIVE_HANDOFF_TRACE_STEPS, + video_prefix="dvrk-needle-pass-native-handoff", + ) as runner: + env = runner.unwrapped + audit = _DirectStateWriteAudit(env) + audit.install() + runner.reset(seed=SEED) + _assert_live_needle_topology(env) + _run_native_grasp_handoff(runner, env) + _assert_live_needle_topology(env) + _assert_native_handoff_audit(audit) + + videos = sorted(video_dir.glob("dvrk-needle-pass-native-handoff-episode-0*.mp4")) + assert videos, f"RecordVideo did not write the qualified handoff video to {video_dir}" + + +@pytest.mark.isaacsim_ci +def test_native_receiver_candidates_complete_a_cuda_guarded_transfer(): + """Screen exact native rows through the guarded CUDA transfer and lift. + + The opt-in probe accepts exact rows from the hash-pinned generator output. + It never creates pose neighbours, alters the free needle, or bypasses the + donor release guard. Qualification requires the donor to be released, the + recipient to retain bilateral force, and the free needle to lift 15 mm. + """ + + candidate_indices, candidate_poses_cpu = _native_probe_candidate_poses() + with _task_env(num_envs=len(candidate_indices)) as env: + env.reset(seed=SEED) + initial_needle_z_w = env.scene["needle"].data.root_pos_w.torch[:, 2].clone() + for _ in range(NATIVE_DONOR_HOLD_SETTLE_STEPS): + _, _, terminated, truncated, _ = env.step(_held_start_action(env)) + assert not terminated.any() + assert not truncated.any() + + candidate_poses = candidate_poses_cpu.to(env.device) + receiver_pos_w, receiver_quat_xyzw = _native_receiver_candidate_targets(env, candidate_poses) + alive = torch.ones(env.num_envs, dtype=torch.bool, device=env.device) + for segment in range(4): + for step in range(NATIVE_RECEIVER_APPROACH_SEGMENT_STEPS): + _, _, terminated, truncated, _ = env.step( + _native_receiver_candidate_approach_action( + env, + receiver_pos_w, + receiver_quat_xyzw, + segment=segment, + fraction=(step + 1) / NATIVE_RECEIVER_APPROACH_SEGMENT_STEPS, + ) + ) + alive &= ~(terminated | truncated) + for _ in range(NATIVE_RECEIVER_CLOSE_SETTLE_STEPS): + _, _, terminated, truncated, _ = env.step( + _native_receiver_candidate_handoff_action( + env, + receiver_pos_w, + receiver_quat_xyzw, + donor_jaw=DONOR_GRASP_JAW_POS, + receiver_jaw=DVRK_PSM_JAW_CLOSED_POS, + ) + ) + alive &= ~(terminated | truncated) + + phase_machine = get_handoff_phase_machine(env, DVRK_HANDOFF_PHASE_CFG) + co_hold = phase_machine.phase == int(HandoffPhase.CO_HOLD) + donor_released_once = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + donor_released_continuously = torch.ones(env.num_envs, dtype=torch.bool, device=env.device) + recipient_grasped_at_release = torch.ones(env.num_envs, dtype=torch.bool, device=env.device) + receiver_retained_continuously = torch.ones(env.num_envs, dtype=torch.bool, device=env.device) + release_receiver_loads = torch.full((env.num_envs, 2), torch.nan, device=env.device) + release_receiver_normal_dot = torch.full((env.num_envs,), torch.nan, device=env.device) + for _ in range(NATIVE_DONOR_RELEASE_SETTLE_STEPS): + active = alive.clone() + _, _, terminated, truncated, _ = env.step( + _native_receiver_candidate_handoff_action( + env, + receiver_pos_w, + receiver_quat_xyzw, + donor_jaw=DVRK_PSM_JAW_OPEN_POS, + receiver_jaw=DVRK_PSM_JAW_CLOSED_POS, + ) + ) + valid_sample = active & ~(terminated | truncated) + loads_during_release, normals_during_release, _ = jaw_needle_contact_measurements(env) + donor_is_released = torch.all( + loads_during_release[:, :2] < DVRK_HANDOFF_PHASE_CFG.disengage_force_n, dim=-1 + ) + receiver_is_bilateral = phase_machine._bilateral_contact( + loads_during_release[:, 2:], + normals_during_release[:, 2:], + phase_machine._receiver_engaged, + ) + receiver_unit_normals = torch.nn.functional.normalize(normals_during_release[:, 2:], dim=-1, eps=1.0e-12) + receiver_normal_dot = torch.sum(receiver_unit_normals[:, 0] * receiver_unit_normals[:, 1], dim=-1) + first_release = valid_sample & donor_is_released & ~donor_released_once + release_receiver_loads[first_release] = loads_during_release[first_release, 2:] + release_receiver_normal_dot[first_release] = receiver_normal_dot[first_release] + recipient_grasped_at_release &= torch.where(first_release, receiver_is_bilateral, True) + donor_released_continuously &= torch.where(valid_sample & donor_released_once, donor_is_released, True) + receiver_retained_continuously &= torch.where( + valid_sample & (donor_released_once | first_release), receiver_is_bilateral, True + ) + donor_released_once |= valid_sample & donor_is_released + alive &= valid_sample + receiver = env.scene["right_psm"] + body_ids, body_names = receiver.find_bodies(DVRK_PSM_TOOL_TIP_BODY_NAME) + assert len(body_ids) == 1, body_names + post_release_relative_pos, post_release_relative_quat = math_utils.subtract_frame_transforms( + receiver.data.body_pos_w.torch[:, body_ids[0]], + receiver.data.body_quat_w.torch[:, body_ids[0]], + env.scene["needle"].data.root_pos_w.torch, + env.scene["needle"].data.root_quat_w.torch, + ) + post_release_velocity_w = torch.cat( + (env.scene["needle"].data.root_lin_vel_w.torch, env.scene["needle"].data.root_ang_vel_w.torch), dim=-1 + ).clone() + terminal_success = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + physical_success = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + retained_lift_counter = torch.zeros(env.num_envs, dtype=torch.long, device=env.device) + required_retained_lift_steps = phase_machine._required_steps(DVRK_HANDOFF_PHASE_CFG.retained_lift_dwell_s) + max_lift_delta_z_m = torch.full((env.num_envs,), -torch.inf, device=env.device) + for step in range(NATIVE_RECEIVER_LIFT_STEPS): + active = alive & ~terminal_success & ~physical_success + loads_before_lift, normals_before_lift, _ = jaw_needle_contact_measurements(env) + donor_is_released_before_lift = torch.all( + loads_before_lift[:, :2] < DVRK_HANDOFF_PHASE_CFG.disengage_force_n, dim=-1 + ) + receiver_is_bilateral_before_lift = phase_machine._bilateral_contact( + loads_before_lift[:, 2:], + normals_before_lift[:, 2:], + phase_machine._receiver_engaged, + ) + tracking = active & donor_released_once + donor_released_continuously &= torch.where(tracking, donor_is_released_before_lift, True) + receiver_retained_continuously &= torch.where(tracking, receiver_is_bilateral_before_lift, True) + lift_delta_z_m = env.scene["needle"].data.root_pos_w.torch[:, 2] - initial_needle_z_w + max_lift_delta_z_m = torch.where( + active, torch.maximum(max_lift_delta_z_m, lift_delta_z_m), max_lift_delta_z_m + ) + retained_lift_condition = ( + tracking + & donor_is_released_before_lift + & receiver_is_bilateral_before_lift + & (lift_delta_z_m >= DVRK_HANDOFF_PHASE_CFG.required_lift_delta_z_m) + ) + updated_retained_lift_counter = torch.where( + retained_lift_condition, retained_lift_counter + 1, torch.zeros_like(retained_lift_counter) + ) + retained_lift_counter = torch.where(active, updated_retained_lift_counter, retained_lift_counter) + physical_success |= ( + (retained_lift_counter >= required_retained_lift_steps) + & donor_released_continuously + & receiver_retained_continuously + ) + active &= ~physical_success + _, _, terminated, truncated, _ = env.step( + _native_receiver_candidate_handoff_action( + env, + receiver_pos_w, + receiver_quat_xyzw, + donor_jaw=DVRK_PSM_JAW_OPEN_POS, + receiver_jaw=DVRK_PSM_JAW_CLOSED_POS, + receiver_lift_offset_m=_native_receiver_lift_offset(step), + ) + ) + succeeded = env.termination_manager.get_term("success") + terminal_success |= active & terminated & succeeded + physical_success |= active & terminated & succeeded + alive &= ~(active & (terminated | truncated)) + loads_during_lift, normals_during_lift, _ = jaw_needle_contact_measurements(env) + donor_is_released = torch.all(loads_during_lift[:, :2] < DVRK_HANDOFF_PHASE_CFG.disengage_force_n, dim=-1) + receiver_is_bilateral = phase_machine._bilateral_contact( + loads_during_lift[:, 2:], + normals_during_lift[:, 2:], + phase_machine._receiver_engaged, + ) + valid_post_step = active & alive & ~terminal_success & ~physical_success + donor_released_continuously &= torch.where(valid_post_step & donor_released_once, donor_is_released, True) + receiver_retained_continuously &= torch.where( + valid_post_step & donor_released_once, receiver_is_bilateral, True + ) + + loads, _, raw_forces_w = jaw_needle_contact_measurements(env) + donor_released = torch.all(loads[:, :2] < DVRK_HANDOFF_PHASE_CFG.disengage_force_n, dim=-1) + lifted = env.scene["needle"].data.root_pos_w.torch[:, 2] - initial_needle_z_w >= 0.015 + qualified = ( + physical_success + & co_hold + & donor_released_once + & donor_released_continuously + & recipient_grasped_at_release + & receiver_retained_continuously + ) + actual_pos_w = receiver.data.body_pos_w.torch[:, body_ids[0]] + actual_quat_w = receiver.data.body_quat_w.torch[:, body_ids[0]] + jaw_body_ids, jaw_body_names = receiver.find_bodies(["psm_tool_gripper1_link", "psm_tool_gripper2_link"]) + assert len(jaw_body_ids) == 2, jaw_body_names + receiver_jaw_body_pos_w = receiver.data.body_pos_w.torch[:, jaw_body_ids] + orientation_dot = torch.abs(torch.sum(actual_quat_w * receiver_quat_xyzw, dim=-1)).clamp(max=1.0) + receiver_jaw_sensor_pos_w = torch.stack( + [env.scene.sensors[name].data.pos_w.torch[:, 0, :] for name in JAW_CONTACT_SENSOR_NAMES[2:]], dim=1 + ) + if env.num_envs == 1: + live_channel_w = _receiver_collision_channel_world(env, 0) + tool_channel_w = _pose_matrix( + actual_pos_w[0].detach().cpu().numpy(), actual_quat_w[0].detach().cpu().numpy() + ) @ _pose_matrix( + np.asarray(DVRK_JAW_CHANNEL_T_T_C_POS_M, dtype=np.float64), + np.asarray(DVRK_JAW_CHANNEL_T_T_C_ROT_XYZW, dtype=np.float64), + ) + expected_channel_w = _pose_matrix( + env.scene["needle"].data.root_pos_w.torch[0].detach().cpu().numpy(), + env.scene["needle"].data.root_quat_w.torch[0].detach().cpu().numpy(), + ) @ _pose_matrix( + candidate_poses[0, :3].detach().cpu().numpy(), candidate_poses[0, 3:].detach().cpu().numpy() + ) + print( + "NATIVE_RECEIVER_LIVE_CHANNELS=" + + repr( + { + "collision_midpoint_w": live_channel_w.round(9).tolist(), + "tool_calibrated_channel_w": tool_channel_w[:3, 3].round(9).tolist(), + "native_expected_channel_w": expected_channel_w[:3, 3].round(9).tolist(), + } + ) + ) + report = [ + { + "candidate": candidate_indices[index], + "alive": bool(alive[index].item()), + "co_hold": bool(co_hold[index].item()), + "success_termination": bool(terminal_success[index].item()), + "physical_success": bool(physical_success[index].item()), + "post_trace_donor_released": bool(donor_released[index].item()), + "donor_released_continuously": bool(donor_released_continuously[index].item()), + "recipient_grasped_at_release": bool(recipient_grasped_at_release[index].item()), + "receiver_retained_continuously": bool(receiver_retained_continuously[index].item()), + "release_receiver_loads_n": release_receiver_loads[index].detach().cpu().tolist(), + "release_receiver_normal_dot": float(release_receiver_normal_dot[index].item()), + "max_lift_delta_z_m": float(max_lift_delta_z_m[index].item()), + "retained_lift_steps": int(retained_lift_counter[index].item()), + "post_trace_lifted": bool(lifted[index].item()), + "post_trace_receiver_loads_n": loads[index, 2:].detach().cpu().tolist(), + "post_trace_receiver_sensor_forces_w_n": raw_forces_w[index, 2:].detach().cpu().tolist(), + "receiver_target_pos_w": receiver_pos_w[index].detach().cpu().tolist(), + "receiver_target_quat_xyzw": receiver_quat_xyzw[index].detach().cpu().tolist(), + "tool_position_error_m": float( + torch.linalg.vector_norm(actual_pos_w[index] - receiver_pos_w[index]).item() + ), + "tool_orientation_error_rad": float((2.0 * torch.acos(orientation_dot[index])).item()), + "receiver_jaws_rad": receiver.data.joint_pos.torch[index, -2:].detach().cpu().tolist(), + "post_release_relative_pos_m": post_release_relative_pos[index].detach().cpu().tolist(), + "post_release_relative_quat_xyzw": post_release_relative_quat[index].detach().cpu().tolist(), + "post_release_velocity_w": post_release_velocity_w[index].detach().cpu().tolist(), + "receiver_jaw_sensor_pos_w": receiver_jaw_sensor_pos_w[index].detach().cpu().tolist(), + "receiver_jaw_body_pos_w": receiver_jaw_body_pos_w[index].detach().cpu().tolist(), + "qualified": bool(qualified[index].item()), + } + for index in range(env.num_envs) + ] + print(f"NATIVE_RECEIVER_CUDA_PROBE={report!r}") + assert qualified.any(), report + + +@pytest.mark.isaacsim_ci +def test_one_env_runtime_contracts_and_100_random_actions(): + """Audit the renderer-free CUDA runtime path with bounded random actions.""" + + with _task_env(num_envs=1) as runner: + env = runner.unwrapped + audit = _DirectStateWriteAudit(env) + audit.install() + + first_observations, _ = runner.reset(seed=SEED) + first_snapshot = _reset_snapshot(env, first_observations) + second_observations, _ = runner.reset(seed=SEED) + second_snapshot = _reset_snapshot(env, second_observations) + _assert_same_tree(second_snapshot, first_snapshot) + + expected_reset_writes = { + (asset_name, method_name) + for layer, asset_name, method_name in _EXPECTED_RESET_STATE_WRITE_SEQUENCE + if layer == "asset" + } + expected_physx_reset_writes = { + (asset_name, method_name) + for layer, asset_name, method_name in _EXPECTED_RESET_STATE_WRITE_SEQUENCE + if layer == "physx" + } + assert audit.reset_windows == [ + _EXPECTED_RESET_STATE_WRITE_SEQUENCE, + _EXPECTED_RESET_STATE_WRITE_SEQUENCE, + ] + assert set(audit.asset_state_calls) == expected_reset_writes + assert all(audit.asset_state_calls.count(write) == 2 for write in expected_reset_writes) + assert set(audit.physx_state_calls) == expected_physx_reset_writes + assert all(audit.physx_state_calls.count(write) == 2 for write in expected_physx_reset_writes) + assert audit.violations == [] + + _assert_live_needle_topology(env) + _assert_live_needle_mass_and_material(env) + _assert_contact_sensor_matrices(env) + _assert_live_action_and_joint_order(env) + _assert_live_psm_jaw_contract(env) + _assert_static_suture_pad(env) + _assert_configured_tool_homes(env) + trace_steps = TRACE_STEPS + _run_finite_action_trace(runner, _bounded_random_actions(env, trace_steps, SEED + 1)) + expected_control_setters = { + (asset_name, method_name) + for asset_name in ("left_psm", "right_psm") + for method_name in _ARTICULATION_PHYSX_CONTROL_SETTERS + } + assert set(audit.physx_control_calls) == expected_control_setters + assert all(audit.physx_control_calls.count(call) >= trace_steps for call in expected_control_setters) + assert audit.violations == [] + assert audit.usd_mutations == [] + _assert_live_needle_topology(env) + + +@pytest.mark.isaacsim_ci +def test_32_env_100_random_actions_are_finite(): + """Exercise the batched absolute-world action ABI without asserting task success.""" + + with _task_env(num_envs=32) as env: + observations, _ = env.reset(seed=SEED) + _assert_finite_tree(observations) + _assert_contact_sensor_matrices(env) + _assert_live_action_and_joint_order(env) + _run_finite_action_trace(env, _bounded_random_actions(env, TRACE_STEPS, SEED + 32)) diff --git a/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_teleop_pipeline.py b/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_teleop_pipeline.py new file mode 100644 index 000000000000..24d84ada16f9 --- /dev/null +++ b/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_teleop_pipeline.py @@ -0,0 +1,142 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Focused integration tests for the dVRK needle-pass IsaacTeleop graph. + +The external dVRK nodes are supplied by NVIDIA/IsaacTeleop PR 769. Generic +task shards skip this module until that API is released; the focused PR lane +installs the immutable test pin before running this file. +""" + +import numpy as np +import pytest + +pytest.importorskip("isaacteleop.retargeters.DVRK") + +from isaaclab.app import AppLauncher + +app_launcher = AppLauncher(headless=True, enable_cameras=False) +simulation_app = app_launcher.app + +from isaacteleop.retargeters import DVRKPSMClutchRetargeter, DVRKPSMGripperRetargeter # noqa: E402 +from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource # noqa: E402 + +from isaaclab_tasks.contrib.needle_pass.config.dvrk.ik_abs_env_cfg import ( # noqa: E402 + _TELEOP_AVAILABLE, + DONOR_GRASP_CLOSEDNESS, + DONOR_GRASP_JAW_POS, + DVRK_PSM_JAW_CLOSED_POS, + LEFT_TOOL_HOME_POS_W, + LEFT_TOOL_HOME_ROT_XYZW, + LEFT_WORKSPACE_LOWER, + LEFT_WORKSPACE_UPPER, + RIGHT_TOOL_HOME_POS_W, + RIGHT_TOOL_HOME_ROT_XYZW, + RIGHT_WORKSPACE_LOWER, + RIGHT_WORKSPACE_UPPER, + _build_dvrk_needle_pass_pipeline, +) + +_EXPECTED_ACTION_ORDER = [ + "left_pos_x", + "left_pos_y", + "left_pos_z", + "left_quat_x", + "left_quat_y", + "left_quat_z", + "left_quat_w", + "left_jaw_1", + "left_jaw_2", + "right_pos_x", + "right_pos_y", + "right_pos_z", + "right_quat_x", + "right_quat_y", + "right_quat_z", + "right_quat_w", + "right_jaw_1", + "right_jaw_2", +] + + +def _reorderer_subgraph(): + pipeline = _build_dvrk_needle_pass_pipeline() + return pipeline, pipeline.output_mapping["action"].module + + +def test_dvrk_pipeline_action_is_18d(): + """The pipeline emits the task's ``7 + 2 + 7 + 2`` action ABI.""" + pipeline = _build_dvrk_needle_pass_pipeline() + + assert _TELEOP_AVAILABLE + assert pipeline.output_types()["action"].types[0].shape == (18,) + + +def test_dvrk_pipeline_output_order_matches_action_terms(): + """The flattened values resolve in left pose/jaws then right pose/jaws order.""" + _, subgraph = _reorderer_subgraph() + try: + output_order = subgraph._target_module._output_order + except AttributeError: + pytest.skip("IsaacTeleop does not expose graph wiring for order inspection") + + assert output_order == _EXPECTED_ACTION_ORDER + + +def test_dvrk_pipeline_routes_world_transformed_controller_sides(): + """Each PSM consumes its own controller after the shared world transform.""" + _, reorderer_subgraph = _reorderer_subgraph() + try: + connections = reorderer_subgraph._input_connections + left_pose = connections["left_pose"].module + left_jaws = connections["left_jaws"].module + right_pose = connections["right_pose"].module + right_jaws = connections["right_jaws"].module + except AttributeError: + pytest.skip("IsaacTeleop does not expose graph wiring for route inspection") + + assert isinstance(left_pose._target_module, DVRKPSMClutchRetargeter) + assert isinstance(left_jaws._target_module, DVRKPSMGripperRetargeter) + assert isinstance(right_pose._target_module, DVRKPSMClutchRetargeter) + assert isinstance(right_jaws._target_module, DVRKPSMGripperRetargeter) + assert list(left_pose._target_module.input_spec()) == [ControllersSource.LEFT] + assert list(left_jaws._target_module.input_spec()) == [ControllersSource.LEFT] + assert list(right_pose._target_module.input_spec()) == [ControllersSource.RIGHT] + assert list(right_jaws._target_module.input_spec()) == [ControllersSource.RIGHT] + + left_transform = left_pose._input_connections[ControllersSource.LEFT].module + right_transform = right_pose._input_connections[ControllersSource.RIGHT].module + assert left_jaws._input_connections[ControllersSource.LEFT].module is left_transform + assert right_jaws._input_connections[ControllersSource.RIGHT].module is right_transform + assert left_transform is right_transform + assert left_transform._input_connections["transform"].module.name == "world_T_anchor" + + +def test_dvrk_pipeline_preserves_side_homes_workspaces_and_reset_jaws(): + """Each side retains its calibrated world limits and intended reset jaw state.""" + _, reorderer_subgraph = _reorderer_subgraph() + try: + connections = reorderer_subgraph._input_connections + left_clutch_cfg = connections["left_pose"].module._target_module._clutch_state._config + right_clutch_cfg = connections["right_pose"].module._target_module._clutch_state._config + left_gripper_cfg = connections["left_jaws"].module._target_module._jaw_intent._config + right_gripper_cfg = connections["right_jaws"].module._target_module._jaw_intent._config + except AttributeError: + pytest.skip("IsaacTeleop does not expose graph node configs for contract inspection") + + np.testing.assert_allclose(left_clutch_cfg.home_position, LEFT_TOOL_HOME_POS_W) + np.testing.assert_allclose(left_clutch_cfg.home_orientation, LEFT_TOOL_HOME_ROT_XYZW) + assert left_clutch_cfg.workspace_lower == LEFT_WORKSPACE_LOWER + assert left_clutch_cfg.workspace_upper == LEFT_WORKSPACE_UPPER + np.testing.assert_allclose(right_clutch_cfg.home_position, RIGHT_TOOL_HOME_POS_W) + np.testing.assert_allclose(right_clutch_cfg.home_orientation, RIGHT_TOOL_HOME_ROT_XYZW) + assert right_clutch_cfg.workspace_lower == RIGHT_WORKSPACE_LOWER + assert right_clutch_cfg.workspace_upper == RIGHT_WORKSPACE_UPPER + + assert DONOR_GRASP_CLOSEDNESS == 1.0 + assert DONOR_GRASP_JAW_POS == DVRK_PSM_JAW_CLOSED_POS + assert left_gripper_cfg.initial_closedness == DONOR_GRASP_CLOSEDNESS + assert left_gripper_cfg.jaw_closed == DONOR_GRASP_JAW_POS + assert right_gripper_cfg.initial_closedness == 0.0 From 4452c02b6ff7db7a1df3cdabb52e70cb61e7e9ca Mon Sep 17 00:00:00 2001 From: Chris von Csefalvay Date: Thu, 16 Jul 2026 10:27:49 -0600 Subject: [PATCH 3/6] test(teleop): add pinned dVRK integration coverage --- .github/actions/run-package-tests/action.yml | 5 + .github/actions/run-tests/action.yml | 117 ++++++++++++------ .github/workflows/build.yaml | 24 ++++ .../install_isaacteleop_pr769_for_tests.sh | 109 ++++++++++++++++ 4 files changed, 218 insertions(+), 37 deletions(-) create mode 100755 scripts/tools/install_isaacteleop_pr769_for_tests.sh diff --git a/.github/actions/run-package-tests/action.yml b/.github/actions/run-package-tests/action.yml index 9ee6dcddd9b8..0c2b8e33a884 100644 --- a/.github/actions/run-package-tests/action.yml +++ b/.github/actions/run-package-tests/action.yml @@ -83,6 +83,10 @@ inputs: description: 'Space-separated pip packages to install inside the Docker container before pytest starts' default: '' required: false + install-isaacteleop-pr769: + description: 'Build and install the immutable NVIDIA/IsaacTeleop PR 769 test pin before pytest' + default: 'false' + required: false wheelhouse-resource: description: 'Optional NGC resource containing wheelhouse/ and manifest.json for offline pip installs' default: '' @@ -302,6 +306,7 @@ runs: test-node-ids-key: ${{ inputs.test-node-ids-key }} volume-mount-source: ${{ github.workspace }} extra-pip-packages: ${{ inputs.extra-pip-packages }} + install-isaacteleop-pr769: ${{ inputs.install-isaacteleop-pr769 }} wheelhouse-host-dir: ${{ steps.extract-wheelhouse.outputs.wheelhouse_host_dir }} wheelhouse-packages: ${{ inputs.wheelhouse-packages }} omni-github-test-type: ${{ inputs.omni-github-test-type }} diff --git a/.github/actions/run-tests/action.yml b/.github/actions/run-tests/action.yml index d3829cc8300e..bc703d1dabaf 100644 --- a/.github/actions/run-tests/action.yml +++ b/.github/actions/run-tests/action.yml @@ -85,6 +85,10 @@ inputs: description: 'Space-separated pip packages to install inside the Docker container before pytest starts' default: '' required: false + install-isaacteleop-pr769: + description: 'Build and install the immutable NVIDIA/IsaacTeleop PR 769 test pin before pytest' + default: 'false' + required: false wheelhouse-host-dir: description: 'Host directory containing wheelhouse/ and manifest.json for offline pip installs' default: '' @@ -137,10 +141,13 @@ runs: local wheelhouse_packages="${19}" local test_k_expr="${20}" local ci_marker="${21}" + local install_isaacteleop_pr769="${22}" local logs_pid="" local wait_pid="" local docker_wait_file="/tmp/.docker_exit_${container_name}" local docker_runtime_dir="" + local test_run_uid="1000" + local test_run_gid="1000" # Kill the container immediately if the runner is cancelled. # The GitHub Actions runner can deliver HUP, INT, or TERM on cancellation @@ -168,6 +175,12 @@ runs: if [ -n "$wheelhouse_packages" ]; then echo "With wheelhouse packages: $wheelhouse_packages" fi + if [ "$install_isaacteleop_pr769" = "true" ]; then + echo "With NVIDIA/IsaacTeleop PR 769 test pin" + elif [ "$install_isaacteleop_pr769" != "false" ]; then + echo "install-isaacteleop-pr769 must be 'true' or 'false'" + return 1 + fi if [ -n "$filter_pattern" ]; then echo "With filter pattern: $filter_pattern" fi @@ -298,6 +311,8 @@ runs: host_uid="$(id -u)" host_gid="$(id -g)" host_user="$(id -un)" + test_run_uid="$host_uid" + test_run_gid="$host_gid" # Kit writes generated cache, config, data, and log files outside # the Isaac Lab source tree. Provide writable runtime storage for # host-uid test runs, mirroring the compose/singularity mounts. @@ -338,6 +353,17 @@ runs: echo "🔵 Running volume-mounted container as host uid:gid ${host_uid}:${host_gid} (${host_user})" fi + if [ "$install_isaacteleop_pr769" = "true" ]; then + # The pinned source build may need to install libx11-dev. Start as + # root for that bootstrap, then drop to the normal test uid:gid + # before creating any reports or cache files in the bind mount. + docker_user_args="--user 0:0" + docker_env_vars="$docker_env_vars \ + -e TEST_INSTALL_ISAACTELEOP_PR769=true \ + -e TEST_RUN_UID=${test_run_uid} \ + -e TEST_RUN_GID=${test_run_gid}" + fi + if [ -n "$wheelhouse_host_dir" ]; then if [ -z "$wheelhouse_packages" ]; then echo "::error::wheelhouse-host-dir was provided but wheelhouse-packages is empty" @@ -387,46 +413,63 @@ runs: -c " set -e cd /workspace/isaaclab - mkdir -p tests - rm _isaac_sim || true - ln -s /isaac-sim _isaac_sim - # Allow OmniHub to start in the test container. Some base images - # set this detect-only flag, which makes cold asset downloads - # fall back to slow repeated retries. - unset HUB__ARGS__DETECT_ONLY - ./isaaclab.sh -p -m pip install pytest pytest-mock junitparser flatdict flaky \"coverage>=7.6.1\" - if [ -n \"\${TEST_WHEELHOUSE_PACKAGES:-}\" ]; then - if [ ! -d \"\${TEST_WHEELHOUSE_PATH:-}\" ]; then - echo \"Wheelhouse path is missing: \${TEST_WHEELHOUSE_PATH:-}\" - exit 1 + if [ \"\${TEST_INSTALL_ISAACTELEOP_PR769:-}\" = \"true\" ]; then + # Keep root's source-build caches out of the runtime home that + # is owned by the non-root test user selected below. + HOME=/root \ + XDG_CACHE_HOME=/root/.cache \ + XDG_DATA_HOME=/root/.local/share \ + bash scripts/tools/install_isaacteleop_pr769_for_tests.sh + fi + + run_test_body() { + mkdir -p tests + rm _isaac_sim || true + ln -s /isaac-sim _isaac_sim + # Allow OmniHub to start in the test container. Some base images + # set this detect-only flag, which makes cold asset downloads + # fall back to slow repeated retries. + unset HUB__ARGS__DETECT_ONLY + ./isaaclab.sh -p -m pip install pytest pytest-mock junitparser flatdict flaky \"coverage>=7.6.1\" + if [ -n \"\${TEST_WHEELHOUSE_PACKAGES:-}\" ]; then + if [ ! -d \"\${TEST_WHEELHOUSE_PATH:-}\" ]; then + echo \"Wheelhouse path is missing: \${TEST_WHEELHOUSE_PATH:-}\" + exit 1 + fi + if [ ! -f \"\${TEST_WHEELHOUSE_MANIFEST:-}\" ]; then + echo \"Wheelhouse manifest is missing: \${TEST_WHEELHOUSE_MANIFEST:-}\" + exit 1 + fi + + echo \"Installing wheelhouse packages offline: \${TEST_WHEELHOUSE_PACKAGES}\" + ./isaaclab.sh -p -m pip uninstall -y \${TEST_WHEELHOUSE_PACKAGES} || true + PIP_NO_INDEX=1 ./isaaclab.sh -p -m pip install --no-index --find-links=\"\${TEST_WHEELHOUSE_PATH}\" --upgrade --force-reinstall \${TEST_WHEELHOUSE_PACKAGES} + + case \" \${TEST_WHEELHOUSE_PACKAGES} \" in + *\" ovphysx \"*) + ./isaaclab.sh -p -c \"import importlib.metadata,json,os,pathlib; from packaging.version import Version; manifest=json.loads(pathlib.Path(os.environ['TEST_WHEELHOUSE_MANIFEST']).read_text(encoding='utf-8')); expected=manifest.get('ovphysx_version'); actual=importlib.metadata.version('ovphysx'); print(f'Resolved ovphysx package version: {actual}'); print(f'Wheelhouse manifest ovphysx version: {expected}'); import ovphysx; runtime=getattr(ovphysx, '__version__', actual); print(f'Imported ovphysx runtime version: {runtime}'); raise SystemExit(0 if Version(actual) == Version(expected) and Version(runtime) == Version(expected) else f'ovphysx version mismatch: installed {actual}, import {runtime}, manifest {expected}')\" + ;; + esac fi - if [ ! -f \"\${TEST_WHEELHOUSE_MANIFEST:-}\" ]; then - echo \"Wheelhouse manifest is missing: \${TEST_WHEELHOUSE_MANIFEST:-}\" - exit 1 + if [ -n \"\${TEST_EXTRA_PIP_PACKAGES:-}\" ]; then + echo \"Installing extra pip packages: \${TEST_EXTRA_PIP_PACKAGES}\" + ./isaaclab.sh -p -m pip install \${TEST_EXTRA_PIP_PACKAGES} + case \" \${TEST_EXTRA_PIP_PACKAGES} \" in + *\" leapp\"*) + echo \"Resolved LEAPP package:\" + ./isaaclab.sh -p -m pip show leapp || true + ;; + esac fi + echo 'Starting pytest with path: $test_path' + ./isaaclab.sh -p -m pytest --ignore=tools/conftest.py $test_path $pytest_options -v --junitxml=tests/$result_file + } - echo \"Installing wheelhouse packages offline: \${TEST_WHEELHOUSE_PACKAGES}\" - ./isaaclab.sh -p -m pip uninstall -y \${TEST_WHEELHOUSE_PACKAGES} || true - PIP_NO_INDEX=1 ./isaaclab.sh -p -m pip install --no-index --find-links=\"\${TEST_WHEELHOUSE_PATH}\" --upgrade --force-reinstall \${TEST_WHEELHOUSE_PACKAGES} - - case \" \${TEST_WHEELHOUSE_PACKAGES} \" in - *\" ovphysx \"*) - ./isaaclab.sh -p -c \"import importlib.metadata,json,os,pathlib; from packaging.version import Version; manifest=json.loads(pathlib.Path(os.environ['TEST_WHEELHOUSE_MANIFEST']).read_text(encoding='utf-8')); expected=manifest.get('ovphysx_version'); actual=importlib.metadata.version('ovphysx'); print(f'Resolved ovphysx package version: {actual}'); print(f'Wheelhouse manifest ovphysx version: {expected}'); import ovphysx; runtime=getattr(ovphysx, '__version__', actual); print(f'Imported ovphysx runtime version: {runtime}'); raise SystemExit(0 if Version(actual) == Version(expected) and Version(runtime) == Version(expected) else f'ovphysx version mismatch: installed {actual}, import {runtime}, manifest {expected}')\" - ;; - esac - fi - if [ -n \"\${TEST_EXTRA_PIP_PACKAGES:-}\" ]; then - echo \"Installing extra pip packages: \${TEST_EXTRA_PIP_PACKAGES}\" - ./isaaclab.sh -p -m pip install \${TEST_EXTRA_PIP_PACKAGES} - case \" \${TEST_EXTRA_PIP_PACKAGES} \" in - *\" leapp\"*) - echo \"Resolved LEAPP package:\" - ./isaaclab.sh -p -m pip show leapp || true - ;; - esac + if [ \"\${TEST_INSTALL_ISAACTELEOP_PR769:-}\" = \"true\" ]; then + export -f run_test_body + exec setpriv --reuid=\"\${TEST_RUN_UID}\" --regid=\"\${TEST_RUN_GID}\" --clear-groups bash -e -c run_test_body fi - echo 'Starting pytest with path: $test_path' - ./isaaclab.sh -p -m pytest --ignore=tools/conftest.py $test_path $pytest_options -v --junitxml=tests/$result_file + run_test_body " # Stream container logs in background. @@ -523,7 +566,7 @@ runs: } # Call the function with provided parameters - run_tests "${{ inputs.test-path }}" "${{ inputs.result-file }}" "${{ inputs.container-name }}" "${{ inputs.image-tag }}" "${{ inputs.reports-dir }}" "$PYTEST_OPTIONS" "${{ inputs.filter-pattern }}" "${{ inputs.exclude-pattern }}" "${{ inputs.curobo-only }}" "${{ inputs.include-files }}" "${{ inputs.quarantined-only }}" "${{ inputs.shard-index }}" "${{ inputs.shard-count }}" "${{ inputs.volume-mount-source }}" "${{ inputs.extra-pip-packages }}" "${{ inputs.test-node-ids-file }}" "${{ inputs.test-node-ids-key }}" "${{ inputs.wheelhouse-host-dir }}" "${{ inputs.wheelhouse-packages }}" "$TEST_K_EXPR_INPUT" "$CI_MARKER_INPUT" + run_tests "${{ inputs.test-path }}" "${{ inputs.result-file }}" "${{ inputs.container-name }}" "${{ inputs.image-tag }}" "${{ inputs.reports-dir }}" "$PYTEST_OPTIONS" "${{ inputs.filter-pattern }}" "${{ inputs.exclude-pattern }}" "${{ inputs.curobo-only }}" "${{ inputs.include-files }}" "${{ inputs.quarantined-only }}" "${{ inputs.shard-index }}" "${{ inputs.shard-count }}" "${{ inputs.volume-mount-source }}" "${{ inputs.extra-pip-packages }}" "${{ inputs.test-node-ids-file }}" "${{ inputs.test-node-ids-key }}" "${{ inputs.wheelhouse-host-dir }}" "${{ inputs.wheelhouse-packages }}" "$TEST_K_EXPR_INPUT" "$CI_MARKER_INPUT" "${{ inputs.install-isaacteleop-pr769 }}" - name: Kill container on cancellation if: cancelled() diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index ff89bcd039df..e94aa34678ed 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -412,6 +412,30 @@ jobs: shard-count: "3" container-name: isaac-lab-tasks-3-test + test-dvrk-needle-pass-teleop: + name: dVRK needle-pass teleop + runs-on: [self-hosted, gpu] + timeout-minutes: 180 + continue-on-error: true + needs: [build, config] + if: >- + github.event_name != 'push' && + needs.build.result == 'success' + steps: + - uses: actions/checkout@v6 + with: + fetch-depth: 1 + lfs: true + - uses: ./.github/actions/run-package-tests + with: + image-tag: ${{ needs.config.outputs.ci_image_tag }} + isaacsim-base-image: ${{ needs.config.outputs.isaacsim_image_name }} + isaacsim-version: ${{ needs.config.outputs.isaacsim_image_tag }} + filter-pattern: "isaaclab_tasks" + include-files: "test_dvrk_needle_pass_teleop_pipeline.py" + install-isaacteleop-pr769: "true" + container-name: isaac-lab-dvrk-needle-pass-teleop-test + test-isaaclab-core: name: isaaclab (core) [1/3] runs-on: [self-hosted, gpu] diff --git a/scripts/tools/install_isaacteleop_pr769_for_tests.sh b/scripts/tools/install_isaacteleop_pr769_for_tests.sh new file mode 100755 index 000000000000..ee39c4c6897f --- /dev/null +++ b/scripts/tools/install_isaacteleop_pr769_for_tests.sh @@ -0,0 +1,109 @@ +#!/usr/bin/env bash +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Test-only pin for the public dVRK retargeter API in NVIDIA/IsaacTeleop PR 769. +# Remove this source build once the first IsaacTeleop release containing that +# API satisfies IsaacLab's normal version constraint. +set -euo pipefail + +readonly ISAAC_TELEOP_REPOSITORY="https://github.com/NVIDIA/IsaacTeleop.git" +readonly ISAAC_TELEOP_PR769_HEAD_SHA="ca175df7afc8198cbba0592cd1b447b11a4f3165" +readonly UV_VERSION="0.11.29" +if [[ -x /isaac-sim/python.sh ]]; then + readonly ISAACLAB_PYTHON="/isaac-sim/python.sh" +else + readonly ISAACLAB_PYTHON="${ISAACLAB_PATH:?ISAACLAB_PATH must be set}/_isaac_sim/python.sh" +fi +readonly ISAAC_TELEOP_PYTHON_VERSION="$( + "${ISAACLAB_PYTHON}" -c 'import sys; print(f"{sys.version_info.major}.{sys.version_info.minor}")' +)" +readonly ISAACLAB_PYTHON_SCRIPTS="$("${ISAACLAB_PYTHON}" -c 'import sysconfig; print(sysconfig.get_path("scripts"))')" +readonly BUILD_ROOT="${ISAACLAB_TELEOP_TEST_CACHE:-/tmp/isaacteleop-pr769-${ISAAC_TELEOP_PR769_HEAD_SHA}}" +readonly SOURCE_DIR="${BUILD_ROOT}/source" +readonly CMAKE_BUILD_DIR="${BUILD_ROOT}/build-python-${ISAAC_TELEOP_PYTHON_VERSION}" +readonly SOURCE_REVISION_FILE="${BUILD_ROOT}/source-revision" + +# The Isaac Sim launcher owns a Python installation whose console-script +# directory is not always on ``PATH`` in a test container. CMake finds the +# same ``uv`` executable that the launcher installs only after this export. +export PATH="${ISAACLAB_PYTHON_SCRIPTS}:${PATH}" + +if ! command -v git >/dev/null || ! dpkg-query --show --showformat='${db:Status-Status}' libx11-dev 2>/dev/null | grep -qx installed; then + # IsaacTeleop's CMake dependencies are fetched through Git and its static + # OpenXR loader selects the Xlib backend. The minimal Isaac Sim runtime + # image used by the test action includes neither build prerequisite. + apt_get=(apt-get) + if (( EUID != 0 )); then + if ! command -v sudo >/dev/null; then + echo "Installing IsaacTeleop build prerequisites requires root or sudo." >&2 + exit 1 + fi + apt_get=(sudo apt-get) + fi + "${apt_get[@]}" update + DEBIAN_FRONTEND=noninteractive "${apt_get[@]}" install --yes --no-install-recommends git libx11-dev +fi + +installed_uv_version="$( + "${ISAACLAB_PYTHON}" -c 'import importlib.metadata; print(importlib.metadata.version("uv"))' 2>/dev/null || true +)" +if [[ "${installed_uv_version}" != "${UV_VERSION}" ]]; then + "${ISAACLAB_PYTHON}" -m pip install "uv==${UV_VERSION}" +fi +test "$("${ISAACLAB_PYTHON}" -c 'import importlib.metadata; print(importlib.metadata.version("uv"))')" = "${UV_VERSION}" + +source_cache_valid=false +if [[ -f "${SOURCE_REVISION_FILE}" ]] \ + && [[ "$(<"${SOURCE_REVISION_FILE}")" = "${ISAAC_TELEOP_PR769_HEAD_SHA}" ]] \ + && [[ -d "${SOURCE_DIR}/.git" ]] \ + && [[ "$(git -C "${SOURCE_DIR}" rev-parse HEAD 2>/dev/null || true)" = "${ISAAC_TELEOP_PR769_HEAD_SHA}" ]] \ + && [[ -z "$(git -C "${SOURCE_DIR}" status --porcelain --untracked-files=all 2>/dev/null || true)" ]]; then + source_cache_valid=true +fi + +if [[ "${source_cache_valid}" != "true" ]]; then + rm -rf "${SOURCE_DIR}" "${CMAKE_BUILD_DIR}" + mkdir -p "${BUILD_ROOT}" + git init --quiet "${SOURCE_DIR}" + git -C "${SOURCE_DIR}" remote add origin "${ISAAC_TELEOP_REPOSITORY}" + git -C "${SOURCE_DIR}" fetch --depth 1 origin "${ISAAC_TELEOP_PR769_HEAD_SHA}" + git -C "${SOURCE_DIR}" checkout --quiet --detach FETCH_HEAD + test "$(git -C "${SOURCE_DIR}" rev-parse HEAD)" = "${ISAAC_TELEOP_PR769_HEAD_SHA}" + test -z "$(git -C "${SOURCE_DIR}" status --porcelain --untracked-files=all)" + printf '%s\n' "${ISAAC_TELEOP_PR769_HEAD_SHA}" > "${SOURCE_REVISION_FILE}" +fi + +test -f "${SOURCE_DIR}/CMakeLists.txt" +test "$(<"${SOURCE_REVISION_FILE}")" = "${ISAAC_TELEOP_PR769_HEAD_SHA}" +test "$(git -C "${SOURCE_DIR}" rev-parse HEAD)" = "${ISAAC_TELEOP_PR769_HEAD_SHA}" +test -z "$(git -C "${SOURCE_DIR}" status --porcelain --untracked-files=all)" + +cmake -S "${SOURCE_DIR}" -B "${CMAKE_BUILD_DIR}" \ + -DISAAC_TELEOP_PYTHON_VERSION="${ISAAC_TELEOP_PYTHON_VERSION}" \ + -DBUILD_EXAMPLES=OFF \ + -DBUILD_PLUGINS=OFF \ + -DBUILD_TESTING=OFF \ + -DBUILD_VIZ=OFF \ + -DENABLE_CLANG_FORMAT_CHECK=OFF \ + -DCMAKE_BUILD_TYPE=Release +cmake --build "${CMAKE_BUILD_DIR}" --target python_wheel --parallel + +wheel_path="$(find "${CMAKE_BUILD_DIR}/wheels" -maxdepth 1 -name 'isaacteleop-*.whl' -print -quit)" +test -n "${wheel_path}" +"${ISAACLAB_PYTHON}" -m pip install --force-reinstall --no-deps "${wheel_path}" +"${ISAACLAB_PYTHON}" - <<'PY' +from isaacteleop.retargeters import ( + DVRKPSMClutchConfig, + DVRKPSMClutchRetargeter, + DVRKPSMGripperConfig, + DVRKPSMGripperRetargeter, +) + +assert DVRKPSMClutchConfig is not None +assert DVRKPSMGripperConfig is not None +assert DVRKPSMClutchRetargeter.OUTPUT_POSE == "ee_pose" +assert DVRKPSMGripperRetargeter.OUTPUT_JAW_TARGETS == "jaw_targets" +PY From d507791870d3892835d444fe1d66c0fbc86abf43 Mon Sep 17 00:00:00 2001 From: Chris von Csefalvay Date: Thu, 16 Jul 2026 10:28:01 -0600 Subject: [PATCH 4/6] docs(teleop): document dVRK needle-pass workflow --- docs/source/features/isaac_teleop.rst | 25 ++++++++ docs/source/how-to/cloudxr_teleoperation.rst | 64 +++++++++++++++++++ docs/source/overview/environments.rst | 5 ++ .../imitation-learning/teleop_imitation.rst | 57 +++++++++++++++++ 4 files changed, 151 insertions(+) diff --git a/docs/source/features/isaac_teleop.rst b/docs/source/features/isaac_teleop.rst index 73f56952c6ad..7e284e49390f 100644 --- a/docs/source/features/isaac_teleop.rst +++ b/docs/source/features/isaac_teleop.rst @@ -75,6 +75,11 @@ starting point, then see the detailed pipeline examples below. - ``Se3AbsRetargeter`` + ``GripperRetargeter`` - 8 - ``stack_ik_abs_env_cfg.py`` + * - Bimanual surgical manipulation (dVRK) + - Motion controllers + - Bimanual ``DVRKPSMClutchRetargeter`` + ``DVRKPSMGripperRetargeter`` + - 18 + - ``ik_abs_env_cfg.py`` * - Bimanual dex + locomotion (e.g. G1 TriHand) - Motion controllers - Bimanual ``Se3AbsRetargeter`` + ``TriHandMotionControllerRetargeter`` + ``LocomotionRootCmdRetargeter`` @@ -261,6 +266,19 @@ retargeters not listed here -- refer to the Outputs a single float (-1.0 closed, 1.0 open). Uses controller trigger (priority) or thumb-index pinch distance from hand tracking. +.. dropdown:: DVRKPSMClutchRetargeter / DVRKPSMGripperRetargeter + + Provides the paired motion-controller mapping for a da Vinci Research Kit (dVRK) Patient + Side Manipulator (PSM). ``DVRKPSMClutchRetargeter`` emits a workspace-bounded 7D absolute + tool-tip pose. The controller squeeze is a deadman clutch: the first valid squeezed frame + captures the controller origin, and releasing squeeze holds the last target before the next + engagement captures a fresh origin. + + ``DVRKPSMGripperRetargeter`` maps relative analogue-trigger motion to the two ordered PSM jaw + targets. Closing intent is applied immediately, while opening intent must cross a deadband + and persist for a configured duration. Tracking loss, an inactive session, or a released + squeeze holds the last pose and jaw targets for that PSM. + .. dropdown:: DexHandRetargeter / DexBiManualRetargeter Retargets full hand tracking (26 joints) to robot-specific hand joint angles using the @@ -305,6 +323,8 @@ The built-in Isaac Lab environments use these retargeters as follows: - ``Se3AbsRetargeter``, ``TriHandMotionControllerRetargeter``, ``TensorReorderer`` * - G1 loco-manipulation - ``Se3AbsRetargeter``, ``TriHandMotionControllerRetargeter``, ``LocomotionRootCmdRetargeter``, ``TensorReorderer`` + * - dVRK PSM needle pass + - ``DVRKPSMClutchRetargeter``, ``DVRKPSMGripperRetargeter``, ``TensorReorderer`` .. _isaac-teleop-env-control-reference: @@ -334,6 +354,11 @@ These environments use the Isaac Teleop XR pipeline with motion controllers or h - Right - **Arm:** right controller grip pose drives end-effector. **Gripper:** right trigger. + * - ``IsaacContrib-NeedlePass-dVRK-IK-Abs`` + - Controllers + - Both + - **Arms:** left/right controller grip pose drives the corresponding PSM while squeeze is held. + **Jaws:** relative left/right trigger motion commands the corresponding paired jaws. * - ``IsaacContrib-PickPlace-GR1T2-Abs`` - Hand tracking - Both diff --git a/docs/source/how-to/cloudxr_teleoperation.rst b/docs/source/how-to/cloudxr_teleoperation.rst index 38f1145284e1..24837d430b90 100644 --- a/docs/source/how-to/cloudxr_teleoperation.rst +++ b/docs/source/how-to/cloudxr_teleoperation.rst @@ -385,6 +385,70 @@ choose the tab that matches your hardware. #. Click **Disconnect** when finished. +.. _teleoperate-dvrk-needle-pass: + +Teleoperate the dVRK needle-pass task +------------------------------------- + +The ``IsaacContrib-NeedlePass-dVRK-IK-Abs`` environment uses paired motion controllers to +operate two da Vinci Research Kit (dVRK) Patient Side Manipulators. It references revisioned +public assets from the Isaac for Healthcare ``0.6.0`` catalogue. Verify the downloaded bytes +against the pinned SHA-256 digests before the first run: + +.. code-block:: bash + + ./isaaclab.sh -p scripts/tools/preflight_dvrk_needle_pass_assets.py + +.. note:: + + Until `NVIDIA/IsaacTeleop PR #769 `__ + is included in a release satisfying Isaac Lab's normal version constraint, the dVRK + retargeters are a temporary source-pinned prerequisite. From the Isaac Lab repository root, + install that immutable revision for local validation with: + + .. code-block:: bash + + ISAACLAB_PATH=$PWD bash scripts/tools/install_isaacteleop_pr769_for_tests.sh + + The helper installs ``git`` and ``libx11-dev`` through ``sudo`` when they are missing, pins + its ``uv`` build tool in Isaac Sim's Python environment, builds the exact source revision + against that Python version, and force-reinstalls the resulting ``isaacteleop`` wheel without + changing its runtime dependencies. + + Use the normal Isaac Lab installation again once a released ``isaacteleop`` package contains + the dVRK retargeters. + +Launch the task through its unified Isaac Teleop pipeline. Do not pass ``--teleop_device``; +that option selects the legacy native-device path. + +.. code-block:: bash + + ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py \ + --task IsaacContrib-NeedlePass-dVRK-IK-Abs \ + --device cuda:0 \ + --visualizer kit \ + --xr + +The controller mapping is: + +* the left controller commands the left PSM, and the right controller commands the right PSM; +* squeeze acts as the deadman clutch for the corresponding PSM; +* the controller grip pose commands the tool-tip pose while squeeze is held; and +* relative index-trigger motion commands the two corresponding jaw targets. + +The first valid squeezed frame captures a controller origin without moving the tool. Releasing +squeeze or losing valid grip tracking holds that PSM's last pose and jaw targets, while the other +side continues independently. The next valid squeeze re-clutches at the held target. ``RESET`` +returns the donor to its needle-holding state and opens the receiver jaws. + +The resulting action has 18 values in this order: + +.. code-block:: text + + [left position xyz, left quaternion xyzw, left jaw_1, left jaw_2, + right position xyz, right quaternion xyzw, right jaw_1, right jaw_2] + + .. _manus-vive-handtracking: Manus Gloves diff --git a/docs/source/overview/environments.rst b/docs/source/overview/environments.rst index b82b1bc83e06..7de5b3c4a340 100644 --- a/docs/source/overview/environments.rst +++ b/docs/source/overview/environments.rst @@ -1126,6 +1126,11 @@ inferencing, including reading from an already trained checkpoint and disabling - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - + * - IsaacContrib-NeedlePass-dVRK-IK-Abs + - + - Manager Based + - + - **physics=** ``physx`` * - IsaacContrib-NutPour-GR1T2-Pink-IK-Abs - - Manager Based diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index 92fa91d0ee35..47113a46373f 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -206,6 +206,63 @@ variant of the task (``IsaacContrib-Stack-Cube-Franka-IK-Abs``): and :ref:`isaac-teleop-new-device` for information on adding new devices. +dVRK needle-pass demonstration +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The bimanual dVRK needle-pass task uses paired motion controllers through the unified Isaac +Teleop pipeline. It references the dVRK PSM, SDF suture needle, and suture pad from the Isaac for +Healthcare ``0.6.0`` asset catalogue at content revision ``c189487``; Isaac Lab does not +redistribute those remote assets. Verify the downloaded bytes before launching the task: + +.. code:: bash + + ./isaaclab.sh -p scripts/tools/preflight_dvrk_needle_pass_assets.py + +Each reset starts with the free dynamic needle physically held between the donor jaws and the +receiver jaws open. Preserve the donor hold, acquire the receiver grasp, and then release the +donor. First check manual control with: + +.. code:: bash + + ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py \ + --task IsaacContrib-NeedlePass-dVRK-IK-Abs \ + --device cuda:0 --visualizer kit --xr + +Then record and replay one successful demonstration: + +.. code:: bash + + # Record one successful demonstration. + ./isaaclab.sh -p scripts/tools/record_demos.py \ + --task IsaacContrib-NeedlePass-dVRK-IK-Abs \ + --dataset_file ./datasets/dvrk_needle_pass.hdf5 \ + --num_demos 1 --num_success_steps 10 --step_hz 30 \ + --device cuda:0 --visualizer kit --xr + + # Replay the recorded demonstration. + ./isaaclab.sh -p scripts/tools/replay_demos.py \ + --task IsaacContrib-NeedlePass-dVRK-IK-Abs \ + --dataset_file ./datasets/dvrk_needle_pass.hdf5 \ + --num_envs 1 --device cuda:0 --validate_success_rate + +The needle remains a free dynamic rigid body throughout the episode. Reset is the only operation +that writes its pose or velocity; the task advances donor hold, co-hold, receiver-only hold, and +retained lift from bilateral jaw contact and simulated needle motion. The task requires CUDA +PhysX because its contact-qualified grasp and hand-off contracts are validated only on that path. + +To retain a reviewable video of the qualified one-environment hand-off trace, set an output +directory before running the focused physics test: + +.. code-block:: bash + + ISAACLAB_DVRK_NEEDLE_PASS_VIDEO_DIR=$PWD/output/dvrk-needle-pass \ + ./isaaclab.sh -p -m pytest \ + source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_physics.py \ + -k native_grasp_generator_handoff_video -q + +The test fails if Gymnasium's ``RecordVideo`` wrapper does not write an MP4 file. + + Collect a Dataset of Human Demonstrations ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ From 1d53a9422e8ff566e534af239b27614a0b4b1b15 Mon Sep 17 00:00:00 2001 From: Chris von Csefalvay Date: Thu, 16 Jul 2026 11:41:13 -0600 Subject: [PATCH 5/6] Fix dVRK runtime safety contracts Move runtime imports behind lazy configuration strings and preserve inherited IK behaviour without CUDA synchronisation. Validate physical inputs and make invalid pose and jaw commands fail safe. Cache phase constants and cover the new action semantics and pre-application configuration loading. --- .../contrib/needle_pass/mdp/__init__.pyi | 8 +- .../contrib/needle_pass/mdp/actions.py | 224 +++++++++++----- .../contrib/needle_pass/mdp/actions_cfg.py | 61 +++++ .../contrib/needle_pass/mdp/events.py | 8 + .../contrib/needle_pass/mdp/grasp_solver.py | 120 ++++++++- .../contrib/needle_pass/mdp/observations.py | 81 +++++- .../contrib/needle_pass/mdp/rewards.py | 20 +- .../contrib/needle_pass/mdp/terminations.py | 197 +++++++++++--- .../needle_pass/needle_pass_env_cfg.py | 59 +---- .../contrib/needle_pass/spawners.py | 79 ++++++ .../needle_pass/test_dvrk_needle_pass.py | 248 +++++++++++++++++- 11 files changed, 934 insertions(+), 171 deletions(-) create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/actions_cfg.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/spawners.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/__init__.pyi index cea11227eda3..6e748c60a3e2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/__init__.pyi @@ -58,15 +58,17 @@ from isaaclab.envs.mdp import * # noqa: F403 from .actions import ( PSM_JAW_JOINT_ORDER, DonorReleaseGuardedPairedJawJointPositionAction, - DonorReleaseGuardedPairedJawJointPositionActionCfg, PairedJawJointPositionAction, - PairedJawJointPositionActionCfg, WorldFrameDifferentialInverseKinematicsAction, - WorldFrameDifferentialInverseKinematicsActionCfg, donor_opening_requested, donor_release_is_allowed, world_pose_xyzw_to_root_pose_xyzw, ) +from .actions_cfg import ( + DonorReleaseGuardedPairedJawJointPositionActionCfg, + PairedJawJointPositionActionCfg, + WorldFrameDifferentialInverseKinematicsActionCfg, +) from .events import reset_needle_pass_to_default from .grasp_solver import ( EXACT_POINT_CONTACT_FORCE_RESIDUAL_TOLERANCE_N, diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/actions.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/actions.py index 8a7f45e2d2b9..7555518f781b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/actions.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/actions.py @@ -14,19 +14,17 @@ import torch import isaaclab.utils.math as math_utils -from isaaclab.envs.mdp.actions.actions_cfg import ( - DifferentialInverseKinematicsActionCfg, - JointPositionActionCfg, -) from isaaclab.envs.mdp.actions.joint_actions import JointPositionAction from isaaclab.envs.mdp.actions.task_space_actions import DifferentialInverseKinematicsAction -from isaaclab.managers.action_manager import ActionTerm -from isaaclab.utils.configclass import configclass if TYPE_CHECKING: from isaaclab.envs import ManagerBasedEnv - from .terminations import HandoffPhaseCfg + from .actions_cfg import ( + DonorReleaseGuardedPairedJawJointPositionActionCfg, + PairedJawJointPositionActionCfg, + WorldFrameDifferentialInverseKinematicsActionCfg, + ) PSM_JAW_JOINT_ORDER = ("psm_tool_gripper1_joint", "psm_tool_gripper2_joint") @@ -39,6 +37,14 @@ def donor_release_is_allowed(phase: torch.Tensor, receiver_grasp: torch.Tensor, contact in the latest post-physics sensor sample. The caller passes the live contact result rather than a phase-derived latch so a contact loss cannot leave an opening request authorised. + + Args: + phase: Current hand-off phase per environment. + receiver_grasp: Whether the receiver currently has bilateral contact. + co_hold_phase: Integer value of the first phase permitting release. + + Returns: + Boolean release permission per environment. """ if phase.shape != receiver_grasp.shape: @@ -60,6 +66,14 @@ def donor_opening_requested( held target*. The public ABI exposes two joint targets, so guarding only a simultaneous paired command would leave an unsafe one-jaw escape path. Deliberate further closing remains an ordinary actuator command. + + Args: + jaw_targets: Ordered donor-jaw targets [rad], shape ``(N, 2)``. + hold_targets: Ordered held-grasp targets [rad], shape ``(N, 2)``. + aperture_threshold_rad: Minimum outward displacement treated as release [rad]. + + Returns: + Boolean release request per environment. """ if jaw_targets.shape != hold_targets.shape or jaw_targets.ndim != 2 or jaw_targets.shape[1] != 2: @@ -68,6 +82,14 @@ def donor_opening_requested( raise ValueError("jaw targets must be finite") if not math.isfinite(aperture_threshold_rad) or aperture_threshold_rad < 0.0: raise ValueError("aperture threshold must be finite and non-negative") + return _donor_opening_requested(jaw_targets, hold_targets, aperture_threshold_rad) + + +def _donor_opening_requested( + jaw_targets: torch.Tensor, hold_targets: torch.Tensor, aperture_threshold_rad: float +) -> torch.Tensor: + """Evaluate validated jaw targets without synchronising a CUDA stream.""" + return (jaw_targets[:, 0] < hold_targets[:, 0] - aperture_threshold_rad) | ( jaw_targets[:, 1] > hold_targets[:, 1] + aperture_threshold_rad ) @@ -83,6 +105,14 @@ def world_pose_xyzw_to_root_pose_xyzw( Each row is converted against its matching live root transform. The helper deliberately accepts batched tensors so differently placed cloned PSMs can never accidentally share one root transform. + + Args: + pose_w_xyzw: World position [m] and xyzw quaternion, shape ``(N, 7)``. + root_pos_w: Live articulation-root position [m], shape ``(N, 3)``. + root_quat_w_xyzw: Live articulation-root xyzw quaternion, shape ``(N, 4)``. + + Returns: + Root-frame position [m] and normalised xyzw quaternion, shape ``(N, 7)``. """ if pose_w_xyzw.ndim != 2 or pose_w_xyzw.shape[1] != 7: @@ -92,11 +122,22 @@ def world_pose_xyzw_to_root_pose_xyzw( if not torch.isfinite(pose_w_xyzw).all(): raise ValueError("world-frame IK actions must be finite") - target_quat_xyzw = pose_w_xyzw[:, 3:7] - target_quat_norm = torch.linalg.vector_norm(target_quat_xyzw, dim=-1, keepdim=True) + target_quat_norm = torch.linalg.vector_norm(pose_w_xyzw[:, 3:7], dim=-1, keepdim=True) if torch.any(target_quat_norm <= 1.0e-9): raise ValueError("world-frame IK action quaternions must be normalisable") - target_quat_xyzw = target_quat_xyzw / target_quat_norm + return _world_pose_xyzw_to_root_pose_xyzw(pose_w_xyzw, root_pos_w, root_quat_w_xyzw) + + +def _world_pose_xyzw_to_root_pose_xyzw( + pose_w_xyzw: torch.Tensor, + root_pos_w: torch.Tensor, + root_quat_w_xyzw: torch.Tensor, +) -> torch.Tensor: + """Convert a validated world pose without synchronising a CUDA stream.""" + + target_quat_xyzw = pose_w_xyzw[:, 3:7] + target_quat_norm = torch.linalg.vector_norm(target_quat_xyzw, dim=-1, keepdim=True) + target_quat_xyzw = target_quat_xyzw / target_quat_norm.clamp_min(1.0e-9) target_pos_b, target_quat_b = math_utils.subtract_frame_transforms( root_pos_w, root_quat_w_xyzw, @@ -121,6 +162,8 @@ class WorldFrameDifferentialInverseKinematicsAction(DifferentialInverseKinematic def __init__(self, cfg: WorldFrameDifferentialInverseKinematicsActionCfg, env: ManagerBasedEnv): if cfg.scale != 1.0: raise ValueError("world-frame absolute IK must use scale=1.0") + if cfg.controller.command_type != "pose" or cfg.controller.use_relative_mode: + raise ValueError("world-frame absolute IK requires an absolute pose controller") super().__init__(cfg, env) def process_actions(self, actions: torch.Tensor) -> None: @@ -128,42 +171,96 @@ def process_actions(self, actions: torch.Tensor) -> None: if actions.shape != self._raw_actions.shape: raise ValueError(f"expected world-frame IK actions with shape {tuple(self._raw_actions.shape)}") - if not torch.isfinite(actions).all(): - raise ValueError("world-frame IK actions must be finite") - quaternion_norm = torch.linalg.vector_norm(actions[:, 3:7], dim=-1, keepdim=True) - if torch.any(quaternion_norm <= 1.0e-9): - raise ValueError("world-frame IK action quaternions must be normalisable") self._raw_actions[:] = actions - self._processed_actions[:, :3] = actions[:, :3] - self._processed_actions[:, 3:7] = actions[:, 3:7] / quaternion_norm + processed_actions = self.raw_actions * self._scale + if self.cfg.clip is not None: + processed_actions = torch.clamp( + processed_actions, + min=self._clip[:, :, 0], + max=self._clip[:, :, 1], + ) + + quaternion_norm = torch.linalg.vector_norm(processed_actions[:, 3:7], dim=-1, keepdim=True) + valid = torch.isfinite(processed_actions).all(dim=-1, keepdim=True) & (quaternion_norm > 1.0e-9) + current_position_w, current_quaternion_w = self._compute_frame_pose_w() + normalised_quaternion = torch.nan_to_num( + processed_actions[:, 3:7] / quaternion_norm.clamp_min(1.0e-9), + nan=0.0, + posinf=0.0, + neginf=0.0, + ) + self._processed_actions[:, :3] = torch.where(valid, processed_actions[:, :3], current_position_w) + self._processed_actions[:, 3:7] = torch.where(valid, normalised_quaternion, current_quaternion_w) def apply_actions(self) -> None: """Convert against the live root and solve the current articulation.""" - target_pose_b = world_pose_xyzw_to_root_pose_xyzw( + target_pose_b = _world_pose_xyzw_to_root_pose_xyzw( self._processed_actions, self._asset.data.root_pos_w.torch, self._asset.data.root_quat_w.torch, ) ee_pos_b, ee_quat_b = self._compute_frame_pose() - self._ik_controller.set_command(target_pose_b, ee_pos_b, ee_quat_b) + ee_quat_norm = torch.linalg.vector_norm(ee_quat_b, dim=-1, keepdim=True) + ee_pose_valid = ( + torch.isfinite(ee_pos_b).all(dim=-1, keepdim=True) + & torch.isfinite(ee_quat_b).all(dim=-1, keepdim=True) + & (ee_quat_norm > 1.0e-9) + ) + safe_ee_pos_b = torch.nan_to_num(ee_pos_b, nan=0.0, posinf=0.0, neginf=0.0) + identity_quaternion = torch.zeros_like(ee_quat_b) + identity_quaternion[:, 3] = 1.0 + safe_ee_quat_b = torch.where( + ee_pose_valid, + torch.nan_to_num( + ee_quat_b / ee_quat_norm.clamp_min(1.0e-9), + nan=0.0, + posinf=0.0, + neginf=0.0, + ), + identity_quaternion, + ) + self._ik_controller.set_command(target_pose_b, safe_ee_pos_b, safe_ee_quat_b) joint_pos = self._asset.data.joint_pos.torch[:, self._joint_ids] - if torch.linalg.vector_norm(ee_quat_b, dim=-1).gt(0.0).all(): - joint_pos_des = self._ik_controller.compute( - ee_pos_b, - ee_quat_b, - self._compute_frame_jacobian(), - joint_pos, - ) - else: - joint_pos_des = joint_pos.clone() + if not self._limits_injected and getattr(self.cfg.controller, "joint_limit_avoidance_gain", 0.0) > 0.0: + limits = self._asset.data.soft_joint_pos_limits.torch[0, self._joint_ids, :] + self._ik_controller.set_joint_pos_limits(limits[:, 0].clone(), limits[:, 1].clone()) + self._limits_injected = True + computed_joint_pos = self._ik_controller.compute( + safe_ee_pos_b, + safe_ee_quat_b, + self._compute_frame_jacobian(), + joint_pos, + ) + joint_pos_des = torch.where(ee_pose_valid, computed_joint_pos, joint_pos) self._asset.set_joint_position_target_index(target=joint_pos_des, joint_ids=self._joint_ids) def reset(self, env_ids: Sequence[int] | None = None) -> None: - """Clear cached raw values for the selected environments.""" + """Reset selected commands to the live tool pose. + + Args: + env_ids: Environment indices to reset, or ``None`` for all environments. + """ super().reset(env_ids) - self._processed_actions[env_ids] = 0.0 + selected = slice(None) if env_ids is None else env_ids + current_position_w, current_quaternion_w = self._compute_frame_pose_w() + self._processed_actions[selected, :3] = current_position_w[selected] + self._processed_actions[selected, 3:7] = current_quaternion_w[selected] + + def _compute_frame_pose_w(self) -> tuple[torch.Tensor, torch.Tensor]: + """Return the live controlled-frame pose directly in world coordinates.""" + + frame_position_w = self._asset.data.body_pos_w.torch[:, self._body_idx] + frame_quaternion_w = self._asset.data.body_quat_w.torch[:, self._body_idx] + if self.cfg.body_offset is not None: + frame_position_w, frame_quaternion_w = math_utils.combine_frame_transforms( + frame_position_w, + frame_quaternion_w, + self._offset_pos, + self._offset_rot, + ) + return frame_position_w, frame_quaternion_w class PairedJawJointPositionAction(JointPositionAction): @@ -177,6 +274,35 @@ def __init__(self, cfg: PairedJawJointPositionActionCfg, env: ManagerBasedEnv): raise ValueError( f"dVRK jaw action must resolve exactly {list(PSM_JAW_JOINT_ORDER)}, got {self._joint_names}" ) + self._last_finite_target = self._asset.data.default_joint_pos.torch[:, self._joint_ids].clone() + self._last_command_finite = torch.ones(self.num_envs, dtype=torch.bool, device=self.device) + + def process_actions(self, actions: torch.Tensor) -> None: + """Apply configured transforms and hold the last finite jaw target. + + Args: + actions: Ordered paired-jaw position commands [rad], shape ``(num_envs, 2)``. + """ + + super().process_actions(actions) + candidate_finite = torch.isfinite(self._processed_actions).all(dim=-1, keepdim=True) + self._processed_actions = torch.where(candidate_finite, self._processed_actions, self._last_finite_target) + self._last_finite_target[:] = self._processed_actions + self._last_command_finite[:] = candidate_finite.squeeze(-1) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Reset selected jaw targets to the configured articulation defaults. + + Args: + env_ids: Environment indices to reset, or ``None`` for all environments. + """ + + super().reset(env_ids) + selected = slice(None) if env_ids is None else env_ids + default_target = self._asset.data.default_joint_pos.torch[selected][:, self._joint_ids] + self._processed_actions[selected] = default_target + self._last_finite_target[selected] = default_target + self._last_command_finite[selected] = True class DonorReleaseGuardedPairedJawJointPositionAction(PairedJawJointPositionAction): @@ -200,9 +326,9 @@ def __init__(self, cfg: DonorReleaseGuardedPairedJawJointPositionActionCfg, env: raise ValueError("donor release guard requires the shared hand-off phase configuration") if not math.isfinite(cfg.release_aperture_threshold_rad) or cfg.release_aperture_threshold_rad < 0.0: raise ValueError("donor release aperture threshold must be finite and non-negative") - hold_target = torch.tensor(cfg.hold_jaw_pos, dtype=torch.float32, device=self.device) - if hold_target.shape != (2,) or not torch.isfinite(hold_target).all(): + if len(cfg.hold_jaw_pos) != 2 or not all(math.isfinite(value) for value in cfg.hold_jaw_pos): raise ValueError("donor release guard requires two finite holding jaw positions") + hold_target = torch.tensor(cfg.hold_jaw_pos, dtype=torch.float32, device=self.device) self._hold_target = hold_target.repeat(self.num_envs, 1) def apply_actions(self) -> None: @@ -213,50 +339,26 @@ def apply_actions(self) -> None: machine = get_handoff_phase_machine(self._env, self.cfg.phase_cfg) loads, normals, _ = jaw_needle_contact_measurements(self._env) receiver_grasp = machine._bilateral_contact(loads[:, 2:4], normals[:, 2:4], machine._receiver_engaged) - release_requested = donor_opening_requested( + release_requested = _donor_opening_requested( self.processed_actions, self._hold_target, self.cfg.release_aperture_threshold_rad, ) release_allowed = donor_release_is_allowed(machine.phase, receiver_grasp, int(HandoffPhase.CO_HOLD)) + unsafe_command = ~self._last_command_finite | (release_requested & ~release_allowed) command = torch.where( - (release_requested & ~release_allowed).unsqueeze(-1), self._hold_target, self.processed_actions + unsafe_command.unsqueeze(-1), + self._hold_target, + self.processed_actions, ) self._asset.set_joint_position_target_index(target=command, joint_ids=self._joint_ids) -@configclass -class WorldFrameDifferentialInverseKinematicsActionCfg(DifferentialInverseKinematicsActionCfg): - """Configuration for live world-to-root absolute differential IK.""" - - class_type: type[ActionTerm] = WorldFrameDifferentialInverseKinematicsAction - - -@configclass -class PairedJawJointPositionActionCfg(JointPositionActionCfg): - """Configuration for the exact ordered paired-jaw action.""" - - class_type: type[ActionTerm] = PairedJawJointPositionAction - - -@configclass -class DonorReleaseGuardedPairedJawJointPositionActionCfg(PairedJawJointPositionActionCfg): - """Exact paired donor jaws with a measured receiver-grasp release interlock.""" - - class_type: type[ActionTerm] = DonorReleaseGuardedPairedJawJointPositionAction - phase_cfg: HandoffPhaseCfg | None = None - release_aperture_threshold_rad: float = 0.0 - hold_jaw_pos: tuple[float, float] = (0.0, 0.0) - - __all__ = [ "DonorReleaseGuardedPairedJawJointPositionAction", - "DonorReleaseGuardedPairedJawJointPositionActionCfg", "PSM_JAW_JOINT_ORDER", "PairedJawJointPositionAction", - "PairedJawJointPositionActionCfg", "WorldFrameDifferentialInverseKinematicsAction", - "WorldFrameDifferentialInverseKinematicsActionCfg", "donor_release_is_allowed", "donor_opening_requested", "world_pose_xyzw_to_root_pose_xyzw", diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/actions_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/actions_cfg.py new file mode 100644 index 000000000000..39fd97953936 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/actions_cfg.py @@ -0,0 +1,61 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Action configurations for the dVRK needle-pass task.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from isaaclab.envs.mdp.actions.actions_cfg import ( + DifferentialInverseKinematicsActionCfg, + JointPositionActionCfg, +) +from isaaclab.utils.configclass import configclass + +if TYPE_CHECKING: + from isaaclab.managers.action_manager import ActionTerm + + from .terminations import HandoffPhaseCfg + + +@configclass +class WorldFrameDifferentialInverseKinematicsActionCfg(DifferentialInverseKinematicsActionCfg): + """Configure live world-to-root absolute differential IK.""" + + class_type: type[ActionTerm] | str = "{DIR}.actions:WorldFrameDifferentialInverseKinematicsAction" + """Action-term implementation, resolved lazily after the simulation application starts.""" + + +@configclass +class PairedJawJointPositionActionCfg(JointPositionActionCfg): + """Configure the exact ordered paired-jaw action.""" + + class_type: type[ActionTerm] | str = "{DIR}.actions:PairedJawJointPositionAction" + """Action-term implementation, resolved lazily after the simulation application starts.""" + + +@configclass +class DonorReleaseGuardedPairedJawJointPositionActionCfg(PairedJawJointPositionActionCfg): + """Configure paired donor jaws with a measured receiver-grasp interlock.""" + + class_type: type[ActionTerm] | str = "{DIR}.actions:DonorReleaseGuardedPairedJawJointPositionAction" + """Action-term implementation, resolved lazily after the simulation application starts.""" + + phase_cfg: HandoffPhaseCfg | None = None + """Shared hand-off phase configuration used by the release interlock.""" + + release_aperture_threshold_rad: float = 0.0 + """Minimum outward displacement from the held target that requests release [rad].""" + + hold_jaw_pos: tuple[float, float] = (0.0, 0.0) + """Ordered donor-jaw positions commanded while release is blocked [rad].""" + + +__all__ = [ + "DonorReleaseGuardedPairedJawJointPositionActionCfg", + "PairedJawJointPositionActionCfg", + "WorldFrameDifferentialInverseKinematicsActionCfg", +] diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/events.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/events.py index 443e1e4b7053..b2dd00305f26 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/events.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/events.py @@ -36,6 +36,14 @@ def reset_needle_pass_to_default( open. The free needle then receives exactly one pose write and one velocity write. The event does not step or settle physics and never applies an action. + + Args: + env: Manager-based needle-pass environment. + env_ids: Environment indices to reset, or ``None`` for all environments. + phase_cfg: Shared physical phase configuration. + left_psm_cfg: Donor PSM scene entity. + right_psm_cfg: Receiver PSM scene entity. + needle_cfg: Free-needle scene entity. """ if env_ids is None: diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/grasp_solver.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/grasp_solver.py index a5f14c2c4f2c..ff745fdebe32 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/grasp_solver.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/grasp_solver.py @@ -52,9 +52,16 @@ class RetentionLoad: """Analytical two-contact load required by gravity and commanded motion.""" external_force_n: float + """Worst-case external force from gravity and commanded motion [N].""" + normal_force_per_jaw_n: float + """Required normal force at each opposed jaw contact [N].""" + friction_coefficient: float + """Static Coulomb friction coefficient used by the calculation.""" + safety_factor: float + """Multiplicative safety factor applied to the required contact load.""" @dataclass(frozen=True, slots=True) @@ -69,16 +76,37 @@ class ForceClosureProof: """ exact_point_contact_feasible: bool + """Whether the exact point-contact tolerances are satisfied.""" + coefficients: tuple[float, ...] + """Non-negative friction-cone generator magnitudes [N].""" + achieved_wrench: tuple[float, float, float, float, float, float] + """Achieved force and torque wrench [N, N*m].""" + residual_wrench: tuple[float, float, float, float, float, float] + """Required-minus-achieved force and torque wrench [N, N*m].""" + required_force_norm_n: float + """Euclidean norm of the required force [N].""" + force_residual_norm_n: float + """Euclidean norm of the force residual [N].""" + torque_residual_norm_n_m: float + """Euclidean norm of the torque residual [N*m].""" + equivalent_moment_arm_residual_m: float + """Torque residual divided by required-force norm [m].""" + force_residual_tolerance_n: float + """Numerical feasibility tolerance for force residual [N].""" + moment_arm_residual_tolerance_m: float + """Numerical feasibility tolerance for equivalent moment arm [m].""" + active_generator_indices: tuple[int, ...] + """Indices of non-zero friction-cone generators in the certificate.""" @property def feasible(self) -> bool: @@ -96,10 +124,19 @@ class FiniteContactAcceptance: """ accepted: bool + """Whether both caller-supplied finite-contact tolerances are satisfied.""" + force_within_tolerance: bool + """Whether the force residual is within its tolerance.""" + moment_arm_within_tolerance: bool + """Whether the equivalent moment-arm residual is within its tolerance.""" + force_residual_tolerance_n: float + """Caller-supplied finite-contact force tolerance [N].""" + moment_arm_residual_tolerance_m: float + """Caller-supplied finite-contact moment-arm tolerance [m].""" def required_retention_load( @@ -110,7 +147,18 @@ def required_retention_load( friction_coefficient: float, safety_factor: float, ) -> RetentionLoad: - """Return the per-jaw normal load for an opposed two-contact grasp.""" + """Return the per-jaw normal load for an opposed two-contact grasp. + + Args: + mass_kg: Retained object mass [kg]. + gravity_m_s2: Gravity magnitude [m/s^2]. + maximum_commanded_acceleration_m_s2: Maximum commanded acceleration [m/s^2]. + friction_coefficient: Static Coulomb friction coefficient. + safety_factor: Multiplicative load safety factor, at least one. + + Returns: + Required external and per-jaw normal loads. + """ values = ( mass_kg, @@ -135,7 +183,16 @@ def friction_cone_generators( friction_coefficient: float, facets: int, ) -> np.ndarray: - """Return a fixed polygonal approximation of one Coulomb friction cone.""" + """Return a fixed polygonal approximation of one Coulomb friction cone. + + Args: + inward_normal: Inward contact normal, shape ``(3,)``. + friction_coefficient: Static Coulomb friction coefficient. + facets: Number of polygonal friction-cone facets. + + Returns: + Dimensionless force directions, shape ``(facets, 3)``. + """ normal = _unit(inward_normal, "inward normal") if not math.isfinite(friction_coefficient) or friction_coefficient <= 0.0: @@ -166,6 +223,14 @@ def two_contact_friction_wrench_generators( contributes eight edges of a polygonal Coulomb cone using the declared *static* friction coefficient. A generator is ordered as ``[force, moment]``, with ``moment = point x force``. + + Args: + contact_points_m: Contact positions relative to the wrench origin [m], shape ``(2, 3)``. + inward_normals: Inward contact normals, shape ``(2, 3)``. + static_friction_coefficient: Static Coulomb friction coefficient. + + Returns: + Force and torque generators [dimensionless, m], shape ``(16, 6)``. """ points = np.asarray(contact_points_m, dtype=np.float64) @@ -213,6 +278,15 @@ def prove_two_contact_force_closure( Infeasible candidates are ranked by the maximum of their two dimensionless tolerance ratios. No force value is ever added to a torque value. + + Args: + contact_points_m: Contact positions relative to the wrench origin [m], shape ``(2, 3)``. + inward_normals: Inward contact normals, shape ``(2, 3)``. + static_friction_coefficient: Static Coulomb friction coefficient. + required_wrench: Required force and torque [N, N*m], shape ``(6,)``. + + Returns: + Deterministic exact point-contact certificate and residuals. """ target = np.asarray(required_wrench, dtype=np.float64) @@ -316,6 +390,14 @@ def assess_finite_contact_acceptance( and therefore requires both physical tolerances from the caller. A conservative task should keep the moment-arm allowance at or below 10 micrometres unless independent contact-patch evidence supports more. + + Args: + proof: Exact point-contact proof to assess. + force_residual_tolerance_n: Accepted force residual [N]. + moment_arm_residual_tolerance_m: Accepted equivalent moment-arm residual [m]. + + Returns: + Explicit finite-contact acceptance decision. """ for name, tolerance in ( @@ -336,7 +418,14 @@ def assess_finite_contact_acceptance( def grasp_matrix(contact_points_m: ArrayLike) -> np.ndarray: - """Return the six-dimensional point-contact grasp matrix for two contacts.""" + """Return the six-dimensional point-contact grasp matrix for two contacts. + + Args: + contact_points_m: Contact positions relative to the wrench origin [m], shape ``(2, 3)``. + + Returns: + Matrix mapping two contact forces [N] to force and torque [N, N*m], shape ``(6, 6)``. + """ points = np.asarray(contact_points_m, dtype=np.float64) if points.shape != (2, 3) or not np.isfinite(points).all(): @@ -356,7 +445,16 @@ def impedance_gains( natural_frequency_rad_s: float, damping_ratio: float, ) -> tuple[float, float]: - """Derive ``Kp`` and ``Kd`` from reflected inertia and pole placement.""" + """Derive rotational ``Kp`` and ``Kd`` from inertia and pole placement. + + Args: + reflected_inertia_kg_m2: Reflected rotational inertia [kg*m^2]. + natural_frequency_rad_s: Target natural frequency [rad/s]. + damping_ratio: Dimensionless damping ratio. + + Returns: + Rotational stiffness and damping [N*m/rad, N*m*s/rad]. + """ inputs = (reflected_inertia_kg_m2, natural_frequency_rad_s, damping_ratio) if not all(math.isfinite(value) and value > 0.0 for value in inputs): @@ -375,7 +473,19 @@ def solve_minimum_closing_target( tolerance: float = 1.0e-6, max_iterations: int = 80, ) -> float: - """Find the smallest bounded closedness whose measured model meets the load.""" + """Find the smallest bounded closedness whose measured model meets the load. + + Args: + normal_load_fn: Deterministic mapping from closedness to normal load [N]. + required_normal_load_n: Minimum acceptable normal load [N]. + lower_closedness: Inclusive lower bound on dimensionless jaw closedness. + upper_closedness: Inclusive upper bound on dimensionless jaw closedness. + tolerance: Absolute closedness tolerance for bisection. + max_iterations: Maximum number of bisection iterations. + + Returns: + Smallest closedness meeting the required load within ``tolerance``. + """ if not 0.0 <= lower_closedness <= upper_closedness <= 1.0: raise ValueError("closedness bounds must be ordered inside [0, 1]") diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/observations.py index f703129cffd3..b5aa4066937b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/observations.py @@ -33,7 +33,15 @@ def joint_position( env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg, ) -> torch.Tensor: - """Return all live articulation joint positions in native USD order.""" + """Return all live articulation joint positions in native USD order. + + Args: + env: Manager-based needle-pass environment. + asset_cfg: Articulation scene entity. + + Returns: + Joint positions [m or rad, depending on joint type]. + """ return _articulation(env, asset_cfg).data.joint_pos.torch @@ -42,7 +50,15 @@ def joint_velocity( env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg, ) -> torch.Tensor: - """Return all live articulation joint velocities in native USD order.""" + """Return all live articulation joint velocities in native USD order. + + Args: + env: Manager-based needle-pass environment. + asset_cfg: Articulation scene entity. + + Returns: + Joint velocities [m/s or rad/s, depending on joint type]. + """ return _articulation(env, asset_cfg).data.joint_vel.torch @@ -52,7 +68,16 @@ def end_effector_pose_w( asset_cfg: SceneEntityCfg, body_name: str = "psm_tool_tip_link", ) -> torch.Tensor: - """Return live tool-tip pose ``[xyz, qx, qy, qz, qw]`` in world frame.""" + """Return live tool-tip pose ``[xyz, qx, qy, qz, qw]`` in world frame. + + Args: + env: Manager-based needle-pass environment. + asset_cfg: Articulation scene entity. + body_name: Tool-tip body name. + + Returns: + World position [m] and xyzw quaternion, shape ``(num_envs, 7)``. + """ asset = _articulation(env, asset_cfg) body_ids, body_names = asset.find_bodies(body_name) @@ -66,7 +91,15 @@ def needle_pose_w( env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("needle"), ) -> torch.Tensor: - """Return simulated needle pose ``[xyz, qx, qy, qz, qw]`` in world frame.""" + """Return simulated needle pose ``[xyz, qx, qy, qz, qw]`` in world frame. + + Args: + env: Manager-based needle-pass environment. + asset_cfg: Needle scene entity. + + Returns: + World position [m] and xyzw quaternion, shape ``(num_envs, 7)``. + """ needle: RigidObject = env.scene[asset_cfg.name] return torch.cat((needle.data.root_pos_w.torch, needle.data.root_quat_w.torch), dim=-1) @@ -76,21 +109,44 @@ def needle_velocity_w( env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("needle"), ) -> torch.Tensor: - """Return simulated needle linear then angular world velocity.""" + """Return simulated needle linear then angular world velocity. + + Args: + env: Manager-based needle-pass environment. + asset_cfg: Needle scene entity. + + Returns: + World linear and angular velocity [m/s, rad/s], shape ``(num_envs, 6)``. + """ needle: RigidObject = env.scene[asset_cfg.name] return torch.cat((needle.data.root_lin_vel_w.torch, needle.data.root_ang_vel_w.torch), dim=-1) def jaw_needle_contact_force(env: ManagerBasedRLEnv) -> torch.Tensor: - """Return four projected normal loads in left-jaw-1 through right-jaw-2 order.""" + """Return four projected normal loads in left-jaw-1 through right-jaw-2 order. + + Args: + env: Manager-based needle-pass environment. + + Returns: + Ordered projected jaw-normal loads [N], shape ``(num_envs, 4)``. + """ loads, _, _ = jaw_needle_contact_measurements(env) return loads def handoff_phase(env: ManagerBasedRLEnv, phase_cfg: HandoffPhaseCfg) -> torch.Tensor: - """Return one physical phase column; INITIAL is reset-held pending fresh contact.""" + """Return one physical phase column; INITIAL awaits fresh contact. + + Args: + env: Manager-based needle-pass environment. + phase_cfg: Shared physical phase configuration. + + Returns: + Integer phase column, shape ``(num_envs, 1)``. + """ return update_handoff_phase(env, phase_cfg).phase.unsqueeze(-1) @@ -100,7 +156,16 @@ def phase_at_least( phase_cfg: HandoffPhaseCfg, phase: HandoffPhase, ) -> torch.Tensor: - """Return a recorder subtask flag derived solely from measured phase state.""" + """Return a recorder subtask flag derived solely from measured phase state. + + Args: + env: Manager-based needle-pass environment. + phase_cfg: Shared physical phase configuration. + phase: Earliest phase that sets the flag. + + Returns: + Float flag column containing zero or one, shape ``(num_envs, 1)``. + """ current = update_handoff_phase(env, phase_cfg).phase return (current >= int(phase)).to(dtype=torch.float32).unsqueeze(-1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/rewards.py index 966cdbbfdb1b..071c309d1c3e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/rewards.py @@ -18,14 +18,30 @@ def handoff_phase_progress(env: ManagerBasedRLEnv, phase_cfg: HandoffPhaseCfg) -> torch.Tensor: - """Return normalised ordered progress; this is not success evidence.""" + """Return normalised ordered progress; this is not success evidence. + + Args: + env: Manager-based needle-pass environment. + phase_cfg: Shared physical phase configuration. + + Returns: + Dimensionless phase progress per environment in ``[0, 1]``. + """ phase = update_handoff_phase(env, phase_cfg).phase return phase.to(dtype=torch.float32) / float(HandoffPhase.RETAINED_LIFT) def retained_lift_bonus(env: ManagerBasedRLEnv, phase_cfg: HandoffPhaseCfg) -> torch.Tensor: - """Return a sparse bonus after the retained-lift dwell has completed.""" + """Return a sparse bonus after the retained-lift dwell has completed. + + Args: + env: Manager-based needle-pass environment. + phase_cfg: Shared physical phase configuration. + + Returns: + Dimensionless zero-or-one bonus per environment. + """ phase = update_handoff_phase(env, phase_cfg).phase return (phase == int(HandoffPhase.RETAINED_LIFT)).to(dtype=torch.float32) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/terminations.py index 058ed5f397ef..7c6846534fa4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/mdp/terminations.py @@ -70,27 +70,62 @@ class HandoffPhaseCfg: """ engage_force_n: float = 1.0e-4 + """Per-jaw normal load that engages a contact [N].""" + disengage_force_n: float = 5.0e-5 + """Per-jaw normal load below which an engaged contact disengages [N].""" + opposed_normal_tolerance_rad: float = math.radians(20.0) + """Maximum angular deviation from opposed jaw reaction axes [rad].""" + donor_dwell_s: float = 8.0 / 240.0 + """Continuous donor-contact dwell required for donor hold [s].""" + co_hold_dwell_s: float = 8.0 / 240.0 + """Continuous bilateral co-hold dwell required for transfer [s].""" + receiver_only_dwell_s: float = 8.0 / 240.0 + """Continuous receiver-only dwell required for ownership [s].""" + retained_lift_dwell_s: float = 10.0 / 240.0 + """Continuous retained-lift dwell required for success [s].""" + receiver_relative_position_target_m: tuple[float, float, float] = (0.0, 0.0, 0.0) + """Target needle position in the receiver tool frame [m].""" + receiver_relative_orientation_target_xyzw: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0) + """Target needle xyzw quaternion in the receiver tool frame.""" + receiver_relative_position_limit_m: float = 0.035 + """Maximum position error from the receiver-relative target [m].""" + receiver_relative_orientation_limit_rad: float = math.radians(60.0) + """Maximum orientation error from the receiver-relative target [rad].""" + maximum_linear_velocity_m_s: float = 0.10 + """Maximum needle linear speed accepted during transfer [m/s].""" + maximum_angular_velocity_rad_s: float = 5.0 + """Maximum needle angular speed accepted during transfer [rad/s].""" + required_lift_delta_z_m: float = 0.015 + """Minimum retained height above the reset reference [m].""" def __post_init__(self) -> None: + force_values = (self.engage_force_n, self.disengage_force_n) + if not all(math.isfinite(value) for value in force_values): + raise ValueError("contact hysteresis thresholds must be finite") if not 0.0 <= self.disengage_force_n < self.engage_force_n: raise ValueError("contact hysteresis requires 0 <= disengage < engage") + if not math.isfinite(self.opposed_normal_tolerance_rad): + raise ValueError("opposed_normal_tolerance_rad must be finite") if not 0.0 < self.opposed_normal_tolerance_rad < math.pi: raise ValueError("opposed_normal_tolerance_rad must lie in (0, pi)") - receiver_position_target = torch.tensor(self.receiver_relative_position_target_m) - receiver_orientation_target = torch.tensor(self.receiver_relative_orientation_target_xyzw) + receiver_position_target = torch.tensor(self.receiver_relative_position_target_m, dtype=torch.float64) + receiver_orientation_target = torch.tensor( + self.receiver_relative_orientation_target_xyzw, + dtype=torch.float64, + ) if receiver_position_target.shape != (3,) or not torch.isfinite(receiver_position_target).all(): raise ValueError("receiver relative position target must be a finite three-vector") if ( @@ -119,10 +154,19 @@ class HandoffMeasurements: """One post-physics batch consumed by :class:`HandoffPhaseMachine`.""" normal_forces_n: torch.Tensor + """Projected jaw-normal loads [N], shape ``(num_envs, 4)``.""" + reaction_normals_w: torch.Tensor + """World-space jaw reaction axes, shape ``(num_envs, 4, 3)``.""" + needle_pose_w: torch.Tensor + """World position [m] and xyzw quaternion, shape ``(num_envs, 7)``.""" + needle_velocity_w: torch.Tensor + """World linear and angular velocity [m/s, rad/s], shape ``(num_envs, 6)``.""" + receiver_pose_w: torch.Tensor + """Receiver world position [m] and xyzw quaternion, shape ``(num_envs, 7)``.""" class HandoffPhaseMachine: @@ -131,6 +175,12 @@ class HandoffPhaseMachine: The machine reads filtered normal contact loads and simulated poses. It never reads the commanded action. All counters and hysteresis state are per environment and support partial resets. + + Args: + num_envs: Number of parallel environments. + device: Torch device used for state buffers. + step_dt: Simulator step period [s]. + cfg: Physical phase thresholds and dwell periods. """ def __init__(self, num_envs: int, device: str, step_dt: float, cfg: HandoffPhaseCfg): @@ -142,6 +192,25 @@ def __init__(self, num_envs: int, device: str, step_dt: float, cfg: HandoffPhase self.device = device self.step_dt = step_dt self.cfg = cfg + self._engage_force_n = torch.full((num_envs,), cfg.engage_force_n, dtype=torch.float32, device=device) + self._disengage_force_n = torch.full((num_envs,), cfg.disengage_force_n, dtype=torch.float32, device=device) + self._receiver_position_target = torch.tensor( + cfg.receiver_relative_position_target_m, + dtype=torch.float32, + device=device, + ).unsqueeze(0) + self._receiver_orientation_target = torch.nn.functional.normalize( + torch.tensor( + cfg.receiver_relative_orientation_target_xyzw, + dtype=torch.float32, + device=device, + ).unsqueeze(0), + dim=-1, + ).repeat(num_envs, 1) + self._donor_required_steps = self._required_steps(cfg.donor_dwell_s) + self._co_hold_required_steps = self._required_steps(cfg.co_hold_dwell_s) + self._receiver_only_required_steps = self._required_steps(cfg.receiver_only_dwell_s) + self._retained_lift_required_steps = self._required_steps(cfg.retained_lift_dwell_s) self.phase = torch.zeros(num_envs, dtype=torch.long, device=device) self._donor_engaged = torch.zeros(num_envs, dtype=torch.bool, device=device) self._receiver_engaged = torch.zeros_like(self._donor_engaged) @@ -161,7 +230,13 @@ def reset( reset_needle_z_w: torch.Tensor, step_token: int | None = None, ) -> None: - """Reset to donor-held INITIAL and await a fresh post-action contact sample.""" + """Reset to donor-held INITIAL and await a fresh contact sample. + + Args: + env_ids: Environment indices to reset. + reset_needle_z_w: Reset needle heights in world coordinates [m]. + step_token: Current simulator-step identifier, if available. + """ env_ids = env_ids.to(device=self.device, dtype=torch.long) reset_z = reset_needle_z_w.to(device=self.device, dtype=torch.float32).reshape(-1) @@ -183,10 +258,12 @@ def _bilateral_contact( normals: torch.Tensor, engaged: torch.Tensor, ) -> torch.Tensor: + if not torch.is_floating_point(loads) or not torch.is_floating_point(normals): + raise TypeError("contact loads and normals must use floating-point dtypes") threshold = torch.where( engaged, - torch.full_like(loads[:, 0], self.cfg.disengage_force_n), - torch.full_like(loads[:, 0], self.cfg.engage_force_n), + self._disengage_force_n, + self._engage_force_n, ) force_ok = torch.logical_and(loads[:, 0] >= threshold, loads[:, 1] >= threshold) unit_normals = torch.nn.functional.normalize(normals, dim=-1, eps=1.0e-12) @@ -202,21 +279,12 @@ def _receiver_bounds(self, measurements: HandoffMeasurements) -> torch.Tensor: measurements.needle_pose_w[:, :3], measurements.needle_pose_w[:, 3:7], ) - position_target = torch.tensor( - self.cfg.receiver_relative_position_target_m, - dtype=needle_pos_r.dtype, - device=needle_pos_r.device, - ) + position_target = self._receiver_position_target.to(dtype=needle_pos_r.dtype) relative_position_ok = torch.linalg.vector_norm(needle_pos_r - position_target, dim=-1) <= ( self.cfg.receiver_relative_position_limit_m ) unit_quat = torch.nn.functional.normalize(needle_quat_r, dim=-1, eps=1.0e-12) - orientation_target = torch.tensor( - self.cfg.receiver_relative_orientation_target_xyzw, - dtype=unit_quat.dtype, - device=unit_quat.device, - ).repeat(self.num_envs, 1) - orientation_target = torch.nn.functional.normalize(orientation_target, dim=-1, eps=1.0e-12) + orientation_target = self._receiver_orientation_target.to(dtype=unit_quat.dtype) relative_angle = math_utils.quat_error_magnitude(unit_quat, orientation_target) relative_orientation_ok = relative_angle <= self.cfg.receiver_relative_orientation_limit_rad linear_velocity_ok = torch.linalg.vector_norm(measurements.needle_velocity_w[:, :3], dim=-1) <= ( @@ -264,7 +332,26 @@ def _clear_progress(self, mask: torch.Tensor, *, after_phase: HandoffPhase) -> N self._retained_lift_counter[mask] = 0 def advance(self, measurements: HandoffMeasurements, step_token: int) -> torch.Tensor: - """Advance each environment at most once for one simulator step token.""" + """Advance each environment at most once for one simulator step token. + + Args: + measurements: Current post-physics contact, pose, and velocity batch. + step_token: Monotonic simulator-step identifier. + + Returns: + Current phase per environment. + """ + + measurement_tensors = { + "normal_forces_n": measurements.normal_forces_n, + "reaction_normals_w": measurements.reaction_normals_w, + "needle_pose_w": measurements.needle_pose_w, + "needle_velocity_w": measurements.needle_velocity_w, + "receiver_pose_w": measurements.receiver_pose_w, + } + for name, tensor in measurement_tensors.items(): + if not torch.is_floating_point(tensor): + raise TypeError(f"{name} must use a floating-point dtype") if measurements.normal_forces_n.shape != (self.num_envs, 4): raise ValueError("normal_forces_n must have shape (num_envs, 4)") @@ -295,7 +382,7 @@ def advance(self, measurements: HandoffMeasurements, step_token: int) -> torch.T initial = active & (self.phase == int(HandoffPhase.INITIAL)) self._count_consecutive(self._donor_counter, donor, initial) - donor_complete = initial & (self._donor_counter >= self._required_steps(self.cfg.donor_dwell_s)) + donor_complete = initial & (self._donor_counter >= self._donor_required_steps) self.phase[donor_complete] = int(HandoffPhase.DONOR_HOLD) donor_phase = active & (self.phase == int(HandoffPhase.DONOR_HOLD)) & ~donor_complete @@ -304,7 +391,7 @@ def advance(self, measurements: HandoffMeasurements, step_token: int) -> torch.T self._clear_progress(donor_lost, after_phase=HandoffPhase.INITIAL) donor_phase = donor_phase & donor self._count_consecutive(self._co_hold_counter, donor & receiver, donor_phase) - co_hold_complete = donor_phase & (self._co_hold_counter >= self._required_steps(self.cfg.co_hold_dwell_s)) + co_hold_complete = donor_phase & (self._co_hold_counter >= self._co_hold_required_steps) self.phase[co_hold_complete] = int(HandoffPhase.CO_HOLD) co_hold_phase = active & (self.phase == int(HandoffPhase.CO_HOLD)) & ~co_hold_complete @@ -317,9 +404,7 @@ def advance(self, measurements: HandoffMeasurements, step_token: int) -> torch.T self._clear_progress(receiver_lost_to_initial, after_phase=HandoffPhase.INITIAL) receiver_only_condition = ~donor & receiver & receiver_bounds self._count_consecutive(self._receiver_only_counter, receiver_only_condition, co_hold_phase & receiver) - receiver_only_complete = co_hold_phase & ( - self._receiver_only_counter >= self._required_steps(self.cfg.receiver_only_dwell_s) - ) + receiver_only_complete = co_hold_phase & (self._receiver_only_counter >= self._receiver_only_required_steps) self.phase[receiver_only_complete] = int(HandoffPhase.RECEIVER_ONLY_HOLD) receiver_phase = active & (self.phase == int(HandoffPhase.RECEIVER_ONLY_HOLD)) & ~receiver_only_complete @@ -340,9 +425,7 @@ def advance(self, measurements: HandoffMeasurements, step_token: int) -> torch.T lifted = measurements.needle_pose_w[:, 2] - self.reset_needle_z_w >= self.cfg.required_lift_delta_z_m retained_lift_condition = receiver_only_condition & lifted self._count_consecutive(self._retained_lift_counter, retained_lift_condition, receiver_phase) - lift_complete = receiver_phase & ( - self._retained_lift_counter >= self._required_steps(self.cfg.retained_lift_dwell_s) - ) + lift_complete = receiver_phase & (self._retained_lift_counter >= self._retained_lift_required_steps) self.phase[lift_complete] = int(HandoffPhase.RETAINED_LIFT) return self.phase @@ -357,6 +440,13 @@ def jaw_needle_contact_measurements( into the jaw solids, and the unmodified filtered world-force vectors. The force matrix is the reaction acting on the jaw sensor body. Each sensor must contain exactly one jaw body and exactly one needle filter. + + Args: + env: Manager-based needle-pass environment. + sensor_names: Ordered names of the four filtered jaw contact sensors. + + Returns: + Projected loads [N], reaction axes, and world force vectors [N]. """ if len(sensor_names) != 4: @@ -407,7 +497,15 @@ def _asset_pose_w(asset: Articulation, body_name: str) -> torch.Tensor: def get_handoff_phase_machine(env: ManagerBasedRLEnv, phase_cfg: HandoffPhaseCfg) -> HandoffPhaseMachine: - """Return the environment-owned phase machine, constructing it once.""" + """Return the environment-owned phase machine, constructing it once. + + Args: + env: Manager-based needle-pass environment. + phase_cfg: Shared physical phase configuration. + + Returns: + Environment-owned hand-off phase machine. + """ attribute_name = "_needle_pass_handoff_phase_machine" machine = getattr(env, attribute_name, None) @@ -426,7 +524,18 @@ def update_handoff_phase( receiver_cfg: SceneEntityCfg = SceneEntityCfg("right_psm"), receiver_body_name: str = "psm_tool_tip_link", ) -> HandoffPhaseMachine: - """Update the shared phase machine idempotently from post-physics buffers.""" + """Update the shared phase machine idempotently from post-physics buffers. + + Args: + env: Manager-based needle-pass environment. + phase_cfg: Shared physical phase configuration. + needle_cfg: Scene entity containing the free needle. + receiver_cfg: Scene entity containing the receiver PSM. + receiver_body_name: Receiver tool body used for relative bounds. + + Returns: + Updated environment-owned hand-off phase machine. + """ machine = get_handoff_phase_machine(env, phase_cfg) step_token = int(env.common_step_counter) @@ -456,7 +565,14 @@ def reset_handoff_phase( reset_needle_z_w: torch.Tensor, phase_cfg: HandoffPhaseCfg, ) -> None: - """Partially reset state and the reset-relative height reference.""" + """Partially reset state and the reset-relative height reference. + + Args: + env: Manager-based needle-pass environment. + env_ids: Environment indices to reset. + reset_needle_z_w: Reset needle heights in world coordinates [m]. + phase_cfg: Shared physical phase configuration. + """ get_handoff_phase_machine(env, phase_cfg).reset( env_ids, @@ -466,7 +582,15 @@ def reset_handoff_phase( def success(env: ManagerBasedRLEnv, phase_cfg: HandoffPhaseCfg) -> torch.Tensor: - """Return true only after the measured retained-lift dwell completes.""" + """Return true only after the measured retained-lift dwell completes. + + Args: + env: Manager-based needle-pass environment. + phase_cfg: Shared physical phase configuration. + + Returns: + Boolean success flag per environment. + """ machine = update_handoff_phase(env, phase_cfg) return machine.phase == int(HandoffPhase.RETAINED_LIFT) @@ -479,8 +603,21 @@ def needle_dropped_or_out_of_bounds( drop_distance_m: float = 0.12, horizontal_distance_m: float = 0.45, ) -> torch.Tensor: - """Terminate a physically dropped needle separately from success.""" + """Terminate a physically dropped needle separately from success. + + Args: + env: Manager-based needle-pass environment. + phase_cfg: Shared physical phase configuration. + needle_cfg: Scene entity containing the free needle. + drop_distance_m: Maximum downward displacement from reset [m]. + horizontal_distance_m: Maximum horizontal displacement from the environment origin [m]. + + Returns: + Boolean failure flag per environment. + """ + if not math.isfinite(drop_distance_m) or not math.isfinite(horizontal_distance_m): + raise ValueError("drop and horizontal bounds must be finite") if drop_distance_m <= 0.0 or horizontal_distance_m <= 0.0: raise ValueError("drop and horizontal bounds must be positive") machine = update_handoff_phase(env, phase_cfg) diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/needle_pass_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/needle_pass_env_cfg.py index 5f121332cbec..d1708745a8fc 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/needle_pass_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/needle_pass_env_cfg.py @@ -9,15 +9,12 @@ from collections.abc import Callable from dataclasses import MISSING -from typing import Any from isaaclab_physx.physics import PhysxCfg from isaaclab_physx.sim.schemas import PhysxCollisionPropertiesCfg, PhysxRigidBodyPropertiesCfg from isaaclab_physx.sim.spawners.materials import PhysxRigidBodyMaterialCfg from isaaclab_teleop import XrCfg -from pxr import Usd, UsdPhysics - import isaaclab.sim as sim_utils from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg from isaaclab.envs import ManagerBasedRLEnvCfg @@ -29,8 +26,7 @@ from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.scene import InteractiveSceneCfg from isaaclab.sensors import ContactSensorCfg -from isaaclab.sim.spawners.from_files import UsdFileCfg, spawn_from_usd -from isaaclab.sim.utils import bind_physics_material, find_matching_prim_paths, get_current_stage +from isaaclab.sim.spawners.from_files import UsdFileCfg from isaaclab.utils.configclass import configclass from isaaclab_tasks.utils import PresetCfg @@ -96,59 +92,15 @@ class NeedlePassPhysicsCfg(PresetCfg): physx: PhysxCfg = default -def spawn_usd_with_rigid_material( - prim_path: str, - cfg: UsdFileWithRigidMaterialCfg, - translation: tuple[float, float, float] | None = None, - orientation: tuple[float, float, float, float] | None = None, - **kwargs: Any, -) -> Usd.Prim: - """Spawn a USD and strongly bind one explicit rigid-body material. - - Current Isaac Lab ``UsdFileCfg`` does not expose a physics-material field. - The stock USD spawner first creates/clones the asset; this wrapper then - creates one material beneath every resolved clone and recursively binds it - to collision descendants. Binding happens during scene construction, not - during reset, and therefore cannot write or adapt needle state. - """ - - # The pinned needle authors its rigid body on a descendant without a - # ``MassAPI``. The stock USD spawner only modifies existing mass schemas, - # so applying ``mass_props`` at the referenced asset root is a no-op. Defer - # mass authoring until the unique rigid-body descendant has been resolved. - spawn_cfg = cfg.replace(mass_props=None) - prim = spawn_from_usd(prim_path, spawn_cfg, translation, orientation, **kwargs) - resolved_prim_paths = find_matching_prim_paths(prim_path) - if not resolved_prim_paths: - raise RuntimeError(f"USD material binding resolved no prims for {prim_path!r}") - stage = get_current_stage() - for resolved_prim_path in resolved_prim_paths: - material_path = f"{resolved_prim_path}/physicsMaterial" - cfg.physics_material.func(material_path, cfg.physics_material) - bind_physics_material( - resolved_prim_path, - material_path, - stronger_than_descendants=True, - ) - if cfg.mass_props is not None: - root_prim = stage.GetPrimAtPath(resolved_prim_path) - rigid_body_prims = [prim for prim in Usd.PrimRange(root_prim) if prim.HasAPI(UsdPhysics.RigidBodyAPI)] - if len(rigid_body_prims) != 1: - raise RuntimeError( - f"needle physical-property binding expected one rigid body beneath {resolved_prim_path!r}, " - f"found {[str(prim.GetPath()) for prim in rigid_body_prims]}" - ) - rigid_body_prim = rigid_body_prims[0] - sim_utils.define_mass_properties(str(rigid_body_prim.GetPath()), cfg.mass_props, stage=stage) - return prim - - @configclass class UsdFileWithRigidMaterialCfg(UsdFileCfg): """Task-local USD spawner with an explicit rigid-body material binding.""" - func: Callable = spawn_usd_with_rigid_material + func: Callable | str = "{DIR}.spawners:spawn_usd_with_rigid_material" + """Spawner implementation, resolved lazily after the simulation application starts.""" + physics_material: PhysxRigidBodyMaterialCfg = MISSING + """Rigid-body contact material applied to every spawned clone.""" @configclass @@ -416,5 +368,4 @@ def __post_init__(self): "RETENTION_LOAD_SAFETY_FACTOR", "TerminationsCfg", "UsdFileWithRigidMaterialCfg", - "spawn_usd_with_rigid_material", ] diff --git a/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/spawners.py b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/spawners.py new file mode 100644 index 000000000000..070a6b8ef7fc --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/contrib/needle_pass/spawners.py @@ -0,0 +1,79 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Runtime USD spawners for the dVRK needle-pass task.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING, Any + +from pxr import Usd, UsdPhysics + +import isaaclab.sim as sim_utils +from isaaclab.sim.spawners.from_files import spawn_from_usd +from isaaclab.sim.utils import bind_physics_material, find_matching_prim_paths, get_current_stage + +if TYPE_CHECKING: + from .needle_pass_env_cfg import UsdFileWithRigidMaterialCfg + + +def spawn_usd_with_rigid_material( + prim_path: str, + cfg: UsdFileWithRigidMaterialCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + **kwargs: Any, +) -> Usd.Prim: + """Spawn a USD and strongly bind one explicit rigid-body material. + + Current Isaac Lab ``UsdFileCfg`` does not expose a physics-material field. + The stock USD spawner first creates or clones the asset; this wrapper then + creates one material beneath every resolved clone and recursively binds it + to collision descendants. Binding happens during scene construction, not + during reset, and therefore cannot write or adapt needle state. + + Args: + prim_path: Destination USD prim path. + cfg: USD and rigid-body material configuration. + translation: Optional translation relative to the parent prim [m]. + orientation: Optional xyzw quaternion relative to the parent prim. + **kwargs: Additional arguments forwarded to the stock USD spawner. + + Returns: + Spawned USD prim. + """ + + # The pinned needle authors its rigid body on a descendant without a + # ``MassAPI``. The stock USD spawner only modifies existing mass schemas, + # so applying ``mass_props`` at the referenced asset root is a no-op. Defer + # mass authoring until the unique rigid-body descendant has been resolved. + spawn_cfg = cfg.replace(mass_props=None) + prim = spawn_from_usd(prim_path, spawn_cfg, translation, orientation, **kwargs) + resolved_prim_paths = find_matching_prim_paths(prim_path) + if not resolved_prim_paths: + raise RuntimeError(f"USD material binding resolved no prims for {prim_path!r}") + stage = get_current_stage() + for resolved_prim_path in resolved_prim_paths: + material_path = f"{resolved_prim_path}/physicsMaterial" + cfg.physics_material.func(material_path, cfg.physics_material) + bind_physics_material( + resolved_prim_path, + material_path, + stronger_than_descendants=True, + ) + if cfg.mass_props is not None: + root_prim = stage.GetPrimAtPath(resolved_prim_path) + rigid_body_prims = [prim for prim in Usd.PrimRange(root_prim) if prim.HasAPI(UsdPhysics.RigidBodyAPI)] + if len(rigid_body_prims) != 1: + raise RuntimeError( + f"needle physical-property binding expected one rigid body beneath {resolved_prim_path!r}, " + f"found {[str(prim.GetPath()) for prim in rigid_body_prims]}" + ) + rigid_body_prim = rigid_body_prims[0] + sim_utils.define_mass_properties(str(rigid_body_prim.GetPath()), cfg.mass_props, stage=stage) + return prim + + +__all__ = ["spawn_usd_with_rigid_material"] diff --git a/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass.py b/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass.py index ad69aa02e973..72555e495375 100644 --- a/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass.py +++ b/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass.py @@ -5,6 +5,10 @@ """Focused contracts for the manager-based dVRK needle-pass task.""" +import json +import subprocess +import sys +import textwrap from types import SimpleNamespace from isaaclab.app import AppLauncher @@ -67,11 +71,13 @@ ) from isaaclab_tasks.contrib.needle_pass.mdp.actions import ( DonorReleaseGuardedPairedJawJointPositionAction, - DonorReleaseGuardedPairedJawJointPositionActionCfg, + PairedJawJointPositionAction, + WorldFrameDifferentialInverseKinematicsAction, donor_opening_requested, donor_release_is_allowed, world_pose_xyzw_to_root_pose_xyzw, ) +from isaaclab_tasks.contrib.needle_pass.mdp.actions_cfg import DonorReleaseGuardedPairedJawJointPositionActionCfg from isaaclab_tasks.contrib.needle_pass.mdp.events import reset_needle_pass_to_default from isaaclab_tasks.contrib.needle_pass.mdp.grasp_solver import ( EXACT_POINT_CONTACT_FORCE_RESIDUAL_TOLERANCE_N, @@ -109,6 +115,62 @@ TASK_ID = "IsaacContrib-NeedlePass-dVRK-IK-Abs" +def test_config_load_is_lazy_before_simulation_app_startup(): + """A fresh process must construct the task config without runtime imports.""" + + script = textwrap.dedent( + f"""\ + import builtins + import json + import sys + import traceback + + import isaaclab_tasks # noqa: F401 + from isaaclab_tasks.utils.parse_cfg import load_cfg_from_registry + + forbidden = ("pxr", "omni", "carb", "isaacsim", "scipy") + violations = {{}} + original_import = builtins.__import__ + + def import_hook(name, *args, **kwargs): + prefix = name.split(".")[0] + if prefix in forbidden and prefix not in violations: + violations[prefix] = "".join(traceback.format_stack()) + return original_import(name, *args, **kwargs) + + error = None + builtins.__import__ = import_hook + try: + load_cfg_from_registry({TASK_ID!r}, "env_cfg_entry_point") + except Exception as exception: + error = repr(exception) + finally: + builtins.__import__ = original_import + + eager_runtime_modules = [ + name + for name in ( + "isaaclab_tasks.contrib.needle_pass.mdp.actions", + "isaaclab_tasks.contrib.needle_pass.spawners", + ) + if name in sys.modules + ] + print("__RESULT__" + json.dumps({{ + "error": error, + "violations": violations, + "eager_runtime_modules": eager_runtime_modules, + }})) + """ + ) + result = subprocess.run([sys.executable, "-c", script], capture_output=True, text=True, timeout=60) + result_line = next((line for line in result.stdout.splitlines() if line.startswith("__RESULT__")), None) + assert result.returncode == 0 and result_line is not None, ( + f"config subprocess failed\nstdout:\n{result.stdout}\nstderr:\n{result.stderr}" + ) + payload = json.loads(result_line.removeprefix("__RESULT__")) + assert payload == {"error": None, "violations": {}, "eager_runtime_modules": []} + + def _proxy(tensor: torch.Tensor) -> SimpleNamespace: """Model the develop-era ProxyArray interface in pure unit mocks.""" @@ -346,6 +408,118 @@ def test_world_targets_use_each_live_psm_root(): assert not np.allclose(actual[0], actual[1]) +def test_world_frame_action_clips_valid_rows_and_holds_invalid_rows(): + """Device-side validation must preserve clip semantics without forwarding NaNs.""" + + action = object.__new__(WorldFrameDifferentialInverseKinematicsAction) + action.cfg = SimpleNamespace(clip={"pose": (-0.5, 0.5)}, body_offset=None) + action._raw_actions = torch.zeros((2, 7), dtype=torch.float32) + action._processed_actions = torch.zeros_like(action._raw_actions) + action._scale = torch.ones_like(action._raw_actions) + action._clip = torch.tensor(((-0.5, 0.5),) + ((-float("inf"), float("inf")),) * 6).repeat(2, 1, 1) + current_position = torch.tensor(((0.1, 0.2, 0.3), (-0.1, -0.2, -0.3)), dtype=torch.float32) + current_quaternion = torch.tensor(((0.0, 0.0, 0.0, 1.0),) * 2, dtype=torch.float32) + action._body_idx = 0 + action._asset = SimpleNamespace( + data=SimpleNamespace( + body_pos_w=_proxy(current_position.unsqueeze(1)), + body_quat_w=_proxy(current_quaternion.unsqueeze(1)), + ) + ) + + action.process_actions( + torch.tensor( + ( + (2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0), + (float("nan"), 1.0, 2.0, 0.0, 0.0, 0.0, 0.0), + ), + dtype=torch.float32, + ) + ) + + torch.testing.assert_close( + action.processed_actions, + torch.tensor( + ( + (0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), + (-0.1, -0.2, -0.3, 0.0, 0.0, 0.0, 1.0), + ), + dtype=torch.float32, + ), + ) + + +@pytest.mark.parametrize( + "controller", + ( + SimpleNamespace(command_type="position", use_relative_mode=False), + SimpleNamespace(command_type="pose", use_relative_mode=True), + ), +) +def test_world_frame_action_rejects_nonabsolute_pose_controllers(controller): + """World-frame targets require an absolute seven-dimensional pose controller.""" + + cfg = SimpleNamespace(scale=1.0, controller=controller) + with pytest.raises(ValueError, match="absolute pose controller"): + WorldFrameDifferentialInverseKinematicsAction(cfg, None) + + +def test_world_frame_action_injects_joint_limits_once(): + """The specialised apply path must retain lazy joint-limit avoidance setup.""" + + class _Controller: + def __init__(self): + self.limit_calls = 0 + self.compute_calls = 0 + + def set_command(self, target, ee_pos, ee_quat): + assert target.shape == (1, 7) + + def set_joint_pos_limits(self, lower, upper): + self.limit_calls += 1 + torch.testing.assert_close(lower, torch.tensor((-1.0, -2.0))) + torch.testing.assert_close(upper, torch.tensor((1.0, 2.0))) + + def compute(self, ee_pos, ee_quat, jacobian, joint_pos): + self.compute_calls += 1 + return joint_pos + 0.1 + + class _Asset: + def __init__(self): + identity = torch.tensor(((0.0, 0.0, 0.0, 1.0),), dtype=torch.float32) + self.data = SimpleNamespace( + root_pos_w=_proxy(torch.zeros((1, 3), dtype=torch.float32)), + root_quat_w=_proxy(identity), + joint_pos=_proxy(torch.zeros((1, 2), dtype=torch.float32)), + soft_joint_pos_limits=_proxy(torch.tensor((((-1.0, 1.0), (-2.0, 2.0)),))), + ) + self.targets = [] + + def set_joint_position_target_index(self, *, target, joint_ids): + self.targets.append(target.clone()) + + action = object.__new__(WorldFrameDifferentialInverseKinematicsAction) + action.cfg = SimpleNamespace(controller=SimpleNamespace(joint_limit_avoidance_gain=1.0)) + action._processed_actions = torch.tensor(((0.1, 0.2, 0.3, 0.0, 0.0, 0.0, 1.0),)) + action._asset = _Asset() + action._joint_ids = [0, 1] + action._limits_injected = False + action._ik_controller = _Controller() + action._compute_frame_pose = lambda: ( + torch.zeros((1, 3), dtype=torch.float32), + torch.tensor(((0.0, 0.0, 0.0, 1.0),), dtype=torch.float32), + ) + action._compute_frame_jacobian = lambda: torch.zeros((1, 6, 2), dtype=torch.float32) + + action.apply_actions() + action.apply_actions() + + assert action._ik_controller.limit_calls == 1 + assert action._ik_controller.compute_calls == 2 + assert len(action._asset.targets) == 2 + torch.testing.assert_close(action._asset.targets[-1], torch.full((1, 2), 0.1)) + + def test_grasp_solver_derives_loads_and_impedance_gains(): load = required_retention_load( mass_kg=assets.NEEDLE_MASS_KG, @@ -677,6 +851,40 @@ def test_bilateral_contact_rejects_nonfinite_measurements(loads, normals): assert result.tolist() == [False] +@pytest.mark.parametrize( + "kwargs", + ( + {"engage_force_n": float("inf")}, + {"disengage_force_n": float("nan")}, + {"opposed_normal_tolerance_rad": float("inf")}, + ), +) +def test_handoff_phase_config_rejects_nonfinite_contact_thresholds(kwargs): + """Non-finite physical thresholds must fail during configuration.""" + + with pytest.raises(ValueError, match="finite"): + HandoffPhaseCfg(**kwargs) + + +def test_handoff_phase_config_accepts_integer_quaternion_values(): + """Value-like integer quaternion components should be validated as floats.""" + + cfg = HandoffPhaseCfg(receiver_relative_orientation_target_xyzw=(0, 0, 0, 1)) + assert cfg.receiver_relative_orientation_target_xyzw == (0, 0, 0, 1) + + +def test_bilateral_contact_rejects_integer_measurements(): + """Integer loads must not truncate the configured force threshold.""" + + machine = HandoffPhaseMachine(1, "cpu", 0.01, HandoffPhaseCfg()) + with pytest.raises(TypeError, match="floating-point"): + machine._bilateral_contact( + torch.tensor(((0, 0),), dtype=torch.int64), + torch.tensor((((1, 0, 0), (-1, 0, 0)),), dtype=torch.int64), + torch.zeros(1, dtype=torch.bool), + ) + + def test_handoff_update_reads_post_physics_buffers_once_per_step(monkeypatch): """Manager terms sharing one phase machine must also share one sensor sample.""" @@ -747,6 +955,24 @@ def test_donor_release_blocks_any_outward_jaw_command(): assert donor_opening_requested(paired_open, held, 0.01).tolist() == [True] +def test_paired_jaw_action_holds_last_target_for_nonfinite_commands(): + """A malformed receiver command must not reach an articulation target.""" + + action = object.__new__(PairedJawJointPositionAction) + action.cfg = SimpleNamespace(clip=None) + action._raw_actions = torch.zeros((2, 2), dtype=torch.float32) + action._processed_actions = torch.zeros_like(action._raw_actions) + action._scale = 1.0 + action._offset = 0.0 + action._last_finite_target = torch.tensor(((-0.2, 0.2), (-0.3, 0.3)), dtype=torch.float32) + action._last_command_finite = torch.ones(2, dtype=torch.bool) + + action.process_actions(torch.tensor(((-0.1, 0.1), (float("nan"), float("inf"))))) + + torch.testing.assert_close(action.processed_actions, torch.tensor(((-0.1, 0.1), (-0.3, 0.3)))) + assert action._last_command_finite.tolist() == [True, False] + + def test_donor_release_guard_clamps_unqualified_jaw_targets_at_the_actuator(monkeypatch): """Exercise the production action term rather than only its pure predicates.""" @@ -761,31 +987,37 @@ def set_joint_position_target_index(self, *, target, joint_ids): hold = torch.tensor(((-0.20, 0.01),), dtype=torch.float32) machine = SimpleNamespace( - phase=torch.tensor((int(HandoffPhase.CO_HOLD),) * 3), - _receiver_engaged=torch.ones(3, dtype=torch.bool), - _bilateral_contact=lambda loads, normals, engaged: torch.tensor((False, True, False)), + phase=torch.tensor((int(HandoffPhase.CO_HOLD),) * 4), + _receiver_engaged=torch.ones(4, dtype=torch.bool), + _bilateral_contact=lambda loads, normals, engaged: torch.tensor((False, True, False, True)), ) env = SimpleNamespace() action = object.__new__(DonorReleaseGuardedPairedJawJointPositionAction) action._env = env - action._hold_target = hold.expand(3, -1).clone() + action._hold_target = hold.expand(4, -1).clone() action._joint_ids = [6, 7] action._asset = _Asset() action._debug_vis_handle = None action.cfg = SimpleNamespace(phase_cfg=object(), release_aperture_threshold_rad=0.01) - action._processed_actions = torch.tensor(((-0.40, 0.01), (-0.40, 0.30), (-0.40, 0.30)), dtype=torch.float32) + action._processed_actions = torch.tensor( + ((-0.40, 0.01), (-0.40, 0.30), (-0.40, 0.30), (-0.40, 0.30)), dtype=torch.float32 + ) + action._last_command_finite = torch.tensor((True, True, True, False)) monkeypatch.setattr(terminations, "get_handoff_phase_machine", lambda *_: machine) monkeypatch.setattr( terminations, "jaw_needle_contact_measurements", - lambda _: (torch.zeros((3, 4)), torch.zeros((3, 4, 3)), torch.zeros((3, 4))), + lambda _: (torch.zeros((4, 4)), torch.zeros((4, 4, 3)), torch.zeros((4, 4))), ) action.apply_actions() command, joint_ids = action._asset.calls.pop() - torch.testing.assert_close(command, torch.tensor(((-0.20, 0.01), (-0.40, 0.30), (-0.20, 0.01)))) + torch.testing.assert_close( + command, + torch.tensor(((-0.20, 0.01), (-0.40, 0.30), (-0.20, 0.01), (-0.20, 0.01))), + ) assert joint_ids == [6, 7] From 189c63d3f069a0b84a166ff594545d2065d60444 Mon Sep 17 00:00:00 2001 From: Chris von Csefalvay Date: Thu, 16 Jul 2026 11:41:44 -0600 Subject: [PATCH 6/6] Harden dVRK teleop validation Make the pinned PR 769 integration job blocking and let the source-build helper select supported active Python environments safely. Accept the unreleased 1.4 wheel without allowing a future major version. Execute the public retargeting graph with controller samples and require a non-empty, decodable qualification video. --- .github/workflows/build.yaml | 1 - docs/source/how-to/cloudxr_teleoperation.rst | 23 +- pyproject.toml | 2 +- .../install_isaacteleop_pr769_for_tests.sh | 91 +++++- .../test_dvrk_needle_pass_physics.py | 17 +- .../test_dvrk_needle_pass_teleop_pipeline.py | 279 ++++++++++++------ 6 files changed, 290 insertions(+), 123 deletions(-) diff --git a/.github/workflows/build.yaml b/.github/workflows/build.yaml index e94aa34678ed..67d2121e2a33 100644 --- a/.github/workflows/build.yaml +++ b/.github/workflows/build.yaml @@ -416,7 +416,6 @@ jobs: name: dVRK needle-pass teleop runs-on: [self-hosted, gpu] timeout-minutes: 180 - continue-on-error: true needs: [build, config] if: >- github.event_name != 'push' && diff --git a/docs/source/how-to/cloudxr_teleoperation.rst b/docs/source/how-to/cloudxr_teleoperation.rst index 24837d430b90..4d044f83975a 100644 --- a/docs/source/how-to/cloudxr_teleoperation.rst +++ b/docs/source/how-to/cloudxr_teleoperation.rst @@ -217,10 +217,9 @@ choose the tab that matches your hardware. .. note:: - The web client URL is versioned. The ``release-1.3.x`` path corresponds to the - Isaac Teleop version Isaac Lab is pinned to (``isaacteleop~=1.3.0`` in the - ``teleop`` extra of the root ``pyproject.toml``). When Isaac Lab bumps its Isaac - Teleop pin, update this link to the matching client release. + The web client URL is versioned. This URL targets ``release-1.3.x``. The ``teleop`` + extra accepts ``isaacteleop>=1.3.0,<2.0.0``; when using a newer 1.x release, select + the matching client release instead. .. tip:: @@ -401,19 +400,21 @@ against the pinned SHA-256 digests before the first run: .. note:: - Until `NVIDIA/IsaacTeleop PR #769 `__ - is included in a release satisfying Isaac Lab's normal version constraint, the dVRK - retargeters are a temporary source-pinned prerequisite. From the Isaac Lab repository root, - install that immutable revision for local validation with: + The dVRK retargeter API in + `NVIDIA/IsaacTeleop PR #769 `__ is not yet + released. Until it is included in a release satisfying Isaac Lab's normal version constraint, + the dVRK retargeters are a temporary source-pinned prerequisite. From the Isaac Lab repository + root, install that immutable revision for local validation with: .. code-block:: bash - ISAACLAB_PATH=$PWD bash scripts/tools/install_isaacteleop_pr769_for_tests.sh + bash scripts/tools/install_isaacteleop_pr769_for_tests.sh The helper installs ``git`` and ``libx11-dev`` through ``sudo`` when they are missing, pins - its ``uv`` build tool in Isaac Sim's Python environment, builds the exact source revision + its ``uv`` build tool in the selected Python environment, builds the exact source revision against that Python version, and force-reinstalls the resulting ``isaacteleop`` wheel without - changing its runtime dependencies. + changing its runtime dependencies. Set ``ISAACLAB_PYTHON`` to an executable interpreter or + launcher to override the automatic environment selection. Use the normal Isaac Lab installation again once a released ``isaacteleop`` package contains the dVRK retargeters. diff --git a/pyproject.toml b/pyproject.toml index 97a50399e946..ebff8d756cfe 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -134,7 +134,7 @@ mimic = [ teleop = [ "isaaclab-teleop", # IsaacTeleop is Linux x86_64 only - "isaacteleop[retargeters,ui,cloudxr]~=1.3.0 ; platform_system == 'Linux' and platform_machine in 'x86_64 AMD64'", + "isaacteleop[retargeters,ui,cloudxr]>=1.3.0,<2.0.0 ; platform_system == 'Linux' and platform_machine in 'x86_64 AMD64'", "dex-retargeting==0.5.0 ; platform_system == 'Linux' and platform_machine in 'x86_64 AMD64'", ] # RLinf VLA post-training (externally contributed). diff --git a/scripts/tools/install_isaacteleop_pr769_for_tests.sh b/scripts/tools/install_isaacteleop_pr769_for_tests.sh index ee39c4c6897f..5c93cb9ee1fc 100755 --- a/scripts/tools/install_isaacteleop_pr769_for_tests.sh +++ b/scripts/tools/install_isaacteleop_pr769_for_tests.sh @@ -12,25 +12,86 @@ set -euo pipefail readonly ISAAC_TELEOP_REPOSITORY="https://github.com/NVIDIA/IsaacTeleop.git" readonly ISAAC_TELEOP_PR769_HEAD_SHA="ca175df7afc8198cbba0592cd1b447b11a4f3165" readonly UV_VERSION="0.11.29" -if [[ -x /isaac-sim/python.sh ]]; then - readonly ISAACLAB_PYTHON="/isaac-sim/python.sh" +readonly SCRIPT_DIR="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd -P)" +readonly ISAACLAB_ROOT="${ISAACLAB_PATH:-$(cd -- "${SCRIPT_DIR}/../.." && pwd -P)}" + +selected_python="" +python_source="" +if [[ -n "${ISAACLAB_PYTHON:-}" ]]; then + selected_python="${ISAACLAB_PYTHON}" + python_source="ISAACLAB_PYTHON" +elif [[ -n "${VIRTUAL_ENV:-}" && -x "${VIRTUAL_ENV}/bin/python" ]]; then + selected_python="${VIRTUAL_ENV}/bin/python" + python_source="VIRTUAL_ENV" +elif [[ -n "${CONDA_PREFIX:-}" && -x "${CONDA_PREFIX}/bin/python" ]]; then + selected_python="${CONDA_PREFIX}/bin/python" + python_source="CONDA_PREFIX" +elif [[ -x "${ISAACLAB_ROOT}/env_isaaclab/bin/python" ]]; then + selected_python="${ISAACLAB_ROOT}/env_isaaclab/bin/python" + python_source="${ISAACLAB_ROOT}/env_isaaclab" +elif [[ -x /isaac-sim/python.sh ]]; then + selected_python="/isaac-sim/python.sh" + python_source="/isaac-sim" +elif [[ -x "${ISAACLAB_ROOT}/_isaac_sim/python.sh" ]]; then + selected_python="${ISAACLAB_ROOT}/_isaac_sim/python.sh" + python_source="${ISAACLAB_ROOT}/_isaac_sim" +elif command -v python3 >/dev/null 2>&1; then + selected_python="$(command -v python3)" + python_source="PATH" else - readonly ISAACLAB_PYTHON="${ISAACLAB_PATH:?ISAACLAB_PATH must be set}/_isaac_sim/python.sh" + echo "Unable to find a Python interpreter for the IsaacTeleop source build." >&2 + echo "Set ISAACLAB_PYTHON to an executable interpreter or launcher." >&2 + exit 1 +fi + +if [[ ! -x "${selected_python}" ]]; then + echo "Python interpreter selected from ${python_source} is not executable: ${selected_python}" >&2 + exit 1 fi -readonly ISAAC_TELEOP_PYTHON_VERSION="$( +readonly ISAACLAB_PYTHON="${selected_python}" + +if ! selected_python_version="$( "${ISAACLAB_PYTHON}" -c 'import sys; print(f"{sys.version_info.major}.{sys.version_info.minor}")' -)" -readonly ISAACLAB_PYTHON_SCRIPTS="$("${ISAACLAB_PYTHON}" -c 'import sysconfig; print(sysconfig.get_path("scripts"))')" +)"; then + echo "Failed to run Python interpreter selected from ${python_source}: ${ISAACLAB_PYTHON}" >&2 + exit 1 +fi +if [[ ! "${selected_python_version}" =~ ^[0-9]+\.[0-9]+$ ]]; then + echo "Selected Python interpreter returned an invalid version: ${selected_python_version}" >&2 + exit 1 +fi +readonly ISAAC_TELEOP_PYTHON_VERSION="${selected_python_version}" + +if ! selected_python_scripts="$( + "${ISAACLAB_PYTHON}" -c 'import sysconfig; path = sysconfig.get_path("scripts"); assert path; print(path)' +)"; then + echo "Failed to locate the console-script directory for ${ISAACLAB_PYTHON}." >&2 + exit 1 +fi +readonly ISAACLAB_PYTHON_SCRIPTS="${selected_python_scripts}" readonly BUILD_ROOT="${ISAACLAB_TELEOP_TEST_CACHE:-/tmp/isaacteleop-pr769-${ISAAC_TELEOP_PR769_HEAD_SHA}}" readonly SOURCE_DIR="${BUILD_ROOT}/source" readonly CMAKE_BUILD_DIR="${BUILD_ROOT}/build-python-${ISAAC_TELEOP_PYTHON_VERSION}" readonly SOURCE_REVISION_FILE="${BUILD_ROOT}/source-revision" -# The Isaac Sim launcher owns a Python installation whose console-script -# directory is not always on ``PATH`` in a test container. CMake finds the -# same ``uv`` executable that the launcher installs only after this export. +# The selected Python environment's console-script directory is not always on +# ``PATH`` in a test container. CMake finds the same ``uv`` executable that +# the interpreter installs only after this export. export PATH="${ISAACLAB_PYTHON_SCRIPTS}:${PATH}" +assert_source_checkout_clean() { + local source_status + if ! source_status="$(git -C "${SOURCE_DIR}" status --porcelain --untracked-files=all)"; then + echo "Failed to inspect IsaacTeleop source checkout: ${SOURCE_DIR}" >&2 + return 1 + fi + if [[ -n "${source_status}" ]]; then + echo "IsaacTeleop source checkout is not clean: ${SOURCE_DIR}" >&2 + printf '%s\n' "${source_status}" >&2 + return 1 + fi +} + if ! command -v git >/dev/null || ! dpkg-query --show --showformat='${db:Status-Status}' libx11-dev 2>/dev/null | grep -qx installed; then # IsaacTeleop's CMake dependencies are fetched through Git and its static # OpenXR loader selects the Xlib backend. The minimal Isaac Sim runtime @@ -56,11 +117,15 @@ fi test "$("${ISAACLAB_PYTHON}" -c 'import importlib.metadata; print(importlib.metadata.version("uv"))')" = "${UV_VERSION}" source_cache_valid=false +source_cache_head="" +source_cache_status="" if [[ -f "${SOURCE_REVISION_FILE}" ]] \ && [[ "$(<"${SOURCE_REVISION_FILE}")" = "${ISAAC_TELEOP_PR769_HEAD_SHA}" ]] \ && [[ -d "${SOURCE_DIR}/.git" ]] \ - && [[ "$(git -C "${SOURCE_DIR}" rev-parse HEAD 2>/dev/null || true)" = "${ISAAC_TELEOP_PR769_HEAD_SHA}" ]] \ - && [[ -z "$(git -C "${SOURCE_DIR}" status --porcelain --untracked-files=all 2>/dev/null || true)" ]]; then + && source_cache_head="$(git -C "${SOURCE_DIR}" rev-parse HEAD 2>/dev/null)" \ + && [[ "${source_cache_head}" = "${ISAAC_TELEOP_PR769_HEAD_SHA}" ]] \ + && source_cache_status="$(git -C "${SOURCE_DIR}" status --porcelain --untracked-files=all 2>/dev/null)" \ + && [[ -z "${source_cache_status}" ]]; then source_cache_valid=true fi @@ -72,14 +137,14 @@ if [[ "${source_cache_valid}" != "true" ]]; then git -C "${SOURCE_DIR}" fetch --depth 1 origin "${ISAAC_TELEOP_PR769_HEAD_SHA}" git -C "${SOURCE_DIR}" checkout --quiet --detach FETCH_HEAD test "$(git -C "${SOURCE_DIR}" rev-parse HEAD)" = "${ISAAC_TELEOP_PR769_HEAD_SHA}" - test -z "$(git -C "${SOURCE_DIR}" status --porcelain --untracked-files=all)" + assert_source_checkout_clean printf '%s\n' "${ISAAC_TELEOP_PR769_HEAD_SHA}" > "${SOURCE_REVISION_FILE}" fi test -f "${SOURCE_DIR}/CMakeLists.txt" test "$(<"${SOURCE_REVISION_FILE}")" = "${ISAAC_TELEOP_PR769_HEAD_SHA}" test "$(git -C "${SOURCE_DIR}" rev-parse HEAD)" = "${ISAAC_TELEOP_PR769_HEAD_SHA}" -test -z "$(git -C "${SOURCE_DIR}" status --porcelain --untracked-files=all)" +assert_source_checkout_clean cmake -S "${SOURCE_DIR}" -B "${CMAKE_BUILD_DIR}" \ -DISAAC_TELEOP_PYTHON_VERSION="${ISAAC_TELEOP_PYTHON_VERSION}" \ diff --git a/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_physics.py b/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_physics.py index 87d59b38df8f..5be2d3385464 100644 --- a/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_physics.py +++ b/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_physics.py @@ -1466,8 +1466,21 @@ def test_one_env_native_grasp_generator_handoff_video(): _assert_live_needle_topology(env) _assert_native_handoff_audit(audit) - videos = sorted(video_dir.glob("dvrk-needle-pass-native-handoff-episode-0*.mp4")) - assert videos, f"RecordVideo did not write the qualified handoff video to {video_dir}" + videos = sorted(video_dir.glob("*.mp4")) + assert len(videos) == 1, f"RecordVideo wrote {len(videos)} MP4 files to {video_dir}, expected exactly one" + video_path = videos[0] + assert video_path.stat().st_size > 0, f"RecordVideo wrote an empty MP4 file to {video_path}" + + import imageio.v2 as imageio + + reader = imageio.get_reader(video_path) + try: + first_frame = reader.get_data(0) + finally: + reader.close() + assert isinstance(first_frame, np.ndarray) + assert first_frame.ndim == 3 + assert first_frame.size > 0 @pytest.mark.isaacsim_ci diff --git a/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_teleop_pipeline.py b/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_teleop_pipeline.py index 24d84ada16f9..284f6c7eaf22 100644 --- a/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_teleop_pipeline.py +++ b/source/isaaclab_tasks/test/contrib/needle_pass/test_dvrk_needle_pass_teleop_pipeline.py @@ -20,14 +20,27 @@ app_launcher = AppLauncher(headless=True, enable_cameras=False) simulation_app = app_launcher.app -from isaacteleop.retargeters import DVRKPSMClutchRetargeter, DVRKPSMGripperRetargeter # noqa: E402 -from isaacteleop.retargeting_engine.deviceio_source_nodes import ControllersSource # noqa: E402 +from isaacteleop.retargeting_engine.interface import ( # noqa: E402 + ComputeContext, + ExecutionEvents, + ExecutionState, + GraphTime, + OutputCombiner, + TensorGroup, +) +from isaacteleop.schema import ( # noqa: E402 + ControllerInputState, + ControllerPose, + ControllerSnapshot, + ControllerSnapshotTrackedT, + Point, + Pose, + Quaternion, +) from isaaclab_tasks.contrib.needle_pass.config.dvrk.ik_abs_env_cfg import ( # noqa: E402 _TELEOP_AVAILABLE, - DONOR_GRASP_CLOSEDNESS, DONOR_GRASP_JAW_POS, - DVRK_PSM_JAW_CLOSED_POS, LEFT_TOOL_HOME_POS_W, LEFT_TOOL_HOME_ROT_XYZW, LEFT_WORKSPACE_LOWER, @@ -39,31 +52,80 @@ _build_dvrk_needle_pass_pipeline, ) -_EXPECTED_ACTION_ORDER = [ - "left_pos_x", - "left_pos_y", - "left_pos_z", - "left_quat_x", - "left_quat_y", - "left_quat_z", - "left_quat_w", - "left_jaw_1", - "left_jaw_2", - "right_pos_x", - "right_pos_y", - "right_pos_z", - "right_quat_x", - "right_quat_y", - "right_quat_z", - "right_quat_w", - "right_jaw_1", - "right_jaw_2", -] - - -def _reorderer_subgraph(): - pipeline = _build_dvrk_needle_pass_pipeline() - return pipeline, pipeline.output_mapping["action"].module +from isaaclab_assets.robots.dvrk import DVRK_PSM_JAW_CLOSED_POS, DVRK_PSM_JAW_OPEN_POS # noqa: E402 + + +def _tracked_controller( + position: tuple[float, float, float], + *, + squeeze: float = 1.0, + trigger: float = 0.5, +) -> ControllerSnapshotTrackedT: + """Build one valid tracked controller sample for public graph execution.""" + + pose = Pose(Point(*position), Quaternion(0.0, 0.0, 0.0, 1.0)) + controller_pose = ControllerPose(pose, True) + inputs = ControllerInputState( + primary_click=False, + secondary_click=False, + thumbstick_click=False, + menu_click=False, + thumbstick_x=0.0, + thumbstick_y=0.0, + squeeze_value=squeeze, + trigger_value=trigger, + ) + return ControllerSnapshotTrackedT(ControllerSnapshot(controller_pose, controller_pose, inputs)) + + +def _pipeline_inputs( + pipeline: OutputCombiner, + left_controller: ControllerSnapshotTrackedT, + right_controller: ControllerSnapshotTrackedT, +) -> dict: + """Build leaf-keyed inputs with an identity anchor-to-world transform.""" + + leaf_nodes = {node.name: node for node in pipeline.get_leaf_nodes()} + assert set(leaf_nodes) == {"controllers", "world_T_anchor"} + + controller_spec = leaf_nodes["controllers"].input_spec() + left_group = TensorGroup(controller_spec["deviceio_controller_left"]) + left_group[0] = left_controller + right_group = TensorGroup(controller_spec["deviceio_controller_right"]) + right_group[0] = right_controller + + transform_group = TensorGroup(leaf_nodes["world_T_anchor"].input_spec()["value"]) + transform_group[0] = np.eye(4, dtype=np.float32) + return { + "controllers": { + "deviceio_controller_left": left_group, + "deviceio_controller_right": right_group, + }, + "world_T_anchor": {"value": transform_group}, + } + + +def _running_context(time_ns: int, *, reset: bool = False) -> ComputeContext: + """Build a deterministic running-session context for one graph step.""" + + return ComputeContext( + graph_time=GraphTime(sim_time_ns=time_ns, real_time_ns=time_ns), + execution_events=ExecutionEvents(reset=reset, execution_state=ExecutionState.RUNNING), + ) + + +def _expected_initial_action() -> np.ndarray: + """Return the task-ordered left pose/jaws then right pose/jaws reset action.""" + + return np.asarray( + LEFT_TOOL_HOME_POS_W + + LEFT_TOOL_HOME_ROT_XYZW + + DONOR_GRASP_JAW_POS + + RIGHT_TOOL_HOME_POS_W + + RIGHT_TOOL_HOME_ROT_XYZW + + DVRK_PSM_JAW_OPEN_POS, + dtype=np.float32, + ) def test_dvrk_pipeline_action_is_18d(): @@ -74,69 +136,96 @@ def test_dvrk_pipeline_action_is_18d(): assert pipeline.output_types()["action"].types[0].shape == (18,) -def test_dvrk_pipeline_output_order_matches_action_terms(): - """The flattened values resolve in left pose/jaws then right pose/jaws order.""" - _, subgraph = _reorderer_subgraph() - try: - output_order = subgraph._target_module._output_order - except AttributeError: - pytest.skip("IsaacTeleop does not expose graph wiring for order inspection") - - assert output_order == _EXPECTED_ACTION_ORDER - - -def test_dvrk_pipeline_routes_world_transformed_controller_sides(): - """Each PSM consumes its own controller after the shared world transform.""" - _, reorderer_subgraph = _reorderer_subgraph() - try: - connections = reorderer_subgraph._input_connections - left_pose = connections["left_pose"].module - left_jaws = connections["left_jaws"].module - right_pose = connections["right_pose"].module - right_jaws = connections["right_jaws"].module - except AttributeError: - pytest.skip("IsaacTeleop does not expose graph wiring for route inspection") - - assert isinstance(left_pose._target_module, DVRKPSMClutchRetargeter) - assert isinstance(left_jaws._target_module, DVRKPSMGripperRetargeter) - assert isinstance(right_pose._target_module, DVRKPSMClutchRetargeter) - assert isinstance(right_jaws._target_module, DVRKPSMGripperRetargeter) - assert list(left_pose._target_module.input_spec()) == [ControllersSource.LEFT] - assert list(left_jaws._target_module.input_spec()) == [ControllersSource.LEFT] - assert list(right_pose._target_module.input_spec()) == [ControllersSource.RIGHT] - assert list(right_jaws._target_module.input_spec()) == [ControllersSource.RIGHT] - - left_transform = left_pose._input_connections[ControllersSource.LEFT].module - right_transform = right_pose._input_connections[ControllersSource.RIGHT].module - assert left_jaws._input_connections[ControllersSource.LEFT].module is left_transform - assert right_jaws._input_connections[ControllersSource.RIGHT].module is right_transform - assert left_transform is right_transform - assert left_transform._input_connections["transform"].module.name == "world_T_anchor" - - -def test_dvrk_pipeline_preserves_side_homes_workspaces_and_reset_jaws(): - """Each side retains its calibrated world limits and intended reset jaw state.""" - _, reorderer_subgraph = _reorderer_subgraph() - try: - connections = reorderer_subgraph._input_connections - left_clutch_cfg = connections["left_pose"].module._target_module._clutch_state._config - right_clutch_cfg = connections["right_pose"].module._target_module._clutch_state._config - left_gripper_cfg = connections["left_jaws"].module._target_module._jaw_intent._config - right_gripper_cfg = connections["right_jaws"].module._target_module._jaw_intent._config - except AttributeError: - pytest.skip("IsaacTeleop does not expose graph node configs for contract inspection") - - np.testing.assert_allclose(left_clutch_cfg.home_position, LEFT_TOOL_HOME_POS_W) - np.testing.assert_allclose(left_clutch_cfg.home_orientation, LEFT_TOOL_HOME_ROT_XYZW) - assert left_clutch_cfg.workspace_lower == LEFT_WORKSPACE_LOWER - assert left_clutch_cfg.workspace_upper == LEFT_WORKSPACE_UPPER - np.testing.assert_allclose(right_clutch_cfg.home_position, RIGHT_TOOL_HOME_POS_W) - np.testing.assert_allclose(right_clutch_cfg.home_orientation, RIGHT_TOOL_HOME_ROT_XYZW) - assert right_clutch_cfg.workspace_lower == RIGHT_WORKSPACE_LOWER - assert right_clutch_cfg.workspace_upper == RIGHT_WORKSPACE_UPPER - - assert DONOR_GRASP_CLOSEDNESS == 1.0 - assert DONOR_GRASP_JAW_POS == DVRK_PSM_JAW_CLOSED_POS - assert left_gripper_cfg.initial_closedness == DONOR_GRASP_CLOSEDNESS - assert left_gripper_cfg.jaw_closed == DONOR_GRASP_JAW_POS - assert right_gripper_cfg.initial_closedness == 0.0 +def test_dvrk_pipeline_executes_tracked_controller_samples(): + """Public execution emits ordered homes, independent side motion, and tracking-loss holds.""" + + pipeline = _build_dvrk_needle_pass_pipeline() + assert isinstance(pipeline, OutputCombiner) + initial_left = _tracked_controller((0.10, 0.20, 0.30)) + initial_right = _tracked_controller((-0.10, -0.20, -0.30)) + + initial_outputs = pipeline.execute_pipeline( + _pipeline_inputs(pipeline, initial_left, initial_right), + _running_context(0, reset=True), + ) + initial_action = initial_outputs["action"][0] + expected_initial_action = _expected_initial_action() + assert initial_action.shape == (18,) + np.testing.assert_allclose(initial_action, expected_initial_action, atol=1.0e-6, rtol=0.0) + + left_delta = np.asarray((0.01, -0.02, 0.03), dtype=np.float32) + moved_left = _tracked_controller(tuple(np.asarray((0.10, 0.20, 0.30)) + left_delta)) + moved_outputs = pipeline.execute_pipeline( + _pipeline_inputs(pipeline, moved_left, initial_right), + _running_context(1_000_000_000), + ) + moved_action = moved_outputs["action"][0] + expected_moved_action = expected_initial_action.copy() + expected_moved_action[:3] += left_delta + np.testing.assert_allclose(moved_action, expected_moved_action, atol=1.0e-6, rtol=0.0) + + right_delta = np.asarray((-0.02, 0.01, 0.04), dtype=np.float32) + moved_right = _tracked_controller(tuple(np.asarray((-0.10, -0.20, -0.30)) + right_delta)) + tracking_loss_outputs = pipeline.execute_pipeline( + _pipeline_inputs(pipeline, ControllerSnapshotTrackedT(), moved_right), + _running_context(2_000_000_000), + ) + tracking_loss_action = tracking_loss_outputs["action"][0] + expected_tracking_loss_action = expected_moved_action.copy() + expected_tracking_loss_action[9:12] += right_delta + np.testing.assert_allclose(tracking_loss_action, expected_tracking_loss_action, atol=1.0e-6, rtol=0.0) + + +def test_dvrk_pipeline_clips_each_side_to_its_world_workspace(): + """Public execution applies each side's configured world-frame bounds.""" + + pipeline = _build_dvrk_needle_pass_pipeline() + initial_left_position = np.asarray((0.10, 0.20, 0.30)) + initial_right_position = np.asarray((-0.10, -0.20, -0.30)) + initial_left = _tracked_controller(tuple(initial_left_position)) + initial_right = _tracked_controller(tuple(initial_right_position)) + pipeline.execute_pipeline( + _pipeline_inputs(pipeline, initial_left, initial_right), + _running_context(0, reset=True), + ) + + extreme_left = _tracked_controller(tuple(initial_left_position + np.asarray((1.0, -1.0, 1.0)))) + extreme_right = _tracked_controller(tuple(initial_right_position + np.asarray((-1.0, 1.0, -1.0)))) + clipped_outputs = pipeline.execute_pipeline( + _pipeline_inputs(pipeline, extreme_left, extreme_right), + _running_context(1_000_000_000), + ) + expected_action = _expected_initial_action() + expected_action[:3] = (LEFT_WORKSPACE_UPPER[0], LEFT_WORKSPACE_LOWER[1], LEFT_WORKSPACE_UPPER[2]) + expected_action[9:12] = (RIGHT_WORKSPACE_LOWER[0], RIGHT_WORKSPACE_UPPER[1], RIGHT_WORKSPACE_LOWER[2]) + np.testing.assert_allclose(clipped_outputs["action"][0], expected_action, atol=1.0e-6, rtol=0.0) + + +def test_dvrk_pipeline_maps_independent_trigger_intent_to_ordered_jaws(): + """Public execution maps trigger changes to the correct two-jaw action slices.""" + + pipeline = _build_dvrk_needle_pass_pipeline() + left_position = (0.10, 0.20, 0.30) + right_position = (-0.10, -0.20, -0.30) + pipeline.execute_pipeline( + _pipeline_inputs( + pipeline, + _tracked_controller(left_position, trigger=0.5), + _tracked_controller(right_position, trigger=0.5), + ), + _running_context(0, reset=True), + ) + + output = pipeline.execute_pipeline( + _pipeline_inputs( + pipeline, + _tracked_controller(left_position, trigger=0.5), + _tracked_controller(right_position, trigger=0.8), + ), + _running_context(100_000_000), + )["action"][0] + expected_action = _expected_initial_action() + expected_action[16:18] = np.asarray(DVRK_PSM_JAW_OPEN_POS) + 0.3 * ( + np.asarray(DVRK_PSM_JAW_CLOSED_POS) - np.asarray(DVRK_PSM_JAW_OPEN_POS) + ) + np.testing.assert_allclose(output, expected_action, atol=1.0e-6, rtol=0.0)