diff --git a/.github/actions/run-tests/action.yml b/.github/actions/run-tests/action.yml index 467122860141..b02944da6a82 100644 --- a/.github/actions/run-tests/action.yml +++ b/.github/actions/run-tests/action.yml @@ -31,6 +31,10 @@ inputs: description: 'Pattern to filter test files (e.g., isaaclab_tasks)' default: '' required: false + install-isaacteleop-pr769: + description: 'Build and install the test-only IsaacTeleop PR 769 merge revision before pytest' + default: 'false' + required: false runs: using: composite @@ -47,6 +51,7 @@ runs: local reports_dir="$5" local pytest_options="$6" local filter_pattern="$7" + local install_isaacteleop_pr769="$8" echo "Running tests in: $test_path" if [ -n "$pytest_options" ]; then @@ -106,12 +111,20 @@ runs: set -e cd /workspace/isaaclab mkdir -p tests + if [ \"$install_isaacteleop_pr769\" = \"true\" ]; then + bash scripts/tools/install_isaacteleop_pr769_for_tests.sh + fi echo 'Starting pytest with path: $test_path' - /isaac-sim/python.sh -m pytest --ignore=tools/conftest.py $test_path $pytest_options -v --junitxml=tests/$result_file || echo 'Pytest completed with exit code: $?' + # tools/conftest.py dispatches each Isaac Sim test in its own process, + # applies TEST_FILTER_PATTERN, and combines the per-test reports. It + # must be loaded: ignoring it would leave the CI jobs with no + # meaningful task/device test coverage. + /isaac-sim/python.sh -m pytest $test_path $pytest_options -v --junitxml=tests/$result_file "; then echo "✅ Docker container completed successfully" else - echo "⚠️ Docker container failed, but continuing to copy results..." + test_status=$? + echo "❌ Docker container failed; preserving reports before failing this action." fi # Copy test results with error handling @@ -151,7 +164,8 @@ runs: # Clean up container echo "🧹 Cleaning up Docker container..." docker rm $container_name 2>/dev/null || echo "⚠️ Container cleanup failed, but continuing..." + return ${test_status:-0} } # Call the function with provided parameters - run_tests "${{ inputs.test-path }}" "${{ inputs.result-file }}" "${{ inputs.container-name }}" "${{ inputs.image-tag }}" "${{ inputs.reports-dir }}" "${{ inputs.pytest-options }}" "${{ inputs.filter-pattern }}" + run_tests "${{ inputs.test-path }}" "${{ inputs.result-file }}" "${{ inputs.container-name }}" "${{ inputs.image-tag }}" "${{ inputs.reports-dir }}" "${{ inputs.pytest-options }}" "${{ inputs.filter-pattern }}" "${{ inputs.install-isaacteleop-pr769 }}" diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index cbaa8f7b8e99..f7ff1b093f77 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -33,7 +33,6 @@ jobs: test-isaaclab-tasks: runs-on: [self-hosted, gpu] timeout-minutes: 180 - continue-on-error: true steps: - name: Checkout Code @@ -118,6 +117,7 @@ jobs: image-tag: ${{ env.DOCKER_IMAGE_TAG }} pytest-options: "" filter-pattern: "not isaaclab_tasks" + install-isaacteleop-pr769: true - name: Copy Test Results from General Tests Container run: | diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index b637aec71792..02d07d3a027c 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -60,6 +60,7 @@ Guidelines for modifications: * Cathy Y. Li * Cheng-Rong Lai * Chenyu Yang +* Chris von Csefalvay * Connor Smith * CY (Chien-Ying) Chen * David Yang diff --git a/docs/source/api/lab/isaaclab.devices.rst b/docs/source/api/lab/isaaclab.devices.rst index 5f04c4733cf6..3e60970f8bab 100644 --- a/docs/source/api/lab/isaaclab.devices.rst +++ b/docs/source/api/lab/isaaclab.devices.rst @@ -17,7 +17,12 @@ Se3SpaceMouse HaplyDevice OpenXRDevice + DVRKOpenXRDevice + DVRKOpenXRDeviceCfg ManusVive + isaaclab.devices.openxr.retargeters.DVRKPSMRetargeter + isaaclab.devices.openxr.retargeters.DVRKPSMRetargeterCfg + isaaclab.devices.openxr.retargeters.DVRKPSMSideRetargeterCfg isaaclab.devices.openxr.retargeters.GripperRetargeter isaaclab.devices.openxr.retargeters.Se3AbsRetargeter isaaclab.devices.openxr.retargeters.Se3RelRetargeter @@ -104,6 +109,20 @@ OpenXR :show-inheritance: :noindex: +dVRK OpenXR +----------- + +.. autoclass:: DVRKOpenXRDevice + :members: + :inherited-members: + :show-inheritance: + :noindex: + +.. autoclass:: DVRKOpenXRDeviceCfg + :members: + :show-inheritance: + :noindex: + Manus + Vive ------------ @@ -116,6 +135,22 @@ Manus + Vive Retargeters ----------- +.. autoclass:: isaaclab.devices.openxr.retargeters.DVRKPSMRetargeter + :members: + :inherited-members: + :show-inheritance: + :noindex: + +.. autoclass:: isaaclab.devices.openxr.retargeters.DVRKPSMRetargeterCfg + :members: + :show-inheritance: + :noindex: + +.. autoclass:: isaaclab.devices.openxr.retargeters.DVRKPSMSideRetargeterCfg + :members: + :show-inheritance: + :noindex: + .. autoclass:: isaaclab.devices.openxr.retargeters.GripperRetargeter :members: :inherited-members: diff --git a/docs/source/how-to/cloudxr_teleoperation.rst b/docs/source/how-to/cloudxr_teleoperation.rst index e13b76305dcb..12feee176afa 100644 --- a/docs/source/how-to/cloudxr_teleoperation.rst +++ b/docs/source/how-to/cloudxr_teleoperation.rst @@ -721,6 +721,72 @@ Isaac Lab provides three main retargeters for hand tracking: Retargeters can be combined to control different robot functions simultaneously. +dVRK bimanual PSM retargeting +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +The :class:`isaaclab.devices.DVRKOpenXRDevice` and +:class:`isaaclab.devices.openxr.retargeters.DVRKPSMRetargeter` provide a reusable +motion-controller interface for two da Vinci Research Kit (dVRK) Patient Side +Manipulators (PSMs). Isaac Lab remains the owner of the OpenXR session and raw +controller source. The retargeter wraps the simulator-independent Cartesian-clutch +and jaw-intent state machines supplied by the ``isaacteleop`` package; it does not +construct a second Isaac Teleop retargeting graph. + +The ``isaacteleop`` installation must expose these public classes from +``isaacteleop.retargeters.DVRK.control``: + +* ``DVRKPSMCartesianClutchConfig`` and ``DVRKPSMCartesianClutchStateMachine`` +* ``DVRKPSMJawIntentConfig`` and ``DVRKPSMJawIntentStateMachine`` + +Isaac Lab imports them only when the dVRK retargeter is constructed, so task +discovery still works when the optional runtime is unavailable. Construction +raises a targeted installation error if the required classes are missing. + +The controller mapping is fixed: + +* the left controller commands the left PSM and the right controller commands the right PSM; +* the grip pose commands the corresponding tool-tip pose; +* squeeze is the clutch and deadman control; +* the index trigger commands paired-jaw intent; and +* thumbsticks and face buttons are not consumed. + +Each controller must provide a valid grip position and orientation. Missing, +malformed, non-finite, or partially valid data holds that side's complete command +while the other side continues independently; inspect ``controller_faults`` on +the OpenXR device before continuing a bimanual procedure. Unexpected XR runtime +errors propagate instead of being silently treated as tracking loss. Isaac Lab converts the OpenXR +scalar-first ``wxyz`` quaternion to Isaac Teleop's scalar-last ``xyzw`` convention. +The returned contiguous ``float32`` action has 18 values in this stable 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] + +Configure each :class:`isaaclab.devices.openxr.retargeters.DVRKPSMSideRetargeterCfg` +with an explicit world-frame home pose, workspace bounds, orientation calibration, +and measured open and closed jaw endpoints. The controller stream, home, and bounds +share the world/XR command frame. A bimanual task must convert each world-frame +tool target through that PSM's current root transform immediately before its IK +solve; the two independently placed PSMs must not share one cached root transform. + +``initial_closedness`` is a per-side reset target. The dVRK needle-pass task deliberately +uses asymmetric values: the donor restores its load-qualified closed grasp around the +needle, while the receiver restores fully open. ``RESET`` returns the controller command +to the same donor-held and receiver-open state as the task articulation. + +Session lifecycle is explicit. ``START`` activates both state machines and the first +valid squeezed sample captures fresh controller origins without moving either tool. +``STOP`` deactivates and disengages the state machines while preserving the last +pose and jaw targets. ``RESET`` restores the configured home targets and fresh-origin +requirement without changing whether the session is active. Set +``teleoperation_active_default=False`` when an application requires the operator to +choose **Start Teleop** before inputs can change the target. + +Command hold is limited to this adapter's output. It does not disable actuator +drives, latch measured joint state, create a grasp constraint, or attach a manipulated +object. + Using Retargeters with Hand Tracking ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/docs/source/overview/imitation-learning/teleop_imitation.rst b/docs/source/overview/imitation-learning/teleop_imitation.rst index 14017e65b5da..184f17a56cf2 100644 --- a/docs/source/overview/imitation-learning/teleop_imitation.rst +++ b/docs/source/overview/imitation-learning/teleop_imitation.rst @@ -114,6 +114,65 @@ To collect demonstrations with teleoperation for the environment ``Isaac-Stack-C # step a: replay the collected dataset ./isaaclab.sh -p scripts/tools/replay_demos.py --task Isaac-Stack-Cube-Franka-IK-Rel-v0 --device cpu --dataset_file ./datasets/dataset.hdf5 +The bimanual dVRK needle-pass task uses the same stock scripts with its paired Quest motion controllers. +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``; the remote assets are not redistributed +with Isaac Lab. Each reset begins with the free dynamic needle physically held between the closed donor +jaws while the receiver jaws remain open. The operator therefore starts by preserving the donor hold, +then acquires the receiver grasp and releases the donor; the task does not begin with an ungrasped needle. +After configuring CloudXR as described in :ref:`cloudxr-teleoperation`, first check manual control with +the stock teleoperation entry point. Asset digest verification is an explicit +online preflight so task discovery and configuration remain offline-safe: + +.. code:: bash + + ./isaaclab.sh -p scripts/tools/preflight_dvrk_needle_pass_assets.py + + ./isaaclab.sh -p scripts/environments/teleoperation/teleop_se3_agent.py \ + --task Isaac-NeedlePass-dVRK-IK-Abs-v0 \ + --teleop_device motion_controllers \ + --device cuda:0 --xr + +Then record and replay one successful demonstration with: + +.. code:: bash + + # record one successful demonstration + ./isaaclab.sh -p scripts/tools/record_demos.py \ + --task Isaac-NeedlePass-dVRK-IK-Abs-v0 \ + --teleop_device motion_controllers \ + --dataset_file ./datasets/dvrk_needle_pass.hdf5 \ + --num_demos 1 --num_success_steps 10 --step_hz 30 --device cuda:0 --xr + + # replay the recorded demonstration + ./isaaclab.sh -p scripts/tools/replay_demos.py \ + --task Isaac-NeedlePass-dVRK-IK-Abs-v0 \ + --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; donor hold, co-hold, +receiver-only hold, and retained lift are advanced from filtered bilateral jaw +contact and simulated needle motion. The recorder therefore exports success +only after a contact-driven transfer and retained receiver lift, without a +grasp joint, attachment, or scripted object following. + +The dVRK needle-pass task requires CUDA PhysX. CPU is intentionally rejected: +the contact-qualified grasp and hand-off contracts are validated only on the +CUDA simulation path. + +To retain a reviewable CUDA-physics video of the one-environment runtime +verification trace, set an output directory before running its test: + +.. code-block:: bash + + ISAACLAB_DVRK_NEEDLE_PASS_VIDEO_DIR=$PWD/output/dvrk-needle-pass \ + ./isaaclab.sh -p -m pytest \ + source/isaaclab_tasks/test/test_dvrk_needle_pass_physics.py \ + -k native_grasp_generator_handoff_video -q + +The test fails if Gymnasium's ``RecordVideo`` wrapper does not write the MP4. + .. note:: 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..a920562576dc --- /dev/null +++ b/scripts/tools/install_isaacteleop_pr769_for_tests.sh @@ -0,0 +1,78 @@ +#!/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: this merge revision includes NVIDIA/IsaacTeleop PR 769. This +# PR is part of the dVRK Teleop Extended Universe and, once merged, this pin +# will be updated to align with the corresponding released IsaacTeleop version. +set -euo pipefail + +readonly ISAAC_TELEOP_REPOSITORY="https://github.com/NVIDIA/IsaacTeleop.git" +readonly ISAAC_TELEOP_PR769_MERGE_SHA="790d6cb4e948de377975c76ed1e9cbf5098e10fc" +readonly ISAACLAB_PYTHON="${ISAACLAB_PATH:?ISAACLAB_PATH must be set}/_isaac_sim/python.sh" +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_MERGE_SHA}}" +readonly SOURCE_DIR="${BUILD_ROOT}/source" +readonly CMAKE_BUILD_DIR="${BUILD_ROOT}/build" +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 update + DEBIAN_FRONTEND=noninteractive apt-get install --yes --no-install-recommends git libx11-dev +fi + +if ! command -v uv >/dev/null; then + "${ISAACLAB_PYTHON}" -m pip install uv +fi + +if [[ ! -f "${SOURCE_REVISION_FILE}" ]] \ + || [[ "$(<"${SOURCE_REVISION_FILE}")" != "${ISAAC_TELEOP_PR769_MERGE_SHA}" ]]; then + rm -rf "${SOURCE_DIR}" "${CMAKE_BUILD_DIR}" + mkdir -p "${BUILD_ROOT}" + if command -v git >/dev/null; then + 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_MERGE_SHA}" + git -C "${SOURCE_DIR}" checkout --quiet --detach FETCH_HEAD + test "$(git -C "${SOURCE_DIR}" rev-parse HEAD)" = "${ISAAC_TELEOP_PR769_MERGE_SHA}" + else + # Isaac Sim runtime images do not all include Git. GitHub's archive + # endpoint names the immutable commit directly, so this fallback pins + # the identical merge revision rather than installing a branch tip. + archive_path="$(mktemp "${BUILD_ROOT}/isaacteleop-${ISAAC_TELEOP_PR769_MERGE_SHA}.XXXXXX.tar.gz")" + curl --fail --location --retry 3 --retry-delay 2 \ + "https://github.com/NVIDIA/IsaacTeleop/archive/${ISAAC_TELEOP_PR769_MERGE_SHA}.tar.gz" \ + --output "${archive_path}" + mkdir -p "${SOURCE_DIR}" + tar --extract --gzip --file "${archive_path}" --strip-components=1 --directory "${SOURCE_DIR}" + rm -f "${archive_path}" + fi + printf '%s\n' "${ISAAC_TELEOP_PR769_MERGE_SHA}" > "${SOURCE_REVISION_FILE}" +fi + +test -f "${SOURCE_DIR}/CMakeLists.txt" +test "$(<"${SOURCE_REVISION_FILE}")" = "${ISAAC_TELEOP_PR769_MERGE_SHA}" + +cmake -S "${SOURCE_DIR}" -B "${CMAKE_BUILD_DIR}" \ + -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}" -c 'from isaacteleop.retargeters.DVRK.control import DVRKPSMCartesianClutchStateMachine' 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..cdc78602a433 --- /dev/null +++ b/scripts/tools/preflight_dvrk_needle_pass_assets.py @@ -0,0 +1,38 @@ +# 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/manager_based/manipulation/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/config/extension.toml b/source/isaaclab/config/extension.toml index cc88c6861801..c09c4c1c940b 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.54.4" +version = "0.54.5" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 92bf95b87281..807b6cbaf12f 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,16 @@ Changelog --------- +0.54.5 (2026-07-13) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab.devices.DVRKOpenXRDevice` and + :class:`~isaaclab.devices.openxr.retargeters.DVRKPSMRetargeter` for reusable, + lifecycle-aware bimanual dVRK PSM control from OpenXR motion controllers. + 0.54.4 (2026-06-01) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/devices/__init__.py b/source/isaaclab/isaaclab/devices/__init__.py index b2605d39ca16..e8112e1f1635 100644 --- a/source/isaaclab/isaaclab/devices/__init__.py +++ b/source/isaaclab/isaaclab/devices/__init__.py @@ -24,7 +24,7 @@ from .gamepad import Se2Gamepad, Se2GamepadCfg, Se3Gamepad, Se3GamepadCfg from .haply import HaplyDevice, HaplyDeviceCfg from .keyboard import Se2Keyboard, Se2KeyboardCfg, Se3Keyboard, Se3KeyboardCfg -from .openxr import ManusVive, ManusViveCfg, OpenXRDevice, OpenXRDeviceCfg +from .openxr import DVRKOpenXRDevice, DVRKOpenXRDeviceCfg, ManusVive, ManusViveCfg, OpenXRDevice, OpenXRDeviceCfg from .retargeter_base import RetargeterBase, RetargeterCfg from .spacemouse import Se2SpaceMouse, Se2SpaceMouseCfg, Se3SpaceMouse, Se3SpaceMouseCfg from .teleop_device_factory import create_teleop_device diff --git a/source/isaaclab/isaaclab/devices/device_base.py b/source/isaaclab/isaaclab/devices/device_base.py index a434bcc73cfd..c8a928e3d65c 100644 --- a/source/isaaclab/isaaclab/devices/device_base.py +++ b/source/isaaclab/isaaclab/devices/device_base.py @@ -11,6 +11,7 @@ from enum import Enum from typing import Any +import numpy as np import torch from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg @@ -157,4 +158,28 @@ class MotionControllerInputIndex(Enum): SQUEEZE = 3 BUTTON_0 = 4 BUTTON_1 = 5 + POSE_VALID = 6 + """Whether both the controller grip position and orientation are valid.""" + + # Compatibility alias for consumers that treated the final slot as padding. PADDING = 6 + + @classmethod + def motion_controller_sample_is_valid(cls, controller_data: Any) -> bool: + """Return whether a controller sample is complete, finite, and tracked.""" + + try: + sample = np.asarray(controller_data, dtype=np.float64) + except (OverflowError, TypeError, ValueError): + return False + expected_shape = (len(cls.MotionControllerDataRowIndex), len(cls.MotionControllerInputIndex)) + return bool( + sample.shape == expected_shape + and np.isfinite(sample).all() + and sample[ + cls.MotionControllerDataRowIndex.INPUTS.value, + cls.MotionControllerInputIndex.POSE_VALID.value, + ] + == 1.0 + and np.linalg.norm(sample[cls.MotionControllerDataRowIndex.POSE.value, 3:7]) > 1.0e-9 + ) diff --git a/source/isaaclab/isaaclab/devices/openxr/__init__.py b/source/isaaclab/isaaclab/devices/openxr/__init__.py index 030fdbdd00b5..7ab6a9d8f9a9 100644 --- a/source/isaaclab/isaaclab/devices/openxr/__init__.py +++ b/source/isaaclab/isaaclab/devices/openxr/__init__.py @@ -5,6 +5,7 @@ """Keyboard device for SE(2) and SE(3) control.""" +from .dvrk_openxr_device import DVRKOpenXRDevice, DVRKOpenXRDeviceCfg from .manus_vive import ManusVive, ManusViveCfg from .openxr_device import OpenXRDevice, OpenXRDeviceCfg from .xr_cfg import XrAnchorRotationMode, XrCfg, remove_camera_configs diff --git a/source/isaaclab/isaaclab/devices/openxr/dvrk_openxr_device.py b/source/isaaclab/isaaclab/devices/openxr/dvrk_openxr_device.py new file mode 100644 index 000000000000..01d5596385ec --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/dvrk_openxr_device.py @@ -0,0 +1,67 @@ +# 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 + +"""OpenXR motion-controller device with lifecycle forwarding for the stateful dVRK retargeter.""" + +from __future__ import annotations + +from dataclasses import dataclass + +import carb + +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.openxr.openxr_device import OpenXRDevice, OpenXRDeviceCfg +from isaaclab.devices.openxr.retargeters.manipulator.dvrk_psm_retargeter import DVRKPSMRetargeter +from isaaclab.devices.retargeter_base import RetargeterBase + + +class DVRKOpenXRDevice(OpenXRDevice): + """OpenXR motion-controller device that owns the dVRK retargeter's session lifecycle. + + Raw controller tracking, anchors, application callbacks, and XRCore resources remain + owned by :class:`OpenXRDevice`. This subclass only forwards START, STOP, + and RESET transitions to its single :class:`DVRKPSMRetargeter`. + """ + + def __init__( + self, + cfg: DVRKOpenXRDeviceCfg, + retargeters: list[RetargeterBase] | None = None, + ): + if retargeters is None or len(retargeters) != 1 or not isinstance(retargeters[0], DVRKPSMRetargeter): + raise ValueError("DVRKOpenXRDevice requires exactly one DVRKPSMRetargeter") + self._dvrk_retargeter = retargeters[0] + super().__init__(cfg=cfg, retargeters=retargeters) + if cfg.teleoperation_active_default: + self._dvrk_retargeter.start() + else: + self._dvrk_retargeter.stop() + + def reset(self) -> None: + """Reset dVRK target state once, then reset the base OpenXR state.""" + self._dvrk_retargeter.reset() + super().reset() + + def _on_teleop_command(self, event: carb.events.IEvent) -> None: + """Apply dVRK START/STOP transitions before base application callbacks.""" + message = event.payload["message"] + if "start" in message: + self._dvrk_retargeter.start() + elif "stop" in message: + self._dvrk_retargeter.stop() + + # The base RESET path invokes the application callback and then calls + # this class's reset() override, keeping the internal transition singular. + super()._on_teleop_command(event) + + +@dataclass +class DVRKOpenXRDeviceCfg(OpenXRDeviceCfg): + """Configuration for paired OpenXR motion controllers driving a bimanual dVRK retargeter.""" + + class_type: type[DeviceBase] = DVRKOpenXRDevice + + +__all__ = ["DVRKOpenXRDevice", "DVRKOpenXRDeviceCfg"] diff --git a/source/isaaclab/isaaclab/devices/openxr/openxr_device.py b/source/isaaclab/isaaclab/devices/openxr/openxr_device.py index 49f423fe8a01..57d7c345c8ce 100644 --- a/source/isaaclab/isaaclab/devices/openxr/openxr_device.py +++ b/source/isaaclab/isaaclab/devices/openxr/openxr_device.py @@ -67,6 +67,7 @@ class OpenXRDevice(DeviceBase): """ TELEOP_COMMAND_EVENT_TYPE = "teleop_command" + _EXPECTED_CONTROLLER_SAMPLE_ERRORS = (AttributeError, KeyError, TypeError, ValueError) def __init__( self, @@ -118,6 +119,13 @@ def __init__( # Button binding support self.__button_subscriptions: dict[str, dict] = {} + # A missing controller sample is safe only when it is observable. The + # dVRK retargeter holds the affected side, while callers can inspect + # this per-side state before continuing bimanual control. + self._controller_faults: dict[DeviceBase.TrackingTarget, str | None] = { + DeviceBase.TrackingTarget.CONTROLLER_LEFT: None, + DeviceBase.TrackingTarget.CONTROLLER_RIGHT: None, + } # Optional anchor synchronizer self._anchor_sync: XrAnchorSynchronizer | None = None @@ -235,6 +243,17 @@ def add_callback(self, key: str, func: Callable): """ self._additional_callbacks[key] = func + @property + def controller_faults(self) -> dict[DeviceBase.TrackingTarget, str | None]: + """Latest expected acquisition fault for each motion-controller side. + + ``None`` means the most recent sample was acquired successfully. + Unexpected runtime/API exceptions propagate instead of being + misrepresented as ordinary tracking loss. + """ + + return self._controller_faults.copy() + def _get_raw_data(self) -> Any: """Get the latest tracking data from the OpenXR runtime. @@ -263,19 +282,12 @@ def _get_raw_data(self) -> Any: data[DeviceBase.TrackingTarget.HEAD] = self._calculate_headpose() if RetargeterBase.Requirement.MOTION_CONTROLLER in self._required_features: - # Optionally include motion controller pose+inputs if devices are available - try: - left_dev = XRCore.get_singleton().get_input_device("/user/hand/left") - right_dev = XRCore.get_singleton().get_input_device("/user/hand/right") - left_ctrl = self._query_controller(left_dev) if left_dev is not None else np.array([]) - right_ctrl = self._query_controller(right_dev) if right_dev is not None else np.array([]) - if left_ctrl.size: - data[DeviceBase.TrackingTarget.CONTROLLER_LEFT] = left_ctrl - if right_ctrl.size: - data[DeviceBase.TrackingTarget.CONTROLLER_RIGHT] = right_ctrl - except Exception: - # Ignore controller data if XRCore/controller APIs are unavailable - pass + left_ctrl = self._get_controller_sample("/user/hand/left", DeviceBase.TrackingTarget.CONTROLLER_LEFT) + if left_ctrl is not None and left_ctrl.size: + data[DeviceBase.TrackingTarget.CONTROLLER_LEFT] = left_ctrl + right_ctrl = self._get_controller_sample("/user/hand/right", DeviceBase.TrackingTarget.CONTROLLER_RIGHT) + if right_ctrl is not None and right_ctrl.size: + data[DeviceBase.TrackingTarget.CONTROLLER_RIGHT] = right_ctrl return data @@ -283,6 +295,51 @@ def _get_raw_data(self) -> Any: Internal helpers. """ + def _get_controller_sample(self, input_path: str, target: DeviceBase.TrackingTarget) -> np.ndarray | None: + """Read one side, preserving independent operation without hiding faults.""" + + try: + input_device = XRCore.get_singleton().get_input_device(input_path) + if input_device is None: + self._record_controller_fault(target, input_path, "InputDeviceUnavailable: no input device") + return None + sample = self._query_controller(input_device) + except self._EXPECTED_CONTROLLER_SAMPLE_ERRORS as error: + fault = f"{type(error).__name__}: {error}" + self._record_controller_fault(target, input_path, fault) + return None + + expected_shape = (len(DeviceBase.MotionControllerDataRowIndex), len(DeviceBase.MotionControllerInputIndex)) + if sample.shape != expected_shape: + self._record_controller_fault( + target, input_path, f"MalformedSample: expected {expected_shape}, got {sample.shape}" + ) + return None + if not np.isfinite(sample).all(): + self._record_controller_fault( + target, input_path, "NonFiniteSample: controller data contains NaN or infinity" + ) + return None + if not DeviceBase.motion_controller_sample_is_valid(sample): + self._record_controller_fault( + target, input_path, "TrackingInvalid: grip pose flags or quaternion are invalid" + ) + return None + self._controller_faults[target] = None + return sample + + def _record_controller_fault( + self, + target: DeviceBase.TrackingTarget, + input_path: str, + fault: str, + ) -> None: + """Record and log a changed expected controller fault once.""" + + if self._controller_faults[target] != fault: + logger.warning("OpenXR controller %s unavailable: %s", input_path, fault) + self._controller_faults[target] = fault + def _calculate_joint_poses( self, hand_device: Any, previous_joint_poses: dict[str, np.ndarray] ) -> dict[str, np.ndarray]: @@ -429,12 +486,19 @@ def _query_controller(self, input_device) -> np.ndarray: """Query motion controller pose and inputs as a 2x7 array. Row 0 (POSE): [x, y, z, w, x, y, z] - Row 1 (INPUTS): [thumbstick_x, thumbstick_y, trigger, squeeze, button_0, button_1, padding] + Row 1 (INPUTS): [thumbstick_x, thumbstick_y, trigger, squeeze, button_0, button_1, pose_valid] + + The grip descriptor is required so stale pose values are never reported + as valid. Callers omit this controller sample if the descriptor API is + unavailable or malformed. """ if input_device is None: return np.array([]) - pose = input_device.get_virtual_world_pose() + pose_desc = input_device.get_virtual_world_pose_desc("grip") + pose = pose_desc.pose_matrix + required_pose_flags = XRPoseValidityFlags.POSITION_VALID | XRPoseValidityFlags.ORIENTATION_VALID + pose_valid = (pose_desc.validity_flags & required_pose_flags) == required_pose_flags position = pose.ExtractTranslation() quat = pose.ExtractRotationQuat() @@ -483,7 +547,7 @@ def _query_controller(self, input_device) -> np.ndarray: squeeze, button_0, button_1, - 0.0, + float(pose_valid), ] return np.array([pose_row, input_row], dtype=np.float32) diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py index 94ef9c0e4e5f..5133cf5861ca 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/__init__.py @@ -23,6 +23,7 @@ G1TriHandUpperBodyRetargeter, G1TriHandUpperBodyRetargeterCfg, ) +from .manipulator.dvrk_psm_retargeter import DVRKPSMRetargeter, DVRKPSMRetargeterCfg, DVRKPSMSideRetargeterCfg from .manipulator.gripper_retargeter import GripperRetargeter, GripperRetargeterCfg from .manipulator.se3_abs_retargeter import Se3AbsRetargeter, Se3AbsRetargeterCfg from .manipulator.se3_rel_retargeter import Se3RelRetargeter, Se3RelRetargeterCfg diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/__init__.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/__init__.py index 426b8ac1002c..1062bd8e03bf 100644 --- a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/__init__.py +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/__init__.py @@ -3,11 +3,12 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Franka manipulator retargeting module. +"""Manipulator retargeting module. -This module provides functionality for retargeting motion to Franka robots. +This module provides functionality for retargeting motion to manipulator robots. """ +from .dvrk_psm_retargeter import DVRKPSMRetargeter, DVRKPSMRetargeterCfg, DVRKPSMSideRetargeterCfg from .gripper_retargeter import GripperRetargeter, GripperRetargeterCfg from .se3_abs_retargeter import Se3AbsRetargeter, Se3AbsRetargeterCfg from .se3_rel_retargeter import Se3RelRetargeter, Se3RelRetargeterCfg diff --git a/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/dvrk_psm_retargeter.py b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/dvrk_psm_retargeter.py new file mode 100644 index 000000000000..5b148d41635e --- /dev/null +++ b/source/isaaclab/isaaclab/devices/openxr/retargeters/manipulator/dvrk_psm_retargeter.py @@ -0,0 +1,292 @@ +# 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 retargeting from OpenXR motion controllers. + +This adapter owns four simulator-independent Isaac Teleop state machines: one +Cartesian clutch and one paired-jaw intent machine for each PSM. It emits a +stable 18-dimensional command in left-pose, left-jaws, right-pose, right-jaws +order. A held command only freezes the target emitted by this adapter; it does +not disable downstream actuator drives or latch measured robot state. +""" + +from __future__ import annotations + +import time +from dataclasses import dataclass +from typing import Any + +import numpy as np +import torch + +from isaaclab.devices.device_base import DeviceBase +from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg + + +@dataclass(frozen=True) +class DVRKPSMSideRetargeterCfg: + """World-frame control configuration for one dVRK PSM.""" + + home_position: tuple[float, float, float] + """World-frame tool-tip home position.""" + + home_orientation: tuple[float, float, float, float] + """World-frame tool-tip home orientation as a scalar-last ``xyzw`` quaternion.""" + + workspace_lower: tuple[float, float, float] + """Inclusive world-frame lower workspace bound.""" + + workspace_upper: tuple[float, float, float] + """Inclusive world-frame upper workspace bound.""" + + jaw_open: tuple[float, float] + """Ordered open targets for gripper joints one and two, in radians.""" + + jaw_closed: tuple[float, float] + """Ordered closed targets for gripper joints one and two, in radians.""" + + translation_scale: float = 1.0 + orientation_offset: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0) + initial_closedness: float = 0.0 + """Exact per-side jaw closedness restored by construction and RESET.""" + + clutch_threshold: float = 0.5 + trigger_deadband: float = 0.05 + opening_intent_duration_s: float = 0.1 + + +@dataclass +class _SideKernels: + pose: Any + jaw: Any + + +class DVRKPSMRetargeter(RetargeterBase): + """Retarget two OpenXR controllers to an ordered 18-D dVRK PSM command. + + The left and right sides are validated and advanced independently. Tracking + loss, malformed input, or clutch release atomically holds that side's full + nine-dimensional pose-and-jaw command. Session lifecycle is explicit: + :meth:`start` enables fresh controller-origin capture, :meth:`stop` holds + and disengages all kernels, and :meth:`reset` restores configured home + targets without changing session activity. + """ + + _POSE_LENGTH = 7 + _JAW_LENGTH = 2 + _CONTROLLER_SHAPE = (2, 7) + _MIN_QUATERNION_NORM = 1.0e-6 + + def __init__(self, cfg: DVRKPSMRetargeterCfg): + """Initialise the adapter and lazily load the Isaac Teleop kernels.""" + super().__init__(cfg) + if cfg.left is None or cfg.right is None: + raise ValueError("DVRKPSMRetargeterCfg requires explicit left and right side configurations") + + try: + from isaacteleop.retargeters.DVRK.control import ( + DVRKPSMCartesianClutchConfig, + DVRKPSMCartesianClutchStateMachine, + DVRKPSMJawIntentConfig, + DVRKPSMJawIntentStateMachine, + ) + except ImportError as error: + raise ModuleNotFoundError( + "DVRKPSMRetargeter requires an Isaac Teleop release containing the dVRK control kernels from " + "NVIDIA/IsaacTeleop PR 769" + ) from error + + def build_side(side_cfg: DVRKPSMSideRetargeterCfg) -> _SideKernels: + pose = DVRKPSMCartesianClutchStateMachine( + DVRKPSMCartesianClutchConfig( + home_position=side_cfg.home_position, + home_orientation=side_cfg.home_orientation, + workspace_lower=side_cfg.workspace_lower, + workspace_upper=side_cfg.workspace_upper, + translation_scale=side_cfg.translation_scale, + orientation_offset=side_cfg.orientation_offset, + clutch_threshold=side_cfg.clutch_threshold, + ) + ) + jaw = DVRKPSMJawIntentStateMachine( + DVRKPSMJawIntentConfig( + jaw_open=side_cfg.jaw_open, + jaw_closed=side_cfg.jaw_closed, + initial_closedness=side_cfg.initial_closedness, + clutch_threshold=side_cfg.clutch_threshold, + trigger_deadband=side_cfg.trigger_deadband, + opening_intent_duration_s=side_cfg.opening_intent_duration_s, + ) + ) + return _SideKernels(pose=pose, jaw=jaw) + + self._left = build_side(cfg.left) + self._right = build_side(cfg.right) + self._session_active = False + self._last_jaw_time_ns: dict[str, int | None] = {"left": None, "right": None} + self._left_pose = self._left.pose.reset() + self._left_jaws = self._left.jaw.reset() + self._right_pose = self._right.pose.reset() + self._right_jaws = self._right.jaw.reset() + + @property + def session_active(self) -> bool: + """Whether controller samples may currently change command targets.""" + return self._session_active + + @property + def action(self) -> torch.Tensor: + """Return the currently held contiguous 18-D command.""" + return self._as_tensor() + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + """Request only motion-controller data from the OpenXR device.""" + return [RetargeterBase.Requirement.MOTION_CONTROLLER] + + def start(self) -> None: + """Activate the session; the next squeezed sample captures a fresh origin.""" + self._session_active = True + + def stop(self) -> None: + """Deactivate the session, disengage every kernel, and hold its command.""" + self._session_active = False + self._left_pose = self._inactive_pose_step(self._left) + self._left_jaws = self._inactive_jaw_step(self._left) + self._right_pose = self._inactive_pose_step(self._right) + self._right_jaws = self._inactive_jaw_step(self._right) + self._clear_jaw_clocks() + + def reset(self) -> None: + """Restore configured home targets while preserving session activity.""" + self._left_pose = self._left.pose.reset() + self._left_jaws = self._left.jaw.reset() + self._right_pose = self._right.pose.reset() + self._right_jaws = self._right.jaw.reset() + self._clear_jaw_clocks() + + def retarget(self, data: Any) -> torch.Tensor: + """Advance both independent PSM sides and return the ordered 18-D command.""" + now_ns = time.monotonic_ns() + self._left_pose, self._left_jaws = self._retarget_side( + data, + DeviceBase.TrackingTarget.CONTROLLER_LEFT, + self._left, + self._elapsed_seconds("left", now_ns), + ) + self._right_pose, self._right_jaws = self._retarget_side( + data, + DeviceBase.TrackingTarget.CONTROLLER_RIGHT, + self._right, + self._elapsed_seconds("right", now_ns), + ) + return self._as_tensor() + + def _retarget_side( + self, + data: Any, + target: DeviceBase.TrackingTarget, + kernels: _SideKernels, + dt_seconds: float, + ) -> tuple[np.ndarray, np.ndarray]: + valid, position, orientation, trigger, squeeze = self._parse_controller(data, target) + pose = kernels.pose.step( + controller_position=position if valid else None, + controller_orientation=orientation if valid else None, + squeeze=squeeze if valid else None, + tracking_valid=valid, + session_active=self._session_active, + ) + jaws = kernels.jaw.step( + trigger=trigger if valid else None, + squeeze=squeeze if valid else None, + tracking_valid=valid, + session_active=self._session_active, + dt_seconds=dt_seconds, + ) + return pose, jaws + + @classmethod + def _parse_controller( + cls, data: Any, target: DeviceBase.TrackingTarget + ) -> tuple[bool, np.ndarray | None, np.ndarray | None, float | None, float | None]: + try: + controller = np.asarray(data[target], dtype=np.float64) + except (KeyError, OverflowError, TypeError, ValueError): + return False, None, None, None, None + if controller.shape != cls._CONTROLLER_SHAPE: + return False, None, None, None, None + + pose = controller[DeviceBase.MotionControllerDataRowIndex.POSE.value] + inputs = controller[DeviceBase.MotionControllerDataRowIndex.INPUTS.value] + trigger = inputs[DeviceBase.MotionControllerInputIndex.TRIGGER.value] + squeeze = inputs[DeviceBase.MotionControllerInputIndex.SQUEEZE.value] + pose_valid = inputs[DeviceBase.MotionControllerInputIndex.POSE_VALID.value] + consumed = np.concatenate((pose, (trigger, squeeze, pose_valid))) + if not np.all(np.isfinite(consumed)) or pose_valid != 1.0: + return False, None, None, None, None + + # IsaacLab receives scalar-first wxyz; Isaac Teleop consumes scalar-last xyzw. + orientation = np.asarray((pose[4], pose[5], pose[6], pose[3]), dtype=np.float64) + scale = float(np.max(np.abs(orientation))) + if scale == 0.0: + return False, None, None, None, None + scaled_norm = float(np.linalg.norm(orientation / scale)) + if not np.isfinite(scaled_norm) or scale < cls._MIN_QUATERNION_NORM / scaled_norm: + return False, None, None, None, None + orientation /= scale * scaled_norm + + return True, pose[:3].copy(), orientation, float(trigger), float(squeeze) + + @staticmethod + def _inactive_pose_step(kernels: _SideKernels) -> np.ndarray: + return kernels.pose.step( + controller_position=None, + controller_orientation=None, + squeeze=None, + tracking_valid=False, + session_active=False, + ) + + @staticmethod + def _inactive_jaw_step(kernels: _SideKernels) -> np.ndarray: + return kernels.jaw.step( + trigger=None, + squeeze=None, + tracking_valid=False, + session_active=False, + dt_seconds=0.0, + ) + + def _elapsed_seconds(self, side: str, now_ns: int) -> float: + previous_ns = self._last_jaw_time_ns[side] + if previous_ns is None: + self._last_jaw_time_ns[side] = now_ns + return 0.0 + if now_ns <= previous_ns: + return 0.0 + self._last_jaw_time_ns[side] = now_ns + return (now_ns - previous_ns) / 1_000_000_000.0 + + def _clear_jaw_clocks(self) -> None: + self._last_jaw_time_ns["left"] = None + self._last_jaw_time_ns["right"] = None + + def _as_tensor(self) -> torch.Tensor: + action = np.concatenate((self._left_pose, self._left_jaws, self._right_pose, self._right_jaws)) + if action.shape != (18,) or not np.all(np.isfinite(action)): + raise RuntimeError("dVRK retargeting kernels emitted an invalid command") + return torch.tensor(action, dtype=torch.float32, device=self._sim_device).contiguous() + + +@dataclass +class DVRKPSMRetargeterCfg(RetargeterCfg): + """Configuration for the bimanual dVRK PSM retargeter.""" + + left: DVRKPSMSideRetargeterCfg | None = None + right: DVRKPSMSideRetargeterCfg | None = None + retargeter_type: type[RetargeterBase] = DVRKPSMRetargeter + + +__all__ = ["DVRKPSMRetargeter", "DVRKPSMRetargeterCfg", "DVRKPSMSideRetargeterCfg"] diff --git a/source/isaaclab/test/devices/test_dvrk_openxr_device.py b/source/isaaclab/test/devices/test_dvrk_openxr_device.py new file mode 100644 index 000000000000..1387abf7aa73 --- /dev/null +++ b/source/isaaclab/test/devices/test_dvrk_openxr_device.py @@ -0,0 +1,174 @@ +# 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 + +from __future__ import annotations + +import builtins +import importlib +from types import SimpleNamespace + +from isaaclab.app import AppLauncher + +simulation_app = AppLauncher(headless=True).app + +import pytest + +pytestmark = pytest.mark.isaacsim_ci + +from isaaclab.devices import DVRKOpenXRDevice, DVRKOpenXRDeviceCfg, create_teleop_device +from isaaclab.devices.openxr.openxr_device import OpenXRDevice +from isaaclab.devices.openxr.retargeters import ( + DVRKPSMRetargeter, + DVRKPSMRetargeterCfg, + DVRKPSMSideRetargeterCfg, +) + + +def _retargeter_cfg() -> DVRKPSMRetargeterCfg: + side = DVRKPSMSideRetargeterCfg( + home_position=(0.0, 0.0, 0.2), + home_orientation=(0.0, 0.0, 0.0, 1.0), + workspace_lower=(-0.1, -0.1, 0.1), + workspace_upper=(0.1, 0.1, 0.3), + jaw_open=(-0.5, 0.5), + jaw_closed=(-0.09, 0.09), + ) + return DVRKPSMRetargeterCfg(left=side, right=side, sim_device="cpu") + + +@pytest.fixture +def retargeter() -> DVRKPSMRetargeter: + pytest.importorskip("isaacteleop.retargeters.DVRK.control") + return DVRKPSMRetargeter(_retargeter_cfg()) + + +@pytest.fixture +def patched_openxr_init(mocker): + def initialise(device, cfg, retargeters=None): + device._retargeters = retargeters or [] + device._required_features = set() + device._additional_callbacks = {} + + return mocker.patch.object(OpenXRDevice, "__init__", autospec=True, side_effect=initialise) + + +def test_configuration_selects_dvrk_device_class(): + assert DVRKOpenXRDeviceCfg().class_type is DVRKOpenXRDevice + + +def test_constructor_requires_exactly_one_dvrk_retargeter(patched_openxr_init): + with pytest.raises(ValueError, match="exactly one DVRKPSMRetargeter"): + DVRKOpenXRDevice(DVRKOpenXRDeviceCfg(), retargeters=[]) + + +def test_factory_constructs_dvrk_device_retargeter_and_callbacks(patched_openxr_init): + pytest.importorskip("isaacteleop.retargeters.DVRK.control") + + def callback(): + pass + + device = create_teleop_device( + "motion_controllers", + { + "motion_controllers": DVRKOpenXRDeviceCfg( + teleoperation_active_default=False, + retargeters=[_retargeter_cfg()], + ) + }, + callbacks={"RESET": callback}, + ) + + assert isinstance(device, DVRKOpenXRDevice) + assert len(device._retargeters) == 1 + assert isinstance(device._retargeters[0], DVRKPSMRetargeter) + assert device._retargeters[0].session_active is False + assert device._additional_callbacks["RESET"] is callback + + +@pytest.mark.parametrize("active_default", (False, True)) +def test_constructor_initialises_explicit_session_state(retargeter, patched_openxr_init, mocker, active_default: bool): + start = mocker.spy(retargeter, "start") + stop = mocker.spy(retargeter, "stop") + + DVRKOpenXRDevice( + DVRKOpenXRDeviceCfg(teleoperation_active_default=active_default), + retargeters=[retargeter], + ) + + assert retargeter.session_active is active_default + assert start.call_count == int(active_default) + assert stop.call_count == int(not active_default) + + +def test_start_stop_and_reset_preserve_application_callbacks_and_order(retargeter, patched_openxr_init, mocker): + events: list[str] = [] + mocker.patch.object(retargeter, "start", side_effect=lambda: events.append("internal start")) + mocker.patch.object(retargeter, "stop", side_effect=lambda: events.append("internal stop")) + mocker.patch.object(retargeter, "reset", side_effect=lambda: events.append("internal reset")) + mocker.patch.object(OpenXRDevice, "reset", autospec=True, side_effect=lambda _: events.append("openxr reset")) + device = DVRKOpenXRDevice( + DVRKOpenXRDeviceCfg(teleoperation_active_default=False), + retargeters=[retargeter], + ) + events.clear() # Ignore the constructor's explicit inactive transition. + device.add_callback("START", lambda: events.append("application start")) + device.add_callback("STOP", lambda: events.append("application stop")) + device.add_callback("RESET", lambda: events.append("application reset")) + + device._on_teleop_command(SimpleNamespace(payload={"message": "start"})) + assert events == ["internal start", "application start"] + + events.clear() + device._on_teleop_command(SimpleNamespace(payload={"message": "stop"})) + assert events == ["internal stop", "application stop"] + + events.clear() + device._on_teleop_command(SimpleNamespace(payload={"message": "reset"})) + assert events == ["application reset", "internal reset", "openxr reset"] + + +def test_direct_reset_forwards_once_before_openxr_reset(retargeter, patched_openxr_init, mocker): + events: list[str] = [] + mocker.patch.object(retargeter, "reset", side_effect=lambda: events.append("internal reset")) + mocker.patch.object(OpenXRDevice, "reset", autospec=True, side_effect=lambda _: events.append("openxr reset")) + device = DVRKOpenXRDevice(DVRKOpenXRDeviceCfg(), retargeters=[retargeter]) + + device.reset() + + assert events == ["internal reset", "openxr reset"] + + +def test_task_enumeration_without_isaacteleop_only_errors_when_factory_constructs_device(monkeypatch): + control_imports: list[str] = [] + real_import = builtins.__import__ + + def import_without_control(name, *args, **kwargs): + if name == "isaacteleop.retargeters.DVRK.control": + control_imports.append(name) + raise ImportError("dependency deliberately absent") + return real_import(name, *args, **kwargs) + + monkeypatch.setattr(builtins, "__import__", import_without_control) + tasks_package = importlib.import_module("isaaclab_tasks") + if getattr(tasks_package, "__file__", None) is None: + pytest.skip("isaaclab_tasks extension is unavailable in this simulator lane") + gym = importlib.import_module("gymnasium") + task_registration = importlib.import_module("isaaclab_tasks.manager_based.manipulation.needle_pass.config.dvrk") + if "Isaac-NeedlePass-dVRK-IK-Abs-v0" not in gym.registry: + importlib.reload(task_registration) + + assert tasks_package.__name__ == "isaaclab_tasks" + assert gym.spec("Isaac-NeedlePass-dVRK-IK-Abs-v0").entry_point == "isaaclab.envs:ManagerBasedRLEnv" + assert control_imports == [] + + device_configs = { + "motion_controllers": DVRKOpenXRDeviceCfg( + teleoperation_active_default=False, + retargeters=[_retargeter_cfg()], + ) + } + with pytest.raises(ModuleNotFoundError, match="Isaac Teleop.*PR 769"): + create_teleop_device("motion_controllers", device_configs) + assert control_imports == ["isaacteleop.retargeters.DVRK.control"] diff --git a/source/isaaclab/test/devices/test_dvrk_psm_retargeter.py b/source/isaaclab/test/devices/test_dvrk_psm_retargeter.py new file mode 100644 index 000000000000..3508b239087e --- /dev/null +++ b/source/isaaclab/test/devices/test_dvrk_psm_retargeter.py @@ -0,0 +1,379 @@ +# 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 + +# Ignore private usage of variables warning. +# pyright: reportPrivateUsage=none + +from __future__ import annotations + +import builtins + +from isaaclab.app import AppLauncher + +simulation_app = AppLauncher(headless=True).app + +import numpy as np +import pytest +import torch + +pytestmark = pytest.mark.isaacsim_ci + +from isaaclab.devices import DeviceBase, RetargeterBase +from isaaclab.devices.openxr.retargeters import ( + DVRKPSMRetargeter, + DVRKPSMRetargeterCfg, + DVRKPSMSideRetargeterCfg, +) + + +def _side_cfg( + *, + x_offset: float, + home_orientation: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0), + orientation_offset: tuple[float, float, float, float] = (0.0, 0.0, 0.0, 1.0), + initial_closedness: float = 0.0, +) -> DVRKPSMSideRetargeterCfg: + return DVRKPSMSideRetargeterCfg( + home_position=(x_offset, 0.0, 0.2), + home_orientation=home_orientation, + workspace_lower=(x_offset - 0.1, -0.1, 0.1), + workspace_upper=(x_offset + 0.1, 0.1, 0.3), + translation_scale=1.0, + orientation_offset=orientation_offset, + jaw_open=(-0.5, 0.5), + jaw_closed=(-0.09, 0.09), + initial_closedness=initial_closedness, + clutch_threshold=0.5, + trigger_deadband=0.05, + opening_intent_duration_s=0.1, + ) + + +def _cfg(*, sim_device: str = "cpu") -> DVRKPSMRetargeterCfg: + return DVRKPSMRetargeterCfg( + sim_device=sim_device, + left=_side_cfg(x_offset=-0.2), + right=_side_cfg(x_offset=0.2), + ) + + +def _controller( + position=(0.0, 0.0, 0.0), + orientation_wxyz=(1.0, 0.0, 0.0, 0.0), + *, + trigger=0.0, + squeeze=1.0, + pose_valid=1.0, +) -> np.ndarray: + pose = np.asarray((*position, *orientation_wxyz), dtype=np.float64) + inputs = np.asarray((0.0, 0.0, trigger, squeeze, 0.0, 0.0, pose_valid), dtype=np.float64) + return np.stack((pose, inputs)) + + +def _both(left: np.ndarray, right: np.ndarray) -> dict: + return { + DeviceBase.TrackingTarget.CONTROLLER_LEFT: left, + DeviceBase.TrackingTarget.CONTROLLER_RIGHT: right, + } + + +def _rotation(axis: tuple[float, float, float], angle: float) -> np.ndarray: + """Build a rotation matrix without using Isaac Teleop quaternion helpers.""" + unit_axis = np.asarray(axis, dtype=np.float64) + unit_axis /= np.linalg.norm(unit_axis) + x, y, z = unit_axis + skew = np.array(((0.0, -z, y), (z, 0.0, -x), (-y, x, 0.0))) + return np.eye(3) + np.sin(angle) * skew + (1.0 - np.cos(angle)) * (skew @ skew) + + +def _matrix_to_quaternion_wxyz(rotation: np.ndarray) -> tuple[float, float, float, float]: + """Convert a test-oracle matrix to wxyz using an eigenvector construction.""" + xx, xy, xz = rotation[0] + yx, yy, yz = rotation[1] + zx, zy, zz = rotation[2] + symmetric = ( + np.array( + ( + (xx - yy - zz, xy + yx, xz + zx, zy - yz), + (xy + yx, yy - xx - zz, yz + zy, xz - zx), + (xz + zx, yz + zy, zz - xx - yy, yx - xy), + (zy - yz, xz - zx, yx - xy, xx + yy + zz), + ) + ) + / 3.0 + ) + eigenvalues, eigenvectors = np.linalg.eigh(symmetric) + xyzw = eigenvectors[:, np.argmax(eigenvalues)] + if xyzw[3] < 0.0: + xyzw *= -1.0 + return float(xyzw[3]), float(xyzw[0]), float(xyzw[1]), float(xyzw[2]) + + +def _quaternion_xyzw_to_matrix(quaternion: torch.Tensor) -> np.ndarray: + """Convert adapter output to a matrix independently of its implementation.""" + x, y, z, w = quaternion.cpu().numpy().astype(np.float64) + norm = np.linalg.norm((x, y, z, w)) + x, y, z, w = np.asarray((x, y, z, w)) / norm + return np.array( + ( + (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)), + ) + ) + + +@pytest.fixture +def retargeter() -> DVRKPSMRetargeter: + pytest.importorskip("isaacteleop.retargeters.DVRK.control") + return DVRKPSMRetargeter(_cfg()) + + +def test_lazy_dependency_error_is_targeted(monkeypatch): + real_import = builtins.__import__ + + def import_with_missing_control(name, *args, **kwargs): + if name == "isaacteleop.retargeters.DVRK.control": + raise ImportError("missing test dependency") + return real_import(name, *args, **kwargs) + + monkeypatch.setattr(builtins, "__import__", import_with_missing_control) + with pytest.raises(ModuleNotFoundError, match="Isaac Teleop.*PR 769"): + DVRKPSMRetargeter(_cfg()) + + +def test_retargeter_requests_only_motion_controller_data(retargeter): + assert retargeter.get_requirements() == [RetargeterBase.Requirement.MOTION_CONTROLLER] + + +def test_inactive_session_holds_then_emits_stable_18d_abi(retargeter): + home = retargeter.action.clone() + pre_start = _both( + _controller(position=(4.0, 5.0, 6.0), trigger=1.0), + _controller(position=(-4.0, -5.0, -6.0), trigger=1.0), + ) + assert torch.equal(retargeter.retarget(pre_start), home) + + retargeter.start() + origins = _both(_controller(position=(0.3, 0.1, 0.4)), _controller(position=(-0.3, 0.1, 0.4))) + assert torch.equal(retargeter.retarget(origins), home) + + moved = _both( + _controller(position=(0.32, 0.09, 0.43), trigger=1.0), + _controller(position=(-0.3, 0.1, 0.4)), + ) + action = retargeter.retarget(moved) + + assert action.shape == (18,) + assert action.dtype == torch.float32 + assert action.device == torch.device("cpu") + assert action.is_contiguous() + assert torch.isfinite(action).all() + # [left xyz, left xyzw, left jaws, right xyz, right xyzw, right jaws] + torch.testing.assert_close(action[:3], torch.tensor((-0.18, -0.01, 0.23))) + torch.testing.assert_close(action[3:7], torch.tensor((0.0, 0.0, 0.0, 1.0))) + torch.testing.assert_close(action[7:9], torch.tensor((-0.09, 0.09))) + torch.testing.assert_close(action[9:12], torch.tensor((0.2, 0.0, 0.2))) + torch.testing.assert_close(action[12:16], torch.tensor((0.0, 0.0, 0.0, 1.0))) + torch.testing.assert_close(action[16:18], torch.tensor((-0.5, 0.5))) + + +def test_non_identity_wxyz_inputs_follow_independent_non_commuting_rotation_oracle(): + pytest.importorskip("isaacteleop.retargeters.DVRK.control") + home_rotation = _rotation((1.0, 1.0, 0.0), np.deg2rad(23.0)) + offset_rotation = _rotation((0.0, 1.0, 0.0), np.deg2rad(67.0)) + origin_rotation = _rotation((0.0, 0.0, 1.0), np.deg2rad(-31.0)) + relative_rotation = _rotation((1.0, 0.0, 0.0), np.deg2rad(41.0)) + current_rotation = origin_rotation @ relative_rotation + assert not np.allclose(offset_rotation @ relative_rotation, relative_rotation @ offset_rotation) + + cfg = DVRKPSMRetargeterCfg( + sim_device="cpu", + left=_side_cfg( + x_offset=-0.2, + home_orientation=_matrix_to_quaternion_wxyz(home_rotation)[1:] + + _matrix_to_quaternion_wxyz(home_rotation)[:1], + orientation_offset=_matrix_to_quaternion_wxyz(offset_rotation)[1:] + + _matrix_to_quaternion_wxyz(offset_rotation)[:1], + ), + right=_side_cfg(x_offset=0.2), + ) + retargeter = DVRKPSMRetargeter(cfg) + retargeter.start() + retargeter.retarget( + _both( + _controller(orientation_wxyz=_matrix_to_quaternion_wxyz(origin_rotation)), + _controller(), + ) + ) + action = retargeter.retarget( + _both( + _controller(orientation_wxyz=_matrix_to_quaternion_wxyz(current_rotation)), + _controller(), + ) + ) + + expected_rotation = home_rotation @ offset_rotation @ relative_rotation @ offset_rotation.T + np.testing.assert_allclose(_quaternion_xyzw_to_matrix(action[3:7]), expected_rotation, atol=2.0e-6) + np.testing.assert_allclose(_quaternion_xyzw_to_matrix(action[12:16]), np.eye(3), atol=2.0e-6) + + +def test_differently_placed_sides_clip_to_independent_world_bounds(retargeter): + retargeter.start() + origins = _both(_controller(), _controller()) + retargeter.retarget(origins) + + action = retargeter.retarget( + _both(_controller(position=(10.0, 10.0, 10.0)), _controller(position=(-10.0, -10.0, -10.0))) + ) + torch.testing.assert_close(action[:3], torch.tensor((-0.1, 0.1, 0.3))) + torch.testing.assert_close(action[9:12], torch.tensor((0.1, -0.1, 0.1))) + + +@pytest.mark.parametrize( + "bad_left", + ( + None, + np.zeros((1, 7)), + _controller(pose_valid=0.0), + _controller(orientation_wxyz=(0.0, 0.0, 0.0, 0.0)), + _controller(position=(np.nan, 0.0, 0.0)), + _controller(trigger=np.inf), + [[10**1000] * 7, [0.0] * 7], + ), +) +def test_invalid_left_holds_full_side_while_right_remains_independent(retargeter, bad_left): + retargeter.start() + origins = _both(_controller(), _controller()) + retargeter.retarget(origins) + previous = retargeter.retarget( + _both(_controller(position=(0.02, 0.0, 0.0), trigger=1.0), _controller(position=(0.01, 0.0, 0.0))) + ) + + data = {DeviceBase.TrackingTarget.CONTROLLER_RIGHT: _controller(position=(0.03, 0.0, 0.0))} + if bad_left is not None: + data[DeviceBase.TrackingTarget.CONTROLLER_LEFT] = bad_left + action = retargeter.retarget(data) + + assert torch.equal(action[:9], previous[:9]) + assert not torch.equal(action[9:12], previous[9:12]) + assert torch.isfinite(action).all() + + +@pytest.mark.parametrize("loss", ("squeeze", "tracking")) +def test_squeeze_release_and_tracking_loss_hold_then_capture_fresh_origin(retargeter, loss): + retargeter.start() + retargeter.retarget(_both(_controller(), _controller())) + moved = retargeter.retarget(_both(_controller(position=(0.02, 0.0, 0.0), trigger=1.0), _controller())) + torch.testing.assert_close(moved[:3], torch.tensor((-0.18, 0.0, 0.2))) + torch.testing.assert_close(moved[7:9], torch.tensor((-0.09, 0.09))) + + loss_sample = ( + _controller(position=(0.08, 0.0, 0.0), trigger=0.0, squeeze=0.0) + if loss == "squeeze" + else _controller(position=(0.08, 0.0, 0.0), trigger=0.0, pose_valid=0.0) + ) + held = retargeter.retarget(_both(loss_sample, _controller())) + assert torch.equal(held[:9], moved[:9]) + + recaptured = retargeter.retarget(_both(_controller(position=(0.08, 0.0, 0.0), trigger=0.0), _controller())) + assert torch.equal(recaptured[:9], moved[:9]) + after_fresh_origin = retargeter.retarget(_both(_controller(position=(0.09, 0.0, 0.0), trigger=0.0), _controller())) + torch.testing.assert_close(after_fresh_origin[:3], torch.tensor((-0.17, 0.0, 0.2))) + assert torch.equal(after_fresh_origin[7:9], moved[7:9]) + + +def test_deliberate_opening_waits_for_dwell_then_emits_configured_targets(mocker): + pytest.importorskip("isaacteleop.retargeters.DVRK.control") + cfg = DVRKPSMRetargeterCfg( + sim_device="cpu", + left=_side_cfg(x_offset=-0.2, initial_closedness=1.0), + right=_side_cfg(x_offset=0.2, initial_closedness=1.0), + ) + retargeter = DVRKPSMRetargeter(cfg) + mocker.patch( + "isaaclab.devices.openxr.retargeters.manipulator.dvrk_psm_retargeter.time.monotonic_ns", + side_effect=(0, 50_000_000, 100_000_000, 160_000_000), + ) + retargeter.start() + + closed = retargeter.retarget(_both(_controller(trigger=1.0), _controller(trigger=1.0))) + first_opening_sample = retargeter.retarget(_both(_controller(trigger=0.0), _controller(trigger=1.0))) + still_dwelling = retargeter.retarget(_both(_controller(trigger=0.0), _controller(trigger=1.0))) + opened = retargeter.retarget(_both(_controller(trigger=0.0), _controller(trigger=1.0))) + + expected_closed = torch.tensor((-0.09, 0.09)) + expected_open = torch.tensor((-0.5, 0.5)) + torch.testing.assert_close(closed[7:9], expected_closed) + torch.testing.assert_close(first_opening_sample[7:9], expected_closed) + torch.testing.assert_close(still_dwelling[7:9], expected_closed) + torch.testing.assert_close(opened[7:9], expected_open) + torch.testing.assert_close(opened[16:18], expected_closed) + + +def test_configured_non_cpu_tensor_device_is_honoured(): + pytest.importorskip("isaacteleop.retargeters.DVRK.control") + retargeter = DVRKPSMRetargeter(_cfg(sim_device="meta")) + + action = retargeter.action + + assert action.shape == (18,) + assert action.dtype == torch.float32 + assert action.device == torch.device("meta") + assert action.is_contiguous() + + +def test_stop_disengages_reset_preserves_activity_and_requires_fresh_origins(retargeter): + retargeter.start() + retargeter.retarget(_both(_controller(), _controller())) + moved = retargeter.retarget( + _both(_controller(position=(0.03, 0.0, 0.0), trigger=1.0), _controller(position=(-0.03, 0.0, 0.0))) + ) + + retargeter.stop() + assert not retargeter.session_active + assert torch.equal(retargeter.action, moved) + assert torch.equal( + retargeter.retarget(_both(_controller(position=(8.0, 8.0, 8.0)), _controller(position=(8.0, 8.0, 8.0)))), + moved, + ) + + retargeter.start() + assert torch.equal(retargeter.retarget(_both(_controller(), _controller())), moved) + retargeter.reset() + assert retargeter.session_active + reset_action = retargeter.action + torch.testing.assert_close(reset_action[:3], torch.tensor((-0.2, 0.0, 0.2))) + torch.testing.assert_close(reset_action[7:9], torch.tensor((-0.5, 0.5))) + torch.testing.assert_close(reset_action[9:12], torch.tensor((0.2, 0.0, 0.2))) + assert torch.equal( + retargeter.retarget(_both(_controller(position=(2.0, 2.0, 2.0)), _controller(position=(-2.0, -2.0, -2.0)))), + reset_action, + ) + + +def test_jaw_clock_uses_independent_high_water_marks_and_clears_on_lifecycle(retargeter, mocker): + left_step = mocker.spy(retargeter._left.jaw, "step") + right_step = mocker.spy(retargeter._right.jaw, "step") + clock = mocker.patch( + "isaaclab.devices.openxr.retargeters.manipulator.dvrk_psm_retargeter.time.monotonic_ns", + side_effect=(100, 100, 90, 150, 200, 250), + ) + sample = _both(_controller(), _controller()) + + for _ in range(4): + retargeter.retarget(sample) + assert [call.kwargs["dt_seconds"] for call in left_step.call_args_list] == [0.0, 0.0, 0.0, 50e-9] + assert [call.kwargs["dt_seconds"] for call in right_step.call_args_list] == [0.0, 0.0, 0.0, 50e-9] + + retargeter.reset() + retargeter.retarget(sample) + assert left_step.call_args.kwargs["dt_seconds"] == 0.0 + assert right_step.call_args.kwargs["dt_seconds"] == 0.0 + retargeter.stop() + retargeter.retarget(sample) + assert left_step.call_args.kwargs["dt_seconds"] == 0.0 + assert right_step.call_args.kwargs["dt_seconds"] == 0.0 + assert clock.call_count == 6 diff --git a/source/isaaclab/test/devices/test_oxr_device.py b/source/isaaclab/test/devices/test_oxr_device.py index 6402d8e0c187..78416c597a9e 100644 --- a/source/isaaclab/test/devices/test_oxr_device.py +++ b/source/isaaclab/test/devices/test_oxr_device.py @@ -18,6 +18,7 @@ simulation_app = app_launcher.app import importlib +from types import SimpleNamespace import numpy as np import pytest @@ -27,7 +28,7 @@ import omni.usd from isaacsim.core.prims import XFormPrim -from isaaclab.devices import OpenXRDevice, OpenXRDeviceCfg +from isaaclab.devices import DeviceBase, OpenXRDevice, OpenXRDeviceCfg from isaaclab.devices.openxr import XrCfg from isaaclab.devices.retargeter_base import RetargeterBase, RetargeterCfg from isaaclab.envs import ManagerBasedEnv, ManagerBasedEnvCfg @@ -53,6 +54,13 @@ def retarget(self, data): return torch.tensor([], device=self._sim_device) +class MotionControllerNoOpRetargeter(NoOpRetargeter): + """A no-op retargeter that requests only motion-controller data.""" + + def get_requirements(self) -> list[RetargeterBase.Requirement]: + return [RetargeterBase.Requirement.MOTION_CONTROLLER] + + @configclass class EmptyManagerCfg: """Empty manager.""" @@ -134,9 +142,19 @@ def get_input_device_mock(device_path): pose_matrix_mock.ExtractRotationQuat.return_value = rotation_quat_mock joint_pose_mock.pose_matrix = pose_matrix_mock + controller_pose_desc_mock = mocker.MagicMock() + controller_pose_desc_mock.validity_flags = ( + xr_pose_validity_flags_mock.POSITION_VALID | xr_pose_validity_flags_mock.ORIENTATION_VALID + ) + controller_pose_desc_mock.pose_matrix = pose_matrix_mock + joint_poses = {"palm": joint_pose_mock, "wrist": joint_pose_mock} left_hand_mock.get_all_virtual_world_poses.return_value = joint_poses right_hand_mock.get_all_virtual_world_poses.return_value = joint_poses + left_hand_mock.get_virtual_world_pose_desc.return_value = controller_pose_desc_mock + right_hand_mock.get_virtual_world_pose_desc.return_value = controller_pose_desc_mock + left_hand_mock.has_input_gesture.return_value = False + right_hand_mock.has_input_gesture.return_value = False head_mock.get_virtual_world_pose.return_value = pose_matrix_mock @@ -153,6 +171,7 @@ def get_input_device_mock(device_path): "left_hand": left_hand_mock, "right_hand": right_hand_mock, "head": head_mock, + "controller_pose_desc": controller_pose_desc_mock, } @@ -274,3 +293,103 @@ def test_get_raw_data(empty_env, mock_xrcore): assert len(head_pose) == 7 np.testing.assert_almost_equal(head_pose[:3], [0.1, 0.2, 0.3]) # Position np.testing.assert_almost_equal(head_pose[3:], [0.9, 0.1, 0.2, 0.3]) # Orientation + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize( + ("validity_flags", "expected_pose_valid"), + ( + (3, 1.0), + (1, 0.0), + (2, 0.0), + (0, 0.0), + ), +) +def test_motion_controller_grip_pose_validity(empty_env, mock_xrcore, validity_flags, expected_pose_valid): + """Controller validity requires both position and orientation flags.""" + env, _ = empty_env + mock_xrcore["controller_pose_desc"].validity_flags = validity_flags + device = OpenXRDevice(OpenXRDeviceCfg(), retargeters=[MotionControllerNoOpRetargeter(RetargeterCfg())]) + + controller = device._query_controller(mock_xrcore["left_hand"]) + + assert controller.shape == (2, 7) + assert controller.dtype == np.float32 + np.testing.assert_array_equal(controller[0], np.array([0.1, 0.2, 0.3, 0.9, 0.1, 0.2, 0.3], dtype=np.float32)) + assert controller[1, DeviceBase.MotionControllerInputIndex.POSE_VALID.value] == expected_pose_valid + mock_xrcore["left_hand"].get_virtual_world_pose_desc.assert_called_with("grip") + assert DeviceBase.MotionControllerInputIndex.PADDING is DeviceBase.MotionControllerInputIndex.POSE_VALID + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("invalid_sample", ("tracking", "degenerate", "nonfinite", "disconnected")) +def test_invalid_controller_sample_is_omitted_and_reported(empty_env, mock_xrcore, invalid_sample): + """Invalid tracking must not reach retargeters as an actionable sample.""" + + env, _ = empty_env + left = mock_xrcore["left_hand"] + if invalid_sample == "tracking": + left.get_virtual_world_pose_desc.return_value = SimpleNamespace( + validity_flags=1, + pose_matrix=mock_xrcore["controller_pose_desc"].pose_matrix, + ) + elif invalid_sample == "degenerate": + zero_quaternion = SimpleNamespace(GetReal=lambda: 0.0, GetImaginary=lambda: (0.0, 0.0, 0.0)) + left.get_virtual_world_pose_desc.return_value = SimpleNamespace( + validity_flags=3, + pose_matrix=SimpleNamespace( + ExtractTranslation=lambda: (0.1, 0.2, 0.3), + ExtractRotationQuat=lambda: zero_quaternion, + ), + ) + elif invalid_sample == "nonfinite": + left.get_input_gesture_value.return_value = float("nan") + left.has_input_gesture.side_effect = lambda gesture, component: (gesture, component) == ("trigger", "value") + else: + original_get_input_device = mock_xrcore["singleton"].get_input_device.side_effect + mock_xrcore["singleton"].get_input_device.side_effect = ( + lambda path: None if path == "/user/hand/left" else original_get_input_device(path) + ) + device = OpenXRDevice(OpenXRDeviceCfg(), retargeters=[MotionControllerNoOpRetargeter(RetargeterCfg())]) + + raw_data = device._get_raw_data() + + assert DeviceBase.TrackingTarget.CONTROLLER_LEFT not in raw_data + assert DeviceBase.TrackingTarget.CONTROLLER_RIGHT in raw_data + assert device.controller_faults[DeviceBase.TrackingTarget.CONTROLLER_LEFT] is not None + assert device.controller_faults[DeviceBase.TrackingTarget.CONTROLLER_RIGHT] is None + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("descriptor_failure", ("missing", "legacy_api")) +def test_missing_controller_descriptor_omits_only_that_side(empty_env, mock_xrcore, descriptor_failure): + """Unavailable descriptor APIs cannot turn a stale pose into a valid sample.""" + env, _ = empty_env + left = mock_xrcore["left_hand"] + if descriptor_failure == "missing": + left.get_virtual_world_pose_desc.return_value = None + else: + left.get_virtual_world_pose_desc.side_effect = AttributeError("legacy mock has only get_virtual_world_pose") + device = OpenXRDevice(OpenXRDeviceCfg(), retargeters=[MotionControllerNoOpRetargeter(RetargeterCfg())]) + + raw_data = device._get_raw_data() + + assert DeviceBase.TrackingTarget.CONTROLLER_LEFT not in raw_data + assert DeviceBase.TrackingTarget.CONTROLLER_RIGHT in raw_data + assert raw_data[DeviceBase.TrackingTarget.CONTROLLER_RIGHT].shape == (2, 7) + assert device.controller_faults[DeviceBase.TrackingTarget.CONTROLLER_LEFT].startswith( + ("AttributeError:", "TypeError:") + ) + assert device.controller_faults[DeviceBase.TrackingTarget.CONTROLLER_RIGHT] is None + + +@pytest.mark.isaacsim_ci +def test_unexpected_motion_controller_errors_propagate(empty_env, mock_xrcore): + """Do not silently convert an XR API regression into one-sided control.""" + + env, _ = empty_env + mock_xrcore["left_hand"].get_virtual_world_pose_desc.side_effect = RuntimeError("XR runtime failure") + device = OpenXRDevice(OpenXRDeviceCfg(), retargeters=[MotionControllerNoOpRetargeter(RetargeterCfg())]) + + with pytest.raises(RuntimeError, match="XR runtime failure"): + device._get_raw_data() diff --git a/source/isaaclab/test/test_ci_dispatcher.py b/source/isaaclab/test/test_ci_dispatcher.py new file mode 100644 index 000000000000..8dc9af71fc2b --- /dev/null +++ b/source/isaaclab/test/test_ci_dispatcher.py @@ -0,0 +1,32 @@ +# 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 + +"""Regression tests for the outer per-file CI dispatcher.""" + +import importlib.util +import sys +from pathlib import Path + + +def _load_dispatcher_module(): + tools_dir = Path(__file__).resolve().parents[3] / "tools" + sys.path.insert(0, str(tools_dir)) + try: + spec = importlib.util.spec_from_file_location("isaaclab_ci_dispatcher", tools_dir / "conftest.py") + assert spec is not None and spec.loader is not None + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + finally: + sys.path.remove(str(tools_dir)) + + +def test_dispatcher_timeout_is_a_failing_exit_status(): + """A timeout path recorded in failed_tests must fail the composite action.""" + + dispatcher = _load_dispatcher_module() + + assert dispatcher.dispatcher_return_code([]) == 0 + assert dispatcher.dispatcher_return_code(["timed_out_test.py"]) == 1 diff --git a/source/isaaclab_assets/config/extension.toml b/source/isaaclab_assets/config/extension.toml index d45724d97347..db44a6569c7f 100644 --- a/source/isaaclab_assets/config/extension.toml +++ b/source/isaaclab_assets/config/extension.toml @@ -1,6 +1,6 @@ [package] # Semantic Versioning is used: https://semver.org/ -version = "0.2.4" +version = "0.2.5" # Description title = "Isaac Lab Assets" diff --git a/source/isaaclab_assets/docs/CHANGELOG.rst b/source/isaaclab_assets/docs/CHANGELOG.rst index 3456213b3e8e..5fc12c1dd6fe 100644 --- a/source/isaaclab_assets/docs/CHANGELOG.rst +++ b/source/isaaclab_assets/docs/CHANGELOG.rst @@ -1,6 +1,14 @@ Changelog --------- +0.2.5 (2026-07-13) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added a configuration for the dVRK Patient Side Manipulator from the Isaac for Healthcare asset catalogue. + 0.2.4 (2025-11-26) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_assets/isaaclab_assets/robots/__init__.py b/source/isaaclab_assets/isaaclab_assets/robots/__init__.py index 77bcf04d0a3e..33e95f7f06f8 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/__init__.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/__init__.py @@ -3,6 +3,9 @@ # # SPDX-License-Identifier: BSD-3-Clause +# This module intentionally re-exports every robot configuration. +# ruff: noqa: F403 + ## # Configuration for different assets. ## @@ -15,6 +18,7 @@ from .cart_double_pendulum import * from .cartpole import * from .cassie import * +from .dvrk import * from .fourier import * from .franka import * from .galbot import * 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..c33489219d83 --- /dev/null +++ b/source/isaaclab_assets/isaaclab_assets/robots/dvrk.py @@ -0,0 +1,126 @@ +# 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 + +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 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, in :data:`DVRK_PSM_JAW_JOINT_NAMES` order.""" + +DVRK_PSM_JAW_CLOSED_POS = (0.0, 0.0) +"""Fully-closed jaw endpoint, 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=sim_utils.ArticulationRootPropertiesCfg( + 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.""" diff --git a/source/isaaclab_tasks/config/extension.toml b/source/isaaclab_tasks/config/extension.toml index 6b1140818ee3..65dfc90a86d6 100644 --- a/source/isaaclab_tasks/config/extension.toml +++ b/source/isaaclab_tasks/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.11.16" +version = "0.11.17" # Description title = "Isaac Lab Environments" diff --git a/source/isaaclab_tasks/docs/CHANGELOG.rst b/source/isaaclab_tasks/docs/CHANGELOG.rst index 8bb095bc372a..392228258a11 100644 --- a/source/isaaclab_tasks/docs/CHANGELOG.rst +++ b/source/isaaclab_tasks/docs/CHANGELOG.rst @@ -1,6 +1,16 @@ Changelog --------- +0.11.17 (2026-07-13) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added the ``Isaac-NeedlePass-dVRK-IK-Abs-v0`` manager-based environment with a stable 18-dimensional + bimanual action interface, an initially donor-held needle pose selected by Isaac Sim's native grasp generator, + and measured jaw-contact hand-off phases. + 0.11.16 (2026-04-21) ~~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/__init__.py new file mode 100644 index 000000000000..f09e482759c3 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/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/manager_based/manipulation/needle_pass/assets.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/assets.py new file mode 100644 index 000000000000..fcf9f80b7415 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/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 Isaac Sim 5.1 lane separately verifies the resolved PhysX 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/manager_based/manipulation/needle_pass/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/config/__init__.py new file mode 100644 index 000000000000..f1f5b5f9cc92 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/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/manager_based/manipulation/needle_pass/config/dvrk/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/config/dvrk/__init__.py new file mode 100644 index 000000000000..eaad46e16755 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/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="Isaac-NeedlePass-dVRK-IK-Abs-v0", + 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/manager_based/manipulation/needle_pass/config/dvrk/ik_abs_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/config/dvrk/ik_abs_env_cfg.py new file mode 100644 index 000000000000..bdfdd7eb9154 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/config/dvrk/ik_abs_env_cfg.py @@ -0,0 +1,455 @@ +# 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 + +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg, RigidObjectCfg +from isaaclab.controllers import DifferentialIKControllerCfg +from isaaclab.devices import DevicesCfg, DVRKOpenXRDeviceCfg +from isaaclab.devices.openxr.retargeters import DVRKPSMRetargeterCfg, DVRKPSMSideRetargeterCfg +from isaaclab.utils 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 + +LEFT_PSM_ROOT_POS = (-0.149189, -0.124189, 0.161400) +RIGHT_PSM_ROOT_POS = (0.149189, -0.124189, 0.161400) +PSM_ROOT_ROT_WXYZ = (1.0, 0.0, 0.0, 0.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 unmodified ``needle_channel_*`` (``T_N_C``) +# rows emitted by Isaac Sim 5.1's native antipodal grasp generator. ``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 manually permuted poses. 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_WXYZ = ( + 0.9499980187763187, + -0.06216530304406737, + -0.299876591345807, + 0.06093660132734946, +) +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_WXYZ = ( + 0.8479089570874903, + -0.01452245833926486, + 0.36639395824675103, + 0.38287722060063434, +) + +# 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_WXYZ = ( + 0.7071067811865476, + 0.0, + 0.7071067811865476, + 0.0, +) + +# 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_WXYZ = ( + 0.7632827686095777, + 0.04784599008904877, + 0.5946084191654624, + 0.24809474119226224, +) +NEEDLE_RESET_POS = (-0.024928808212280273, -0.0031707286834716797, 0.05836881697177887) +NEEDLE_RESET_ROT_WXYZ = ( + 0.7318665981292725, + 0.028613094240427017, + 0.6356675028800964, + 0.2438839077949524, +) +# 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_WXYZ = ( + 0.7496980735529877, + 0.0413061008969846, + 0.613496097694513, + 0.24468171703916028, +) +# 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_WXYZ = ( + 0.8453344702720642, + -0.25603383779525757, + 0.3513960838317871, + -0.31044724583625793, +) +RECEIVER_TOOL_TARGET_POS_W = (-0.02371715009212494, -0.008205749094486237, 0.052002355456352234) +RECEIVER_TOOL_TARGET_ROT_WXYZ = ( + 0.7730914950370789, + 0.4864428639411926, + 0.3236384689807892, + 0.2469031661748886, +) + +# 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_wxyz=RECEIVER_NEEDLE_TARGET_ROT_WXYZ, + 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_WXYZ, + joint_pos=_joint_home(arm_home, jaw_home), + joint_vel={".*": 0.0}, + ), + actuators={**DVRK_PSM_CFG.actuators, "jaws": DVRK_NEEDLE_PASS_JAW_ACTUATOR}, + ) + + +def _side_retargeter_cfg( + home_position: tuple[float, float, float], + home_orientation: tuple[float, float, float, float], + workspace_lower: tuple[float, float, float], + workspace_upper: tuple[float, float, float], + initial_closedness: float, +) -> DVRKPSMSideRetargeterCfg: + if not all(lower <= home <= upper for lower, home, upper in zip(workspace_lower, home_position, workspace_upper)): + raise ValueError("dVRK tool home must lie inside its world workspace") + return DVRKPSMSideRetargeterCfg( + home_position=home_position, + home_orientation=home_orientation, + workspace_lower=workspace_lower, + workspace_upper=workspace_upper, + translation_scale=1.0, + orientation_offset=(0.0, 0.0, 0.0, 1.0), + jaw_open=DVRK_PSM_JAW_OPEN_POS, + jaw_closed=DVRK_PSM_JAW_CLOSED_POS, + initial_closedness=initial_closedness, + clutch_threshold=0.5, + trigger_deadband=0.05, + opening_intent_duration_s=0.12, + ) + + +@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_WXYZ, + 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, + ) + + retargeter_cfg = DVRKPSMRetargeterCfg( + sim_device=self.sim.device, + left=_side_retargeter_cfg( + LEFT_TOOL_HOME_POS_W, + LEFT_TOOL_HOME_ROT_XYZW, + LEFT_WORKSPACE_LOWER, + LEFT_WORKSPACE_UPPER, + DONOR_GRASP_CLOSEDNESS, + ), + right=_side_retargeter_cfg( + RIGHT_TOOL_HOME_POS_W, + RIGHT_TOOL_HOME_ROT_XYZW, + RIGHT_WORKSPACE_LOWER, + RIGHT_WORKSPACE_UPPER, + 0.0, + ), + ) + self.teleop_devices = DevicesCfg( + devices={ + "motion_controllers": DVRKOpenXRDeviceCfg( + teleoperation_active_default=False, + sim_device=self.sim.device, + xr_cfg=self.xr, + retargeters=[retargeter_cfg], + ) + } + ) + + +__all__ = [ + "DVRKNeedlePassEnvCfg", + "DVRK_HANDOFF_PHASE_CFG", + "DVRK_JAW_CHANNEL_T_T_C_POS_M", + "DVRK_JAW_CHANNEL_T_T_C_ROT_WXYZ", + "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_WXYZ", + "DONOR_GRASP_OUTWARD_NORMALS_N", + "DONOR_GRASP_T_N_C_POS_M", + "DONOR_GRASP_T_N_C_ROT_WXYZ", + "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_WXYZ", + "PSM_ROOT_ROT_WXYZ", + "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_WXYZ", + "RECEIVER_ACQUISITION_NEEDLE_POS_W", + "RECEIVER_ACQUISITION_NEEDLE_ROT_WXYZ", + "RECEIVER_NEEDLE_TARGET_POS_T", + "RECEIVER_NEEDLE_TARGET_ROT_WXYZ", + "RECEIVER_TOOL_TARGET_POS_W", + "RECEIVER_TOOL_TARGET_ROT_WXYZ", +] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/mdp/__init__.py new file mode 100644 index 000000000000..173f58434199 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/mdp/__init__.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 + +"""MDP terms for the manager-based dVRK needle-pass environment.""" + +from isaaclab.envs.mdp import * # noqa: F401, F403 + +from .actions import * # noqa: F401, F403 +from .events import * # noqa: F401, F403 +from .grasp_solver import * # noqa: F401, F403 +from .observations import * # noqa: F401, F403 +from .rewards import * # noqa: F401, F403 +from .terminations import * # noqa: F401, F403 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/mdp/actions.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/mdp/actions.py new file mode 100644 index 000000000000..5b00d948bbe3 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/mdp/actions.py @@ -0,0 +1,262 @@ +# 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 import configclass + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedEnv + +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_wxyz( + pose_w_xyzw: torch.Tensor, + root_pos_w: torch.Tensor, + root_quat_w_wxyz: torch.Tensor, +) -> torch.Tensor: + """Convert absolute world poses from the public xyzw ABI to root-frame wxyz. + + 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_wxyz.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_quat_wxyz = torch.cat((target_quat_xyzw[:, 3:4], target_quat_xyzw[:, 0:3]), dim=-1) + target_pos_b, target_quat_b = math_utils.subtract_frame_transforms( + root_pos_w, + root_quat_w_wxyz, + pose_w_xyzw[:, :3], + target_quat_wxyz, + ) + 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 wxyz 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_wxyz( + self._processed_actions, + self._asset.data.root_pos_w, + self._asset.data.root_quat_w, + ) + 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[:, 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(joint_pos_des, 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(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: object | 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_wxyz", +] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/mdp/events.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/mdp/events.py new file mode 100644 index 000000000000..95cf3087bcaf --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/mdp/events.py @@ -0,0 +1,62 @@ +# 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[env_ids].clone() + joint_vel = torch.zeros_like(joint_pos) + psm.write_joint_state_to_sim(joint_pos, joint_vel, env_ids=env_ids) + psm.set_joint_position_target(joint_pos, env_ids=env_ids) + psm.set_joint_velocity_target(joint_vel, env_ids=env_ids) + + needle: RigidObject = env.scene[needle_cfg.name] + needle_state = needle.data.default_root_state[env_ids].clone() + needle_state[:, :3] += env.scene.env_origins[env_ids] + needle.write_root_pose_to_sim(needle_state[:, :7], env_ids=env_ids) + needle.write_root_velocity_to_sim(torch.zeros_like(needle_state[:, 7:13]), env_ids=env_ids) + reset_handoff_phase(env, env_ids, needle_state[:, 2], phase_cfg) + + +__all__ = ["reset_needle_pass_to_default"] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/mdp/grasp_solver.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/mdp/grasp_solver.py new file mode 100644 index 000000000000..a5f14c2c4f2c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/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/manager_based/manipulation/needle_pass/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/mdp/observations.py new file mode 100644 index 000000000000..365a2f934244 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/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 + + +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 + + +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, qw, qx, qy, qz]`` 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[:, body_id], asset.data.body_quat_w[:, body_id]), dim=-1) + + +def needle_pose_w( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("needle"), +) -> torch.Tensor: + """Return simulated needle pose ``[xyz, qw, qx, qy, qz]`` in world frame.""" + + needle: RigidObject = env.scene[asset_cfg.name] + return torch.cat((needle.data.root_pos_w, needle.data.root_quat_w), 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, needle.data.root_ang_vel_w), 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/manager_based/manipulation/needle_pass/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/mdp/rewards.py new file mode 100644 index 000000000000..966cdbbfdb1b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/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/manager_based/manipulation/needle_pass/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/mdp/terminations.py new file mode 100644 index 000000000000..424fdb46dff4 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/mdp/terminations.py @@ -0,0 +1,495 @@ +# 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 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.""" + + 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_wxyz: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.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_wxyz) + 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 wxyz 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_wxyz, + 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 = sensor.data.force_matrix_w + if force_matrix is None or force_matrix.shape != (env.num_envs, 1, 1, 3): + actual_shape = None if force_matrix is None else tuple(force_matrix.shape) + raise RuntimeError( + f"contact sensor {sensor_name!r} must expose a (num_envs, 1, 1, 3) needle force matrix; " + f"got {actual_shape}" + ) + if sensor.data.quat_w is None or sensor.data.quat_w.shape != (env.num_envs, 1, 4): + raise RuntimeError(f"contact sensor {sensor_name!r} must track its world pose") + 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.data.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[:, body_ids[0]], asset.data.body_quat_w[:, 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, needle.data.root_quat_w), dim=-1), + needle_velocity_w=torch.cat((needle.data.root_lin_vel_w, needle.data.root_ang_vel_w), 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] + dropped = needle.data.root_pos_w[:, 2] < machine.reset_needle_z_w - drop_distance_m + horizontal_offset = needle.data.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_state_w).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/manager_based/manipulation/needle_pass/needle_pass_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/needle_pass_env_cfg.py new file mode 100644 index 000000000000..46003f5ea3c0 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/needle_pass/needle_pass_env_cfg.py @@ -0,0 +1,399 @@ +# 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 pxr import Usd, UsdPhysics + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.devices.openxr import XrCfg +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.spawners.materials import RigidBodyMaterialCfg +from isaaclab.sim.utils import bind_physics_material, find_matching_prim_paths, get_current_stage +from isaaclab.utils import configclass + +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.""" + + +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, +): + """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: RigidBodyMaterialCfg = MISSING + + +@configclass +class NeedlePassSceneCfg(InteractiveSceneCfg): + """Two fixed PSMs, one free needle, and four filtered jaw sensors.""" + + left_psm: ArticulationCfg = MISSING + right_psm: ArticulationCfg = MISSING + + needle = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Needle", + spawn=UsdFileWithRigidMaterialCfg( + usd_path=NEEDLE_ASSET.url, + scale=NEEDLE_SCALE, + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + 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=sim_utils.CollisionPropertiesCfg(collision_enabled=True), + mass_props=sim_utils.MassPropertiesCfg(mass=NEEDLE_MASS_KG), + physics_material=sim_utils.RigidBodyMaterialCfg( + 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=(1.0, 0.0, 0.0, 0.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}/Needle"], + 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}/Needle"], + 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}/Needle"], + 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}/Needle"], + 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.physx.solver_type = 1 + self.sim.physx.solve_articulation_contact_last = True + self.sim.physx.enable_external_forces_every_iteration = True + self.sim.physx.enable_enhanced_determinism = True + self.sim.physx.min_position_iteration_count = 4 + self.sim.physx.min_velocity_iteration_count = 1 + self.sim.physx.bounce_threshold_velocity = 0.01 + self.sim.physx.friction_correlation_distance = 0.002 + # 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", + "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 0002c5d58d9a..b413735cf7c2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/utils/parse_cfg.py @@ -145,8 +145,24 @@ def parse_env_cfg( if isinstance(cfg, dict): raise RuntimeError(f"Configuration for the task: '{task_name}' is not a class. Please provide a class.") + # Some tasks deliberately rely on CUDA PhysX contact behaviour. Rejecting + # a CPU override here prevents their documented control and qualification + # workflows from silently running against an unsupported backend. + 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 + # Teleoperation configurations are commonly constructed from the default + # simulation device inside an environment configuration's ``__post_init__``. + # Keep their output tensors on the command-line-selected device after this + # late override, including every nested retargeter used by OpenXR devices. + teleop_devices = getattr(cfg, "teleop_devices", None) + if teleop_devices is not None: + for device_cfg in teleop_devices.devices.values(): + device_cfg.sim_device = device + for retargeter_cfg in getattr(device_cfg, "retargeters", None) or (): + retargeter_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/test_dvrk_needle_pass.py b/source/isaaclab_tasks/test/test_dvrk_needle_pass.py new file mode 100644 index 000000000000..b6f923d94aa8 --- /dev/null +++ b/source/isaaclab_tasks/test/test_dvrk_needle_pass.py @@ -0,0 +1,1034 @@ +# 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.manager_based.manipulation.needle_pass import assets +from isaaclab_tasks.manager_based.manipulation.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_WXYZ, + DONOR_GRASP_OUTWARD_NORMALS_N, + DONOR_GRASP_T_N_C_POS_M, + DONOR_GRASP_T_N_C_ROT_WXYZ, + DONOR_HELD_RESET_JAW_POS, + DVRK_HANDOFF_PHASE_CFG, + DVRK_JAW_CHANNEL_T_T_C_POS_M, + DVRK_JAW_CHANNEL_T_T_C_ROT_WXYZ, + 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_WXYZ, + RECEIVER_ACQUISITION_NEEDLE_POS_W, + RECEIVER_ACQUISITION_NEEDLE_ROT_WXYZ, + RECEIVER_GRASP_CANDIDATE_INDEX, + RECEIVER_GRASP_T_N_C_POS_M, + RECEIVER_GRASP_T_N_C_ROT_WXYZ, + RECEIVER_NEEDLE_TARGET_POS_T, + RECEIVER_NEEDLE_TARGET_ROT_WXYZ, + RECEIVER_TOOL_TARGET_POS_W, + RECEIVER_TOOL_TARGET_ROT_WXYZ, + RIGHT_TOOL_HOME_POS_W, + DVRKNeedlePassEnvCfg, +) +from isaaclab_tasks.manager_based.manipulation.needle_pass.mdp.actions import ( + DonorReleaseGuardedPairedJawJointPositionAction, + DonorReleaseGuardedPairedJawJointPositionActionCfg, + donor_opening_requested, + donor_release_is_allowed, + world_pose_xyzw_to_root_pose_wxyz, +) +from isaaclab_tasks.manager_based.manipulation.needle_pass.mdp.events import reset_needle_pass_to_default +from isaaclab_tasks.manager_based.manipulation.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.manager_based.manipulation.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.manager_based.manipulation.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, + UsdFileWithRigidMaterialCfg, +) +from isaaclab_tasks.utils import parse_env_cfg + +TASK_ID = "Isaac-NeedlePass-dVRK-IK-Abs-v0" + + +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 device key 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 list(cfg.teleop_devices.devices) == ["motion_controllers"] + + 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_wxyz == ( + RECEIVER_NEEDLE_TARGET_ROT_WXYZ + ) + 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_override_propagates_to_dvrk_teleoperation(): + """Keep the stock recorder's teleop action on the required CUDA device.""" + + device = "cuda:0" + cfg = parse_env_cfg(TASK_ID, device=device, num_envs=1) + device_cfg = cfg.teleop_devices.devices["motion_controllers"] + + assert cfg.sim.device == device + assert device_cfg.sim_device == device + assert len(device_cfg.retargeters) == 1 + assert device_cfg.retargeters[0].sim_device == device + + +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.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 == ["{ENV_REGEX_NS}/Needle"] + assert sensor_cfg.track_pose is True + assert cfg.sim.dt == pytest.approx(1.0 / 240.0) + assert cfg.decimation == 1 + assert cfg.sim.physx.solver_type == 1 + 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_wxyz(quaternion: np.ndarray) -> np.ndarray: + w, x, y, z = 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_wxyz = np.array( + ((1.0, 0.0, 0.0, 0.0), (np.sqrt(0.5), 0.0, 0.0, np.sqrt(0.5))), + dtype=np.float32, + ) + target_quat_wxyz = np.array( + ((np.sqrt(0.5), np.sqrt(0.5), 0.0, 0.0), (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_wxyz[:, (1, 2, 3, 0)]), axis=1) + actual = world_pose_xyzw_to_root_pose_wxyz( + torch.tensor(pose_xyzw), + torch.tensor(root_pos), + torch.tensor(root_quat_wxyz), + ).numpy() + + for index in range(2): + root_transform = np.eye(4) + root_transform[:3, :3] = _matrix_from_quat_wxyz(root_quat_wxyz[index]) + root_transform[:3, 3] = root_pos[index] + target_transform = np.eye(4) + target_transform[:3, :3] = _matrix_from_quat_wxyz(target_quat_wxyz[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_wxyz(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_wxyz): + transform = np.eye(4) + transform[:3, :3] = _matrix_from_quat_wxyz(np.asarray(quaternion_wxyz)) + 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_WXYZ, RECEIVER_GRASP_T_N_C_ROT_WXYZ): + 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) + retargeter_cfg = cfg.teleop_devices.devices["motion_controllers"].retargeters[0] + assert retargeter_cfg.left.initial_closedness == DONOR_GRASP_CLOSEDNESS + assert retargeter_cfg.right.initial_closedness == 0.0 + left_retargeter_reset = np.asarray(retargeter_cfg.left.jaw_open) + DONOR_GRASP_CLOSEDNESS * ( + np.asarray(retargeter_cfg.left.jaw_closed) - np.asarray(retargeter_cfg.left.jaw_open) + ) + np.testing.assert_allclose(left_retargeter_reset, DONOR_GRASP_JAW_POS, atol=1.0e-15) + 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.""" + + left_home_wxyz = ( + LEFT_TOOL_HOME_ROT_XYZW[3], + LEFT_TOOL_HOME_ROT_XYZW[0], + LEFT_TOOL_HOME_ROT_XYZW[1], + LEFT_TOOL_HOME_ROT_XYZW[2], + ) + transform_w_donor_tool = _transform_from_pose(LEFT_TOOL_HOME_POS_W, left_home_wxyz) + transform_tool_channel = _transform_from_pose( + DVRK_JAW_CHANNEL_T_T_C_POS_M, + DVRK_JAW_CHANNEL_T_T_C_ROT_WXYZ, + ) + transform_needle_donor_channel = _transform_from_pose( + DONOR_GRASP_T_N_C_POS_M, + DONOR_GRASP_T_N_C_ROT_WXYZ, + ) + transform_needle_receiver_channel = _transform_from_pose( + RECEIVER_GRASP_T_N_C_POS_M, + RECEIVER_GRASP_T_N_C_ROT_WXYZ, + ) + + 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_WXYZ) + 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_WXYZ) + 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_WXYZ, + ) + 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_WXYZ, + ) + 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_WXYZ, + ) + 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_wxyz(np.asarray(NEEDLE_RESET_ROT_WXYZ)).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[:, 3] = 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.manager_based.manipulation.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=torch.zeros((1, 3)), + root_quat_w=torch.tensor(((1.0, 0.0, 0.0, 0.0),)), + root_lin_vel_w=torch.zeros((1, 3)), + root_ang_vel_w=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, 1.0, 0.0, 0.0, 0.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.manager_based.manipulation.needle_pass.mdp import terminations + + class _Asset: + def __init__(self): + self.calls = [] + + def set_joint_position_target(self, command, joint_ids): + self.calls.append((command.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_wxyz=(np.cos(half_angle), 0.0, 0.0, np.sin(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, 1.0, 0.0, 0.0, 0.0),), dtype=torch.float32) + needle_pose = torch.tensor( + ((0.01, -0.02, 0.03, np.cos(half_angle), 0.0, 0.0, np.sin(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=default_joint_pos) + + def write_joint_state_to_sim(self, joint_pos, joint_vel, env_ids): + calls.append((self.name, "state")) + torch.testing.assert_close(joint_pos, self.expected_joint_pos) + torch.testing.assert_close(joint_vel, torch.zeros_like(joint_pos)) + + def set_joint_position_target(self, joint_pos, env_ids): + calls.append((self.name, "position_target")) + torch.testing.assert_close(joint_pos, self.expected_joint_pos) + + def set_joint_velocity_target(self, joint_vel, env_ids): + calls.append((self.name, "velocity_target")) + + class MockNeedle: + def __init__(self): + self.data = SimpleNamespace( + default_root_state=torch.tensor(((0.1, 0.2, 0.3, 1.0, 0.0, 0.0, 0.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0),)) + ) + + def write_root_pose_to_sim(self, pose, env_ids): + calls.append(("needle", "pose")) + torch.testing.assert_close(pose[:, :3], torch.tensor(((1.1, 2.2, 3.3),))) + + def write_root_velocity_to_sim(self, velocity, env_ids): + calls.append(("needle", "velocity")) + torch.testing.assert_close(velocity, torch.zeros_like(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", "state"), + ("left", "position_target"), + ("left", "velocity_target"), + ("right", "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)) + for name, normal, load in zip( + JAW_CONTACT_SENSOR_NAMES, + JAW_BODY_REACTION_NORMALS_LOCAL, + expected_loads, + strict=True, + ): + force = torch.tensor(normal, dtype=torch.float32) * load + sensors[name] = SimpleNamespace( + data=SimpleNamespace( + force_matrix_w=force.reshape(1, 1, 1, 3), + quat_w=torch.tensor((1.0, 0.0, 0.0, 0.0)).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], torch.tensor(JAW_BODY_REACTION_NORMALS_LOCAL)) + torch.testing.assert_close( + force_vectors[0], torch.tensor(JAW_BODY_REACTION_NORMALS_LOCAL) * expected_loads[:, None] + ) + + for sensor in sensors.values(): + sensor.data.force_matrix_w *= -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/test_dvrk_needle_pass_physics.py b/source/isaaclab_tasks/test/test_dvrk_needle_pass_physics.py new file mode 100644 index 000000000000..225f7670da1c --- /dev/null +++ b/source/isaaclab_tasks/test/test_dvrk_needle_pass_physics.py @@ -0,0 +1,1762 @@ +# 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 on + # headless Isaac Sim 5.1, 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 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.manager_based.manipulation.needle_pass import assets +from isaaclab_tasks.manager_based.manipulation.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_WXYZ, + ISAAC_GRASP_CANDIDATES_SHA256, + LEFT_TOOL_HOME_POS_W, + LEFT_TOOL_HOME_ROT_XYZW, + RECEIVER_TOOL_TARGET_POS_W, + RECEIVER_TOOL_TARGET_ROT_WXYZ, + RIGHT_TOOL_HOME_POS_W, + RIGHT_TOOL_HOME_ROT_XYZW, +) +from isaaclab_tasks.manager_based.manipulation.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 = "Isaac-NeedlePass-dVRK-IK-Abs-v0" +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_state_to_sim", + "write_root_com_state_to_sim", + "write_root_link_state_to_sim", + "write_root_pose_to_sim", + "write_root_link_pose_to_sim", + "write_root_com_pose_to_sim", + "write_root_velocity_to_sim", + "write_root_com_velocity_to_sim", + "write_root_link_velocity_to_sim", + "write_joint_state_to_sim", + "write_joint_position_to_sim", + "write_joint_velocity_to_sim", +) +_RIGID_OBJECT_STATE_WRITERS = ( + "write_root_state_to_sim", + "write_root_com_state_to_sim", + "write_root_link_state_to_sim", + "write_root_pose_to_sim", + "write_root_link_pose_to_sim", + "write_root_com_pose_to_sim", + "write_root_velocity_to_sim", + "write_root_com_velocity_to_sim", + "write_root_link_velocity_to_sim", +) +_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_state_to_sim"), + ("asset", "left_psm", "write_joint_position_to_sim"), + ("physx", "left_psm", "set_dof_positions"), + ("asset", "left_psm", "write_joint_velocity_to_sim"), + ("physx", "left_psm", "set_dof_velocities"), + ("asset", "right_psm", "write_joint_state_to_sim"), + ("asset", "right_psm", "write_joint_position_to_sim"), + ("physx", "right_psm", "set_dof_positions"), + ("asset", "right_psm", "write_joint_velocity_to_sim"), + ("physx", "right_psm", "set_dof_velocities"), + ("asset", "needle", "write_root_pose_to_sim"), + ("asset", "needle", "write_root_link_pose_to_sim"), + ("physx", "needle", "set_transforms"), + ("asset", "needle", "write_root_velocity_to_sim"), + ("asset", "needle", "write_root_com_velocity_to_sim"), + ("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, + 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_WXYZ[1], + RECEIVER_TOOL_TARGET_ROT_WXYZ[2], + RECEIVER_TOOL_TARGET_ROT_WXYZ[3], + RECEIVER_TOOL_TARGET_ROT_WXYZ[0], + ), + 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_WXYZ[1], + RECEIVER_TOOL_TARGET_ROT_WXYZ[2], + RECEIVER_TOOL_TARGET_ROT_WXYZ[3], + RECEIVER_TOOL_TARGET_ROT_WXYZ[0], + ), + 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 + needle_quat_w = needle.data.root_quat_w + 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_WXYZ, 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, torch.cat((tool_quat_w[:, 1:], tool_quat_w[:, :1]), dim=-1) + + +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_wxyz: np.ndarray) -> np.ndarray: + """Return a column-vector homogeneous transform from a PhysX body pose.""" + + w, x, y, z = quaternion_wxyz / np.linalg.norm(quaternion_wxyz) + 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_ns}/env_{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[env_index, body_id].detach().cpu().numpy(), + receiver.data.body_quat_w[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 for an opt-in CUDA receiver feasibility probe.""" + + 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)) + rows = {int(row["candidate_index"]): row for row in csv.DictReader(candidate_file.open(encoding="utf-8"))} + 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_qw"]), + float(rows[index]["needle_channel_qx"]), + float(rows[index]["needle_channel_qy"]), + float(rows[index]["needle_channel_qz"]), + ] + for index in indices + ], + dtype=torch.float32, + ) + 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[:, body_ids[0]], + receiver.data.body_quat_w[:, body_ids[0]], + env.scene["needle"].data.root_pos_w, + env.scene["needle"].data.root_quat_w, + ) + 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_wxyz, device=env.device + ).expand_as(needle_quat_r) + needle = env.scene["needle"] + receiver_pose_w = torch.cat( + (receiver.data.body_pos_w[:, body_ids[0]], receiver.data.body_quat_w[:, 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, needle.data.root_quat_w), dim=-1), + needle_velocity_w=torch.cat((needle.data.root_lin_vel_w, needle.data.root_ang_vel_w), 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[:, body_ids[0]].detach().cpu().tolist(), + "receiver_tool_quat_wxyz": receiver.data.body_quat_w[:, body_ids[0]].detach().cpu().tolist(), + "receiver_joint_pos": receiver.data.joint_pos.detach().cpu().tolist(), + "donor_joint_pos": env.scene["left_psm"].data.joint_pos.detach().cpu().tolist(), + "needle_pos_w": env.scene["needle"].data.root_pos_w.detach().cpu().tolist(), + "needle_quat_wxyz": env.scene["needle"].data.root_quat_w.detach().cpu().tolist(), + "needle_velocity_w": torch.cat( + (env.scene["needle"].data.root_lin_vel_w, env.scene["needle"].data.root_ang_vel_w), 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, + "left_joint_vel": env.scene["left_psm"].data.joint_vel, + "right_joint_pos": env.scene["right_psm"].data.joint_pos, + "right_joint_vel": env.scene["right_psm"].data.joint_vel, + "needle_root_state_w": env.scene["needle"].data.root_state_w, + } + ) + + +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_physx_view, + "physx", + asset_name, + _ARTICULATION_PHYSX_STATE_SETTERS, + ) + self._wrap_control_methods(asset.root_physx_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_physx_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 = f"{self._env.scene.env_ns}/env_0/Needle" + 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 PhysxSchema.PhysxPhysicsAttachment(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 _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 _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 = f"{env.scene.env_ns}/env_0/Needle" + needle_root = stage.GetPrimAtPath(needle_root_path) + assert needle_root.IsValid() + assert str(needle_root.GetParent().GetPath()) == f"{env.scene.env_ns}/env_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())) + + attachment = PhysxSchema.PhysxPhysicsAttachment(prim) + if attachment: + targets = ( + *_relationship_targets(attachment.GetActor0Rel()), + *_relationship_targets(attachment.GetActor1Rel()), + ) + 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_physx_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"] + coms = needle.root_physx_view.get_coms() + assert coms.shape == (needle.root_physx_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 = needle.root_physx_view.get_masses() + assert needle.root_physx_view.count == env.num_envs + assert masses.shape == (needle.root_physx_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 = needle.root_physx_view.get_material_properties() + assert live_materials.shape == ( + needle.root_physx_view.count, + needle.root_physx_view.max_shapes, + 3, + ) + assert needle.root_physx_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 = f"{env.scene.env_ns}/env_0/Needle" + 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_physx_view.filter_count == 1 + force_matrix_w = sensor.data.force_matrix_w + 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[:, 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_ns}/env_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_ns}/env_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[:, 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) + + x, y, z, w = orientation_xyzw + expected_orientation = torch.tensor((w, x, y, z), device=env.device).expand(env.num_envs, -1) + actual_orientation = torch.nn.functional.normalize( + articulation.data.body_quat_w[:, 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.clone() + initial_needle_orientation = env.scene["needle"].data.root_quat_w.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 + 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_wxyz": initial_needle_orientation.detach().cpu().tolist(), + "final_needle_orientation_wxyz": env.scene["needle"].data.root_quat_w.detach().cpu().tolist(), + "final_needle_velocity_w": torch.cat( + (env.scene["needle"].data.root_lin_vel_w, env.scene["needle"].data.root_ang_vel_w), 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[:, 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[:, body_ids[0]], + receiver.data.body_quat_w[:, body_ids[0]], + env.scene["needle"].data.root_pos_w, + env.scene["needle"].data.root_quat_w, + ) + post_release_velocity_w = torch.cat( + (env.scene["needle"].data.root_lin_vel_w, env.scene["needle"].data.root_ang_vel_w), 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[:, 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[:, 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[:, body_ids[0]] + actual_quat_w = receiver.data.body_quat_w[:, 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[:, jaw_body_ids] + target_quat_w = torch.cat((receiver_quat_xyzw[:, 3:], receiver_quat_xyzw[:, :3]), dim=-1) + orientation_dot = torch.abs(torch.sum(actual_quat_w * target_quat_w, dim=-1)).clamp(max=1.0) + receiver_jaw_sensor_pos_w = torch.stack( + [env.scene.sensors[name].data.pos_w[:, 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_WXYZ, dtype=np.float64), + ) + expected_channel_w = _pose_matrix( + env.scene["needle"].data.root_pos_w[0].detach().cpu().numpy(), + env.scene["needle"].data.root_quat_w[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[index, -2:].detach().cpu().tolist(), + "post_release_relative_pos_m": post_release_relative_pos[index].detach().cpu().tolist(), + "post_release_relative_quat_wxyz": 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/tools/conftest.py b/tools/conftest.py index a61c94f2c474..5dcba9ff18c3 100644 --- a/tools/conftest.py +++ b/tools/conftest.py @@ -256,6 +256,12 @@ def run_individual_tests(test_files, workspace_root, isaacsim_ci): return failed_tests, test_status +def dispatcher_return_code(failed_tests): + """Return non-zero for every failed, timed-out, or unreported test file.""" + + return 1 if failed_tests else 0 + + def pytest_sessionstart(session): """Intercept pytest startup to execute tests in the correct order.""" # Get the workspace root directory (one level up from tools) @@ -417,4 +423,7 @@ def pytest_sessionstart(session): print(summary_str) # Exit pytest after custom execution to prevent normal pytest from overwriting our report - pytest.exit("Custom test execution completed", returncode=0 if num_failing == 0 else 1) + # ``failed_tests`` also contains timed-out subprocesses and missing or + # unreadable JUnit reports. The composite CI action now trusts this exit + # status, so every dispatcher-level failure must remain non-zero. + pytest.exit("Custom test execution completed", returncode=dispatcher_return_code(failed_tests))