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